Merged with master, cleanup of varlength prototypemavlink_variable_length

838 files changed, 83225 insertions, 17640 deletions

diff --git a/.gitignore b/.gitignore
index 0372b60c8..71326517f 100644
--- a/.gitignore
+++ b/.gitignore
@@ -19,9 +19,20 @@ Archives/*
 Build/*
 core
 cscope.out
-dot.gdbinit
 Firmware.sublime-workspace
 Images/*.bin
 Images/*.px4
 mavlink/include/mavlink/v0.9/
-NuttX
\ No newline at end of file
+/nuttx-configs/px4io-v2/src/.depend
+/nuttx-configs/px4io-v2/src/Make.dep
+/nuttx-configs/px4io-v2/src/libboard.a
+/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_sorted_by_file
+.pydevproject
diff --git a/Debug/ARMv7M b/Debug/ARMv7M
index 803d96453..4c539b345 100644
--- a/Debug/ARMv7M
+++ b/Debug/ARMv7M
@@ -56,7 +56,7 @@ define vecstate
 			if $mmfsr & (1<<3)
 				printf " during exception return"
 			end
-			if $mmfsr & (1<<0)
+			if $mmfsr & (1<<1)
 				printf " during data access"
 			end
 			if $mmfsr & (1<<0)
diff --git a/Debug/Nuttx.py b/Debug/Nuttx.py
new file mode 100644
index 000000000..093edd0e0
--- /dev/null
+++ b/Debug/Nuttx.py
@@ -0,0 +1,371 @@
+# GDB/Python functions for dealing with NuttX
+
+import gdb, gdb.types
+
+class NX_register_set(object):
+	"""Copy of the registers for a given context"""
+
+	v7_regmap = {
+		'R13':		0,
+		'SP':		0,
+		'PRIORITY':	1,
+		'R4':		2,
+		'R5':		3,
+		'R6':		4,
+		'R7':		5,
+		'R8':		6,
+		'R9':		7,
+		'R10':		8,
+		'R11':		9,
+		'EXC_RETURN':	10,
+		'R0':		11,
+		'R1':		12,
+		'R2':		13,
+		'R3':		14,
+		'R12':		15,
+		'R14':		16,
+		'LR':		16,
+		'R15':		17,
+		'PC':		17,
+		'XPSR':		18,
+	}
+
+	v7em_regmap = {
+		'R13':		0,
+		'SP':		0,
+		'PRIORITY':	1,
+		'R4':		2,
+		'R5':		3,
+		'R6':		4,
+		'R7':		5,
+		'R8':		6,
+		'R9':		7,
+		'R10':		8,
+		'R11':		9,
+		'EXC_RETURN':	10,
+		'R0':		27,
+		'R1':		28,
+		'R2':		29,
+		'R3':		30,
+		'R12':		31,
+		'R14':		32,
+		'LR':		32,
+		'R15':		33,
+		'PC':		33,
+		'XPSR':		34,
+	}
+
+	regs = dict()
+
+	def __init__(self, xcpt_regs):
+		if xcpt_regs is None:
+			self.regs['R0']         = long(gdb.parse_and_eval('$r0'))
+			self.regs['R1']         = long(gdb.parse_and_eval('$r1'))
+			self.regs['R2']         = long(gdb.parse_and_eval('$r2'))
+			self.regs['R3']         = long(gdb.parse_and_eval('$r3'))
+			self.regs['R4']         = long(gdb.parse_and_eval('$r4'))
+			self.regs['R5']         = long(gdb.parse_and_eval('$r5'))
+			self.regs['R6']         = long(gdb.parse_and_eval('$r6'))
+			self.regs['R7']         = long(gdb.parse_and_eval('$r7'))
+			self.regs['R8']         = long(gdb.parse_and_eval('$r8'))
+			self.regs['R9']         = long(gdb.parse_and_eval('$r9'))
+			self.regs['R10']        = long(gdb.parse_and_eval('$r10'))
+			self.regs['R11']        = long(gdb.parse_and_eval('$r11'))
+			self.regs['R12']        = long(gdb.parse_and_eval('$r12'))
+			self.regs['R13']        = long(gdb.parse_and_eval('$r13'))
+			self.regs['SP']         = long(gdb.parse_and_eval('$sp'))
+			self.regs['R14']        = long(gdb.parse_and_eval('$r14'))
+			self.regs['LR']         = long(gdb.parse_and_eval('$lr'))
+			self.regs['R15']        = long(gdb.parse_and_eval('$r15'))
+			self.regs['PC']         = long(gdb.parse_and_eval('$pc'))
+			self.regs['XPSR']       = long(gdb.parse_and_eval('$xpsr'))
+		else:
+			for key in self.v7em_regmap.keys():
+				self.regs[key] = int(xcpt_regs[self.v7em_regmap[key]])
+
+
+	@classmethod
+	def with_xcpt_regs(cls, xcpt_regs):
+		return cls(xcpt_regs)
+
+	@classmethod
+	def for_current(cls):
+		return cls(None)
+
+	def __format__(self, format_spec):
+		return format_spec.format(
+			registers	 = self.registers
+			)
+
+	@property
+	def registers(self):
+		return self.regs
+
+
+class NX_task(object):
+	"""Reference to a NuttX task and methods for introspecting it"""
+
+	def __init__(self, tcb_ptr):
+		self._tcb = tcb_ptr.dereference()
+		self._group = self._tcb['group'].dereference()
+		self.pid = tcb_ptr['pid']
+
+	@classmethod
+	def for_tcb(cls, tcb):
+		"""return a task with the given TCB pointer"""
+		pidhash_sym = gdb.lookup_global_symbol('g_pidhash')
+		pidhash_value = pidhash_sym.value()
+		pidhash_type = pidhash_sym.type
+		for i in range(pidhash_type.range()[0],pidhash_type.range()[1]):
+			pidhash_entry = pidhash_value[i]
+			if pidhash_entry['tcb'] == tcb:
+				return cls(pidhash_entry['tcb'])
+		return None
+
+	@classmethod
+	def for_pid(cls, pid):
+		"""return a task for the given PID"""
+		pidhash_sym = gdb.lookup_global_symbol('g_pidhash')
+		pidhash_value = pidhash_sym.value()
+		pidhash_type = pidhash_sym.type
+		for i in range(pidhash_type.range()[0],pidhash_type.range()[1]):
+			pidhash_entry = pidhash_value[i]
+			if pidhash_entry['pid'] == pid:
+				return cls(pidhash_entry['tcb'])
+		return None
+
+	@staticmethod
+	def pids():
+		"""return a list of all PIDs"""
+		pidhash_sym = gdb.lookup_global_symbol('g_pidhash')
+		pidhash_value = pidhash_sym.value()
+		pidhash_type = pidhash_sym.type
+		result = []
+		for i in range(pidhash_type.range()[0],pidhash_type.range()[1]):
+			entry = pidhash_value[i]
+			pid = int(entry['pid'])
+			if pid is not -1:
+				result.append(pid)
+		return result
+
+	@staticmethod
+	def tasks():
+		"""return a list of all tasks"""
+		tasks = []
+		for pid in NX_task.pids():
+			tasks.append(NX_task.for_pid(pid))
+		return tasks
+
+	def _state_is(self, state):
+		"""tests the current state of the task against the passed-in state name"""
+		statenames = gdb.types.make_enum_dict(gdb.lookup_type('enum tstate_e'))
+		if self._tcb['task_state'] == statenames[state]:
+			return True
+		return False
+
+	@property
+	def stack_used(self):
+		"""calculate the stack used by the thread"""
+		if 'stack_used' not in self.__dict__:
+			stack_base = self._tcb['stack_alloc_ptr'].cast(gdb.lookup_type('unsigned char').pointer())
+			if stack_base == 0:
+				self.__dict__['stack_used'] = 0
+			else:
+				stack_limit = self._tcb['adj_stack_size']
+				for offset in range(0, stack_limit):
+					if stack_base[offset] != 0xff:
+						break
+				self.__dict__['stack_used'] = stack_limit - offset
+		return self.__dict__['stack_used']
+
+	@property
+	def name(self):
+		"""return the task's name"""
+		return self._tcb['name'].string()
+
+	@property
+	def state(self):
+		"""return the name of the task's current state"""
+		statenames = gdb.types.make_enum_dict(gdb.lookup_type('enum tstate_e'))
+		for name,value in statenames.iteritems():
+			if value == self._tcb['task_state']:
+				return name
+		return 'UNKNOWN'
+
+	@property
+	def waiting_for(self):
+		"""return a description of what the task is waiting for, if it is waiting"""
+		if self._state_is('TSTATE_WAIT_SEM'):
+			waitsem = self._tcb['waitsem'].dereference()
+			waitsem_holder = waitsem['holder']
+			holder = NX_task.for_tcb(waitsem_holder['htcb'])
+			if holder is not None:
+				return '{}({})'.format(waitsem.address, holder.name)
+			else:
+				return '{}(<bad holder>)'.format(waitsem.address)
+		if self._state_is('TSTATE_WAIT_SIG'):
+			return 'signal'
+		return None
+
+	@property
+	def is_waiting(self):
+		"""tests whether the task is waiting for something"""
+		if self._state_is('TSTATE_WAIT_SEM') or self._state_is('TSTATE_WAIT_SIG'):
+			return True
+
+	@property
+	def is_runnable(self):
+		"""tests whether the task is runnable"""
+		if (self._state_is('TSTATE_TASK_PENDING') or 
+			self._state_is('TSTATE_TASK_READYTORUN') or 
+			self._state_is('TSTATE_TASK_RUNNING')):
+			return True
+		return False
+
+	@property
+	def file_descriptors(self):
+		"""return a dictionary of file descriptors and inode pointers"""
+		filelist = self._group['tg_filelist']
+		filearray = filelist['fl_files']
+		result = dict()
+		for i in range(filearray.type.range()[0],filearray.type.range()[1]):
+			inode = long(filearray[i]['f_inode'])
+			if inode != 0:
+				result[i] = inode
+		return result
+
+	@property
+	def registers(self):
+		if 'registers' not in self.__dict__:
+			registers = dict()
+			if self._state_is('TSTATE_TASK_RUNNING'):
+				registers = NX_register_set.for_current().registers
+			else:
+				context = self._tcb['xcp']
+				regs = context['regs']
+				registers = NX_register_set.with_xcpt_regs(regs).registers
+
+			self.__dict__['registers'] = registers
+		return self.__dict__['registers']
+
+	def __repr__(self):
+		return "<NX_task {}>".format(self.pid)
+
+	def __str__(self):
+		return "{}:{}".format(self.pid, self.name)
+
+	def __format__(self, format_spec):
+		return format_spec.format(
+			pid              = self.pid,
+			name             = self.name,
+			state            = self.state,
+			waiting_for      = self.waiting_for,
+			stack_used       = self.stack_used,
+			stack_limit      = self._tcb['adj_stack_size'],
+			file_descriptors = self.file_descriptors,
+			registers	 = self.registers
+			)
+
+class NX_show_task (gdb.Command):
+	"""(NuttX) prints information about a task"""
+
+	def __init__(self):
+		super(NX_show_task, self).__init__("show task", gdb.COMMAND_USER)
+
+	def invoke(self, arg, from_tty):
+		t = NX_task.for_pid(int(arg))
+		if t is not None:
+			my_fmt = 'PID:{pid}  name:{name}  state:{state}\n'
+			my_fmt += '  stack used {stack_used} of {stack_limit}\n'
+			if t.is_waiting:
+				my_fmt += '  waiting for {waiting_for}\n'
+			my_fmt += '  open files: {file_descriptors}\n'
+			my_fmt += '  R0  {registers[R0]:#010x} {registers[R1]:#010x} {registers[R2]:#010x} {registers[R3]:#010x}\n'
+			my_fmt += '  R4  {registers[R4]:#010x} {registers[R5]:#010x} {registers[R6]:#010x} {registers[R7]:#010x}\n'
+			my_fmt += '  R8  {registers[R8]:#010x} {registers[R9]:#010x} {registers[R10]:#010x} {registers[R11]:#010x}\n'
+			my_fmt += '  R12 {registers[PC]:#010x}\n'
+			my_fmt += '  SP  {registers[SP]:#010x} LR {registers[LR]:#010x} PC {registers[PC]:#010x} XPSR {registers[XPSR]:#010x}\n'
+			print format(t, my_fmt)
+
+class NX_show_tasks (gdb.Command):
+	"""(NuttX) prints a list of tasks"""
+
+	def __init__(self):
+		super(NX_show_tasks, self).__init__('show tasks', gdb.COMMAND_USER)
+
+	def invoke(self, args, from_tty):
+		tasks = NX_task.tasks()
+		for t in tasks:
+			print format(t, '{pid:<2} {name:<16} {state:<20} {stack_used:>4}/{stack_limit:<4}')
+
+NX_show_task()
+NX_show_tasks()
+
+class NX_show_heap (gdb.Command):
+	"""(NuttX) prints the heap"""
+
+	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:
+			self._allocflag = 0x8000
+			self._allocnodesize = 4
+
+	def _node_allocated(self, allocnode):
+		if allocnode['preceding'] & self._allocflag:
+			return True
+		return False
+
+	def _node_size(self, allocnode):
+		return allocnode['size'] & ~self._allocflag
+
+	def _print_allocations(self, region_start, region_end):
+		if region_start >= region_end:
+			raise gdb.GdbError('heap region {} corrupt'.format(hex(region_start)))
+		nodecount = region_end - region_start
+		print 'heap {} - {}'.format(region_start, region_end)
+		cursor = 1
+		while cursor < nodecount:
+			allocnode = region_start[cursor]
+			if self._node_allocated(allocnode):
+				state = ''
+			else:
+				state = '(free)'
+			print '  {} {} {}'.format(allocnode.address + 8, self._node_size(allocnode), state)
+			cursor += self._node_size(allocnode) / self._allocnodesize
+
+	def invoke(self, args, from_tty):
+		heap = gdb.lookup_global_symbol('g_mmheap').value()
+		nregions = heap['mm_nregions']
+		region_starts = heap['mm_heapstart']
+		region_ends = heap['mm_heapend']
+		print '{} heap(s)'.format(nregions)
+		# walk the heaps
+		for i in range(0, nregions):
+			self._print_allocations(region_starts[i], region_ends[i])
+
+NX_show_heap()
+
+class NX_show_interrupted_thread (gdb.Command):
+	"""(NuttX) prints the register state of an interrupted thread when in interrupt/exception context"""
+
+	def __init__(self):
+		super(NX_show_interrupted_thread, self).__init__('show interrupted-thread', gdb.COMMAND_USER)
+
+	def invoke(self, args, from_tty):
+		regs = gdb.lookup_global_symbol('current_regs').value()
+		if regs is 0:
+			raise gdb.GdbError('not in interrupt context')
+		else:
+			registers = NX_register_set.with_xcpt_regs(regs)
+			my_fmt = ''
+			my_fmt += '  R0  {registers[R0]:#010x} {registers[R1]:#010x} {registers[R2]:#010x} {registers[R3]:#010x}\n'
+			my_fmt += '  R4  {registers[R4]:#010x} {registers[R5]:#010x} {registers[R6]:#010x} {registers[R7]:#010x}\n'
+			my_fmt += '  R8  {registers[R8]:#010x} {registers[R9]:#010x} {registers[R10]:#010x} {registers[R11]:#010x}\n'
+			my_fmt += '  R12 {registers[PC]:#010x}\n'
+			my_fmt += '  SP  {registers[SP]:#010x} LR {registers[LR]:#010x} PC {registers[PC]:#010x} XPSR {registers[XPSR]:#010x}\n'
+			print format(registers, my_fmt)
+
+NX_show_interrupted_thread()
diff --git a/Debug/PX4 b/Debug/PX4
index 085cffe43..e99228ee0 100644
--- a/Debug/PX4
+++ b/Debug/PX4
@@ -27,7 +27,7 @@ define _perf_print
 	# PC_COUNT
 	if $hdr->type == 0
 		set $count = (struct perf_ctr_count *)$hdr
-		printf "%llu events,\n", $count->event_count;
+		printf "%llu events\n", $count->event_count
 	end
 	# PC_ELPASED
 	if $hdr->type == 1
diff --git a/Debug/dot.gdbinit b/Debug/dot.gdbinit
new file mode 100644
index 000000000..d70410bc7
--- /dev/null
+++ b/Debug/dot.gdbinit
@@ -0,0 +1,13 @@
+# copy the file to .gdbinit in your Firmware tree, and adjust the path
+# below to match your system
+# For example:
+# target extended /dev/serial/by-id/usb-Black_Sphere_Technologies_Black_Magic_Probe_DDE5A1C4-if00
+# target extended /dev/ttyACM4
+
+
+monitor swdp_scan
+attach 1
+monitor vector_catch disable hard
+set mem inaccessible-by-default off
+set print pretty
+source Debug/PX4
diff --git a/Debug/olimex-px4fmu-debug.cfg b/Debug/olimex-px4fmu-debug.cfg
new file mode 100644
index 000000000..61d70070d
--- /dev/null
+++ b/Debug/olimex-px4fmu-debug.cfg
@@ -0,0 +1,22 @@
+# program a bootable device load on a mavstation
+# To run type openocd -f mavprogram.cfg
+
+source [find interface/olimex-arm-usb-ocd-h.cfg]
+source [find px4fmu-v1-board.cfg]
+
+init
+halt
+
+# Find the flash inside this CPU
+flash probe 0
+
+# erase it (128 pages) then program and exit
+
+#flash erase_sector 0 0 127
+# stm32f1x mass_erase 0 
+
+# It seems that Pat's image has a start address offset of 0x1000 but the vectors need to be at zero, so fixbin.sh moves things around
+#flash write_bank 0 fixed.bin 0
+#flash write_image firmware.elf
+#shutdown
+
diff --git a/Debug/openocd.gdbinit b/Debug/openocd.gdbinit
new file mode 100644
index 000000000..92d78b58d
--- /dev/null
+++ b/Debug/openocd.gdbinit
@@ -0,0 +1,21 @@
+target remote :3333
+
+# Don't let GDB get confused while stepping
+define hook-step
+  mon cortex_m maskisr on
+end
+define hookpost-step
+  mon cortex_m maskisr off
+end
+
+mon init
+mon stm32_init
+# mon reset halt
+mon poll
+mon cortex_m maskisr auto
+set mem inaccessible-by-default off
+set print pretty
+source Debug/PX4
+
+echo PX4 resumed, press ctrl-c to interrupt\n
+continue
diff --git a/Debug/px4fmu-v1-board.cfg b/Debug/px4fmu-v1-board.cfg
new file mode 100644
index 000000000..19b862a2d
--- /dev/null
+++ b/Debug/px4fmu-v1-board.cfg
@@ -0,0 +1,38 @@
+# The latest defaults in OpenOCD 0.7.0 are actually prettymuch correct for the px4fmu
+
+# increase working area to 32KB for faster flash programming
+set WORKAREASIZE 0x8000
+
+source [find target/stm32f4x.cfg]
+
+# needed for px4
+reset_config trst_only
+
+proc stm32_reset {} {
+  reset halt
+# FIXME - needed to init periphs on reset
+# 0x40023800 RCC base
+# 0x24 RCC_APB2 0x75933
+# RCC_APB2 0
+}
+
+# perform init that is required on each connection to the target
+proc stm32_init {} {
+
+  # force jtag to not shutdown during sleep
+  #uint32_t cr = getreg32(STM32_DBGMCU_CR);
+  #cr |= DBGMCU_CR_STANDBY | DBGMCU_CR_STOP | DBGMCU_CR_SLEEP;
+  #putreg32(cr, STM32_DBGMCU_CR);
+  mww 0xe0042004 00000007
+}
+
+# if srst is not fitted use SYSRESETREQ to
+# perform a soft reset
+cortex_m reset_config sysresetreq
+
+# Let GDB directly program elf binaries
+gdb_memory_map enable
+
+# doesn't work yet
+gdb_flash_program disable
+
diff --git a/Debug/runopenocd.sh b/Debug/runopenocd.sh
new file mode 100755
index 000000000..6258fccfb
--- /dev/null
+++ b/Debug/runopenocd.sh
@@ -0,0 +1,5 @@
+#!/bin/bash
+
+DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
+
+openocd -f interface/olimex-arm-usb-ocd-h.cfg -f $DIR/px4fmu-v1-board.cfg
diff --git a/Debug/stm32f4x.cfg b/Debug/stm32f4x.cfg
deleted file mode 100644
index 28bfcfbbb..000000000
--- a/Debug/stm32f4x.cfg
+++ /dev/null
@@ -1,64 +0,0 @@
-# script for stm32f2xxx
-
-if { [info exists CHIPNAME] } {
-   set  _CHIPNAME $CHIPNAME
-} else {
-   set  _CHIPNAME stm32f4xxx
-}
-
-if { [info exists ENDIAN] } {
-   set  _ENDIAN $ENDIAN
-} else {
-   set  _ENDIAN little
-}
-
-# Work-area is a space in RAM used for flash programming
-# By default use 64kB
-if { [info exists WORKAREASIZE] } {
-   set  _WORKAREASIZE $WORKAREASIZE
-} else {
-   set  _WORKAREASIZE 0x10000
-}
-
-# JTAG speed should be <= F_CPU/6. F_CPU after reset is 8MHz, so use F_JTAG = 1MHz
-#
-# Since we may be running of an RC oscilator, we crank down the speed a
-# bit more to be on the safe side. Perhaps superstition, but if are
-# running off a crystal, we can run closer to the limit. Note
-# that there can be a pretty wide band where things are more or less stable.
-jtag_khz 1000
-
-jtag_nsrst_delay 100
-jtag_ntrst_delay 100
-
-#jtag scan chain
-if { [info exists CPUTAPID ] } {
-   set _CPUTAPID $CPUTAPID
-} else {
-  # See STM Document RM0033
-  # Section 32.6.3 - corresponds to Cortex-M3 r2p0
-   set _CPUTAPID 0x4ba00477
-}
-jtag newtap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_CPUTAPID
-
-if { [info exists BSTAPID ] } {
-   set _BSTAPID $BSTAPID
-} else {
-  # See STM Document RM0033
-  # Section 32.6.2
-  # 
-  set _BSTAPID 0x06413041
-}
-jtag newtap $_CHIPNAME bs -irlen 5 -expected-id $_BSTAPID
-
-set _TARGETNAME $_CHIPNAME.cpu
-target create $_TARGETNAME cortex_m3 -endian $_ENDIAN -chain-position $_TARGETNAME -rtos auto
-
-$_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0
-
-set _FLASHNAME $_CHIPNAME.flash
-flash bank $_FLASHNAME stm32f2x 0 0 0 0 $_TARGETNAME
-
-# if srst is not fitted use SYSRESETREQ to
-# perform a soft reset
-cortex_m3 reset_config sysresetreq
diff --git a/Documentation/Doxyfile b/Documentation/Doxyfile
index 4802ef818..b674fbc48 100644
--- a/Documentation/Doxyfile
+++ b/Documentation/Doxyfile
@@ -429,19 +429,19 @@ SORT_BY_SCOPE_NAME     = NO
 # disable (NO) the todo list. This list is created by putting \todo
 # commands in the documentation.
 
-GENERATE_TODOLIST      = YES
+GENERATE_TODOLIST      = NO
 
 # The GENERATE_TESTLIST tag can be used to enable (YES) or
 # disable (NO) the test list. This list is created by putting \test
 # commands in the documentation.
 
-GENERATE_TESTLIST      = YES
+GENERATE_TESTLIST      = NO
 
 # The GENERATE_BUGLIST tag can be used to enable (YES) or
 # disable (NO) the bug list. This list is created by putting \bug
 # commands in the documentation.
 
-GENERATE_BUGLIST       = YES
+GENERATE_BUGLIST       = NO
 
 # The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or
 # disable (NO) the deprecated list. This list is created by putting
@@ -569,7 +569,7 @@ WARN_LOGFILE           = doxy.log
 # directories like "/usr/src/myproject". Separate the files or directories
 # with spaces.
 
-INPUT                  = ../apps
+INPUT                  = ../src
 
 # This tag can be used to specify the character encoding of the source files
 # that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is
@@ -599,9 +599,8 @@ RECURSIVE              = YES
 # excluded from the INPUT source files. This way you can easily exclude a
 # subdirectory from a directory tree whose root is specified with the INPUT tag.
 
-EXCLUDE                = ../dist/ \
-						 ../docs/html/ \
-						 html
+EXCLUDE                = ../src/lib/mathlib/CMSIS \
+			 ../src/modules/attitude_estimator_ekf/codegen
 
 # The EXCLUDE_SYMLINKS tag can be used select whether or not files or
 # directories that are symbolic links (a Unix filesystem feature) are excluded
diff --git a/Documentation/commander_app.odg b/Documentation/commander_app.odg
deleted file mode 100644
index 17459f7cf..000000000
--- a/Documentation/commander_app.odg
+++ /dev/null
diff --git a/Documentation/commander_app.png b/Documentation/commander_app.png
deleted file mode 100644
index 6503817da..000000000
--- a/Documentation/commander_app.png
+++ /dev/null
diff --git a/Documentation/control_flow.graffle b/Documentation/control_flow.graffle
index 253523110..6188c4430 100644
--- a/Documentation/control_flow.graffle
+++ b/Documentation/control_flow.graffle
@@ -5,7 +5,7 @@
 	<key>ApplicationVersion</key>
 	<array>
 		<string>com.omnigroup.OmniGraffle</string>
-		<string>139.16.0.171715</string>
+		<string>139.18.0.187838</string>
 	</array>
 	<key>CreationDate</key>
 	<string>2013-02-22 17:51:02 +0000</string>
@@ -26,9 +26,9 @@
 	<key>MasterSheets</key>
 	<array/>
 	<key>ModificationDate</key>
-	<string>2013-04-20 15:38:47 +0000</string>
+	<string>2013-08-25 05:58:40 +0000</string>
 	<key>Modifier</key>
-	<string>Lorenz Meier</string>
+	<string>Lorenz</string>
 	<key>NotesVisible</key>
 	<string>NO</string>
 	<key>OriginVisible</key>
@@ -55,7 +55,7 @@
 		<key>NSPaperName</key>
 		<array>
 			<string>string</string>
-			<string>iso-a3</string>
+			<string>BADB3137-012D-43BB-AFDE-2C0069A874AD</string>
 		</array>
 		<key>NSPaperSize</key>
 		<array>
@@ -123,7 +123,7 @@
 			<array>
 				<dict>
 					<key>Bounds</key>
-					<string>{{320.41170773935767, 646.55758982070449}, {100, 24}}</string>
+					<string>{{320.41170773792788, 646.55758667263535}, {100, 24}}</string>
 					<key>Class</key>
 					<string>ShapedGraphic</string>
 					<key>FitText</key>
@@ -171,7 +171,7 @@
 					<key>Text</key>
 					<dict>
 						<key>Text</key>
-						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -193,8 +193,8 @@
 					<integer>45</integer>
 					<key>Points</key>
 					<array>
-						<string>{283.47949897905681, 658.66217570036315}</string>
-						<string>{459.99996984645378, 658.44980851233595}</string>
+						<string>{283.47949897630235, 658.66217426345122}</string>
+						<string>{459.99996984638915, 658.4498036008282}</string>
 					</array>
 					<key>Style</key>
 					<dict>
@@ -243,7 +243,7 @@
 					<key>Text</key>
 					<dict>
 						<key>Text</key>
-						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -289,7 +289,7 @@
 						<key>Pad</key>
 						<integer>0</integer>
 						<key>Text</key>
-						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;\red68\green147\blue53;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural
@@ -340,7 +340,7 @@
 						<key>Pad</key>
 						<integer>0</integer>
 						<key>Text</key>
-						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;\red68\green147\blue53;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural
@@ -389,7 +389,7 @@
 						<key>Pad</key>
 						<integer>0</integer>
 						<key>Text</key>
-						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;\red68\green147\blue53;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -437,7 +437,7 @@
 						<key>Pad</key>
 						<integer>0</integer>
 						<key>Text</key>
-						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;\red68\green147\blue53;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -461,8 +461,8 @@
 					<integer>38</integer>
 					<key>Points</key>
 					<array>
-						<string>{584.52245687751122, 125.49999965650382}</string>
-						<string>{584.56069426576869, 157.00000090441219}</string>
+						<string>{584.50076858662646, 125.50000000000021}</string>
+						<string>{584.50207726115275, 157.00000000001324}</string>
 					</array>
 					<key>Style</key>
 					<dict>
@@ -511,7 +511,7 @@
 					<key>Text</key>
 					<dict>
 						<key>Text</key>
-						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -532,8 +532,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string>
 					<integer>34</integer>
 					<key>Points</key>
 					<array>
-						<string>{210.47275259888471, 495.87499981747118}</string>
-						<string>{210.43193555768113, 543.06250166479344}</string>
+						<string>{210.49312053369275, 495.87499999999562}</string>
+						<string>{210.48281498396668, 543.06249999958038}</string>
 					</array>
 					<key>Style</key>
 					<dict>
@@ -582,7 +582,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string>
 					<key>Text</key>
 					<dict>
 						<key>Text</key>
-						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -602,8 +602,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string>
 					<integer>32</integer>
 					<key>Points</key>
 					<array>
-						<string>{210.46968123779743, 580.06249992316259}</string>
-						<string>{210.44442316539605, 627.25000030102035}</string>
+						<string>{210.47945786724097, 580.0625}</string>
+						<string>{210.47913680605853, 627.24999999999989}</string>
 					</array>
 					<key>Style</key>
 					<dict>
@@ -637,8 +637,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string>
 					<integer>30</integer>
 					<key>Points</key>
 					<array>
-						<string>{210.48620562018775, 690.25}</string>
-						<string>{210.49612530146715, 737.4375}</string>
+						<string>{210.48620103078346, 690.25}</string>
+						<string>{210.49611383706232, 737.4375}</string>
 					</array>
 					<key>Style</key>
 					<dict>
@@ -694,7 +694,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string>
 					<key>Text</key>
 					<dict>
 						<key>Text</key>
-						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -731,7 +731,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string>
 					<key>Text</key>
 					<dict>
 						<key>Text</key>
-						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -789,7 +789,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string>
 					<key>Text</key>
 					<dict>
 						<key>Text</key>
-						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -801,7 +801,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string>
 				</dict>
 				<dict>
 					<key>Bounds</key>
-					<string>{{152.49994329565658, 397.00120984204113}, {116, 24}}</string>
+					<string>{{152.49998995675264, 397.00120984204113}, {116, 24}}</string>
 					<key>Class</key>
 					<string>ShapedGraphic</string>
 					<key>FitText</key>
@@ -849,7 +849,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string>
 					<key>Text</key>
 					<dict>
 						<key>Text</key>
-						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -877,8 +877,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string>
 					<real>-1</real>
 					<key>Points</key>
 					<array>
-						<string>{584.49994992834695, 220}</string>
-						<string>{584.4997453697481, 348.6875000000008}</string>
+						<string>{584.49999370649277, 220}</string>
+						<string>{584.49999370649277, 348.6875}</string>
 					</array>
 					<key>Style</key>
 					<dict>
@@ -971,7 +971,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string>
 					<key>Text</key>
 					<dict>
 						<key>Text</key>
-						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -991,8 +991,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string>
 					<integer>17</integer>
 					<key>Points</key>
 					<array>
-						<string>{210.49997491180866, 385.6875}</string>
-						<string>{210.49991091996927, 432.875}</string>
+						<string>{210.49998995675264, 385.6875}</string>
+						<string>{210.49998995675264, 432.875}</string>
 					</array>
 					<key>Style</key>
 					<dict>
@@ -1029,7 +1029,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string>
 					<key>Points</key>
 					<array>
 						<string>{283.49999999999511, 270.00001907176534}</string>
-						<string>{339.50000000000028, 270.00001907176534}</string>
+						<string>{339.49999999999994, 270.00001907176534}</string>
 					</array>
 					<key>Style</key>
 					<dict>
@@ -1098,8 +1098,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string>
 					<integer>13</integer>
 					<key>Points</key>
 					<array>
-						<string>{210.49171354592596, 207}</string>
-						<string>{210.49498558851923, 238.4999999999998}</string>
+						<string>{210.49209551674531, 207}</string>
+						<string>{210.49601794208507, 238.5}</string>
 					</array>
 					<key>Style</key>
 					<dict>
@@ -1133,8 +1133,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string>
 					<integer>12</integer>
 					<key>Points</key>
 					<array>
-						<string>{210.49498558851246, 138.5000000000002}</string>
-						<string>{210.48997117702493, 170.00000000001342}</string>
+						<string>{210.49601794208485, 138.5}</string>
+						<string>{210.49203588416967, 170}</string>
 					</array>
 					<key>Style</key>
 					<dict>
@@ -1190,7 +1190,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string>
 					<key>Text</key>
 					<dict>
 						<key>Text</key>
-						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -1234,7 +1234,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string>
 					<key>Text</key>
 					<dict>
 						<key>Text</key>
-						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -1278,7 +1278,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string>
 					<key>Text</key>
 					<dict>
 						<key>Text</key>
-						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -1322,7 +1322,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string>
 					<key>Text</key>
 					<dict>
 						<key>Text</key>
-						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -1360,7 +1360,7 @@ PX4IO or PX4FMU}</string>
 					<key>Text</key>
 					<dict>
 						<key>Text</key>
-						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -1395,7 +1395,7 @@ PX4IO or PX4FMU}</string>
 					<key>Text</key>
 					<dict>
 						<key>Text</key>
-						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -1430,7 +1430,7 @@ PX4IO or PX4FMU}</string>
 					<key>Text</key>
 					<dict>
 						<key>Text</key>
-						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -1675,7 +1675,7 @@ PX4IO or PX4FMU}</string>
 								<key>Pad</key>
 								<integer>0</integer>
 								<key>Text</key>
-								<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+								<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -1774,7 +1774,7 @@ PX4IO or PX4FMU}</string>
 									<key>Text</key>
 									<dict>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -2145,7 +2145,7 @@ PX4IO or PX4FMU}</string>
 									<key>Text</key>
 									<dict>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -2536,7 +2536,7 @@ PX4IO or PX4FMU}</string>
 									<key>Text</key>
 									<dict>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -2927,7 +2927,7 @@ PX4IO or PX4FMU}</string>
 									<key>Text</key>
 									<dict>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -3361,7 +3361,7 @@ PX4IO or PX4FMU}</string>
 						<key>Pad</key>
 						<integer>0</integer>
 						<key>Text</key>
-						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -3404,7 +3404,7 @@ PX4IO or PX4FMU}</string>
 									<key>Text</key>
 									<dict>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -3791,7 +3791,7 @@ PX4IO or PX4FMU}</string>
 									<key>Text</key>
 									<dict>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -4178,7 +4178,7 @@ PX4IO or PX4FMU}</string>
 									<key>Text</key>
 									<dict>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -4565,7 +4565,7 @@ PX4IO or PX4FMU}</string>
 									<key>Text</key>
 									<dict>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -4963,7 +4963,7 @@ PX4IO or PX4FMU}</string>
 								<key>Pad</key>
 								<integer>0</integer>
 								<key>Text</key>
-								<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+								<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -5062,7 +5062,7 @@ PX4IO or PX4FMU}</string>
 									<key>Text</key>
 									<dict>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -5421,7 +5421,7 @@ PX4IO or PX4FMU}</string>
 									<key>Text</key>
 									<dict>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -5780,7 +5780,7 @@ PX4IO or PX4FMU}</string>
 									<key>Text</key>
 									<dict>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -6175,7 +6175,7 @@ PX4IO or PX4FMU}</string>
 									<key>Text</key>
 									<dict>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -6660,7 +6660,7 @@ PX4IO or PX4FMU}</string>
 									<key>Text</key>
 									<dict>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -7019,7 +7019,7 @@ PX4IO or PX4FMU}</string>
 									<key>Text</key>
 									<dict>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -7406,7 +7406,7 @@ PX4IO or PX4FMU}</string>
 									<key>Text</key>
 									<dict>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -7793,7 +7793,7 @@ PX4IO or PX4FMU}</string>
 									<key>Text</key>
 									<dict>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -8300,7 +8300,7 @@ PX4IO or PX4FMU}</string>
 						<key>Pad</key>
 						<integer>0</integer>
 						<key>Text</key>
-						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -8404,7 +8404,7 @@ PX4IO or PX4FMU}</string>
 									<key>Text</key>
 									<dict>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -8795,7 +8795,7 @@ PX4IO or PX4FMU}</string>
 									<key>Text</key>
 									<dict>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -9166,7 +9166,7 @@ PX4IO or PX4FMU}</string>
 									<key>Text</key>
 									<dict>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -9537,7 +9537,7 @@ PX4IO or PX4FMU}</string>
 									<key>Text</key>
 									<dict>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -9928,7 +9928,7 @@ PX4IO or PX4FMU}</string>
 									<key>Text</key>
 									<dict>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -10367,7 +10367,7 @@ PX4IO or PX4FMU}</string>
 									<key>Text</key>
 									<dict>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -10779,7 +10779,7 @@ PX4IO or PX4FMU}</string>
 						<key>Pad</key>
 						<integer>0</integer>
 						<key>Text</key>
-						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -10883,7 +10883,7 @@ PX4IO or PX4FMU}</string>
 									<key>Text</key>
 									<dict>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -11270,7 +11270,7 @@ PX4IO or PX4FMU}</string>
 									<key>Text</key>
 									<dict>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -11629,7 +11629,7 @@ PX4IO or PX4FMU}</string>
 									<key>Text</key>
 									<dict>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -11988,7 +11988,7 @@ PX4IO or PX4FMU}</string>
 									<key>Text</key>
 									<dict>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -12375,7 +12375,7 @@ PX4IO or PX4FMU}</string>
 									<key>Text</key>
 									<dict>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -12810,7 +12810,7 @@ PX4IO or PX4FMU}</string>
 									<key>Text</key>
 									<dict>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -13263,6 +13263,1697 @@ PX4IO or PX4FMU}</string>
 					<key>Graphics</key>
 					<array>
 						<dict>
+							<key>Bounds</key>
+							<string>{{281.4621559838219, 891.87422762618871}, {48, 14}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>FitText</key>
+							<string>YES</string>
+							<key>Flow</key>
+							<string>Resize</string>
+							<key>ID</key>
+							<integer>1534</integer>
+							<key>Shape</key>
+							<string>Rectangle</string>
+							<key>Style</key>
+							<dict>
+								<key>fill</key>
+								<dict>
+									<key>Draws</key>
+									<string>NO</string>
+								</dict>
+								<key>shadow</key>
+								<dict>
+									<key>Draws</key>
+									<string>NO</string>
+								</dict>
+								<key>stroke</key>
+								<dict>
+									<key>Draws</key>
+									<string>NO</string>
+								</dict>
+							</dict>
+							<key>Text</key>
+							<dict>
+								<key>Pad</key>
+								<integer>0</integer>
+								<key>Text</key>
+								<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\b\fs24 \cf0 QUAD
+\b0  H}</string>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+							<key>Wrap</key>
+							<string>NO</string>
+						</dict>
+						<dict>
+							<key>Class</key>
+							<string>Group</string>
+							<key>Graphics</key>
+							<array>
+								<dict>
+									<key>Bounds</key>
+									<string>{{282.10284531231378, 729.4992324185539}, {50.666667938232422, 36}}</string>
+									<key>Class</key>
+									<string>ShapedGraphic</string>
+									<key>ID</key>
+									<integer>1536</integer>
+									<key>Rotation</key>
+									<real>270</real>
+									<key>Shape</key>
+									<string>HorizontalTriangle</string>
+									<key>Style</key>
+									<dict>
+										<key>fill</key>
+										<dict>
+											<key>Color</key>
+											<dict>
+												<key>b</key>
+												<string>0.13195</string>
+												<key>g</key>
+												<string>0.165669</string>
+												<key>r</key>
+												<string>1</string>
+											</dict>
+										</dict>
+									</dict>
+									<key>Text</key>
+									<dict>
+										<key>VerticalPad</key>
+										<integer>0</integer>
+									</dict>
+								</dict>
+								<dict>
+									<key>Bounds</key>
+									<string>{{271.43617794923421, 713.47324050519865}, {72, 72}}</string>
+									<key>Class</key>
+									<string>ShapedGraphic</string>
+									<key>ID</key>
+									<integer>1537</integer>
+									<key>Shape</key>
+									<string>Rectangle</string>
+									<key>Style</key>
+									<dict>
+										<key>stroke</key>
+										<dict>
+											<key>CornerRadius</key>
+											<real>9</real>
+										</dict>
+									</dict>
+								</dict>
+							</array>
+							<key>ID</key>
+							<integer>1535</integer>
+							<key>Rotation</key>
+							<real>315</real>
+						</dict>
+						<dict>
+							<key>Class</key>
+							<string>Group</string>
+							<key>Graphics</key>
+							<array>
+								<dict>
+									<key>Bounds</key>
+									<string>{{387.95096684220994, 833.99511602139876}, {24, 24}}</string>
+									<key>Class</key>
+									<string>ShapedGraphic</string>
+									<key>ID</key>
+									<integer>1539</integer>
+									<key>Shape</key>
+									<string>Circle</string>
+									<key>Style</key>
+									<dict>
+										<key>shadow</key>
+										<dict>
+											<key>Draws</key>
+											<string>NO</string>
+										</dict>
+									</dict>
+									<key>Text</key>
+									<dict>
+										<key>Text</key>
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 4}</string>
+										<key>VerticalPad</key>
+										<integer>0</integer>
+									</dict>
+								</dict>
+								<dict>
+									<key>Class</key>
+									<string>Group</string>
+									<key>Graphics</key>
+									<array>
+										<dict>
+											<key>Class</key>
+											<string>LineGraphic</string>
+											<key>ID</key>
+											<integer>1541</integer>
+											<key>Points</key>
+											<array>
+												<string>{447.56635253821332, 822.0678021003348}</string>
+												<string>{445.35664384700522, 823.21685061976291}</string>
+											</array>
+											<key>Style</key>
+											<dict>
+												<key>stroke</key>
+												<dict>
+													<key>HeadArrow</key>
+													<string>0</string>
+													<key>Legacy</key>
+													<true/>
+													<key>TailArrow</key>
+													<string>0</string>
+												</dict>
+											</dict>
+										</dict>
+										<dict>
+											<key>Bounds</key>
+											<string>{{359.66328626413321, 875.75030562593793}, {16, 18.5}}</string>
+											<key>Class</key>
+											<string>ShapedGraphic</string>
+											<key>HFlip</key>
+											<string>YES</string>
+											<key>ID</key>
+											<integer>1542</integer>
+											<key>Rotation</key>
+											<real>45</real>
+											<key>Shape</key>
+											<string>HorizontalTriangle</string>
+											<key>Style</key>
+											<dict>
+												<key>shadow</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+											</dict>
+											<key>Text</key>
+											<dict>
+												<key>VerticalPad</key>
+												<integer>0</integer>
+											</dict>
+										</dict>
+										<dict>
+											<key>Bounds</key>
+											<string>{{349.2634215034181, 795.53620935597417}, {101.5, 101.5}}</string>
+											<key>Class</key>
+											<string>ShapedGraphic</string>
+											<key>HFlip</key>
+											<string>YES</string>
+											<key>ID</key>
+											<integer>1543</integer>
+											<key>Rotation</key>
+											<real>135</real>
+											<key>Shape</key>
+											<string>AdjustableArc</string>
+											<key>ShapeData</key>
+											<dict>
+												<key>endAngle</key>
+												<real>72</real>
+												<key>startAngle</key>
+												<real>281</real>
+											</dict>
+											<key>Style</key>
+											<dict>
+												<key>fill</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+												<key>shadow</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+											</dict>
+											<key>Text</key>
+											<dict>
+												<key>VerticalPad</key>
+												<integer>0</integer>
+											</dict>
+											<key>TextRelativeArea</key>
+											<string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+										</dict>
+										<dict>
+											<key>Bounds</key>
+											<string>{{346.5134215034181, 792.78620935597417}, {107, 107}}</string>
+											<key>Class</key>
+											<string>ShapedGraphic</string>
+											<key>HFlip</key>
+											<string>YES</string>
+											<key>ID</key>
+											<integer>1544</integer>
+											<key>Rotation</key>
+											<real>135</real>
+											<key>Shape</key>
+											<string>AdjustableArc</string>
+											<key>ShapeData</key>
+											<dict>
+												<key>endAngle</key>
+												<real>72</real>
+												<key>startAngle</key>
+												<real>281</real>
+											</dict>
+											<key>Style</key>
+											<dict>
+												<key>fill</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+												<key>shadow</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+											</dict>
+											<key>Text</key>
+											<dict>
+												<key>VerticalPad</key>
+												<integer>0</integer>
+											</dict>
+											<key>TextRelativeArea</key>
+											<string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+										</dict>
+									</array>
+									<key>ID</key>
+									<integer>1540</integer>
+									<key>Rotation</key>
+									<real>315</real>
+								</dict>
+								<dict>
+									<key>Class</key>
+									<string>Group</string>
+									<key>Graphics</key>
+									<array>
+										<dict>
+											<key>Class</key>
+											<string>LineGraphic</string>
+											<key>ID</key>
+											<integer>1546</integer>
+											<key>Points</key>
+											<array>
+												<string>{352.51941963025371, 870.09214125245364}</string>
+												<string>{354.7291283214617, 868.94309273302531}</string>
+											</array>
+											<key>Style</key>
+											<dict>
+												<key>stroke</key>
+												<dict>
+													<key>HeadArrow</key>
+													<string>0</string>
+													<key>Legacy</key>
+													<true/>
+													<key>TailArrow</key>
+													<string>0</string>
+												</dict>
+											</dict>
+										</dict>
+										<dict>
+											<key>Bounds</key>
+											<string>{{424.42248590433371, 797.90963772685075}, {16, 18.5}}</string>
+											<key>Class</key>
+											<string>ShapedGraphic</string>
+											<key>HFlip</key>
+											<string>YES</string>
+											<key>ID</key>
+											<integer>1547</integer>
+											<key>Rotation</key>
+											<real>225</real>
+											<key>Shape</key>
+											<string>HorizontalTriangle</string>
+											<key>Style</key>
+											<dict>
+												<key>shadow</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+											</dict>
+											<key>Text</key>
+											<dict>
+												<key>VerticalPad</key>
+												<integer>0</integer>
+											</dict>
+										</dict>
+										<dict>
+											<key>Bounds</key>
+											<string>{{349.32235066504887, 795.12373399681428}, {101.5, 101.5}}</string>
+											<key>Class</key>
+											<string>ShapedGraphic</string>
+											<key>HFlip</key>
+											<string>YES</string>
+											<key>ID</key>
+											<integer>1548</integer>
+											<key>Rotation</key>
+											<real>315</real>
+											<key>Shape</key>
+											<string>AdjustableArc</string>
+											<key>ShapeData</key>
+											<dict>
+												<key>endAngle</key>
+												<real>72</real>
+												<key>startAngle</key>
+												<real>281</real>
+											</dict>
+											<key>Style</key>
+											<dict>
+												<key>fill</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+												<key>shadow</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+											</dict>
+											<key>Text</key>
+											<dict>
+												<key>VerticalPad</key>
+												<integer>0</integer>
+											</dict>
+											<key>TextRelativeArea</key>
+											<string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+										</dict>
+										<dict>
+											<key>Bounds</key>
+											<string>{{346.57235066504887, 792.37373399681428}, {107, 107}}</string>
+											<key>Class</key>
+											<string>ShapedGraphic</string>
+											<key>HFlip</key>
+											<string>YES</string>
+											<key>ID</key>
+											<integer>1549</integer>
+											<key>Rotation</key>
+											<real>315</real>
+											<key>Shape</key>
+											<string>AdjustableArc</string>
+											<key>ShapeData</key>
+											<dict>
+												<key>endAngle</key>
+												<real>72</real>
+												<key>startAngle</key>
+												<real>281</real>
+											</dict>
+											<key>Style</key>
+											<dict>
+												<key>fill</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+												<key>shadow</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+											</dict>
+											<key>Text</key>
+											<dict>
+												<key>VerticalPad</key>
+												<integer>0</integer>
+											</dict>
+											<key>TextRelativeArea</key>
+											<string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+										</dict>
+									</array>
+									<key>ID</key>
+									<integer>1545</integer>
+									<key>Rotation</key>
+									<real>315</real>
+								</dict>
+								<dict>
+									<key>Bounds</key>
+									<string>{{349.03928365032027, 795.08342417943641}, {101.82337649086284, 101.82337649086284}}</string>
+									<key>Class</key>
+									<string>ShapedGraphic</string>
+									<key>ID</key>
+									<integer>1550</integer>
+									<key>Rotation</key>
+									<real>315</real>
+									<key>Shape</key>
+									<string>Circle</string>
+									<key>Style</key>
+									<dict>
+										<key>fill</key>
+										<dict>
+											<key>Color</key>
+											<dict>
+												<key>a</key>
+												<string>0.78</string>
+												<key>b</key>
+												<string>0.234512</string>
+												<key>g</key>
+												<string>0.673128</string>
+												<key>r</key>
+												<string>0.148947</string>
+											</dict>
+										</dict>
+										<key>shadow</key>
+										<dict>
+											<key>Draws</key>
+											<string>NO</string>
+										</dict>
+										<key>stroke</key>
+										<dict>
+											<key>Draws</key>
+											<string>NO</string>
+										</dict>
+									</dict>
+									<key>Text</key>
+									<dict>
+										<key>VerticalPad</key>
+										<integer>0</integer>
+									</dict>
+								</dict>
+							</array>
+							<key>ID</key>
+							<integer>1538</integer>
+							<key>Rotation</key>
+							<real>315</real>
+						</dict>
+						<dict>
+							<key>Class</key>
+							<string>Group</string>
+							<key>Graphics</key>
+							<array>
+								<dict>
+									<key>Bounds</key>
+									<string>{{202.98738428569231, 833.92908910567212}, {24, 24}}</string>
+									<key>Class</key>
+									<string>ShapedGraphic</string>
+									<key>ID</key>
+									<integer>1552</integer>
+									<key>Shape</key>
+									<string>Circle</string>
+									<key>Style</key>
+									<dict>
+										<key>shadow</key>
+										<dict>
+											<key>Draws</key>
+											<string>NO</string>
+										</dict>
+									</dict>
+									<key>Text</key>
+									<dict>
+										<key>Text</key>
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 2}</string>
+										<key>VerticalPad</key>
+										<integer>0</integer>
+									</dict>
+									<key>VFlip</key>
+									<string>YES</string>
+								</dict>
+								<dict>
+									<key>Class</key>
+									<string>Group</string>
+									<key>Graphics</key>
+									<array>
+										<dict>
+											<key>Class</key>
+											<string>LineGraphic</string>
+											<key>ID</key>
+											<integer>1554</integer>
+											<key>Points</key>
+											<array>
+												<string>{167.72554658547227, 821.64822718882488}</string>
+												<string>{169.93525527668044, 822.79727570825287}</string>
+											</array>
+											<key>Style</key>
+											<dict>
+												<key>stroke</key>
+												<dict>
+													<key>HeadArrow</key>
+													<string>0</string>
+													<key>Legacy</key>
+													<true/>
+													<key>TailArrow</key>
+													<string>0</string>
+												</dict>
+											</dict>
+										</dict>
+										<dict>
+											<key>Bounds</key>
+											<string>{{239.62861285955245, 875.33073071442777}, {16, 18.5}}</string>
+											<key>Class</key>
+											<string>ShapedGraphic</string>
+											<key>HFlip</key>
+											<string>YES</string>
+											<key>ID</key>
+											<integer>1555</integer>
+											<key>Rotation</key>
+											<real>135</real>
+											<key>Shape</key>
+											<string>HorizontalTriangle</string>
+											<key>Style</key>
+											<dict>
+												<key>shadow</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+											</dict>
+											<key>Text</key>
+											<dict>
+												<key>VerticalPad</key>
+												<integer>0</integer>
+											</dict>
+											<key>VFlip</key>
+											<string>YES</string>
+										</dict>
+										<dict>
+											<key>Bounds</key>
+											<string>{{164.52847762026795, 795.1166344444639}, {101.5, 101.5}}</string>
+											<key>Class</key>
+											<string>ShapedGraphic</string>
+											<key>HFlip</key>
+											<string>YES</string>
+											<key>ID</key>
+											<integer>1556</integer>
+											<key>Rotation</key>
+											<real>45</real>
+											<key>Shape</key>
+											<string>AdjustableArc</string>
+											<key>ShapeData</key>
+											<dict>
+												<key>endAngle</key>
+												<real>72</real>
+												<key>startAngle</key>
+												<real>281</real>
+											</dict>
+											<key>Style</key>
+											<dict>
+												<key>fill</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+												<key>shadow</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+											</dict>
+											<key>Text</key>
+											<dict>
+												<key>VerticalPad</key>
+												<integer>0</integer>
+											</dict>
+											<key>TextRelativeArea</key>
+											<string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+											<key>VFlip</key>
+											<string>YES</string>
+										</dict>
+										<dict>
+											<key>Bounds</key>
+											<string>{{161.77847762026795, 792.3666344444639}, {107, 107}}</string>
+											<key>Class</key>
+											<string>ShapedGraphic</string>
+											<key>HFlip</key>
+											<string>YES</string>
+											<key>ID</key>
+											<integer>1557</integer>
+											<key>Rotation</key>
+											<real>45</real>
+											<key>Shape</key>
+											<string>AdjustableArc</string>
+											<key>ShapeData</key>
+											<dict>
+												<key>endAngle</key>
+												<real>72</real>
+												<key>startAngle</key>
+												<real>281</real>
+											</dict>
+											<key>Style</key>
+											<dict>
+												<key>fill</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+												<key>shadow</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+											</dict>
+											<key>Text</key>
+											<dict>
+												<key>VerticalPad</key>
+												<integer>0</integer>
+											</dict>
+											<key>TextRelativeArea</key>
+											<string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+											<key>VFlip</key>
+											<string>YES</string>
+										</dict>
+									</array>
+									<key>ID</key>
+									<integer>1553</integer>
+									<key>Rotation</key>
+									<real>225</real>
+									<key>VFlip</key>
+									<string>YES</string>
+								</dict>
+								<dict>
+									<key>Class</key>
+									<string>Group</string>
+									<key>Graphics</key>
+									<array>
+										<dict>
+											<key>Class</key>
+											<string>LineGraphic</string>
+											<key>ID</key>
+											<integer>1559</integer>
+											<key>Points</key>
+											<array>
+												<string>{262.41893329590295, 870.02611253847215}</string>
+												<string>{260.20922460469507, 868.87706401904404}</string>
+											</array>
+											<key>Style</key>
+											<dict>
+												<key>stroke</key>
+												<dict>
+													<key>HeadArrow</key>
+													<string>0</string>
+													<key>Legacy</key>
+													<true/>
+													<key>TailArrow</key>
+													<string>0</string>
+												</dict>
+											</dict>
+										</dict>
+										<dict>
+											<key>Bounds</key>
+											<string>{{174.51586702182317, 797.84360901286959}, {16, 18.5}}</string>
+											<key>Class</key>
+											<string>ShapedGraphic</string>
+											<key>HFlip</key>
+											<string>YES</string>
+											<key>ID</key>
+											<integer>1560</integer>
+											<key>Rotation</key>
+											<real>315</real>
+											<key>Shape</key>
+											<string>HorizontalTriangle</string>
+											<key>Style</key>
+											<dict>
+												<key>shadow</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+											</dict>
+											<key>Text</key>
+											<dict>
+												<key>VerticalPad</key>
+												<integer>0</integer>
+											</dict>
+											<key>VFlip</key>
+											<string>YES</string>
+										</dict>
+										<dict>
+											<key>Bounds</key>
+											<string>{{164.11600226110755, 795.05770528283301}, {101.5, 101.5}}</string>
+											<key>Class</key>
+											<string>ShapedGraphic</string>
+											<key>HFlip</key>
+											<string>YES</string>
+											<key>ID</key>
+											<integer>1561</integer>
+											<key>Rotation</key>
+											<real>225</real>
+											<key>Shape</key>
+											<string>AdjustableArc</string>
+											<key>ShapeData</key>
+											<dict>
+												<key>endAngle</key>
+												<real>72</real>
+												<key>startAngle</key>
+												<real>281</real>
+											</dict>
+											<key>Style</key>
+											<dict>
+												<key>fill</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+												<key>shadow</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+											</dict>
+											<key>Text</key>
+											<dict>
+												<key>VerticalPad</key>
+												<integer>0</integer>
+											</dict>
+											<key>TextRelativeArea</key>
+											<string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+											<key>VFlip</key>
+											<string>YES</string>
+										</dict>
+										<dict>
+											<key>Bounds</key>
+											<string>{{161.36600226110755, 792.30770528283301}, {107, 107}}</string>
+											<key>Class</key>
+											<string>ShapedGraphic</string>
+											<key>HFlip</key>
+											<string>YES</string>
+											<key>ID</key>
+											<integer>1562</integer>
+											<key>Rotation</key>
+											<real>225</real>
+											<key>Shape</key>
+											<string>AdjustableArc</string>
+											<key>ShapeData</key>
+											<dict>
+												<key>endAngle</key>
+												<real>72</real>
+												<key>startAngle</key>
+												<real>281</real>
+											</dict>
+											<key>Style</key>
+											<dict>
+												<key>fill</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+												<key>shadow</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+											</dict>
+											<key>Text</key>
+											<dict>
+												<key>VerticalPad</key>
+												<integer>0</integer>
+											</dict>
+											<key>TextRelativeArea</key>
+											<string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+											<key>VFlip</key>
+											<string>YES</string>
+										</dict>
+									</array>
+									<key>ID</key>
+									<integer>1558</integer>
+									<key>Rotation</key>
+									<real>225</real>
+									<key>VFlip</key>
+									<string>YES</string>
+								</dict>
+								<dict>
+									<key>Bounds</key>
+									<string>{{164.07569244372991, 795.01739580669891}, {101.82337649086284, 101.82337649086284}}</string>
+									<key>Class</key>
+									<string>ShapedGraphic</string>
+									<key>ID</key>
+									<integer>1563</integer>
+									<key>Rotation</key>
+									<real>225</real>
+									<key>Shape</key>
+									<string>Circle</string>
+									<key>Style</key>
+									<dict>
+										<key>fill</key>
+										<dict>
+											<key>Color</key>
+											<dict>
+												<key>a</key>
+												<string>0.78</string>
+												<key>b</key>
+												<string>0.673128</string>
+												<key>g</key>
+												<string>0.534129</string>
+												<key>r</key>
+												<string>0.217952</string>
+											</dict>
+										</dict>
+										<key>shadow</key>
+										<dict>
+											<key>Draws</key>
+											<string>NO</string>
+										</dict>
+										<key>stroke</key>
+										<dict>
+											<key>Draws</key>
+											<string>NO</string>
+										</dict>
+									</dict>
+									<key>Text</key>
+									<dict>
+										<key>VerticalPad</key>
+										<integer>0</integer>
+									</dict>
+									<key>VFlip</key>
+									<string>YES</string>
+								</dict>
+							</array>
+							<key>ID</key>
+							<integer>1551</integer>
+							<key>Rotation</key>
+							<real>315</real>
+						</dict>
+						<dict>
+							<key>Class</key>
+							<string>Group</string>
+							<key>Graphics</key>
+							<array>
+								<dict>
+									<key>Bounds</key>
+									<string>{{424.02444829866442, 666.20122319568361}, {24, 24}}</string>
+									<key>Class</key>
+									<string>ShapedGraphic</string>
+									<key>ID</key>
+									<integer>1565</integer>
+									<key>Shape</key>
+									<string>Circle</string>
+									<key>Style</key>
+									<dict>
+										<key>shadow</key>
+										<dict>
+											<key>Draws</key>
+											<string>NO</string>
+										</dict>
+									</dict>
+									<key>Text</key>
+									<dict>
+										<key>Text</key>
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 1}</string>
+										<key>VerticalPad</key>
+										<integer>0</integer>
+									</dict>
+									<key>VFlip</key>
+									<string>YES</string>
+								</dict>
+								<dict>
+									<key>Class</key>
+									<string>Group</string>
+									<key>Graphics</key>
+									<array>
+										<dict>
+											<key>Class</key>
+											<string>LineGraphic</string>
+											<key>ID</key>
+											<integer>1567</integer>
+											<key>Points</key>
+											<array>
+												<string>{388.76261059844421, 653.92036127883637}</string>
+												<string>{390.97231928965209, 655.06940979826459}</string>
+											</array>
+											<key>Style</key>
+											<dict>
+												<key>stroke</key>
+												<dict>
+													<key>HeadArrow</key>
+													<string>0</string>
+													<key>Legacy</key>
+													<true/>
+													<key>TailArrow</key>
+													<string>0</string>
+												</dict>
+											</dict>
+										</dict>
+										<dict>
+											<key>Bounds</key>
+											<string>{{460.66567687252439, 707.60286480443915}, {16, 18.5}}</string>
+											<key>Class</key>
+											<string>ShapedGraphic</string>
+											<key>HFlip</key>
+											<string>YES</string>
+											<key>ID</key>
+											<integer>1568</integer>
+											<key>Rotation</key>
+											<real>135</real>
+											<key>Shape</key>
+											<string>HorizontalTriangle</string>
+											<key>Style</key>
+											<dict>
+												<key>shadow</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+											</dict>
+											<key>Text</key>
+											<dict>
+												<key>VerticalPad</key>
+												<integer>0</integer>
+											</dict>
+											<key>VFlip</key>
+											<string>YES</string>
+										</dict>
+										<dict>
+											<key>Bounds</key>
+											<string>{{385.56554163323983, 627.3887685344755}, {101.5, 101.5}}</string>
+											<key>Class</key>
+											<string>ShapedGraphic</string>
+											<key>HFlip</key>
+											<string>YES</string>
+											<key>ID</key>
+											<integer>1569</integer>
+											<key>Rotation</key>
+											<real>45</real>
+											<key>Shape</key>
+											<string>AdjustableArc</string>
+											<key>ShapeData</key>
+											<dict>
+												<key>endAngle</key>
+												<real>72</real>
+												<key>startAngle</key>
+												<real>281</real>
+											</dict>
+											<key>Style</key>
+											<dict>
+												<key>fill</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+												<key>shadow</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+											</dict>
+											<key>Text</key>
+											<dict>
+												<key>VerticalPad</key>
+												<integer>0</integer>
+											</dict>
+											<key>TextRelativeArea</key>
+											<string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+											<key>VFlip</key>
+											<string>YES</string>
+										</dict>
+										<dict>
+											<key>Bounds</key>
+											<string>{{382.81554163323983, 624.6387685344755}, {107, 107}}</string>
+											<key>Class</key>
+											<string>ShapedGraphic</string>
+											<key>HFlip</key>
+											<string>YES</string>
+											<key>ID</key>
+											<integer>1570</integer>
+											<key>Rotation</key>
+											<real>45</real>
+											<key>Shape</key>
+											<string>AdjustableArc</string>
+											<key>ShapeData</key>
+											<dict>
+												<key>endAngle</key>
+												<real>72</real>
+												<key>startAngle</key>
+												<real>281</real>
+											</dict>
+											<key>Style</key>
+											<dict>
+												<key>fill</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+												<key>shadow</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+											</dict>
+											<key>Text</key>
+											<dict>
+												<key>VerticalPad</key>
+												<integer>0</integer>
+											</dict>
+											<key>TextRelativeArea</key>
+											<string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+											<key>VFlip</key>
+											<string>YES</string>
+										</dict>
+									</array>
+									<key>ID</key>
+									<integer>1566</integer>
+									<key>Rotation</key>
+									<real>225</real>
+									<key>VFlip</key>
+									<string>YES</string>
+								</dict>
+								<dict>
+									<key>Class</key>
+									<string>Group</string>
+									<key>Graphics</key>
+									<array>
+										<dict>
+											<key>Class</key>
+											<string>LineGraphic</string>
+											<key>ID</key>
+											<integer>1572</integer>
+											<key>Points</key>
+											<array>
+												<string>{483.455997308875, 702.29824662848364}</string>
+												<string>{481.24628861766723, 701.14919810905553}</string>
+											</array>
+											<key>Style</key>
+											<dict>
+												<key>stroke</key>
+												<dict>
+													<key>HeadArrow</key>
+													<string>0</string>
+													<key>Legacy</key>
+													<true/>
+													<key>TailArrow</key>
+													<string>0</string>
+												</dict>
+											</dict>
+										</dict>
+										<dict>
+											<key>Bounds</key>
+											<string>{{395.55293103479517, 630.11574310288108}, {16, 18.5}}</string>
+											<key>Class</key>
+											<string>ShapedGraphic</string>
+											<key>HFlip</key>
+											<string>YES</string>
+											<key>ID</key>
+											<integer>1573</integer>
+											<key>Rotation</key>
+											<real>315</real>
+											<key>Shape</key>
+											<string>HorizontalTriangle</string>
+											<key>Style</key>
+											<dict>
+												<key>shadow</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+											</dict>
+											<key>Text</key>
+											<dict>
+												<key>VerticalPad</key>
+												<integer>0</integer>
+											</dict>
+											<key>VFlip</key>
+											<string>YES</string>
+										</dict>
+										<dict>
+											<key>Bounds</key>
+											<string>{{385.15306627407972, 627.32983937284462}, {101.5, 101.5}}</string>
+											<key>Class</key>
+											<string>ShapedGraphic</string>
+											<key>HFlip</key>
+											<string>YES</string>
+											<key>ID</key>
+											<integer>1574</integer>
+											<key>Rotation</key>
+											<real>225</real>
+											<key>Shape</key>
+											<string>AdjustableArc</string>
+											<key>ShapeData</key>
+											<dict>
+												<key>endAngle</key>
+												<real>72</real>
+												<key>startAngle</key>
+												<real>281</real>
+											</dict>
+											<key>Style</key>
+											<dict>
+												<key>fill</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+												<key>shadow</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+											</dict>
+											<key>Text</key>
+											<dict>
+												<key>VerticalPad</key>
+												<integer>0</integer>
+											</dict>
+											<key>TextRelativeArea</key>
+											<string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+											<key>VFlip</key>
+											<string>YES</string>
+										</dict>
+										<dict>
+											<key>Bounds</key>
+											<string>{{382.40306627407972, 624.57983937284462}, {107, 107}}</string>
+											<key>Class</key>
+											<string>ShapedGraphic</string>
+											<key>HFlip</key>
+											<string>YES</string>
+											<key>ID</key>
+											<integer>1575</integer>
+											<key>Rotation</key>
+											<real>225</real>
+											<key>Shape</key>
+											<string>AdjustableArc</string>
+											<key>ShapeData</key>
+											<dict>
+												<key>endAngle</key>
+												<real>72</real>
+												<key>startAngle</key>
+												<real>281</real>
+											</dict>
+											<key>Style</key>
+											<dict>
+												<key>fill</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+												<key>shadow</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+											</dict>
+											<key>Text</key>
+											<dict>
+												<key>VerticalPad</key>
+												<integer>0</integer>
+											</dict>
+											<key>TextRelativeArea</key>
+											<string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+											<key>VFlip</key>
+											<string>YES</string>
+										</dict>
+									</array>
+									<key>ID</key>
+									<integer>1571</integer>
+									<key>Rotation</key>
+									<real>225</real>
+									<key>VFlip</key>
+									<string>YES</string>
+								</dict>
+								<dict>
+									<key>Bounds</key>
+									<string>{{385.11275645670196, 627.28952989671052}, {101.82337649086284, 101.82337649086284}}</string>
+									<key>Class</key>
+									<string>ShapedGraphic</string>
+									<key>ID</key>
+									<integer>1576</integer>
+									<key>Rotation</key>
+									<real>225</real>
+									<key>Shape</key>
+									<string>Circle</string>
+									<key>Style</key>
+									<dict>
+										<key>fill</key>
+										<dict>
+											<key>Color</key>
+											<dict>
+												<key>a</key>
+												<string>0.78</string>
+												<key>b</key>
+												<string>0.673128</string>
+												<key>g</key>
+												<string>0.534129</string>
+												<key>r</key>
+												<string>0.217952</string>
+											</dict>
+										</dict>
+										<key>shadow</key>
+										<dict>
+											<key>Draws</key>
+											<string>NO</string>
+										</dict>
+										<key>stroke</key>
+										<dict>
+											<key>Draws</key>
+											<string>NO</string>
+										</dict>
+									</dict>
+									<key>Text</key>
+									<dict>
+										<key>VerticalPad</key>
+										<integer>0</integer>
+									</dict>
+									<key>VFlip</key>
+									<string>YES</string>
+								</dict>
+							</array>
+							<key>ID</key>
+							<integer>1564</integer>
+							<key>Rotation</key>
+							<real>315</real>
+						</dict>
+						<dict>
+							<key>Class</key>
+							<string>Group</string>
+							<key>Graphics</key>
+							<array>
+								<dict>
+									<key>Bounds</key>
+									<string>{{170.01738471163631, 670.02445557113811}, {24, 24}}</string>
+									<key>Class</key>
+									<string>ShapedGraphic</string>
+									<key>ID</key>
+									<integer>1578</integer>
+									<key>Shape</key>
+									<string>Circle</string>
+									<key>Style</key>
+									<dict>
+										<key>shadow</key>
+										<dict>
+											<key>Draws</key>
+											<string>NO</string>
+										</dict>
+									</dict>
+									<key>Text</key>
+									<dict>
+										<key>Text</key>
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 3}</string>
+										<key>VerticalPad</key>
+										<integer>0</integer>
+									</dict>
+								</dict>
+								<dict>
+									<key>Class</key>
+									<string>Group</string>
+									<key>Graphics</key>
+									<array>
+										<dict>
+											<key>Class</key>
+											<string>LineGraphic</string>
+											<key>ID</key>
+											<integer>1580</integer>
+											<key>Points</key>
+											<array>
+												<string>{229.63277040763995, 658.09714165007426}</string>
+												<string>{227.42306171643185, 659.24619016950237}</string>
+											</array>
+											<key>Style</key>
+											<dict>
+												<key>stroke</key>
+												<dict>
+													<key>HeadArrow</key>
+													<string>0</string>
+													<key>Legacy</key>
+													<true/>
+													<key>TailArrow</key>
+													<string>0</string>
+												</dict>
+											</dict>
+										</dict>
+										<dict>
+											<key>Bounds</key>
+											<string>{{141.72970413355978, 711.77964517567739}, {16, 18.5}}</string>
+											<key>Class</key>
+											<string>ShapedGraphic</string>
+											<key>HFlip</key>
+											<string>YES</string>
+											<key>ID</key>
+											<integer>1581</integer>
+											<key>Rotation</key>
+											<real>45</real>
+											<key>Shape</key>
+											<string>HorizontalTriangle</string>
+											<key>Style</key>
+											<dict>
+												<key>shadow</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+											</dict>
+											<key>Text</key>
+											<dict>
+												<key>VerticalPad</key>
+												<integer>0</integer>
+											</dict>
+										</dict>
+										<dict>
+											<key>Bounds</key>
+											<string>{{131.32983937284456, 631.5655489057134}, {101.5, 101.5}}</string>
+											<key>Class</key>
+											<string>ShapedGraphic</string>
+											<key>HFlip</key>
+											<string>YES</string>
+											<key>ID</key>
+											<integer>1582</integer>
+											<key>Rotation</key>
+											<real>135</real>
+											<key>Shape</key>
+											<string>AdjustableArc</string>
+											<key>ShapeData</key>
+											<dict>
+												<key>endAngle</key>
+												<real>72</real>
+												<key>startAngle</key>
+												<real>281</real>
+											</dict>
+											<key>Style</key>
+											<dict>
+												<key>fill</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+												<key>shadow</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+											</dict>
+											<key>Text</key>
+											<dict>
+												<key>VerticalPad</key>
+												<integer>0</integer>
+											</dict>
+											<key>TextRelativeArea</key>
+											<string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+										</dict>
+										<dict>
+											<key>Bounds</key>
+											<string>{{128.57983937284456, 628.8155489057134}, {107, 107}}</string>
+											<key>Class</key>
+											<string>ShapedGraphic</string>
+											<key>HFlip</key>
+											<string>YES</string>
+											<key>ID</key>
+											<integer>1583</integer>
+											<key>Rotation</key>
+											<real>135</real>
+											<key>Shape</key>
+											<string>AdjustableArc</string>
+											<key>ShapeData</key>
+											<dict>
+												<key>endAngle</key>
+												<real>72</real>
+												<key>startAngle</key>
+												<real>281</real>
+											</dict>
+											<key>Style</key>
+											<dict>
+												<key>fill</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+												<key>shadow</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+											</dict>
+											<key>Text</key>
+											<dict>
+												<key>VerticalPad</key>
+												<integer>0</integer>
+											</dict>
+											<key>TextRelativeArea</key>
+											<string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+										</dict>
+									</array>
+									<key>ID</key>
+									<integer>1579</integer>
+									<key>Rotation</key>
+									<real>315</real>
+								</dict>
+								<dict>
+									<key>Class</key>
+									<string>Group</string>
+									<key>Graphics</key>
+									<array>
+										<dict>
+											<key>Class</key>
+											<string>LineGraphic</string>
+											<key>ID</key>
+											<integer>1585</integer>
+											<key>Points</key>
+											<array>
+												<string>{134.58583749968011, 706.12148080219276}</string>
+												<string>{136.79554619088805, 704.97243228276466}</string>
+											</array>
+											<key>Style</key>
+											<dict>
+												<key>stroke</key>
+												<dict>
+													<key>HeadArrow</key>
+													<string>0</string>
+													<key>Legacy</key>
+													<true/>
+													<key>TailArrow</key>
+													<string>0</string>
+												</dict>
+											</dict>
+										</dict>
+										<dict>
+											<key>Bounds</key>
+											<string>{{206.48890377375986, 633.93897727658998}, {16, 18.5}}</string>
+											<key>Class</key>
+											<string>ShapedGraphic</string>
+											<key>HFlip</key>
+											<string>YES</string>
+											<key>ID</key>
+											<integer>1586</integer>
+											<key>Rotation</key>
+											<real>225</real>
+											<key>Shape</key>
+											<string>HorizontalTriangle</string>
+											<key>Style</key>
+											<dict>
+												<key>shadow</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+											</dict>
+											<key>Text</key>
+											<dict>
+												<key>VerticalPad</key>
+												<integer>0</integer>
+											</dict>
+										</dict>
+										<dict>
+											<key>Bounds</key>
+											<string>{{131.38876853447516, 631.15307354655363}, {101.5, 101.5}}</string>
+											<key>Class</key>
+											<string>ShapedGraphic</string>
+											<key>HFlip</key>
+											<string>YES</string>
+											<key>ID</key>
+											<integer>1587</integer>
+											<key>Rotation</key>
+											<real>315</real>
+											<key>Shape</key>
+											<string>AdjustableArc</string>
+											<key>ShapeData</key>
+											<dict>
+												<key>endAngle</key>
+												<real>72</real>
+												<key>startAngle</key>
+												<real>281</real>
+											</dict>
+											<key>Style</key>
+											<dict>
+												<key>fill</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+												<key>shadow</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+											</dict>
+											<key>Text</key>
+											<dict>
+												<key>VerticalPad</key>
+												<integer>0</integer>
+											</dict>
+											<key>TextRelativeArea</key>
+											<string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+										</dict>
+										<dict>
+											<key>Bounds</key>
+											<string>{{128.63876853447516, 628.40307354655363}, {107, 107}}</string>
+											<key>Class</key>
+											<string>ShapedGraphic</string>
+											<key>HFlip</key>
+											<string>YES</string>
+											<key>ID</key>
+											<integer>1588</integer>
+											<key>Rotation</key>
+											<real>315</real>
+											<key>Shape</key>
+											<string>AdjustableArc</string>
+											<key>ShapeData</key>
+											<dict>
+												<key>endAngle</key>
+												<real>72</real>
+												<key>startAngle</key>
+												<real>281</real>
+											</dict>
+											<key>Style</key>
+											<dict>
+												<key>fill</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+												<key>shadow</key>
+												<dict>
+													<key>Draws</key>
+													<string>NO</string>
+												</dict>
+											</dict>
+											<key>Text</key>
+											<dict>
+												<key>VerticalPad</key>
+												<integer>0</integer>
+											</dict>
+											<key>TextRelativeArea</key>
+											<string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+										</dict>
+									</array>
+									<key>ID</key>
+									<integer>1584</integer>
+									<key>Rotation</key>
+									<real>315</real>
+								</dict>
+								<dict>
+									<key>Bounds</key>
+									<string>{{131.1057015197467, 631.11276372917564}, {101.82337649086284, 101.82337649086284}}</string>
+									<key>Class</key>
+									<string>ShapedGraphic</string>
+									<key>ID</key>
+									<integer>1589</integer>
+									<key>Rotation</key>
+									<real>315</real>
+									<key>Shape</key>
+									<string>Circle</string>
+									<key>Style</key>
+									<dict>
+										<key>fill</key>
+										<dict>
+											<key>Color</key>
+											<dict>
+												<key>a</key>
+												<string>0.78</string>
+												<key>b</key>
+												<string>0.234512</string>
+												<key>g</key>
+												<string>0.673128</string>
+												<key>r</key>
+												<string>0.148947</string>
+											</dict>
+										</dict>
+										<key>shadow</key>
+										<dict>
+											<key>Draws</key>
+											<string>NO</string>
+										</dict>
+										<key>stroke</key>
+										<dict>
+											<key>Draws</key>
+											<string>NO</string>
+										</dict>
+									</dict>
+									<key>Text</key>
+									<dict>
+										<key>VerticalPad</key>
+										<integer>0</integer>
+									</dict>
+								</dict>
+							</array>
+							<key>ID</key>
+							<integer>1577</integer>
+							<key>Rotation</key>
+							<real>315</real>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{253.2315878349163, 736.67782196944336}, {8, 134}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>1590</integer>
+							<key>Rotation</key>
+							<real>225</real>
+							<key>Shape</key>
+							<string>Rectangle</string>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{371.51084102985902, 644.26864825135988}, {8, 134}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>1591</integer>
+							<key>Rotation</key>
+							<real>240</real>
+							<key>Shape</key>
+							<string>Rectangle</string>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{353.64074716687509, 736.67780398678804}, {8, 134}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>1592</integer>
+							<key>Rotation</key>
+							<real>315</real>
+							<key>Shape</key>
+							<string>Rectangle</string>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{234.00404905414581, 645.26862880510475}, {8, 134}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>1593</integer>
+							<key>Rotation</key>
+							<real>300</real>
+							<key>Shape</key>
+							<string>Rectangle</string>
+						</dict>
+					</array>
+					<key>ID</key>
+					<integer>1533</integer>
+				</dict>
+				<dict>
+					<key>Class</key>
+					<string>Group</string>
+					<key>Graphics</key>
+					<array>
+						<dict>
 							<key>Class</key>
 							<string>Group</string>
 							<key>Graphics</key>
@@ -13353,7 +15044,7 @@ PX4IO or PX4FMU}</string>
 											<key>Text</key>
 											<dict>
 												<key>Text</key>
-												<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+												<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -13744,7 +15435,7 @@ PX4IO or PX4FMU}</string>
 											<key>Text</key>
 											<dict>
 												<key>Text</key>
-												<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+												<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -14135,7 +15826,7 @@ PX4IO or PX4FMU}</string>
 											<key>Text</key>
 											<dict>
 												<key>Text</key>
-												<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+												<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -14526,7 +16217,7 @@ PX4IO or PX4FMU}</string>
 											<key>Text</key>
 											<dict>
 												<key>Text</key>
-												<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+												<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -14928,7 +16619,7 @@ PX4IO or PX4FMU}</string>
 										<key>Pad</key>
 										<integer>0</integer>
 										<key>Text</key>
-										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+										<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -14966,7 +16657,7 @@ PX4IO or PX4FMU}</string>
 											<key>Text</key>
 											<dict>
 												<key>Text</key>
-												<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+												<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -15337,7 +17028,7 @@ PX4IO or PX4FMU}</string>
 											<key>Text</key>
 											<dict>
 												<key>Text</key>
-												<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+												<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -15708,7 +17399,7 @@ PX4IO or PX4FMU}</string>
 											<key>Text</key>
 											<dict>
 												<key>Text</key>
-												<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+												<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -16115,7 +17806,7 @@ PX4IO or PX4FMU}</string>
 											<key>Text</key>
 											<dict>
 												<key>Text</key>
-												<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+												<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390
 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
 {\colortbl;\red255\green255\blue255;}
 \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
@@ -16581,6 +18272,94 @@ PX4IO or PX4FMU}</string>
 			<key>VPages</key>
 			<integer>1</integer>
 		</dict>
+		<dict>
+			<key>ActiveLayerIndex</key>
+			<integer>0</integer>
+			<key>AutoAdjust</key>
+			<true/>
+			<key>BackgroundGraphic</key>
+			<dict>
+				<key>Bounds</key>
+				<string>{{0, 0}, {806, 1132}}</string>
+				<key>Class</key>
+				<string>SolidGraphic</string>
+				<key>ID</key>
+				<integer>2</integer>
+				<key>Style</key>
+				<dict>
+					<key>shadow</key>
+					<dict>
+						<key>Draws</key>
+						<string>NO</string>
+					</dict>
+					<key>stroke</key>
+					<dict>
+						<key>Draws</key>
+						<string>NO</string>
+					</dict>
+				</dict>
+			</dict>
+			<key>BaseZoom</key>
+			<integer>0</integer>
+			<key>CanvasOrigin</key>
+			<string>{0, 0}</string>
+			<key>ColumnAlign</key>
+			<integer>1</integer>
+			<key>ColumnSpacing</key>
+			<real>36</real>
+			<key>DisplayScale</key>
+			<string>1 0/72 in = 1.0000 in</string>
+			<key>GraphicsList</key>
+			<array/>
+			<key>GridInfo</key>
+			<dict/>
+			<key>HPages</key>
+			<integer>1</integer>
+			<key>KeepToScale</key>
+			<false/>
+			<key>Layers</key>
+			<array>
+				<dict>
+					<key>Lock</key>
+					<string>NO</string>
+					<key>Name</key>
+					<string>Ebene 1</string>
+					<key>Print</key>
+					<string>YES</string>
+					<key>View</key>
+					<string>YES</string>
+				</dict>
+			</array>
+			<key>LayoutInfo</key>
+			<dict>
+				<key>Animate</key>
+				<string>NO</string>
+				<key>circoMinDist</key>
+				<real>18</real>
+				<key>circoSeparation</key>
+				<real>0.0</real>
+				<key>layoutEngine</key>
+				<string>dot</string>
+				<key>neatoSeparation</key>
+				<real>0.0</real>
+				<key>twopiSeparation</key>
+				<real>0.0</real>
+			</dict>
+			<key>Orientation</key>
+			<integer>2</integer>
+			<key>PrintOnePage</key>
+			<false/>
+			<key>RowAlign</key>
+			<integer>1</integer>
+			<key>RowSpacing</key>
+			<real>36</real>
+			<key>SheetTitle</key>
+			<string>Arbeitsfläche 6</string>
+			<key>UniqueID</key>
+			<integer>6</integer>
+			<key>VPages</key>
+			<integer>1</integer>
+		</dict>
 	</array>
 	<key>SmartAlignmentGuidesActive</key>
 	<string>YES</string>
@@ -16600,7 +18379,7 @@ PX4IO or PX4FMU}</string>
 			</dict>
 		</array>
 		<key>Frame</key>
-		<string>{{96, 56}, {1043, 822}}</string>
+		<string>{{170, 4}, {1043, 774}}</string>
 		<key>ListView</key>
 		<true/>
 		<key>OutlineWidth</key>
@@ -16614,7 +18393,7 @@ PX4IO or PX4FMU}</string>
 		<key>SidebarWidth</key>
 		<integer>120</integer>
 		<key>VisibleRegion</key>
-		<string>{{-51, -33}, {908, 683}}</string>
+		<string>{{-51, 366}, {908, 635}}</string>
 		<key>Zoom</key>
 		<real>1</real>
 		<key>ZoomValues</key>
@@ -16644,6 +18423,11 @@ PX4IO or PX4FMU}</string>
 				<real>1</real>
 				<real>1</real>
 			</array>
+			<array>
+				<string>Arbeitsfläche 6</string>
+				<real>1</real>
+				<real>1</real>
+			</array>
 		</array>
 	</dict>
 </dict>
diff --git a/Documentation/dsm_bind.odt b/Documentation/dsm_bind.odt
new file mode 100644
index 000000000..587a38883
--- /dev/null
+++ b/Documentation/dsm_bind.odt
diff --git a/Documentation/dsm_bind.pdf b/Documentation/dsm_bind.pdf
new file mode 100644
index 000000000..76155569e
--- /dev/null
+++ b/Documentation/dsm_bind.pdf
diff --git a/Documentation/fixed_wing_control.odg b/Documentation/fixed_wing_control.odg
new file mode 100644
index 000000000..0918edcac
--- /dev/null
+++ b/Documentation/fixed_wing_control.odg
diff --git a/Documentation/flight_mode_state_machine.odg b/Documentation/flight_mode_state_machine.odg
index b630ecb40..f9fa7a032 100644
--- a/Documentation/flight_mode_state_machine.odg
+++ b/Documentation/flight_mode_state_machine.odg
diff --git a/Documentation/flight_mode_state_machine.pdf b/Documentation/flight_mode_state_machine.pdf
index cd234ada7..af41953ee 100644
--- a/Documentation/flight_mode_state_machine.pdf
+++ b/Documentation/flight_mode_state_machine.pdf
diff --git a/Documentation/mixing_architecture.graffle b/Documentation/mixing_architecture.graffle
new file mode 100644
index 000000000..da8027bf7
--- /dev/null
+++ b/Documentation/mixing_architecture.graffle
@@ -0,0 +1,4398 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
+<plist version="1.0">
+<dict>
+	<key>ActiveLayerIndex</key>
+	<integer>0</integer>
+	<key>ApplicationVersion</key>
+	<array>
+		<string>com.omnigroup.OmniGraffle</string>
+		<string>139.17.0.185490</string>
+	</array>
+	<key>AutoAdjust</key>
+	<true/>
+	<key>BackgroundGraphic</key>
+	<dict>
+		<key>Bounds</key>
+		<string>{{0, 0}, {1118, 783}}</string>
+		<key>Class</key>
+		<string>SolidGraphic</string>
+		<key>ID</key>
+		<integer>2</integer>
+		<key>Style</key>
+		<dict>
+			<key>shadow</key>
+			<dict>
+				<key>Draws</key>
+				<string>NO</string>
+			</dict>
+			<key>stroke</key>
+			<dict>
+				<key>Draws</key>
+				<string>NO</string>
+			</dict>
+		</dict>
+	</dict>
+	<key>BaseZoom</key>
+	<integer>0</integer>
+	<key>CanvasOrigin</key>
+	<string>{0, 0}</string>
+	<key>ColumnAlign</key>
+	<integer>1</integer>
+	<key>ColumnSpacing</key>
+	<real>36</real>
+	<key>CreationDate</key>
+	<string>2013-06-04 09:23:13 +0000</string>
+	<key>Creator</key>
+	<string>Lorenz Meier</string>
+	<key>DisplayScale</key>
+	<string>1 0/72 in = 1.0000 in</string>
+	<key>GraphDocumentVersion</key>
+	<integer>8</integer>
+	<key>GraphicsList</key>
+	<array>
+		<dict>
+			<key>Class</key>
+			<string>LineGraphic</string>
+			<key>Head</key>
+			<dict>
+				<key>ID</key>
+				<integer>508</integer>
+			</dict>
+			<key>ID</key>
+			<integer>635</integer>
+			<key>Points</key>
+			<array>
+				<string>{106.17826841821868, 273.42634001636537}</string>
+				<string>{213.16101457128596, 357.82365814026997}</string>
+			</array>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>HeadArrow</key>
+					<string>FilledArrow</string>
+					<key>Legacy</key>
+					<true/>
+					<key>TailArrow</key>
+					<string>0</string>
+				</dict>
+			</dict>
+			<key>Tail</key>
+			<dict>
+				<key>ID</key>
+				<integer>613</integer>
+			</dict>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>LineGraphic</string>
+			<key>Head</key>
+			<dict>
+				<key>ID</key>
+				<integer>507</integer>
+			</dict>
+			<key>ID</key>
+			<integer>634</integer>
+			<key>Points</key>
+			<array>
+				<string>{131.96398352136816, 273.42634001634866}</string>
+				<string>{238.946729674436, 357.82365813972365}</string>
+			</array>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>HeadArrow</key>
+					<string>FilledArrow</string>
+					<key>Legacy</key>
+					<true/>
+					<key>TailArrow</key>
+					<string>0</string>
+				</dict>
+			</dict>
+			<key>Tail</key>
+			<dict>
+				<key>ID</key>
+				<integer>612</integer>
+			</dict>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>Group</string>
+			<key>Graphics</key>
+			<array>
+				<dict>
+					<key>Bounds</key>
+					<string>{{482, 231.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>617</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{456.21429061889648, 231.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>618</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{430.42857551574707, 231.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>619</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{404.64286041259766, 231.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>620</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{378.85714530944824, 231.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>621</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{353.07143020629883, 231.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>622</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{327.28571510314941, 231.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>623</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{301.5, 231.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>624</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{482, 267.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>625</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{456.21429061889648, 267.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>626</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{430.42857551574707, 267.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>627</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{404.64286041259766, 267.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>628</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{378.85714530944824, 267.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>629</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{353.07143020629883, 267.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>630</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{327.28571510314941, 267.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>631</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{301.5, 267.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>632</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{291.5, 235.24999816050627}, {208, 36}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>633</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>9</real>
+						</dict>
+					</dict>
+					<key>Text</key>
+					<dict>
+						<key>Text</key>
+						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 Actuator Control Group 1}</string>
+					</dict>
+				</dict>
+			</array>
+			<key>ID</key>
+			<integer>616</integer>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>LineGraphic</string>
+			<key>Head</key>
+			<dict>
+				<key>ID</key>
+				<integer>587</integer>
+			</dict>
+			<key>ID</key>
+			<integer>592</integer>
+			<key>Points</key>
+			<array>
+				<string>{605.82627753848249, 74.5}</string>
+				<string>{716.81658859616107, 159.07205874840687}</string>
+			</array>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>HeadArrow</key>
+					<string>FilledArrow</string>
+					<key>Legacy</key>
+					<true/>
+					<key>TailArrow</key>
+					<string>0</string>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>LineGraphic</string>
+			<key>Head</key>
+			<dict>
+				<key>ID</key>
+				<integer>581</integer>
+			</dict>
+			<key>ID</key>
+			<integer>591</integer>
+			<key>Points</key>
+			<array>
+				<string>{576.14671565757703, 75.907219770052052}</string>
+				<string>{565.99614475502062, 157.66483897835485}</string>
+			</array>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>HeadArrow</key>
+					<string>FilledArrow</string>
+					<key>Legacy</key>
+					<true/>
+					<key>TailArrow</key>
+					<string>0</string>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>LineGraphic</string>
+			<key>ID</key>
+			<integer>590</integer>
+			<key>Points</key>
+			<array>
+				<string>{565.49194626385997, 201.66102838314328}</string>
+				<string>{565.21582473837202, 338.8005880984627}</string>
+			</array>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>HeadArrow</key>
+					<string>FilledArrow</string>
+					<key>Legacy</key>
+					<true/>
+					<key>TailArrow</key>
+					<string>0</string>
+				</dict>
+			</dict>
+			<key>Tail</key>
+			<dict>
+				<key>ID</key>
+				<integer>573</integer>
+			</dict>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>LineGraphic</string>
+			<key>ID</key>
+			<integer>589</integer>
+			<key>Points</key>
+			<array>
+				<string>{720.20623661457341, 201.6610283823652}</string>
+				<string>{719.9301058948314, 338.80058788427203}</string>
+			</array>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>HeadArrow</key>
+					<string>FilledArrow</string>
+					<key>Legacy</key>
+					<true/>
+					<key>TailArrow</key>
+					<string>0</string>
+				</dict>
+			</dict>
+			<key>Tail</key>
+			<dict>
+				<key>ID</key>
+				<integer>579</integer>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{743, 158.16102937420345}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>588</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{717.21429061889648, 158.16102937420345}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>587</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{691.42857551574707, 158.16102937420345}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>586</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{665.64286041259766, 158.16102937420345}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>585</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{639.85714530944824, 158.16102937420345}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>584</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{614.07143020629883, 158.16102937420345}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>583</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{588.28571510314941, 158.16102937420345}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>582</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{562.5, 158.16102937420345}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>581</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{743, 194.16102937420345}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>580</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{717.21429061889648, 194.16102937420345}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>579</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{691.42857551574707, 194.16102937420345}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>578</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{665.64286041259766, 194.16102937420345}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>577</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{639.85714530944824, 194.16102937420345}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>576</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{614.07143020629883, 194.16102937420345}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>575</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{588.28571510314941, 194.16102937420345}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>574</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{562.5, 194.16102937420345}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>573</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{552.5, 162.16102937420345}, {208, 36}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>572</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>9</real>
+				</dict>
+			</dict>
+			<key>Text</key>
+			<dict>
+				<key>Text</key>
+				<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 RC Scaling &amp; Mapping}</string>
+			</dict>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>LineGraphic</string>
+			<key>Head</key>
+			<dict>
+				<key>ID</key>
+				<integer>509</integer>
+			</dict>
+			<key>ID</key>
+			<integer>569</integer>
+			<key>Points</key>
+			<array>
+				<string>{80.392553315069293, 273.42634001641937}</string>
+				<string>{187.37529946813521, 357.82365814202342}</string>
+			</array>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>HeadArrow</key>
+					<string>FilledArrow</string>
+					<key>Legacy</key>
+					<true/>
+					<key>TailArrow</key>
+					<string>0</string>
+				</dict>
+			</dict>
+			<key>Tail</key>
+			<dict>
+				<key>ID</key>
+				<integer>614</integer>
+			</dict>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>LineGraphic</string>
+			<key>Head</key>
+			<dict>
+				<key>ID</key>
+				<integer>506</integer>
+			</dict>
+			<key>ID</key>
+			<integer>596</integer>
+			<key>Points</key>
+			<array>
+				<string>{378.46463924118007, 273.4271429546975}</string>
+				<string>{271.51750416091619, 357.82285520170717}</string>
+			</array>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>HeadArrow</key>
+					<string>FilledArrow</string>
+					<key>Legacy</key>
+					<true/>
+					<key>TailArrow</key>
+					<string>0</string>
+				</dict>
+			</dict>
+			<key>Tail</key>
+			<dict>
+				<key>ID</key>
+				<integer>629</integer>
+			</dict>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>Group</string>
+			<key>Graphics</key>
+			<array>
+				<dict>
+					<key>Bounds</key>
+					<string>{{254.5, 231.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>599</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{228.71429061889648, 231.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>600</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{202.92857551574707, 231.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>601</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{177.14286041259766, 231.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>602</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{151.35714530944824, 231.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>603</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{125.57143020629883, 231.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>604</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{99.785715103149414, 231.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>605</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{74, 231.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>606</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{254.5, 267.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>607</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{228.71429061889648, 267.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>608</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{202.92857551574707, 267.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>609</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{177.14286041259766, 267.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>610</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{151.35714530944824, 267.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>611</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{125.57143020629883, 267.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>612</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{99.785715103149414, 267.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>613</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{74, 267.24999816050627}, {6, 7}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>614</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>1</real>
+						</dict>
+					</dict>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{64, 235.24999816050627}, {208, 36}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>615</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>9</real>
+						</dict>
+					</dict>
+					<key>Text</key>
+					<dict>
+						<key>Text</key>
+						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 Actuator Control Group 0}</string>
+					</dict>
+				</dict>
+			</array>
+			<key>ID</key>
+			<integer>598</integer>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>LineGraphic</string>
+			<key>Head</key>
+			<dict>
+				<key>ID</key>
+				<integer>474</integer>
+			</dict>
+			<key>ID</key>
+			<integer>411</integer>
+			<key>Points</key>
+			<array>
+				<string>{322.59687445702775, 444.84226761008853}</string>
+				<string>{379.20669915150745, 526.90773238985753}</string>
+			</array>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>HeadArrow</key>
+					<string>FilledArrow</string>
+					<key>Legacy</key>
+					<true/>
+					<key>TailArrow</key>
+					<string>0</string>
+				</dict>
+			</dict>
+			<key>Tail</key>
+			<dict>
+				<key>ID</key>
+				<integer>510</integer>
+			</dict>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>LineGraphic</string>
+			<key>Head</key>
+			<dict>
+				<key>ID</key>
+				<integer>477</integer>
+			</dict>
+			<key>ID</key>
+			<integer>412</integer>
+			<key>Points</key>
+			<array>
+				<string>{294.62113860994009, 444.9967148570675}</string>
+				<string>{304.03957452010235, 526.7532851429479}</string>
+			</array>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>HeadArrow</key>
+					<string>FilledArrow</string>
+					<key>Legacy</key>
+					<true/>
+					<key>TailArrow</key>
+					<string>0</string>
+				</dict>
+			</dict>
+			<key>Tail</key>
+			<dict>
+				<key>ID</key>
+				<integer>511</integer>
+			</dict>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>LineGraphic</string>
+			<key>Head</key>
+			<dict>
+				<key>ID</key>
+				<integer>490</integer>
+			</dict>
+			<key>ID</key>
+			<integer>413</integer>
+			<key>Points</key>
+			<array>
+				<string>{290.76782961936522, 443.67073351699491}</string>
+				<string>{183.53574398902853, 528.07926647675936}</string>
+			</array>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>HeadArrow</key>
+					<string>FilledArrow</string>
+					<key>Legacy</key>
+					<true/>
+					<key>TailArrow</key>
+					<string>0</string>
+				</dict>
+			</dict>
+			<key>Tail</key>
+			<dict>
+				<key>ID</key>
+				<integer>511</integer>
+			</dict>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>LineGraphic</string>
+			<key>Head</key>
+			<dict>
+				<key>ID</key>
+				<integer>491</integer>
+			</dict>
+			<key>ID</key>
+			<integer>414</integer>
+			<key>Points</key>
+			<array>
+				<string>{264.9821145162158, 443.67073351699491}</string>
+				<string>{157.75002888587912, 528.07926647675936}</string>
+			</array>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>HeadArrow</key>
+					<string>FilledArrow</string>
+					<key>Legacy</key>
+					<true/>
+					<key>TailArrow</key>
+					<string>0</string>
+				</dict>
+			</dict>
+			<key>Tail</key>
+			<dict>
+				<key>ID</key>
+				<integer>512</integer>
+			</dict>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>LineGraphic</string>
+			<key>Head</key>
+			<dict>
+				<key>ID</key>
+				<integer>492</integer>
+			</dict>
+			<key>ID</key>
+			<integer>415</integer>
+			<key>Points</key>
+			<array>
+				<string>{239.19639941306636, 443.67073351699491}</string>
+				<string>{131.9643137827297, 528.07926647675936}</string>
+			</array>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>HeadArrow</key>
+					<string>FilledArrow</string>
+					<key>Legacy</key>
+					<true/>
+					<key>TailArrow</key>
+					<string>0</string>
+				</dict>
+			</dict>
+			<key>Tail</key>
+			<dict>
+				<key>ID</key>
+				<integer>513</integer>
+			</dict>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>LineGraphic</string>
+			<key>Head</key>
+			<dict>
+				<key>ID</key>
+				<integer>493</integer>
+			</dict>
+			<key>ID</key>
+			<integer>416</integer>
+			<key>Points</key>
+			<array>
+				<string>{213.41068430991695, 443.67073351699491}</string>
+				<string>{106.1785986795803, 528.07926647675936}</string>
+			</array>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>HeadArrow</key>
+					<string>FilledArrow</string>
+					<key>Legacy</key>
+					<true/>
+					<key>TailArrow</key>
+					<string>0</string>
+				</dict>
+			</dict>
+			<key>Tail</key>
+			<dict>
+				<key>ID</key>
+				<integer>514</integer>
+			</dict>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>LineGraphic</string>
+			<key>Head</key>
+			<dict>
+				<key>ID</key>
+				<integer>440</integer>
+			</dict>
+			<key>ID</key>
+			<integer>417</integer>
+			<key>Points</key>
+			<array>
+				<string>{304.49194626386003, 570.74999900893988}</string>
+				<string>{304.21582473837134, 707.88955872425925}</string>
+			</array>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>HeadArrow</key>
+					<string>FilledArrow</string>
+					<key>Legacy</key>
+					<true/>
+					<key>TailArrow</key>
+					<string>0</string>
+				</dict>
+			</dict>
+			<key>Tail</key>
+			<dict>
+				<key>ID</key>
+				<integer>485</integer>
+			</dict>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>LineGraphic</string>
+			<key>Head</key>
+			<dict>
+				<key>ID</key>
+				<integer>429</integer>
+			</dict>
+			<key>ID</key>
+			<integer>418</integer>
+			<key>Points</key>
+			<array>
+				<string>{381.84909342488743, 570.74999900816329}</string>
+				<string>{381.57303538097813, 707.88955851048831}</string>
+			</array>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>HeadArrow</key>
+					<string>FilledArrow</string>
+					<key>Legacy</key>
+					<true/>
+					<key>TailArrow</key>
+					<string>0</string>
+				</dict>
+			</dict>
+			<key>Tail</key>
+			<dict>
+				<key>ID</key>
+				<integer>482</integer>
+			</dict>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>Group</string>
+			<key>Graphics</key>
+			<array>
+				<dict>
+					<key>Class</key>
+					<string>Group</string>
+					<key>Graphics</key>
+					<array>
+						<dict>
+							<key>Bounds</key>
+							<string>{{379.35432048604451, 719.00347978513389}, {5.0056232242123722, 4.9183513058348947}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>421</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{396.74606011338682, 726.18083608931011}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>422</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{391.6252245834844, 724.37750550833903}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>423</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{386.29980802056684, 722.50213058402392}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>424</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{375.67643661590967, 718.76105153253593}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>425</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{370.35101205342835, 716.88567379113056}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>426</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{365.23018452308935, 715.08234602724974}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>427</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{345.93778549185652, 717.4003115566619}, {71.838699340820256, 8.1246847180976189}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>428</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Diamond</string>
+							<key>Style</key>
+							<dict>
+								<key>stroke</key>
+								<dict>
+									<key>CornerRadius</key>
+									<real>5</real>
+								</dict>
+							</dict>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+					</array>
+					<key>ID</key>
+					<integer>420</integer>
+					<key>Rotation</key>
+					<real>19.399997711181641</real>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{371.98798359082144, 708.38955751823028}, {19.087577050122338, 39.91038694100309}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>429</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>fill</key>
+						<dict>
+							<key>Color</key>
+							<dict>
+								<key>b</key>
+								<string>0.153172</string>
+								<key>g</key>
+								<string>0.153172</string>
+								<key>r</key>
+								<string>0.153172</string>
+							</dict>
+							<key>FillType</key>
+							<integer>2</integer>
+							<key>GradientAngle</key>
+							<real>145</real>
+							<key>GradientColor</key>
+							<dict>
+								<key>b</key>
+								<string>0.416928</string>
+								<key>g</key>
+								<string>0.416928</string>
+								<key>r</key>
+								<string>0.416928</string>
+							</dict>
+						</dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>2</real>
+						</dict>
+					</dict>
+				</dict>
+			</array>
+			<key>ID</key>
+			<integer>419</integer>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>Group</string>
+			<key>Graphics</key>
+			<array>
+				<dict>
+					<key>Class</key>
+					<string>Group</string>
+					<key>Graphics</key>
+					<array>
+						<dict>
+							<key>Bounds</key>
+							<string>{{301.99718791759841, 719.00348000010274}, {5.0056232242123722, 4.9183513058348947}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>432</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{319.38892754494066, 726.18083630427896}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>433</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{314.26809201503823, 724.37750572330788}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>434</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{308.94267545212068, 722.50213079899277}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>435</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{298.3193040474635, 718.76105174750478}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>436</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{292.99387948498219, 716.88567400609941}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>437</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{287.87305195464319, 715.08234624221859}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>438</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{268.58065292341041, 717.40031177163075}, {71.838699340820256, 8.1246847180976189}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>439</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Diamond</string>
+							<key>Style</key>
+							<dict>
+								<key>stroke</key>
+								<dict>
+									<key>CornerRadius</key>
+									<real>5</real>
+								</dict>
+							</dict>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+					</array>
+					<key>ID</key>
+					<integer>431</integer>
+					<key>Rotation</key>
+					<real>19.399997711181641</real>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{294.63085102237534, 708.38955773319913}, {19.087577050122338, 39.91038694100309}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>440</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>fill</key>
+						<dict>
+							<key>Color</key>
+							<dict>
+								<key>b</key>
+								<string>0.153172</string>
+								<key>g</key>
+								<string>0.153172</string>
+								<key>r</key>
+								<string>0.153172</string>
+							</dict>
+							<key>FillType</key>
+							<integer>2</integer>
+							<key>GradientAngle</key>
+							<real>145</real>
+							<key>GradientColor</key>
+							<dict>
+								<key>b</key>
+								<string>0.416928</string>
+								<key>g</key>
+								<string>0.416928</string>
+								<key>r</key>
+								<string>0.416928</string>
+							</dict>
+						</dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>2</real>
+						</dict>
+					</dict>
+				</dict>
+			</array>
+			<key>ID</key>
+			<integer>430</integer>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>Group</string>
+			<key>Graphics</key>
+			<array>
+				<dict>
+					<key>Class</key>
+					<string>Group</string>
+					<key>Graphics</key>
+					<array>
+						<dict>
+							<key>Bounds</key>
+							<string>{{151.85433513439526, 676.40348606783687}, {5.0056232242123722, 4.9183513058348947}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>443</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{169.24607476173759, 683.58084237201308}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>444</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{164.12523923183511, 681.777511791042}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>445</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{158.79982266891756, 679.90213686672689}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>446</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{148.17645126426038, 676.16105781523891}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>447</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{142.85102670177906, 674.28568007383353}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>448</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{137.73019917144009, 672.48235230995272}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>449</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{118.43780014020726, 674.80031783936488}, {71.838699340820256, 8.1246847180976189}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>450</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Diamond</string>
+							<key>Style</key>
+							<dict>
+								<key>stroke</key>
+								<dict>
+									<key>CornerRadius</key>
+									<real>5</real>
+								</dict>
+							</dict>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+					</array>
+					<key>ID</key>
+					<integer>442</integer>
+					<key>Rotation</key>
+					<real>19.399997711181641</real>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{144.48799823917219, 665.78956380093325}, {19.087577050122338, 39.91038694100309}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>451</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>fill</key>
+						<dict>
+							<key>Color</key>
+							<dict>
+								<key>b</key>
+								<string>0.153172</string>
+								<key>g</key>
+								<string>0.153172</string>
+								<key>r</key>
+								<string>0.153172</string>
+							</dict>
+							<key>FillType</key>
+							<integer>2</integer>
+							<key>GradientAngle</key>
+							<real>145</real>
+							<key>GradientColor</key>
+							<dict>
+								<key>b</key>
+								<string>0.416928</string>
+								<key>g</key>
+								<string>0.416928</string>
+								<key>r</key>
+								<string>0.416928</string>
+							</dict>
+						</dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>2</real>
+						</dict>
+					</dict>
+				</dict>
+			</array>
+			<key>ID</key>
+			<integer>441</integer>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>Group</string>
+			<key>Graphics</key>
+			<array>
+				<dict>
+					<key>Class</key>
+					<string>Group</string>
+					<key>Graphics</key>
+					<array>
+						<dict>
+							<key>Bounds</key>
+							<string>{{126.06861430919994, 649.30352977035523}, {5.0056232242123722, 4.9183513058348947}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>454</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{143.46035393654228, 656.48088607453144}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>455</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{138.3395184066398, 654.67755549356036}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>456</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{133.01410184372224, 652.80218056924525}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>457</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{122.39073043906507, 649.06110151775727}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>458</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{117.06530587658375, 647.18572377635189}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>459</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{111.94447834624478, 645.38239601247108}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>460</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{92.652079315011946, 647.70036154188324}, {71.838699340820256, 8.1246847180976189}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>461</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Diamond</string>
+							<key>Style</key>
+							<dict>
+								<key>stroke</key>
+								<dict>
+									<key>CornerRadius</key>
+									<real>5</real>
+								</dict>
+							</dict>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+					</array>
+					<key>ID</key>
+					<integer>453</integer>
+					<key>Rotation</key>
+					<real>19.399997711181641</real>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{118.70227741397687, 638.68960750345161}, {19.087577050122338, 39.91038694100309}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>462</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>fill</key>
+						<dict>
+							<key>Color</key>
+							<dict>
+								<key>b</key>
+								<string>0.153172</string>
+								<key>g</key>
+								<string>0.153172</string>
+								<key>r</key>
+								<string>0.153172</string>
+							</dict>
+							<key>FillType</key>
+							<integer>2</integer>
+							<key>GradientAngle</key>
+							<real>145</real>
+							<key>GradientColor</key>
+							<dict>
+								<key>b</key>
+								<string>0.416928</string>
+								<key>g</key>
+								<string>0.416928</string>
+								<key>r</key>
+								<string>0.416928</string>
+							</dict>
+						</dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>2</real>
+						</dict>
+					</dict>
+				</dict>
+			</array>
+			<key>ID</key>
+			<integer>452</integer>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>LineGraphic</string>
+			<key>Head</key>
+			<dict>
+				<key>ID</key>
+				<integer>494</integer>
+			</dict>
+			<key>ID</key>
+			<integer>463</integer>
+			<key>Points</key>
+			<array>
+				<string>{187.62496920676753, 443.67073351699486}</string>
+				<string>{80.392883576430918, 528.07926647675936}</string>
+			</array>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>HeadArrow</key>
+					<string>FilledArrow</string>
+					<key>Legacy</key>
+					<true/>
+					<key>TailArrow</key>
+					<string>0</string>
+				</dict>
+			</dict>
+			<key>Tail</key>
+			<dict>
+				<key>ID</key>
+				<integer>515</integer>
+			</dict>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>LineGraphic</string>
+			<key>Head</key>
+			<dict>
+				<key>ID</key>
+				<integer>451</integer>
+			</dict>
+			<key>ID</key>
+			<integer>464</integer>
+			<key>Points</key>
+			<array>
+				<string>{154.34620841108131, 570.74999811167606}</string>
+				<string>{154.08771587848975, 665.2895656892573}</string>
+			</array>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>HeadArrow</key>
+					<string>FilledArrow</string>
+					<key>Legacy</key>
+					<true/>
+					<key>TailArrow</key>
+					<string>0</string>
+				</dict>
+			</dict>
+			<key>Tail</key>
+			<dict>
+				<key>ID</key>
+				<integer>499</integer>
+			</dict>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>LineGraphic</string>
+			<key>Head</key>
+			<dict>
+				<key>ID</key>
+				<integer>462</integer>
+			</dict>
+			<key>ID</key>
+			<integer>465</integer>
+			<key>Points</key>
+			<array>
+				<string>{128.55726775049197, 570.74999689176411}</string>
+				<string>{128.31848992771231, 638.1896106116875}</string>
+			</array>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>HeadArrow</key>
+					<string>FilledArrow</string>
+					<key>Legacy</key>
+					<true/>
+					<key>TailArrow</key>
+					<string>0</string>
+				</dict>
+			</dict>
+			<key>Tail</key>
+			<dict>
+				<key>ID</key>
+				<integer>500</integer>
+			</dict>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>LineGraphic</string>
+			<key>Head</key>
+			<dict>
+				<key>ID</key>
+				<integer>538</integer>
+			</dict>
+			<key>ID</key>
+			<integer>466</integer>
+			<key>Points</key>
+			<array>
+				<string>{102.76581371331335, 570.74999379087535}</string>
+				<string>{102.56212456317996, 611.68961371257615}</string>
+			</array>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>HeadArrow</key>
+					<string>FilledArrow</string>
+					<key>Legacy</key>
+					<true/>
+					<key>TailArrow</key>
+					<string>0</string>
+				</dict>
+			</dict>
+			<key>Tail</key>
+			<dict>
+				<key>ID</key>
+				<integer>501</integer>
+			</dict>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>LineGraphic</string>
+			<key>Head</key>
+			<dict>
+				<key>ID</key>
+				<integer>549</integer>
+			</dict>
+			<key>ID</key>
+			<integer>467</integer>
+			<key>Points</key>
+			<array>
+				<string>{76.966539516560758, 570.74998251647355}</string>
+				<string>{76.84574963416425, 585.18962498697681}</string>
+			</array>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>HeadArrow</key>
+					<string>FilledArrow</string>
+					<key>Legacy</key>
+					<true/>
+					<key>TailArrow</key>
+					<string>0</string>
+				</dict>
+			</dict>
+			<key>Tail</key>
+			<dict>
+				<key>ID</key>
+				<integer>502</integer>
+			</dict>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>LineGraphic</string>
+			<key>Head</key>
+			<dict>
+				<key>ID</key>
+				<integer>550</integer>
+			</dict>
+			<key>ID</key>
+			<integer>468</integer>
+			<key>Points</key>
+			<array>
+				<string>{180.14284120598458, 570.75}</string>
+				<string>{180.14284120598458, 685.34997558593727}</string>
+			</array>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>HeadArrow</key>
+					<string>FilledArrow</string>
+					<key>Legacy</key>
+					<true/>
+					<key>TailArrow</key>
+					<string>0</string>
+				</dict>
+			</dict>
+			<key>Tail</key>
+			<dict>
+				<key>ID</key>
+				<integer>498</integer>
+			</dict>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>LineGraphic</string>
+			<key>Head</key>
+			<dict>
+				<key>ID</key>
+				<integer>527</integer>
+			</dict>
+			<key>ID</key>
+			<integer>469</integer>
+			<key>Points</key>
+			<array>
+				<string>{205.92052173345891, 570.74999897014993}</string>
+				<string>{205.64439858308342, 707.8895785636937}</string>
+			</array>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>HeadArrow</key>
+					<string>FilledArrow</string>
+					<key>Legacy</key>
+					<true/>
+					<key>TailArrow</key>
+					<string>0</string>
+				</dict>
+			</dict>
+			<key>Tail</key>
+			<dict>
+				<key>ID</key>
+				<integer>497</integer>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{482, 527.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>470</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{456.21429061889648, 527.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>471</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{430.42857551574707, 527.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>472</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{404.64286041259766, 527.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>473</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{378.85714530944824, 527.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>474</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{353.07143020629883, 527.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>475</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{327.28571510314941, 527.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>476</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{301.5, 527.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>477</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{482, 563.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>478</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{456.21429061889648, 563.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>479</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{430.42857551574707, 563.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>480</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{404.64286041259766, 563.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>481</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{378.85714530944824, 563.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>482</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{353.07143020629883, 563.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>483</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{327.28571510314941, 563.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>484</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{301.5, 563.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>485</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{291.5, 531.25}, {208, 36}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>486</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>9</real>
+				</dict>
+			</dict>
+			<key>Text</key>
+			<dict>
+				<key>Text</key>
+				<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 Actuator Output Group 1 (e.g. FMU)}</string>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{254.5, 527.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>487</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{228.71429061889648, 527.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>488</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{202.92857551574707, 527.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>489</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{177.14286041259766, 527.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>490</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{151.35714530944824, 527.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>491</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{125.57143020629883, 527.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>492</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{99.785715103149414, 527.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>493</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{74, 527.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>494</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{254.5, 563.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>495</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{228.71429061889648, 563.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>496</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{202.92857551574707, 563.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>497</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{177.14286041259766, 563.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>498</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{151.35714530944824, 563.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>499</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{125.57143020629883, 563.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>500</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{99.785715103149414, 563.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>501</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{74, 563.25}, {6, 7}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>502</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>1</real>
+				</dict>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{64, 531.25}, {208, 36}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>503</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>stroke</key>
+				<dict>
+					<key>CornerRadius</key>
+					<real>9</real>
+				</dict>
+			</dict>
+			<key>Text</key>
+			<dict>
+				<key>Text</key>
+				<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 Actuator Output Group 0 (e.g. IO)}</string>
+			</dict>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>Group</string>
+			<key>Graphics</key>
+			<array>
+				<dict>
+					<key>Class</key>
+					<string>Group</string>
+					<key>Graphics</key>
+					<array>
+						<dict>
+							<key>Bounds</key>
+							<string>{{265.12499809265137, 357}, {6, 7}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>506</integer>
+							<key>Shape</key>
+							<string>Rectangle</string>
+							<key>Style</key>
+							<dict>
+								<key>stroke</key>
+								<dict>
+									<key>CornerRadius</key>
+									<real>1</real>
+								</dict>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{239.33928298950195, 357}, {6, 7}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>507</integer>
+							<key>Shape</key>
+							<string>Rectangle</string>
+							<key>Style</key>
+							<dict>
+								<key>stroke</key>
+								<dict>
+									<key>CornerRadius</key>
+									<real>1</real>
+								</dict>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{213.55356788635254, 357}, {6, 7}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>508</integer>
+							<key>Shape</key>
+							<string>Rectangle</string>
+							<key>Style</key>
+							<dict>
+								<key>stroke</key>
+								<dict>
+									<key>CornerRadius</key>
+									<real>1</real>
+								</dict>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{187.76785278320312, 357}, {6, 7}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>509</integer>
+							<key>Shape</key>
+							<string>Rectangle</string>
+							<key>Style</key>
+							<dict>
+								<key>stroke</key>
+								<dict>
+									<key>CornerRadius</key>
+									<real>1</real>
+								</dict>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{316.9464282989502, 437.5}, {6, 7}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>510</integer>
+							<key>Shape</key>
+							<string>Rectangle</string>
+							<key>Style</key>
+							<dict>
+								<key>stroke</key>
+								<dict>
+									<key>CornerRadius</key>
+									<real>1</real>
+								</dict>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{291.16071319580078, 437.5}, {6, 7}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>511</integer>
+							<key>Shape</key>
+							<string>Rectangle</string>
+							<key>Style</key>
+							<dict>
+								<key>stroke</key>
+								<dict>
+									<key>CornerRadius</key>
+									<real>1</real>
+								</dict>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{265.37499809265137, 437.5}, {6, 7}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>512</integer>
+							<key>Shape</key>
+							<string>Rectangle</string>
+							<key>Style</key>
+							<dict>
+								<key>stroke</key>
+								<dict>
+									<key>CornerRadius</key>
+									<real>1</real>
+								</dict>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{239.58928298950195, 437.5}, {6, 7}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>513</integer>
+							<key>Shape</key>
+							<string>Rectangle</string>
+							<key>Style</key>
+							<dict>
+								<key>stroke</key>
+								<dict>
+									<key>CornerRadius</key>
+									<real>1</real>
+								</dict>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{213.80356788635254, 437.5}, {6, 7}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>514</integer>
+							<key>Shape</key>
+							<string>Rectangle</string>
+							<key>Style</key>
+							<dict>
+								<key>stroke</key>
+								<dict>
+									<key>CornerRadius</key>
+									<real>1</real>
+								</dict>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{188.01785278320312, 437.5}, {6, 7}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>515</integer>
+							<key>Shape</key>
+							<string>Rectangle</string>
+							<key>Style</key>
+							<dict>
+								<key>stroke</key>
+								<dict>
+									<key>CornerRadius</key>
+									<real>1</real>
+								</dict>
+							</dict>
+						</dict>
+					</array>
+					<key>ID</key>
+					<integer>505</integer>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{177.14285278320312, 359.75}, {208, 82}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>516</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>9</real>
+						</dict>
+					</dict>
+					<key>Text</key>
+					<dict>
+						<key>Text</key>
+						<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 Vehicle Mixer}</string>
+					</dict>
+				</dict>
+			</array>
+			<key>ID</key>
+			<integer>504</integer>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>Group</string>
+			<key>Graphics</key>
+			<array>
+				<dict>
+					<key>Class</key>
+					<string>Group</string>
+					<key>Graphics</key>
+					<array>
+						<dict>
+							<key>Bounds</key>
+							<string>{{203.42576152599673, 719.00349980074725}, {5.0056232242123722, 4.9183513058348947}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>519</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{220.81750115333907, 726.18085610492346}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>520</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{215.69666562343659, 724.37752552395239}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>521</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{210.37124906051903, 722.50215059963728}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>522</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{199.74787765586186, 718.76107154814929}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>523</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{194.42245309338054, 716.88569380674392}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>524</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{189.30162556304157, 715.0823660428631}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>525</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{170.00922653180874, 717.40033157227526}, {71.838699340820256, 8.1246847180976189}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>526</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Diamond</string>
+							<key>Style</key>
+							<dict>
+								<key>stroke</key>
+								<dict>
+									<key>CornerRadius</key>
+									<real>5</real>
+								</dict>
+							</dict>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+					</array>
+					<key>ID</key>
+					<integer>518</integer>
+					<key>Rotation</key>
+					<real>19.399997711181641</real>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{196.05942463077366, 708.38957753384364}, {19.087577050122338, 39.91038694100309}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>527</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>fill</key>
+						<dict>
+							<key>Color</key>
+							<dict>
+								<key>b</key>
+								<string>0.153172</string>
+								<key>g</key>
+								<string>0.153172</string>
+								<key>r</key>
+								<string>0.153172</string>
+							</dict>
+							<key>FillType</key>
+							<integer>2</integer>
+							<key>GradientAngle</key>
+							<real>145</real>
+							<key>GradientColor</key>
+							<dict>
+								<key>b</key>
+								<string>0.416928</string>
+								<key>g</key>
+								<string>0.416928</string>
+								<key>r</key>
+								<string>0.416928</string>
+							</dict>
+						</dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>2</real>
+						</dict>
+					</dict>
+				</dict>
+			</array>
+			<key>ID</key>
+			<integer>517</integer>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>Group</string>
+			<key>Graphics</key>
+			<array>
+				<dict>
+					<key>Class</key>
+					<string>Group</string>
+					<key>Graphics</key>
+					<array>
+						<dict>
+							<key>Bounds</key>
+							<string>{{100.28290111339905, 622.80352977035523}, {5.0056232242123722, 4.9183513058348947}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>530</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{117.67464074074138, 629.98088607453144}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>531</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{112.5538052108389, 628.17755549356036}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>532</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{107.22838864792135, 626.30218056924525}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>533</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{96.605017243264172, 622.56110151775727}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>534</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{91.279592680782855, 620.68572377635189}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>535</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{86.158765150443884, 618.88239601247108}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>536</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{66.866366119211051, 621.20036154188324}, {71.838699340820256, 8.1246847180976189}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>537</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Diamond</string>
+							<key>Style</key>
+							<dict>
+								<key>stroke</key>
+								<dict>
+									<key>CornerRadius</key>
+									<real>5</real>
+								</dict>
+							</dict>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+					</array>
+					<key>ID</key>
+					<integer>529</integer>
+					<key>Rotation</key>
+					<real>19.399997711181641</real>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{92.916564218175978, 612.18960750345161}, {19.087577050122338, 39.91038694100309}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>538</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>fill</key>
+						<dict>
+							<key>Color</key>
+							<dict>
+								<key>b</key>
+								<string>0.153172</string>
+								<key>g</key>
+								<string>0.153172</string>
+								<key>r</key>
+								<string>0.153172</string>
+							</dict>
+							<key>FillType</key>
+							<integer>2</integer>
+							<key>GradientAngle</key>
+							<real>145</real>
+							<key>GradientColor</key>
+							<dict>
+								<key>b</key>
+								<string>0.416928</string>
+								<key>g</key>
+								<string>0.416928</string>
+								<key>r</key>
+								<string>0.416928</string>
+							</dict>
+						</dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>2</real>
+						</dict>
+					</dict>
+				</dict>
+			</array>
+			<key>ID</key>
+			<integer>528</integer>
+		</dict>
+		<dict>
+			<key>Class</key>
+			<string>Group</string>
+			<key>Graphics</key>
+			<array>
+				<dict>
+					<key>Class</key>
+					<string>Group</string>
+					<key>Graphics</key>
+					<array>
+						<dict>
+							<key>Bounds</key>
+							<string>{{74.49718852803673, 596.30352977035523}, {5.0056232242123722, 4.9183513058348947}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>541</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{91.888928155379034, 603.48088607453144}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>542</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{86.768092625476555, 601.67755549356036}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>543</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{81.442676062559002, 599.80218056924525}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>544</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{70.819304657901824, 596.06110151775727}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>545</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{65.493880095420508, 594.18572377635189}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>546</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{60.373052565081537, 592.38239601247108}, {1.9456694882012362, 1.7352345929064885}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>547</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Circle</string>
+							<key>Style</key>
+							<dict/>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+						<dict>
+							<key>Bounds</key>
+							<string>{{41.080653533848704, 594.70036154188324}, {71.838699340820256, 8.1246847180976189}}</string>
+							<key>Class</key>
+							<string>ShapedGraphic</string>
+							<key>ID</key>
+							<integer>548</integer>
+							<key>Rotation</key>
+							<real>19.399997711181641</real>
+							<key>Shape</key>
+							<string>Diamond</string>
+							<key>Style</key>
+							<dict>
+								<key>stroke</key>
+								<dict>
+									<key>CornerRadius</key>
+									<real>5</real>
+								</dict>
+							</dict>
+							<key>Text</key>
+							<dict>
+								<key>VerticalPad</key>
+								<integer>0</integer>
+							</dict>
+						</dict>
+					</array>
+					<key>ID</key>
+					<integer>540</integer>
+					<key>Rotation</key>
+					<real>19.399997711181641</real>
+				</dict>
+				<dict>
+					<key>Bounds</key>
+					<string>{{67.130851632813631, 585.68960750345161}, {19.087577050122338, 39.91038694100309}}</string>
+					<key>Class</key>
+					<string>ShapedGraphic</string>
+					<key>ID</key>
+					<integer>549</integer>
+					<key>Shape</key>
+					<string>Rectangle</string>
+					<key>Style</key>
+					<dict>
+						<key>fill</key>
+						<dict>
+							<key>Color</key>
+							<dict>
+								<key>b</key>
+								<string>0.153172</string>
+								<key>g</key>
+								<string>0.153172</string>
+								<key>r</key>
+								<string>0.153172</string>
+							</dict>
+							<key>FillType</key>
+							<integer>2</integer>
+							<key>GradientAngle</key>
+							<real>145</real>
+							<key>GradientColor</key>
+							<dict>
+								<key>b</key>
+								<string>0.416928</string>
+								<key>g</key>
+								<string>0.416928</string>
+								<key>r</key>
+								<string>0.416928</string>
+							</dict>
+						</dict>
+						<key>stroke</key>
+						<dict>
+							<key>CornerRadius</key>
+							<real>2</real>
+						</dict>
+					</dict>
+				</dict>
+			</array>
+			<key>ID</key>
+			<integer>539</integer>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{64, 104}, {54, 36}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>1</integer>
+			<key>Shape</key>
+			<string>Rectangle</string>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{125.57142639160156, 24}, {36, 36}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>6</integer>
+			<key>Shape</key>
+			<string>Diamond</string>
+			<key>Style</key>
+			<dict/>
+			<key>Text</key>
+			<dict>
+				<key>VerticalPad</key>
+				<integer>0</integer>
+			</dict>
+		</dict>
+		<dict>
+			<key>Bounds</key>
+			<string>{{158.14285217276469, 695.84997558593727}, {44, 24}}</string>
+			<key>Class</key>
+			<string>ShapedGraphic</string>
+			<key>ID</key>
+			<integer>550</integer>
+			<key>Rotation</key>
+			<real>90</real>
+			<key>Shape</key>
+			<string>Rectangle</string>
+			<key>Style</key>
+			<dict>
+				<key>fill</key>
+				<dict>
+					<key>Color</key>
+					<dict>
+						<key>b</key>
+						<string>0.243722</string>
+						<key>g</key>
+						<string>0.864482</string>
+						<key>r</key>
+						<string>1</string>
+					</dict>
+				</dict>
+				<key>stroke</key>
+				<dict>
+					<key>Color</key>
+					<dict>
+						<key>b</key>
+						<string>0.051159</string>
+						<key>g</key>
+						<string>0.160546</string>
+						<key>r</key>
+						<string>0.18663</string>
+					</dict>
+					<key>CornerRadius</key>
+					<real>5</real>
+				</dict>
+			</dict>
+			<key>Text</key>
+			<dict>
+				<key>Text</key>
+				<string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 ESC}</string>
+			</dict>
+		</dict>
+	</array>
+	<key>GridInfo</key>
+	<dict/>
+	<key>GuidesLocked</key>
+	<string>NO</string>
+	<key>GuidesVisible</key>
+	<string>YES</string>
+	<key>HPages</key>
+	<integer>2</integer>
+	<key>ImageCounter</key>
+	<integer>1</integer>
+	<key>KeepToScale</key>
+	<false/>
+	<key>Layers</key>
+	<array>
+		<dict>
+			<key>Lock</key>
+			<string>NO</string>
+			<key>Name</key>
+			<string>Layer 1</string>
+			<key>Print</key>
+			<string>YES</string>
+			<key>View</key>
+			<string>YES</string>
+		</dict>
+	</array>
+	<key>LayoutInfo</key>
+	<dict>
+		<key>Animate</key>
+		<string>NO</string>
+		<key>circoMinDist</key>
+		<real>18</real>
+		<key>circoSeparation</key>
+		<real>0.0</real>
+		<key>layoutEngine</key>
+		<string>dot</string>
+		<key>neatoSeparation</key>
+		<real>0.0</real>
+		<key>twopiSeparation</key>
+		<real>0.0</real>
+	</dict>
+	<key>LinksVisible</key>
+	<string>NO</string>
+	<key>MagnetsVisible</key>
+	<string>NO</string>
+	<key>MasterSheets</key>
+	<array/>
+	<key>ModificationDate</key>
+	<string>2013-06-04 16:51:32 +0000</string>
+	<key>Modifier</key>
+	<string>Lorenz Meier</string>
+	<key>NotesVisible</key>
+	<string>NO</string>
+	<key>Orientation</key>
+	<integer>2</integer>
+	<key>OriginVisible</key>
+	<string>NO</string>
+	<key>PageBreaks</key>
+	<string>YES</string>
+	<key>PrintInfo</key>
+	<dict>
+		<key>NSBottomMargin</key>
+		<array>
+			<string>float</string>
+			<string>41</string>
+		</array>
+		<key>NSHorizonalPagination</key>
+		<array>
+			<string>coded</string>
+			<string>BAtzdHJlYW10eXBlZIHoA4QBQISEhAhOU051bWJlcgCEhAdOU1ZhbHVlAISECE5TT2JqZWN0AIWEASqEhAFxlwCG</string>
+		</array>
+		<key>NSLeftMargin</key>
+		<array>
+			<string>float</string>
+			<string>18</string>
+		</array>
+		<key>NSPaperSize</key>
+		<array>
+			<string>size</string>
+			<string>{595, 842}</string>
+		</array>
+		<key>NSPrintReverseOrientation</key>
+		<array>
+			<string>int</string>
+			<string>0</string>
+		</array>
+		<key>NSRightMargin</key>
+		<array>
+			<string>float</string>
+			<string>18</string>
+		</array>
+		<key>NSTopMargin</key>
+		<array>
+			<string>float</string>
+			<string>18</string>
+		</array>
+	</dict>
+	<key>PrintOnePage</key>
+	<false/>
+	<key>ReadOnly</key>
+	<string>NO</string>
+	<key>RowAlign</key>
+	<integer>1</integer>
+	<key>RowSpacing</key>
+	<real>36</real>
+	<key>SheetTitle</key>
+	<string>Canvas 1</string>
+	<key>SmartAlignmentGuidesActive</key>
+	<string>YES</string>
+	<key>SmartDistanceGuidesActive</key>
+	<string>YES</string>
+	<key>UniqueID</key>
+	<integer>1</integer>
+	<key>UseEntirePage</key>
+	<false/>
+	<key>VPages</key>
+	<integer>1</integer>
+	<key>WindowInfo</key>
+	<dict>
+		<key>CurrentSheet</key>
+		<integer>0</integer>
+		<key>ExpandedCanvases</key>
+		<array>
+			<dict>
+				<key>name</key>
+				<string>Canvas 1</string>
+			</dict>
+		</array>
+		<key>Frame</key>
+		<string>{{176, 63}, {1581, 1355}}</string>
+		<key>ListView</key>
+		<true/>
+		<key>OutlineWidth</key>
+		<integer>142</integer>
+		<key>RightSidebar</key>
+		<false/>
+		<key>ShowRuler</key>
+		<true/>
+		<key>Sidebar</key>
+		<true/>
+		<key>SidebarWidth</key>
+		<integer>120</integer>
+		<key>VisibleRegion</key>
+		<string>{{-164, -216}, {1446, 1216}}</string>
+		<key>Zoom</key>
+		<real>1</real>
+		<key>ZoomValues</key>
+		<array>
+			<array>
+				<string>Canvas 1</string>
+				<real>1</real>
+				<real>2</real>
+			</array>
+		</array>
+	</dict>
+</dict>
+</plist>
diff --git a/Documentation/position_control.odg b/Documentation/position_control.odg
deleted file mode 100644
index 5fb002c5e..000000000
--- a/Documentation/position_control.odg
+++ /dev/null
diff --git a/Documentation/position_control.png b/Documentation/position_control.png
deleted file mode 100644
index d8d1a8b0c..000000000
--- a/Documentation/position_control.png
+++ /dev/null
diff --git a/Documentation/position_estimator_app.odg b/Documentation/position_estimator_app.odg
deleted file mode 100644
index a6fc67373..000000000
--- a/Documentation/position_estimator_app.odg
+++ /dev/null
diff --git a/Documentation/position_estimator_app.pdf b/Documentation/position_estimator_app.pdf
deleted file mode 100644
index bc07209ee..000000000
--- a/Documentation/position_estimator_app.pdf
+++ /dev/null
diff --git a/Documentation/position_estimator_app.png b/Documentation/position_estimator_app.png
deleted file mode 100644
index 377549f63..000000000
--- a/Documentation/position_estimator_app.png
+++ /dev/null
diff --git a/Documentation/rc_mode_switch.odg b/Documentation/rc_mode_switch.odg
index e35a83372..29d738c39 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 823b1d868..856dd55c5 100644
--- a/Documentation/rc_mode_switch.pdf
+++ b/Documentation/rc_mode_switch.pdf
diff --git a/Documentation/state_machine.odg b/Documentation/state_machine.odg
deleted file mode 100644
index 2f55a13dd..000000000
--- a/Documentation/state_machine.odg
+++ /dev/null
diff --git a/Documentation/state_machine.png b/Documentation/state_machine.png
deleted file mode 100644
index 4daeddfc9..000000000
--- a/Documentation/state_machine.png
+++ /dev/null
diff --git a/Firmware.sublime-project b/Firmware.sublime-project
index 72bacee9f..7292307d5 100644
--- a/Firmware.sublime-project
+++ b/Firmware.sublime-project
@@ -32,7 +32,8 @@
 	"settings":
 	{
 		"tab_size": 8,
-		"translate_tabs_to_spaces": false
+		"translate_tabs_to_spaces": false,
+	       "highlight_line": true
 	},
 	"build_systems":
 	[
diff --git a/Images/px4fmu-v2.prototype b/Images/px4fmu-v2.prototype
new file mode 100644
index 000000000..5109b77d1
--- /dev/null
+++ b/Images/px4fmu-v2.prototype
@@ -0,0 +1,12 @@
+{
+    "board_id": 9, 
+    "magic": "PX4FWv1", 
+    "description": "Firmware for the PX4FMUv2 board", 
+    "image": "", 
+    "build_time": 0, 
+    "summary": "PX4FMUv2",
+    "version": "0.1",
+    "image_size": 0,
+    "git_identity": "",
+    "board_revision": 0
+}
diff --git a/Images/px4io-v2.prototype b/Images/px4io-v2.prototype
new file mode 100644
index 000000000..af87924e9
--- /dev/null
+++ b/Images/px4io-v2.prototype
@@ -0,0 +1,12 @@
+{
+    "board_id": 10, 
+    "magic": "PX4FWv2", 
+    "description": "Firmware for the PX4IOv2 board", 
+    "image": "", 
+    "build_time": 0, 
+    "summary": "PX4IOv2",
+    "version": "2.0",
+    "image_size": 0,
+    "git_identity": "",
+    "board_revision": 0
+}
diff --git a/Images/px4iov2.prototype b/Images/px4iov2.prototype
new file mode 100644
index 000000000..af87924e9
--- /dev/null
+++ b/Images/px4iov2.prototype
@@ -0,0 +1,12 @@
+{
+    "board_id": 10, 
+    "magic": "PX4FWv2", 
+    "description": "Firmware for the PX4IOv2 board", 
+    "image": "", 
+    "build_time": 0, 
+    "summary": "PX4IOv2",
+    "version": "2.0",
+    "image_size": 0,
+    "git_identity": "",
+    "board_revision": 0
+}
diff --git a/Makefile b/Makefile
index dea344390..0a8562251 100644
--- a/Makefile
+++ b/Makefile
@@ -40,14 +40,27 @@ export PX4_BASE		 := $(realpath $(dir $(lastword $(MAKEFILE_LIST))))/
 include $(PX4_BASE)makefiles/setup.mk
 
 #
-# Canned firmware configurations that we build.
+# Get a version string provided by git
+# This assumes that git command is available and that
+# the directory holding this file also contains .git directory
 #
-CONFIGS			?= $(subst config_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)config_*.mk))))
+GIT_DESC := $(shell git log -1 --pretty=format:%H)
+ifneq ($(words $(GIT_DESC)),1)
+    GIT_DESC := "unknown_git_version"
+endif
+export GIT_DESC
 
 #
-# Boards that we build NuttX export kits for.
+# Canned firmware configurations that we (know how to) build.
 #
-BOARDS			:= $(subst board_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)board_*.mk))))
+KNOWN_CONFIGS		:= $(subst config_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)config_*.mk))))
+CONFIGS			?= $(KNOWN_CONFIGS)
+
+#
+# Boards that we (know how to) build NuttX export kits for.
+#
+KNOWN_BOARDS		:= $(subst board_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)board_*.mk))))
+BOARDS			?= $(KNOWN_BOARDS)
 
 #
 # Debugging
@@ -87,10 +100,11 @@ endif
 #
 # Built products
 #
-STAGED_FIRMWARES	 = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4)
-FIRMWARES		 = $(foreach config,$(CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4)
+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:			$(STAGED_FIRMWARES)
+all:			$(DESIRED_FIRMWARES)
 
 #
 # Copy FIRMWARES into the image directory.
@@ -100,7 +114,7 @@ all:			$(STAGED_FIRMWARES)
 #     is taken care of.
 #
 $(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4
-	@echo %% Copying $@
+	@$(ECHO) %% Copying $@
 	$(Q) $(COPY) $< $@
 	$(Q) $(COPY) $(patsubst %.px4,%.bin,$<) $(patsubst %.px4,%.bin,$@)
 
@@ -111,17 +125,30 @@ $(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4
 $(BUILD_DIR)%.build/firmware.px4: config   = $(patsubst $(BUILD_DIR)%.build/firmware.px4,%,$@)
 $(BUILD_DIR)%.build/firmware.px4: work_dir = $(BUILD_DIR)$(config).build/
 $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4:
-	@echo %%%%
-	@echo %%%% Building $(config) in $(work_dir)
-	@echo %%%%
-	$(Q) mkdir -p $(work_dir)
-	$(Q) make -r -C $(work_dir) \
+	@$(ECHO) %%%%
+	@$(ECHO) %%%% Building $(config) in $(work_dir)
+	@$(ECHO) %%%%
+	$(Q) $(MKDIR) -p $(work_dir)
+	$(Q) $(MAKE) -r -C $(work_dir) \
 		-f $(PX4_MK_DIR)firmware.mk \
 		CONFIG=$(config) \
 		WORK_DIR=$(work_dir) \
 		$(FIRMWARE_GOAL)
 
 #
+# Make FMU firmwares depend on the corresponding IO firmware.
+#
+# This is a pretty vile hack, since it hard-codes knowledge of the FMU->IO dependency
+# and forces the _default config in all cases. There has to be a better way to do this...
+#
+FMU_VERSION		 = $(patsubst px4fmu-%,%,$(word 1, $(subst _, ,$(1))))
+define FMU_DEP
+$(BUILD_DIR)$(1).build/firmware.px4: $(IMAGE_DIR)px4io-$(call FMU_VERSION,$(1))_default.px4
+endef
+FMU_CONFIGS		:= $(filter px4fmu%,$(CONFIGS))
+$(foreach config,$(FMU_CONFIGS),$(eval $(call FMU_DEP,$(config))))
+
+#
 # Build the NuttX export archives.
 #
 # Note that there are no explicit dependencies extended from these
@@ -132,8 +159,6 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4:
 # XXX Should support fetching/unpacking from a separate directory to permit
 #     downloads of the prebuilt archives as well...
 #
-# XXX PX4IO configuration name is bad - NuttX configs should probably all be "px4"
-#
 NUTTX_ARCHIVES		 = $(foreach board,$(BOARDS),$(ARCHIVE_DIR)$(board).export)
 .PHONY:			archives
 archives:		$(NUTTX_ARCHIVES)
@@ -145,16 +170,55 @@ ifneq ($(filter archives,$(MAKECMDGOALS)),)
 endif
 
 $(ARCHIVE_DIR)%.export:	board = $(notdir $(basename $@))
-$(ARCHIVE_DIR)%.export:	configuration = $(if $(filter $(board),px4io),io,nsh)
-$(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) $(NUTTX_APPS)
-	@echo %% Configuring NuttX for $(board)
+$(ARCHIVE_DIR)%.export:	configuration = nsh
+$(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC)
+	@$(ECHO) %% Configuring NuttX for $(board)
 	$(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export)
-	$(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean
+	$(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean
+	$(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(board) .)
 	$(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration))
-	@echo %% Exporting NuttX for $(board)
-	$(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) export
-	$(Q) mkdir -p $(dir $@)
+	@$(ECHO) %% Exporting NuttX for $(board)
+	$(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export
+	$(Q) $(MKDIR) -p $(dir $@)
 	$(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@
+	$(Q) (cd $(NUTTX_SRC)/configs && $(RMDIR) $(board))
+
+#
+# The user can run the NuttX 'menuconfig' tool for a single board configuration with
+# make BOARDS=<boardname> menuconfig
+#
+ifeq ($(MAKECMDGOALS),menuconfig)
+ifneq ($(words $(BOARDS)),1)
+$(error BOARDS must specify exactly one board for the menuconfig goal)
+endif
+BOARD			 = $(BOARDS)
+menuconfig: $(NUTTX_SRC)
+	@$(ECHO) %% Configuring NuttX for $(BOARD)
+	$(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export)
+	$(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean
+	$(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(BOARD) .)
+	$(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(BOARD)/nsh)
+	@$(ECHO) %% Running menuconfig for $(BOARD)
+	$(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) menuconfig
+	@$(ECHO) %% Saving configuration file
+	$(Q)$(COPY) $(NUTTX_SRC).config $(PX4_BASE)nuttx-configs/$(BOARD)/nsh/defconfig
+else
+menuconfig:
+	@$(ECHO) ""
+	@$(ECHO) "The menuconfig goal must be invoked without any other goal being specified"
+	@$(ECHO) ""
+endif
+
+$(NUTTX_SRC):
+	@$(ECHO) ""
+	@$(ECHO) "NuttX sources missing - clone https://github.com/PX4/NuttX.git and try again."
+	@$(ECHO) ""
+
+#
+# Testing targets
+#
+testbuild:
+	$(Q) (cd $(PX4_BASE) && $(MAKE) distclean && $(MAKE) archives && $(MAKE) -j8)
 
 #
 # Cleanup targets.  'clean' should remove all built products and force
@@ -169,47 +233,51 @@ clean:
 .PHONY:	distclean
 distclean: clean
 	$(Q) $(REMOVE) $(ARCHIVE_DIR)*.export
-	$(Q) make -C $(NUTTX_SRC) -r $(MQUIET) distclean
+	$(Q) $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean
+	$(Q) (cd $(NUTTX_SRC)/configs && $(FIND) . -maxdepth 1 -type l -delete)
 
 #
 # Print some help text
 #
 .PHONY: help
 help:
-	@echo ""
-	@echo " PX4 firmware builder"
-	@echo " ===================="
-	@echo ""
-	@echo "  Available targets:"
-	@echo "  ------------------"
-	@echo ""
-	@echo "  archives"
-	@echo "    Build the NuttX RTOS archives that are used by the firmware build."
-	@echo ""
-	@echo "  all"
-	@echo "    Build all firmware configs: $(CONFIGS)"
-	@echo "    A limited set of configs can be built with CONFIGS=<list-of-configs>"
-	@echo ""
+	@$(ECHO) ""
+	@$(ECHO) " PX4 firmware builder"
+	@$(ECHO) " ===================="
+	@$(ECHO) ""
+	@$(ECHO) "  Available targets:"
+	@$(ECHO) "  ------------------"
+	@$(ECHO) ""
+	@$(ECHO) "  archives"
+	@$(ECHO) "    Build the NuttX RTOS archives that are used by the firmware build."
+	@$(ECHO) ""
+	@$(ECHO) "  all"
+	@$(ECHO) "    Build all firmware configs: $(CONFIGS)"
+	@$(ECHO) "    A limited set of configs can be built with CONFIGS=<list-of-configs>"
+	@$(ECHO) ""
 	@for config in $(CONFIGS); do \
-		echo "  $$config"; \
-		echo "    Build just the $$config firmware configuration."; \
-		echo ""; \
+		$(ECHO) "  $$config"; \
+		$(ECHO) "    Build just the $$config firmware configuration."; \
+		$(ECHO) ""; \
 	done
-	@echo "  clean"
-	@echo "    Remove all firmware build pieces."
-	@echo ""
-	@echo "  distclean"
-	@echo "    Remove all compilation products, including NuttX RTOS archives."
-	@echo ""
-	@echo "  upload"
-	@echo "    When exactly one config is being built, add this target to upload the"
-	@echo "    firmware to the board when the build is complete. Not supported for"
-	@echo "    all configurations."
-	@echo ""
-	@echo "  Common options:"
-	@echo "  ---------------"
-	@echo ""
-	@echo "  V=1"
-	@echo "    If V is set, more verbose output is printed during the build. This can"
-	@echo "    help when diagnosing issues with the build or toolchain."
-	@echo ""
+	@$(ECHO) "  clean"
+	@$(ECHO) "    Remove all firmware build pieces."
+	@$(ECHO) ""
+	@$(ECHO) "  distclean"
+	@$(ECHO) "    Remove all compilation products, including NuttX RTOS archives."
+	@$(ECHO) ""
+	@$(ECHO) "  upload"
+	@$(ECHO) "    When exactly one config is being built, add this target to upload the"
+	@$(ECHO) "    firmware to the board when the build is complete. Not supported for"
+	@$(ECHO) "    all configurations."
+	@$(ECHO) ""
+	@$(ECHO) "  testbuild"
+	@$(ECHO) "    Perform a complete clean build of the entire tree."
+	@$(ECHO) ""
+	@$(ECHO) "  Common options:"
+	@$(ECHO) "  ---------------"
+	@$(ECHO) ""
+	@$(ECHO) "  V=1"
+	@$(ECHO) "    If V is set, more verbose output is printed during the build. This can"
+	@$(ECHO) "    help when diagnosing issues with the build or toolchain."
+	@$(ECHO) ""
diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
new file mode 100644
index 000000000..ebe8a1a1e
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
@@ -0,0 +1,45 @@
+#!nsh
+#
+# HILStar / X-Plane
+#
+# 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/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery
new file mode 100644
index 000000000..63798bb3c
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery
@@ -0,0 +1,31 @@
+#!nsh
+#
+# Team Blacksheep Discovery Quadcopter
+#
+# Maintainers: Simon Wilks <sjwilks@gmail.com>
+#
+
+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
+	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
new file mode 100644
index 000000000..67c24fab3
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris
@@ -0,0 +1,53 @@
+#!nsh
+#
+# 3DR Iris Quadcopter
+#
+# Maintainers: Anton Babushkin <anton.babushkin@me.com>
+#
+
+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
+	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/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil
new file mode 100644
index 000000000..8c0797d7c
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil
@@ -0,0 +1,46 @@
+#!nsh
+#
+# HIL Quadcopter X
+#
+# Maintainers: 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
+
+set HIL yes
+
+set VEHICLE_TYPE mc
+set MIXER FMU_quad_x
diff --git a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil
new file mode 100644
index 000000000..46da24d35
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil
@@ -0,0 +1,45 @@
+#!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
new file mode 100644
index 000000000..bce3015fc
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil
@@ -0,0 +1,46 @@
+#!nsh
+#
+# HIL Quadcopter +
+#
+# Maintainers: 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
+
+set HIL yes
+
+set VEHICLE_TYPE mc
+set MIXER FMU_quad_+
diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil
new file mode 100644
index 000000000..46da24d35
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil
@@ -0,0 +1,45 @@
+#!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/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/11001_hexa_cox
new file mode 100644
index 000000000..2dc83a517
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox
@@ -0,0 +1,39 @@
+#!nsh
+#
+# UNTESTED UNTESTED!
+#
+# 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
+
+set VEHICLE_TYPE mc
+set MIXER hexa_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/12001_octo_cox b/ROMFS/px4fmu_common/init.d/12001_octo_cox
new file mode 100644
index 000000000..a55fc5a30
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox
@@ -0,0 +1,37 @@
+#!nsh
+#
+# 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
+
+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
new file mode 100644
index 000000000..0e5bf60d6
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar
@@ -0,0 +1,39 @@
+#!nsh
+#
+# 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
+ 
+set VEHICLE_TYPE fw
+set MIXER FMU_RET
diff --git a/ROMFS/px4fmu_common/init.d/2101_hk_bixler b/ROMFS/px4fmu_common/init.d/2101_hk_bixler
new file mode 100644
index 000000000..1ed923b19
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/2101_hk_bixler
@@ -0,0 +1,40 @@
+#!nsh
+
+echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker"
+
+#
+# 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
new file mode 100644
index 000000000..1ed923b19
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker
@@ -0,0 +1,40 @@
+#!nsh
+
+echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker"
+
+#
+# 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/3030_io_camflyer b/ROMFS/px4fmu_common/init.d/3030_io_camflyer
new file mode 100644
index 000000000..cbcc6189b
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer
@@ -0,0 +1,40 @@
+#!nsh
+
+echo "[init] PX4FMU v1, v2 with or without IO on Camflyer"
+
+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
new file mode 100644
index 000000000..4ebbe9c61
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/3031_phantom
@@ -0,0 +1,44 @@
+#!nsh
+#
+# Phantom FPV Flying Wing
+#
+# Maintainers: Simon Wilks <sjwilks@gmail.com>
+#
+
+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_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_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
+fi
+
+set VEHICLE_TYPE fw
+set MIXER FMU_Q
diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5
new file mode 100644
index 000000000..143310af9
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5
@@ -0,0 +1,43 @@
+#!nsh
+#
+# Skywalker X5 Flying Wing
+#
+# Maintainers: Thomas Gubler <thomasgubler@gmail.com>, Julian Oes <joes@student.ethz.ch>
+#
+
+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_X5
diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing
new file mode 100644
index 000000000..e53763278
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/3033_wingwing
@@ -0,0 +1,43 @@
+#!nsh
+#
+# Wing Wing (aka Z-84) Flying Wing
+#
+# Maintainers: Simon Wilks <sjwilks@gmail.com>
+#
+
+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/3034_fx79 b/ROMFS/px4fmu_common/init.d/3034_fx79
new file mode 100644
index 000000000..8d179d1fd
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/3034_fx79
@@ -0,0 +1,43 @@
+#!nsh
+#
+# FX-79 Buffalo Flying Wing
+#
+# Maintainers: 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
+
+set VEHICLE_TYPE fw
+set MIXER FMU_FX79
diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone
new file mode 100644
index 000000000..f6f09ac22
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/4008_ardrone
@@ -0,0 +1,57 @@
+#!nsh
+
+echo "[init] 08_ardrone: PX4FMU on PX4IOAR carrier board"
+ 
+#
+# 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
+#
+mavlink start -d /dev/ttyS0 -b 57600
+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 common for all multirotors apps
+#
+sh /etc/init.d/rc.multirotor
+ 
+# Exit, because /dev/ttyS0 is needed for MAVLink
+exit
diff --git a/ROMFS/px4fmu_common/init.d/4009_ardrone_flow b/ROMFS/px4fmu_common/init.d/4009_ardrone_flow
new file mode 100644
index 000000000..2886bcb75
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/4009_ardrone_flow
@@ -0,0 +1,83 @@
+#!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
new file mode 100644
index 000000000..ab1db94d0
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330
@@ -0,0 +1,51 @@
+#!nsh
+#
+# DJI Flame Wheel F330 Quadcopter
+#
+# Maintainers: Anton Babushkin <anton.babushkin@me.com>
+#
+
+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_YAWRATE_P 0.2
+	param set MC_YAWRATE_I 0.05
+	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_MAX 1900
diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450
new file mode 100644
index 000000000..299771c1d
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450
@@ -0,0 +1,37 @@
+#!nsh
+#
+# DJI Flame Wheel F450 Quadcopter
+#
+# 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_x
+
+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/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550
new file mode 100644
index 000000000..7e020cf59
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550
@@ -0,0 +1,33 @@
+#!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/5001_quad_+_pwm b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm
new file mode 100644
index 000000000..2e5f6ca4f
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm
@@ -0,0 +1,37 @@
+#!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_pwm b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm
new file mode 100644
index 000000000..447796709
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm
@@ -0,0 +1,37 @@
+#!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_+_pwm b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm
new file mode 100644
index 000000000..c4e9560d1
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm
@@ -0,0 +1,37 @@
+#!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_pwm b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm
new file mode 100644
index 000000000..ea56195d4
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm
@@ -0,0 +1,37 @@
+#!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_+_pwm b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm
new file mode 100644
index 000000000..f7693875c
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm
@@ -0,0 +1,37 @@
+#!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.FMU_quad_x b/ROMFS/px4fmu_common/init.d/rc.FMU_quad_x
deleted file mode 100644
index 980197d68..000000000
--- a/ROMFS/px4fmu_common/init.d/rc.FMU_quad_x
+++ /dev/null
@@ -1,67 +0,0 @@
-#!nsh
-#
-# Flight startup script for PX4FMU with PWM outputs.
-#
- 
-# Disable the USB interface
-set USB no
- 
-# Disable autostarting other apps
-set MODE custom
- 
-echo "[init] doing PX4FMU Quad startup..."
- 
-#
-# Start the ORB
-#
-uorb start
- 
-#
-# Load microSD params
-#
-echo "[init] loading microSD params"
-param select /fs/microsd/params
-if [ -f /fs/microsd/params ]
-then
-	param load /fs/microsd/params
-fi
- 
-#
-# Force some key parameters to sane values
-# MAV_TYPE     1 = fixed wing, 2 = quadrotor, 13 = hexarotor
-#              see https://pixhawk.ethz.ch/mavlink/
-#
-param set MAV_TYPE 2
- 
-#
-# Start MAVLink
-#
-mavlink start -d /dev/ttyS0 -b 57600
-usleep 5000
- 
-#
-# Start the sensors and test them.
-#
-sh /etc/init.d/rc.sensors
- 
-#
-# Start the commander.
-#
-commander start
- 
-#
-# Start the attitude estimator
-#
-attitude_estimator_ekf start
- 
-echo "[init] starting PWM output"
-fmu mode_pwm
-mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
- 
-#
-# Start attitude control
-#
-multirotor_att_control start
- 
-echo "[init] startup done, exiting"
-exit
\ No newline at end of file
diff --git a/ROMFS/px4fmu_common/init.d/rc.IO_QUAD b/ROMFS/px4fmu_common/init.d/rc.IO_QUAD
deleted file mode 100644
index 5f2de0d7e..000000000
--- a/ROMFS/px4fmu_common/init.d/rc.IO_QUAD
+++ /dev/null
@@ -1,107 +0,0 @@
-#!nsh
- 
-# Disable USB and autostart
-set USB no
-set MODE quad
- 
-#
-# Start the ORB (first app to start)
-#
-uorb start
- 
-#
-# Load microSD params
-#
-echo "[init] loading microSD params"
-param select /fs/microsd/params
-if [ -f /fs/microsd/params ]
-then
-	param load /fs/microsd/params
-fi
- 
-#
-# Force some key parameters to sane values
-# MAV_TYPE     1 = fixed wing, 2 = quadrotor, 13 = hexarotor
-#              see https://pixhawk.ethz.ch/mavlink/
-#
-param set MAV_TYPE 2
- 
-#
-# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell)
-#
-if [ -f /fs/microsd/px4io.bin ]
-then
-	echo "PX4IO Firmware found. Checking Upgrade.."
-	if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
-	then
-		echo "No newer version, skipping upgrade."
-	else
-		echo "Loading /fs/microsd/px4io.bin"
-		if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log
-		then
-			cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
-			echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log
-		else
-			echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log
-			echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode"
-		fi
-	fi
-fi
- 
-#
-# Start MAVLink (depends on orb)
-#
-mavlink start -d /dev/ttyS1 -b 57600
-usleep 5000
- 
-#
-# Start the commander (depends on orb, mavlink)
-#
-commander start
- 
-#
-# Start PX4IO interface (depends on orb, commander)
-#
-px4io start
- 
-#
-# Allow PX4IO to recover from midair restarts.
-# this is very unlikely, but quite safe and robust.
-px4io recovery
- 
-#
-# Start the sensors (depends on orb, px4io)
-#
-sh /etc/init.d/rc.sensors
- 
-#
-# Start GPS interface (depends on orb)
-#
-gps start
- 
-#
-# Start the attitude estimator (depends on orb)
-#
-attitude_estimator_ekf start
- 
-#
-# Load mixer and start controllers (depends on px4io)
-#
-mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix
-multirotor_att_control start
- 
-#
-# Start logging
-#
-#sdlog start -s 4
- 
-#
-# Start system state
-#
-if blinkm start
-then
-	echo "using BlinkM for state indication"
-	blinkm systemstate
-else
-	echo "no BlinkM found, OK."
-fi
\ No newline at end of file
diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IO b/ROMFS/px4fmu_common/init.d/rc.PX4IO
deleted file mode 100644
index 925a5703e..000000000
--- a/ROMFS/px4fmu_common/init.d/rc.PX4IO
+++ /dev/null
@@ -1,107 +0,0 @@
-#!nsh
- 
-# Disable USB and autostart
-set USB no
-set MODE camflyer
- 
-#
-# Start the ORB (first app to start)
-#
-uorb start
- 
-#
-# Load microSD params
-#
-echo "[init] loading microSD params"
-param select /fs/microsd/params
-if [ -f /fs/microsd/params ]
-then
-	param load /fs/microsd/params
-fi
- 
-#
-# Force some key parameters to sane values
-# MAV_TYPE     1 = fixed wing, 2 = quadrotor, 13 = hexarotor
-#              see https://pixhawk.ethz.ch/mavlink/
-#
-param set MAV_TYPE 1
- 
-#
-# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell)
-#
-if [ -f /fs/microsd/px4io.bin ]
-then
-	echo "PX4IO Firmware found. Checking Upgrade.."
-	if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
-	then
-		echo "No newer version, skipping upgrade."
-	else
-		echo "Loading /fs/microsd/px4io.bin"
-		if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log
-		then
-			cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
-			echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log
-		else
-			echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log
-			echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode"
-		fi
-	fi
-fi
- 
-#
-# Start MAVLink (depends on orb)
-#
-mavlink start -d /dev/ttyS1 -b 57600
-usleep 5000
- 
-#
-# Start the commander (depends on orb, mavlink)
-#
-commander start
- 
-#
-# Start PX4IO interface (depends on orb, commander)
-#
-px4io start
- 
-#
-# Allow PX4IO to recover from midair restarts.
-# this is very unlikely, but quite safe and robust.
-px4io recovery
- 
-#
-# Start the sensors (depends on orb, px4io)
-#
-sh /etc/init.d/rc.sensors
- 
-#
-# Start GPS interface (depends on orb)
-#
-gps start
- 
-#
-# Start the attitude estimator (depends on orb)
-#
-kalman_demo start
- 
-#
-# Load mixer and start controllers (depends on px4io)
-#
-mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
-control_demo start
- 
-#
-# Start logging
-#
-#sdlog start -s 4
- 
-#
-# Start system state
-#
-if blinkm start
-then
-	echo "using BlinkM for state indication"
-	blinkm systemstate
-else
-	echo "no BlinkM found, OK."
-fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR b/ROMFS/px4fmu_common/init.d/rc.PX4IOAR
deleted file mode 100644
index f55ac2ae3..000000000
--- a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR
+++ /dev/null
@@ -1,99 +0,0 @@
-#!nsh
-#
-# Flight startup script for PX4FMU on PX4IOAR carrier board.
-#
-
-# Disable the USB interface
-set USB no
-
-# Disable autostarting other apps
-set MODE ardrone
- 
-echo "[init] doing PX4IOAR startup..."
- 
-#
-# Start the ORB
-#
-uorb start
- 
-#
-# Load microSD params
-#
-echo "[init] loading microSD params"
-param select /fs/microsd/params
-if [ -f /fs/microsd/params ]
-then
-	param load /fs/microsd/params
-fi
-
-#
-# Force some key parameters to sane values
-# MAV_TYPE     1 = fixed wing, 2 = quadrotor, 13 = hexarotor
-#              see https://pixhawk.ethz.ch/mavlink/
-#
-param set MAV_TYPE 2
-
-#
-# Configure PX4FMU for operation with PX4IOAR
-#
-fmu mode_gpio_serial
- 
-#
-# Start the sensors.
-#
-sh /etc/init.d/rc.sensors
- 
-#
-# Start MAVLink
-#
-mavlink start -d /dev/ttyS0 -b 57600
-usleep 5000
- 
-#
-# Start the commander.
-#
-commander start
- 
-#
-# Start the attitude estimator
-#
-attitude_estimator_ekf start
- 
-#
-# Fire up the multi rotor attitude controller
-#
-multirotor_att_control start
- 
-#
-# Fire up the AR.Drone interface.
-#
-ardrone_interface start -d /dev/ttyS1
-
-#
-# Start logging
-#
-sdlog start -s 10
-
-#
-# Start GPS capture
-#
-gps start
-
-#
-# Start system state
-#
-if blinkm start
-then
-	echo "using BlinkM for state indication"
-	blinkm systemstate
-else
-	echo "no BlinkM found, OK."
-fi
- 
-#
-# startup is done; we don't want the shell because we
-# use the same UART for telemetry
-#
-echo "[init] startup done"
-
-exit
diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart
new file mode 100644
index 000000000..38b1cb57e
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.autostart
@@ -0,0 +1,204 @@
+#
+# Check if auto-setup from one of the standard scripts is wanted
+# SYS_AUTOSTART = 0 means no autostart (default)
+#
+# AUTOSTART PARTITION:
+#  0   	..	 999		Reserved (historical)
+#  1000	..	 1999		Simulation setups
+#  2000	..	 2999		Standard planes
+#  3000	..	 3999		Flying wing
+#  4000	..	 4999		Quad X
+#  5000	..	 5999		Quad +
+#  6000	..	 6999		Hexa X
+#  7000	..	 7999		Hexa +
+#  8000	..	 8999		Octo X
+#  9000	..	 9999		Octo +
+# 10000	..	10999		Wide arm / H frame
+# 11000	..	11999		Hexa Cox
+# 12000	..	12999		Octo Cox
+
+#
+# Simulation setups
+#
+
+if param compare SYS_AUTOSTART 1000
+then
+	sh /etc/init.d/1000_rc_fw_easystar.hil
+fi
+
+if param compare SYS_AUTOSTART 1001
+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
+fi
+
+if param compare SYS_AUTOSTART 1004
+then
+	sh /etc/init.d/1004_rc_fw_Rascal110.hil
+fi
+
+#
+# Standard plane
+#
+
+if param compare SYS_AUTOSTART 2100 100
+then
+	sh /etc/init.d/2100_mpx_easystar
+	set MODE custom
+fi
+
+if param compare SYS_AUTOSTART 2101 101
+then
+	sh /etc/init.d/2101_hk_bixler
+	set MODE custom
+fi
+
+if param compare SYS_AUTOSTART 2102 102
+then
+	sh /etc/init.d/2102_3dr_skywalker
+	set MODE custom
+fi
+
+#
+# Flying wing
+#
+
+if param compare SYS_AUTOSTART 3030 30
+then
+	sh /etc/init.d/3030_io_camflyer
+fi
+
+if param compare SYS_AUTOSTART 3031 31
+then
+	sh /etc/init.d/3031_phantom
+fi
+
+if param compare SYS_AUTOSTART 3032 32
+then
+	sh /etc/init.d/3032_skywalker_x5
+fi
+
+if param compare SYS_AUTOSTART 3033 33
+then
+	sh /etc/init.d/3033_wingwing
+fi
+
+if param compare SYS_AUTOSTART 3034 34
+then
+	sh /etc/init.d/3034_fx79
+fi
+
+#
+# Quad X
+#
+
+if param compare SYS_AUTOSTART 4008 8
+then
+	#sh /etc/init.d/4008_ardrone
+fi
+
+if param compare SYS_AUTOSTART 4009 9
+then
+	#sh /etc/init.d/4009_ardrone_flow
+fi
+
+if param compare SYS_AUTOSTART 4010 10
+then
+	sh /etc/init.d/4010_dji_f330
+fi
+
+if param compare SYS_AUTOSTART 4011 11
+then
+	sh /etc/init.d/4011_dji_f450
+fi
+
+if param compare SYS_AUTOSTART 4012 12
+then
+	sh /etc/init.d/4012_hk_x550
+fi
+
+#
+# Quad +
+#
+
+if param compare SYS_AUTOSTART 5001
+then
+	sh /etc/init.d/5001_quad_+_pwm
+fi
+
+#
+# Hexa X
+#
+
+if param compare SYS_AUTOSTART 6001
+then
+	sh /etc/init.d/6001_hexa_x_pwm
+fi
+
+#
+# Hexa +
+#
+
+if param compare SYS_AUTOSTART 7001
+then
+	sh /etc/init.d/7001_hexa_+_pwm
+fi
+
+#
+# Octo X
+#
+
+if param compare SYS_AUTOSTART 8001
+then
+	sh /etc/init.d/8001_octo_x_pwm
+fi
+
+#
+# Octo +
+#
+
+if param compare SYS_AUTOSTART 9001
+then
+	sh /etc/init.d/9001_octo_+_pwm
+fi
+
+#
+# Wide arm / H frame
+#
+
+if param compare SYS_AUTOSTART 10015 15
+then
+	sh /etc/init.d/10015_tbs_discovery
+fi
+
+if param compare SYS_AUTOSTART 10016 16
+then
+	sh /etc/init.d/10016_3dr_iris
+fi
+
+#
+# Hexa Coaxial
+#
+
+if param compare SYS_AUTOSTART 11001
+then
+	sh /etc/init.d/11001_hexa_cox
+fi
+
+#
+# Octo Coaxial
+#
+
+if param compare SYS_AUTOSTART 12001
+then
+	sh /etc/init.d/12001_octo_cox
+fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.boarddetect b/ROMFS/px4fmu_common/init.d/rc.boarddetect
deleted file mode 100644
index f233e51df..000000000
--- a/ROMFS/px4fmu_common/init.d/rc.boarddetect
+++ /dev/null
@@ -1,66 +0,0 @@
-#!nsh
-#
-# If we are still in flight mode, work out what airframe 
-# configuration we have and start up accordingly.
-#
-if [ $MODE != autostart ]
-then
-	echo "[init] automatic startup cancelled by user script"
-else
-	echo "[init] detecting attached hardware..."
-
-	#
-	# Assume that we are PX4FMU in standalone mode
-	#
-	set BOARD PX4FMU
-
-	#
-	# Are we attached to a PX4IOAR (AR.Drone carrier board)?
-	#
-	if boardinfo test name PX4IOAR
-	then
-		set BOARD PX4IOAR
-		if [ -f /etc/init.d/rc.PX4IOAR ]
-		then
-			echo "[init] reading /etc/init.d/rc.PX4IOAR"
-			usleep 500
-			sh /etc/init.d/rc.PX4IOAR
-		fi
-	else
-		echo "[init] PX4IOAR not detected"
-	fi
-
-	#
-	# Are we attached to a PX4IO?
-	#
-	if boardinfo test name PX4IO
-	then
-		set BOARD PX4IO
-		if [ -f /etc/init.d/rc.PX4IO ]
-		then
-			echo "[init] reading /etc/init.d/rc.PX4IO"
-			usleep 500
-			sh /etc/init.d/rc.PX4IO
-		fi
-	else
-		echo "[init] PX4IO not detected"
-	fi
-
-	#
-	# Looks like we are stand-alone
-	#
-	if [ $BOARD == PX4FMU ]
-	then
-		echo "[init] no expansion board detected"
-		if [ -f /etc/init.d/rc.standalone ]
-		then
-			echo "[init] reading /etc/init.d/rc.standalone"
-			sh /etc/init.d/rc.standalone
-		fi
-	fi
-
-	#
-	# We may not reach here if the airframe-specific script exits the shell.
-	#
-	echo "[init] startup done."
-fi
\ No newline at end of file
diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps
new file mode 100644
index 000000000..d354fb06f
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps
@@ -0,0 +1,19 @@
+#!nsh
+#
+# Standard apps for fixed wing
+#
+
+#
+# Start the attitude and position estimator
+#
+att_pos_estimator_ekf 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.hil b/ROMFS/px4fmu_common/init.d/rc.hil
deleted file mode 100644
index 7614ac0fe..000000000
--- a/ROMFS/px4fmu_common/init.d/rc.hil
+++ /dev/null
@@ -1,68 +0,0 @@
-#!nsh
-#
-# USB HIL start
-#
-
-echo "[HIL] starting.."
-
-uorb start
-
-# Tell MAVLink that this link is "fast"
-mavlink start -b 230400 -d /dev/console
-
-# Create a fake HIL /dev/pwm_output interface
-hil mode_pwm
-
-#
-# Load microSD params
-#
-echo "[init] loading microSD params"
-param select /fs/microsd/params
-if [ -f /fs/microsd/params ]
-then
-	param load /fs/microsd/params
-fi
-
-#
-# Force some key parameters to sane values
-# MAV_TYPE     1 = fixed wing, 2 = quadrotor, 13 = hexarotor
-#              see https://pixhawk.ethz.ch/mavlink/
-#
-param set MAV_TYPE 1
-
-#
-# Start the commander (depends on orb, mavlink)
-#
-commander start
-
-#
-# Check if we got an IO
-#
-if [ px4io start ]
-then
-	echo "IO started"
-else
-	fmu mode_serial
-	echo "FMU started"
-end
-
-#
-# Start the sensors (depends on orb, px4io)
-#
-sh /etc/init.d/rc.sensors
-
-#
-# Start the attitude estimator (depends on orb)
-#
-kalman_demo start
- 
-#
-# Load mixer and start controllers (depends on px4io)
-#
-mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix
-control_demo start
-
-echo "[HIL] setup done, running"
-
-# Exit shell to make it available to MAVLink
-exit 
diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface
new file mode 100644
index 000000000..d25f01dde
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.interface
@@ -0,0 +1,72 @@
+#!nsh
+#
+# Script to configure control interface
+#
+
+if [ $MIXER != none ]
+then
+	#
+	# Load mixer
+	#
+	set MIXERSD /fs/microsd/etc/mixers/$MIXER.mix
+	
+	#Use the mixer file from the SD-card if it exists
+	if [ -f $MIXERSD ]
+	then
+		set MIXER_FILE $MIXERSD
+	else
+		set MIXER_FILE /etc/mixers/$MIXER.mix
+	fi
+	
+	if [ $OUTPUT_MODE == mkblctrl ]
+	then
+		set OUTPUT_DEV /dev/mkblctrl
+	else
+		set OUTPUT_DEV /dev/pwm_output
+	fi
+	
+	if mixer load $OUTPUT_DEV $MIXER_FILE
+	then
+		echo "[init] Mixer loaded: $MIXER_FILE"
+	else
+		echo "[init] Error loading mixer: $MIXER_FILE"
+		tone_alarm $TUNE_OUT_ERROR
+	fi
+else
+	echo "[init] Mixer not defined"
+	tone_alarm $TUNE_OUT_ERROR
+fi
+
+if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ]
+then
+	if [ $PWM_OUTPUTS != none ]
+	then
+		#
+		# Set PWM output frequency
+		#
+		if [ $PWM_RATE != none ]
+		then
+			echo "[init] Set PWM rate: $PWM_RATE"
+			pwm rate -c $PWM_OUTPUTS -r $PWM_RATE
+		fi
+		
+		#
+		# Set disarmed, min and max PWM values
+		#
+		if [ $PWM_DISARMED != none ]
+		then
+			echo "[init] Set PWM disarmed: $PWM_DISARMED"
+			pwm disarmed -c $PWM_OUTPUTS -p $PWM_DISARMED
+		fi
+		if [ $PWM_MIN != none ]
+		then
+			echo "[init] Set PWM min: $PWM_MIN"
+			pwm min -c $PWM_OUTPUTS -p $PWM_MIN
+		fi
+		if [ $PWM_MAX != none ]
+		then
+			echo "[init] Set PWM max: $PWM_MAX"
+			pwm max -c $PWM_OUTPUTS -p $PWM_MAX
+		fi
+	fi
+fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io
new file mode 100644
index 000000000..c9d964f8e
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.io
@@ -0,0 +1,21 @@
+#
+# Init PX4IO interface
+#
+
+#
+# Allow PX4IO to recover from midair restarts.
+# this is very unlikely, but quite safe and robust.
+#
+px4io recovery
+
+#
+# Adjust PX4IO update rate limit
+#
+set PX4IO_LIMIT 400
+if hw_ver compare PX4FMU_V1
+then
+	set PX4IO_LIMIT 200
+fi
+
+echo "[init] Set PX4IO update rate limit: $PX4IO_LIMIT Hz"
+px4io limit $PX4IO_LIMIT
diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging
index 09c2d00d1..ac620844c 100644
--- a/ROMFS/px4fmu_common/init.d/rc.logging
+++ b/ROMFS/px4fmu_common/init.d/rc.logging
@@ -1,9 +1,14 @@
 #!nsh
 #
-# Initialise logging services.
+# Initialize logging services.
 #
 
 if [ -d /fs/microsd ]
 then
-	sdlog start
+	if hw_ver compare PX4FMU_V1
+	then
+		sdlog2 start -r 50 -a -b 16 -t
+	else
+		sdlog2 start -r 200 -a -b 16 -t
+	fi
 fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps
new file mode 100644
index 000000000..8b51d57e5
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps
@@ -0,0 +1,24 @@
+#!nsh
+#
+# Standard apps for multirotors
+#
+
+#
+# Start the attitude estimator
+#
+attitude_estimator_ekf start
+ 
+#
+# Start position estimator
+#
+position_estimator_inav start
+
+#
+# Start attitude control
+#
+multirotor_att_control start
+
+#
+# Start position control
+#
+multirotor_pos_control start
diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index 62c7184b8..badbf92c3 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -7,25 +7,47 @@
 # Start sensor drivers here.
 #
 
-#
-# Check for UORB
-#
-if uorb start
-then
-	echo "uORB started"
-fi
-
 ms5611 start
 adc start
 
+# Mag might be external
+if hmc5883 start
+then
+	echo "[init] Using HMC5883"
+fi
+
 if mpu6000 start
 then
-	echo "using MPU6000 and HMC5883L"
-	hmc5883 start
+	echo "[init] Using MPU6000"
+fi
+
+if l3gd20 start
+then
+	echo "[init] Using L3GD20(H)"
+fi
+
+if hw_ver compare PX4FMU_V2
+then
+	if lsm303d start
+	then
+		echo "[init] Using LSM303D"
+	fi
+fi
+
+# Start airspeed sensors
+if meas_airspeed start
+then
+	echo "[init] Using MEAS airspeed sensor"
 else
-	echo "using L3GD20 and LSM303D"
-	l3gd20 start
-	lsm303 start
+	if ets_airspeed start
+	then
+		echo "[init] Using ETS airspeed sensor (bus 3)"
+	else
+		if ets_airspeed start -b 1
+		then
+			echo "[init] Using ETS airspeed sensor (bus 1)"
+		fi
+	fi
 fi
 
 #
@@ -34,9 +56,7 @@ fi
 # ALWAYS start this task before the
 # preflight_check.
 #
-sensors start
-
-#
-# Check sensors - run AFTER 'sensors start'
-#
-preflight_check
\ No newline at end of file
+if sensors start
+then
+	preflight_check &
+fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb
index 31af3991a..0cd8a0e04 100644
--- a/ROMFS/px4fmu_common/init.d/rc.usb
+++ b/ROMFS/px4fmu_common/init.d/rc.usb
@@ -5,8 +5,36 @@
 
 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"
-mavlink start -b 230400 -d /dev/console
+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.."
 
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 0efcc5db9..d12785368 100755..100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -1,88 +1,521 @@
 #!nsh
 #
 # PX4FMU startup script.
-#
-# This script is responsible for:
-#
-# - mounting the microSD card (if present)
-# - running the user startup script from the microSD card (if present)
-# - detecting the configuration of the system and picking a suitable
-#   startup script to continue with
-#
-# Note: DO NOT add configuration-specific commands to this script;
-#       add them to the per-configuration scripts instead.
-#
 
 #
 # Default to auto-start mode.  An init script on the microSD card
 # can change this to prevent automatic startup of the flight script.
 #
 set MODE autostart
-set USB autoconnect
 
-#
-
-#
+set RC_FILE /fs/microsd/etc/rc.txt
+set CONFIG_FILE /fs/microsd/etc/config.txt
+set EXTRAS_FILE /fs/microsd/etc/extras.txt
 
+set TUNE_OUT_ERROR ML<<CP4CP4CP4CP4CP4
 
 #
 # Try to mount the microSD card.
 #
-echo "[init] looking for microSD..."
+echo "[init] Looking for microSD..."
 if mount -t vfat /dev/mmcsd0 /fs/microsd
 then
-	echo "[init] card mounted at /fs/microsd"
+	set LOG_FILE /fs/microsd/bootlog.txt
+	echo "[init] microSD card mounted at /fs/microsd"
 	# Start playing the startup tune
 	tone_alarm start
 else
-	echo "[init] no microSD card found"
+	set LOG_FILE /dev/null
+	echo "[init] No microSD card found"
 	# Play SOS
-	tone_alarm 2
+	tone_alarm error
 fi
 
 #
 # Look for an init script on the microSD card.
+# Disable autostart if the script found.
 #
-# To prevent automatic startup in the current flight mode,
-# the script should set MODE to some other value.
-#
-if [ -f /fs/microsd/etc/rc ]
+if [ -f $RC_FILE ]
 then
-	echo "[init] reading /fs/microsd/etc/rc"
-	sh /fs/microsd/etc/rc
-fi
-# Also consider rc.txt files
-if [ -f /fs/microsd/etc/rc.txt ]
-then
-	echo "[init] reading /fs/microsd/etc/rc.txt"
-	sh /fs/microsd/etc/rc.txt
+	echo "[init] Executing init script: $RC_FILE"
+	sh $RC_FILE
+	set MODE custom
+else
+	echo "[init] Init script not found: $RC_FILE"
 fi
 
-#
-# Check for USB host
-#
-if [ $USB != autoconnect ]
+# if this is an APM build then there will be a rc.APM script
+# from an EXTERNAL_SCRIPTS build option
+if [ -f /etc/init.d/rc.APM ]
 then
-	echo "[init] not connecting USB"
-else
 	if sercon
 	then
 		echo "[init] USB interface connected"
+	fi
+
+	echo "[init] Running rc.APM"
+	# if APM startup is successful then nsh will exit
+	sh /etc/init.d/rc.APM
+fi
+
+if [ $MODE == autostart ]
+then
+	echo "[init] AUTOSTART mode"
+
+	#
+	# Start CDC/ACM serial driver
+	#
+	sercon
+	
+	#
+	# Start the ORB (first app to start)
+	#
+	uorb start
+	
+	#
+	# Load parameters
+	#
+	set PARAM_FILE /fs/microsd/params
+	if mtd start
+	then
+		set PARAM_FILE /fs/mtd_params
+	fi
+	
+	param select $PARAM_FILE
+	if param load
+	then
+		echo "[init] Parameters loaded: $PARAM_FILE"
+	else
+		echo "[init] ERROR: Parameters loading failed: $PARAM_FILE"
+	fi
+	
+	#
+	# Start system state indicator
+	#
+	if rgbled start
+	then
+		echo "[init] Using external RGB Led"
 	else
-		if [ -f /dev/ttyACM0 ]
-			echo "[init] NSH via USB"
+		if blinkm start
 		then
+			echo "[init] Using blinkm"
+			blinkm systemstate
+		fi
+	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
+	set PWM_DISARMED none
+	set PWM_MIN none
+	set PWM_MAX none
+	set MKBLCTRL_MODE none
+	set FMU_MODE pwm
+	set MAV_TYPE none
+	
+	#
+	# Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts
+	#
+	if param compare SYS_AUTOCONFIG 1
+	then
+		set DO_AUTOCONFIG yes
+	else
+		set DO_AUTOCONFIG 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"
+	else
+		sh /etc/init.d/rc.autostart
+	fi
+
+	#
+	# Override parameters from user configuration file
+	#
+	if [ -f $CONFIG_FILE ]
+	then
+		echo "[init] Reading config: $CONFIG_FILE"
+		sh $CONFIG_FILE
+	else
+		echo "[init] Config file not found: $CONFIG_FILE"
+	fi
+
+	#
+	# If autoconfig parameter was set, reset it and save parameters
+	#
+	if [ $DO_AUTOCONFIG == yes ]
+	then
+		param set SYS_AUTOCONFIG 0
+		param save
+	fi
+	
+	set IO_PRESENT no
+	
+	if [ $USE_IO == yes ]
+	then
+		#
+		# Check if PX4IO present and update firmware if needed
+		#
+		if [ -f /etc/extras/px4io-v2_default.bin ]
+		then
+			set IO_FILE /etc/extras/px4io-v2_default.bin
+		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] No USB connected"
+			echo "[init] Trying to update"
+			echo "PX4IO Trying to update" >> $LOG_FILE
+			
+			tone_alarm MLL32CP8MB
+			
+			if px4io forceupdate 14662 $IO_FILE
+			then
+				usleep 500000
+				if px4io checkcrc $IO_FILE
+				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"
+					echo "PX4IO update failed" >> $LOG_FILE
+					tone_alarm $TUNE_OUT_ERROR
+				fi
+			else
+				echo "[init] ERROR: PX4IO update failed"
+				echo "PX4IO update failed" >> $LOG_FILE
+				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
+	#
+	if [ $OUTPUT_MODE == none ]
+	then
+		if [ $USE_IO == yes ]
+		then
+			set OUTPUT_MODE io
+		else
+			set OUTPUT_MODE fmu
 		fi
 	fi
-fi
 
-# if this is an APM build then there will be a rc.APM script
-# from an EXTERNAL_SCRIPTS build option
-if [ -f /etc/init.d/rc.APM ]
-then
-	echo Running rc.APM
-	# if APM startup is successful then nsh will exit
-	sh /etc/init.d/rc.APM
+	if [ $OUTPUT_MODE == io -a $IO_PRESENT != yes ]
+	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
+		then
+			set FMU_MODE serial
+		fi
+	fi
+
+	if [ $HIL == yes ]
+	then
+		set OUTPUT_MODE hil
+		if hw_ver compare PX4FMU_V1
+		then
+			set FMU_MODE serial
+		fi
+	else
+		# Try to get an USB console if not in HIL mode
+		nshterm /dev/ttyACM0 &
+	fi
+	
+	#
+	# 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
+		if [ $OUTPUT_MODE == io ]
+		then
+			echo "[init] Use PX4IO PWM as primary output"
+			if px4io start
+			then
+				echo "[init] PX4IO started"
+				sh /etc/init.d/rc.io
+			else
+				echo "[init] ERROR: PX4IO start failed"
+				tone_alarm $TUNE_OUT_ERROR
+			fi
+		fi
+		if [ $OUTPUT_MODE == fmu ]
+		then
+			echo "[init] Use FMU PWM as primary output"
+			if fmu mode_$FMU_MODE
+			then
+				echo "[init] FMU mode_$FMU_MODE started"
+			else
+				echo "[init] ERROR: FMU mode_$FMU_MODE start failed"
+				tone_alarm $TUNE_OUT_ERROR
+			fi
+			
+			if hw_ver compare PX4FMU_V1
+			then
+				if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ]
+				then
+					set TTYS1_BUSY yes
+				fi
+				if [ $FMU_MODE == pwm_gpio ]
+				then
+					set TTYS1_BUSY yes
+				fi
+			fi
+		fi
+		if [ $OUTPUT_MODE == mkblctrl ]
+		then
+			echo "[init] Use MKBLCTRL as primary output"
+			set MKBLCTRL_ARG ""
+			if [ $MKBLCTRL_MODE == x ]
+			then
+				set MKBLCTRL_ARG "-mkmode x"
+			fi
+			if [ $MKBLCTRL_MODE == + ]
+			then
+				set MKBLCTRL_ARG "-mkmode +"
+			fi
+			
+			if mkblctrl $MKBLCTRL_ARG
+			then
+				echo "[init] MKBLCTRL started"
+			else
+				echo "[init] ERROR: MKBLCTRL start failed"
+				tone_alarm $TUNE_OUT_ERROR
+			fi
+			
+		fi		
+		if [ $OUTPUT_MODE == hil ]
+		then
+			echo "[init] Use HIL as primary output"
+			if hil mode_pwm
+			then
+				echo "[init] HIL output started"
+			else
+				echo "[init] ERROR: HIL output start failed"
+				tone_alarm $TUNE_OUT_ERROR
+			fi
+		fi
+		
+		#
+		# Start IO or FMU for RC PPM input if needed
+		#
+		if [ $IO_PRESENT == yes ]
+		then
+			if [ $OUTPUT_MODE != io ]
+			then
+				if px4io start
+				then
+					echo "[init] PX4IO started"
+					sh /etc/init.d/rc.io
+				else
+					echo "[init] ERROR: PX4IO start failed"
+					tone_alarm $TUNE_OUT_ERROR
+				fi
+			fi
+		else
+			if [ $OUTPUT_MODE != fmu ]
+			then
+				if fmu mode_$FMU_MODE
+				then
+					echo "[init] FMU mode_$FMU_MODE started"
+				else
+					echo "[init] ERROR: FMU mode_$FMU_MODE start failed"
+					tone_alarm $TUNE_OUT_ERROR
+				fi
+	
+				if hw_ver compare PX4FMU_V1
+				then
+					if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ]
+					then
+						set TTYS1_BUSY yes
+					fi
+					if [ $FMU_MODE == pwm_gpio ]
+					then
+						set TTYS1_BUSY yes
+					fi
+				fi
+			fi
+		fi
+	fi
+	
+	#
+	# MAVLink
+	#
+	set EXIT_ON_END no
+	
+	if [ $HIL == yes ]
+	then
+		sleep 1
+		mavlink start -b 230400 -d /dev/ttyACM0 
+		usleep 5000
+	else
+		if [ $TTYS1_BUSY == yes ]
+		then
+			# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
+			mavlink start -d /dev/ttyS0
+			usleep 5000
+			
+			# Exit from nsh to free port for mavlink
+			set EXIT_ON_END yes
+		else
+			# Start MAVLink on default port: ttyS1
+			mavlink start
+			usleep 5000
+		fi
+	fi
+	
+	#
+	# Sensors, Logging, GPS
+	#
+	echo "[init] Start sensors"
+	sh /etc/init.d/rc.sensors
+
+	if [ $HIL == no ]
+	then
+		echo "[init] Start logging"
+		sh /etc/init.d/rc.logging
+		
+		echo "[init] Start GPS"
+		gps start
+	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
+	fi
+
+	#
+	# Multicopters setup
+	#
+	if [ $VEHICLE_TYPE == mc ]
+	then
+		echo "[init] Vehicle type: MULTICOPTER"
+
+		if [ $MIXER == none ]
+		then
+			# Set default mixer for multicopter if not defined
+			set MIXER quad_x
+		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_hexa_x -o $MIXER == FMU_hexa_+ ]
+			then
+				set MAV_TYPE 13
+			fi
+			if [ $MIXER == hexa_cox ]
+			then
+				set MAV_TYPE 13
+			fi
+			if [ $MIXER == FMU_octo_x -o $MIXER == FMU_octo_+ ]
+			then
+				set MAV_TYPE 14
+			fi
+			if [ $MIXER == FMU_octo_cox ]
+			then
+				set MAV_TYPE 14
+			fi
+		fi
+
+		param set MAV_TYPE $MAV_TYPE
+		
+		# Load mixer and configure outputs
+		sh /etc/init.d/rc.interface
+		
+		# Start standard multicopter apps
+		sh /etc/init.d/rc.mc_apps
+	fi
+
+	#
+	# Generic setup (autostart ID not found)
+	#
+	if [ $VEHICLE_TYPE == none ]
+	then
+		echo "[init] Vehicle type: GENERIC"
+
+		# Load mixer and configure outputs
+		sh /etc/init.d/rc.interface
+	fi
+
+	# Start any custom addons
+	if [ -f $EXTRAS_FILE ]
+	then
+		echo "[init] Starting addons script: $EXTRAS_FILE"
+		sh $EXTRAS_FILE
+	else
+		echo "[init] Addons script not found: $EXTRAS_FILE"
+	fi
+
+	if [ $EXIT_ON_END == yes ]
+	then
+		exit
+	fi
+
+# End of autostart
 fi
diff --git a/ROMFS/px4fmu_common/logging/conv.zip b/ROMFS/px4fmu_common/logging/conv.zip
new file mode 100644
index 000000000..7f485184c
--- /dev/null
+++ b/ROMFS/px4fmu_common/logging/conv.zip
diff --git a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix
index 75e82bb00..0ec663e35 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix
@@ -62,3 +62,23 @@ range.  Inputs below zero are treated as zero.
 M: 1
 O:      10000  10000      0 -10000  10000
 S: 0 3      0  20000 -10000 -10000  10000
+
+
+Gimbal / flaps / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_AET.mix b/ROMFS/px4fmu_common/mixers/FMU_AET.mix
index 20cb88b91..c73cb2a62 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_AET.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_AET.mix
@@ -58,3 +58,23 @@ range.  Inputs below zero are treated as zero.
 M: 1
 O:      10000  10000      0 -10000  10000
 S: 0 3      0  20000 -10000 -10000  10000
+
+
+Gimbal / flaps / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_FX79.mix b/ROMFS/px4fmu_common/mixers/FMU_FX79.mix
new file mode 100755
index 000000000..0a1dca98d
--- /dev/null
+++ b/ROMFS/px4fmu_common/mixers/FMU_FX79.mix
@@ -0,0 +1,72 @@
+FX-79 Delta-wing mixer for PX4FMU
+=================================
+
+Designed for FX-79.
+
+TODO (sjwilks): Add mixers for flaps.
+
+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   5000   8000      0 -10000  10000
+S: 0 1   8000   8000      0 -10000  10000
+
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 0 0   8000   5000      0 -10000  10000
+S: 0 1  -8000  -8000      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
+
+Gimbal / flaps / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_Q.mix b/ROMFS/px4fmu_common/mixers/FMU_Q.mix
index ebcb66b24..b8ecbc879 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_Q.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_Q.mix
@@ -25,13 +25,13 @@ for the elevons.
 
 M: 2
 O:      10000  10000      0 -10000  10000
-S: 0 0  -5000  -8000      0 -10000  10000
-S: 0 1   8000   8000      0 -10000  10000
+S: 0 0  -8000  -8000      0 -10000  10000
+S: 0 1   6000   6000      0 -10000  10000
 
 M: 2
 O:      10000  10000      0 -10000  10000
-S: 0 0  -8000  -5000      0 -10000  10000
-S: 0 1  -8000  -8000      0 -10000  10000
+S: 0 0  -8000  -8000      0 -10000  10000
+S: 0 1  -6000  -6000      0 -10000  10000
 
 Output 2
 --------
@@ -50,3 +50,21 @@ M: 1
 O:      10000  10000      0 -10000  10000
 S: 0 3      0  20000 -10000 -10000  10000
 
+Gimbal / flaps / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_RET.mix b/ROMFS/px4fmu_common/mixers/FMU_RET.mix
index 95beb8927..f07c34ac8 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_RET.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_RET.mix
@@ -51,3 +51,23 @@ range.  Inputs below zero are treated as zero.
 M: 1
 O:      10000  10000      0 -10000  10000
 S: 0 3      0  20000 -10000 -10000  10000
+
+
+Gimbal / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_X5.mix b/ROMFS/px4fmu_common/mixers/FMU_X5.mix
index 9f81e1dc3..80e3bac09 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_X5.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_X5.mix
@@ -23,13 +23,13 @@ for the elevons.
 
 M: 2
 O:      10000  10000      0 -10000  10000
-S: 0 0  -3000  -5000      0 -10000  10000
-S: 0 1  -5000  -5000      0 -10000  10000
+S: 0 0  -8000  -8000      0 -10000  10000
+S: 0 1   6000   6000      0 -10000  10000
 
 M: 2
 O:      10000  10000      0 -10000  10000
-S: 0 0  -5000  -3000      0 -10000  10000
-S: 0 1   5000   5000      0 -10000  10000
+S: 0 0  -8000  -8000      0 -10000  10000
+S: 0 1  -6000  -6000      0 -10000  10000
 
 Output 2
 --------
@@ -48,3 +48,21 @@ M: 1
 O:      10000  10000      0 -10000  10000
 S: 0 3      0  20000 -10000 -10000  10000
 
+Gimbal / flaps / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_delta.mix b/ROMFS/px4fmu_common/mixers/FMU_delta.mix
index 981466704..f0aa6650d 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_delta.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_delta.mix
@@ -48,3 +48,23 @@ M: 1
 O:      10000  10000      0 -10000  10000
 S: 0 3      0  20000 -10000 -10000  10000
 
+
+Gimbal / flaps / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
+
diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix
deleted file mode 100644
index b5e38ce9e..000000000
--- a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix
+++ /dev/null
@@ -1,7 +0,0 @@
-Multirotor mixer for PX4FMU
-===========================
-
-This file defines a single mixer for a hexacopter in the + configuration. All controls
-are mixed 100%.
-
-R: 6+ 10000 10000 10000 0
diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix
deleted file mode 100644
index 8e8d122ad..000000000
--- a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix
+++ /dev/null
@@ -1,7 +0,0 @@
-Multirotor mixer for PX4FMU
-===========================
-
-This file defines a single mixer for a hexacopter in the X configuration.  All controls
-are mixed 100%.
-
-R: 6x 10000 10000 10000 0
diff --git a/ROMFS/px4fmu_common/mixers/FMU_hexa_+.mix b/ROMFS/px4fmu_common/mixers/FMU_hexa_+.mix
new file mode 100644
index 000000000..e608b459f
--- /dev/null
+++ b/ROMFS/px4fmu_common/mixers/FMU_hexa_+.mix
@@ -0,0 +1,3 @@
+# Hexa +
+
+R: 6+ 10000 10000 10000 0
diff --git a/ROMFS/px4fmu_common/mixers/FMU_hexa_x.mix b/ROMFS/px4fmu_common/mixers/FMU_hexa_x.mix
new file mode 100644
index 000000000..16e6e22f9
--- /dev/null
+++ b/ROMFS/px4fmu_common/mixers/FMU_hexa_x.mix
@@ -0,0 +1,3 @@
+# Hexa X
+
+R: 6x 10000 10000 10000 0
diff --git a/ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix b/ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix
new file mode 100644
index 000000000..f7845450d
--- /dev/null
+++ b/ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix
@@ -0,0 +1,3 @@
+# Octo coaxial
+
+R: 8c 10000 10000 10000 0
diff --git a/ROMFS/px4fmu_common/mixers/FMU_octo_x.mix b/ROMFS/px4fmu_common/mixers/FMU_octo_x.mix
index edc71f013..c9a348aa4 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_octo_x.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_octo_x.mix
@@ -1,7 +1,3 @@
-Multirotor mixer for PX4FMU
-===========================
-
-This file defines a single mixer for a octocopter in the X configuration.  All controls
-are mixed 100%.
+# Octo X
 
 R: 8x 10000 10000 10000 0
diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix
index dfdf1d58e..cd9a9cfab 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix
@@ -5,3 +5,23 @@ This file defines a single mixer for a quadrotor in the + configuration. All con
 are mixed 100%.
 
 R: 4+ 10000 10000 10000 0
+
+
+Gimbal / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix
index 2a4a0f341..520aba635 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix
@@ -5,3 +5,22 @@ This file defines a single mixer for a quadrotor in the V configuration.  All co
 are mixed 100%.
 
 R: 4v 10000 10000 10000 0
+
+Gimbal / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix
index 81b4af30b..58e6af74b 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix
@@ -4,3 +4,22 @@ Multirotor mixer for PX4FMU
 This file defines a single mixer for a quadrotor with a wide configuration.  All controls are mixed 100%.
 
 R: 4w 10000 10000 10000 0
+
+Gimbal / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix
index 12a3bee20..fa21c8012 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix
@@ -5,3 +5,22 @@ This file defines a single mixer for a quadrotor in the X configuration.  All co
 are mixed 100%.
 
 R: 4x 10000 10000 10000 0
+
+Gimbal / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/IO_pass.mix b/ROMFS/px4fmu_common/mixers/IO_pass.mix
new file mode 100644
index 000000000..39f875ddb
--- /dev/null
+++ b/ROMFS/px4fmu_common/mixers/IO_pass.mix
@@ -0,0 +1,38 @@
+Passthrough mixer for PX4IO
+============================
+
+This file defines passthrough mixers suitable for testing.
+
+Channel group 0, channels 0-7 are passed directly through to the outputs.
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 0  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 1  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 2  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 3  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/README b/ROMFS/px4fmu_common/mixers/README
deleted file mode 100644
index 6649c53c2..000000000
--- a/ROMFS/px4fmu_common/mixers/README
+++ /dev/null
@@ -1,154 +0,0 @@
-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.
diff --git a/ROMFS/px4fmu_common/mixers/hexa_cox.mix b/ROMFS/px4fmu_common/mixers/hexa_cox.mix
new file mode 100644
index 000000000..497786feb
--- /dev/null
+++ b/ROMFS/px4fmu_common/mixers/hexa_cox.mix
@@ -0,0 +1,3 @@
+# Hexa coaxial
+
+R: 6c 10000 10000 10000 0
diff --git a/ROMFS/px4fmu_common/init.d/rc.standalone b/ROMFS/px4fmu_test/init.d/rc.standalone
index 67e95215b..67e95215b 100644
--- a/ROMFS/px4fmu_common/init.d/rc.standalone
+++ b/ROMFS/px4fmu_test/init.d/rc.standalone
diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS
new file mode 100644
index 000000000..56482d140
--- /dev/null
+++ b/ROMFS/px4fmu_test/init.d/rcS
@@ -0,0 +1,77 @@
+#!nsh
+#
+# PX4FMU startup script for test hackery.
+#
+uorb start
+
+if sercon
+then
+	echo "[init] USB interface connected"
+
+	# Try to get an USB console
+	nshterm /dev/ttyACM0 &
+fi
+
+#
+# Try to mount the microSD card.
+#
+echo "[init] looking for microSD..."
+if mount -t vfat /dev/mmcsd0 /fs/microsd
+then
+	echo "[init] card mounted at /fs/microsd"
+	# Start playing the startup tune
+	tone_alarm start
+else
+	echo "[init] no microSD card found"
+	# Play SOS
+	tone_alarm error
+fi
+
+#
+# Start a minimal system
+#
+
+if [ -f /etc/extras/px4io-v2_default.bin ]
+then
+	set io_file /etc/extras/px4io-v2_default.bin
+else
+	set io_file /etc/extras/px4io-v1_default.bin
+fi
+
+if px4io start
+then
+	echo "PX4IO OK"
+fi
+
+if px4io checkcrc $io_file
+then
+	echo "PX4IO CRC OK"
+else
+	echo "PX4IO CRC failure"
+	tone_alarm MBABGP
+	if px4io forceupdate 14662 $io_file
+	then
+		usleep 500000
+		if px4io start
+		then
+			echo "PX4IO restart OK"
+			tone_alarm MSPAA
+		else
+			echo "PX4IO restart failed"
+			tone_alarm MNGGG
+			sleep 5
+			reboot
+		fi
+	else
+		echo "PX4IO update failed"
+		tone_alarm MNGGG
+	fi
+fi
+
+#
+# The presence of this file suggests we're running a mount stress test
+#
+if [ -f /fs/microsd/mount_test_cmds.txt ]
+then
+	tests mount
+fi
diff --git a/Tools/fix_code_style.sh b/Tools/fix_code_style.sh
index 832ee79da..0b6743013 100755
--- a/Tools/fix_code_style.sh
+++ b/Tools/fix_code_style.sh
@@ -16,4 +16,5 @@ astyle \
     --ignore-exclude-errors-x	\
     --lineend=linux		\
     --exclude=EASTL		\
+    --add-brackets \
     $*
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/logconv.py b/Tools/logconv.py
deleted file mode 100644
index c47d22a45..000000000
--- a/Tools/logconv.py
+++ /dev/null
@@ -1,59 +0,0 @@
-#!/usr/bin/env python
-
-"""Convert binary log generated by sdlog to CSV format
-    
-Usage: python logconv.py <log.bin>"""
-
-__author__  = "Anton Babushkin"
-__version__ = "0.1"
-
-import struct, sys
-
-def _unpack_packet(data):
-    s = ""
-    s += "Q"    #.timestamp = buf.raw.timestamp,
-    s += "fff"  #.gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]},
-    s += "fff"  #.accel = {buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], buf.raw.accelerometer_m_s2[2]},
-    s += "fff"  #.mag = {buf.raw.magnetometer_ga[0], buf.raw.magnetometer_ga[1], buf.raw.magnetometer_ga[2]},
-    s += "f"    #.baro = buf.raw.baro_pres_mbar,
-    s += "f"    #.baro_alt = buf.raw.baro_alt_meter,
-    s += "f"    #.baro_temp = buf.raw.baro_temp_celcius,
-    s += "ffff" #.control = {buf.act_controls.control[0], buf.act_controls.control[1], buf.act_controls.control[2], buf.act_controls.control[3]},
-    s += "ffffffff"    #.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]},
-    s += "f"    #.vbat = buf.batt.voltage_v,
-    s += "f"    #.bat_current = buf.batt.current_a,
-    s += "f"    #.bat_discharged = buf.batt.discharged_mah,
-    s += "ffff" #.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]},
-    s += "fff"  #.local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
-    s += "iii"  #.gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt},
-    s += "fff"  #.attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw},
-    s += "fffffffff"    #.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]},
-    s += "fff"  #.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},
-    s += "ffff" #.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]},
-    s += "ffffff"    #.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},
-    s += "f"    #.diff_pressure = buf.diff_pres.differential_pressure_pa,
-    s += "f"    #.ind_airspeed = buf.airspeed.indicated_airspeed_m_s,
-    s += "f"    #.true_airspeed = buf.airspeed.true_airspeed_m_s
-    s += "iii"  # to align to 280
-    d = struct.unpack(s, data)
-    return d
-
-def _main():
-    if len(sys.argv) < 2:
-        print "Usage:\npython logconv.py <log.bin>"
-        return
-    fn = sys.argv[1]
-    sysvector_size = 280
-    f = open(fn, "r")
-    while True:
-        data = f.read(sysvector_size)
-        if len(data) < sysvector_size:
-            break
-        a = []
-        for i in _unpack_packet(data):
-            a.append(str(i))
-        print ";".join(a)
-    f.close()
-
-if __name__ == "__main__":
-    _main()
diff --git a/Tools/px4params/.gitignore b/Tools/px4params/.gitignore
new file mode 100644
index 000000000..5d0378b4a
--- /dev/null
+++ b/Tools/px4params/.gitignore
@@ -0,0 +1,3 @@
+parameters.wiki
+parameters.xml
+cookies.txt
\ No newline at end of file
diff --git a/Tools/px4params/README.md b/Tools/px4params/README.md
new file mode 100644
index 000000000..a23b44799
--- /dev/null
+++ b/Tools/px4params/README.md
@@ -0,0 +1,9 @@
+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
diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/dokuwikiout.py
new file mode 100644
index 000000000..c5cf65ea6
--- /dev/null
+++ b/Tools/px4params/dokuwikiout.py
@@ -0,0 +1,62 @@
+import output
+from xml.sax.saxutils import escape
+
+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."
+        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"
+            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
+                else:
+                    result += " | "
+                result += " |\n"
+            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
diff --git a/Tools/px4params/dokuwikiout_listings.py b/Tools/px4params/dokuwikiout_listings.py
new file mode 100644
index 000000000..33f76b415
--- /dev/null
+++ b/Tools/px4params/dokuwikiout_listings.py
@@ -0,0 +1,27 @@
+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/output.py b/Tools/px4params/output.py
new file mode 100644
index 000000000..c09246871
--- /dev/null
+++ b/Tools/px4params/output.py
@@ -0,0 +1,5 @@
+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/parser.py
new file mode 100644
index 000000000..251be672f
--- /dev/null
+++ b/Tools/px4params/parser.py
@@ -0,0 +1,220 @@
+import sys
+import re
+
+class ParameterGroup(object):
+    """
+    Single parameter group
+    """
+    def __init__(self, name):
+        self.name = name
+        self.params = []
+
+    def AddParameter(self, param):
+        """
+        Add parameter to the group
+        """
+        self.params.append(param)
+
+    def GetName(self):
+        """
+        Get parameter group name
+        """
+        return self.name
+
+    def GetParams(self):
+        """
+        Returns the parsed list of parameters. Every parameter is a Parameter
+        object. Note that returned object is not a copy. Modifications affect
+        state of the parser.
+        """
+        return sorted(self.params,
+                cmp=lambda x, y: cmp(x.GetFieldValue("code"),
+                    y.GetFieldValue("code")))
+
+class Parameter(object):
+    """
+    Single parameter
+    """
+
+    # Define sorting order of the fields
+    priority = {
+        "code": 10,
+        "type": 9,
+        "short_desc": 8,
+        "long_desc": 7,
+        "default": 6,
+        "min": 5,
+        "max": 4,
+        # all others == 0 (sorted alphabetically)
+    }
+
+    def __init__(self):
+        self.fields = {}
+
+    def SetField(self, code, value):
+        """
+        Set named field value
+        """
+        self.fields[code] = value
+
+    def GetFieldCodes(self):
+        """
+        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))
+
+    def GetFieldValue(self, code):
+        """
+        Return value of the given field code or None if not found.
+        """
+        return self.fields.get(code)
+
+class Parser(object):
+    """
+    Parses provided data and stores all found parameters internally.
+    """
+
+    re_split_lines = re.compile(r'[\r\n]+')
+    re_comment_start = re.compile(r'^\/\*\*')
+    re_comment_content = re.compile(r'^\*\s*(.*)') 
+    re_comment_tag = re.compile(r'@([a-zA-Z][a-zA-Z0-9_]*)\s*(.*)')
+    re_comment_end = re.compile(r'(.*?)\s*\*\/')
+    re_parameter_definition = re.compile(r'PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*,\s*([^ ,\)]+)\s*\)\s*;')
+    re_cut_type_specifier = re.compile(r'[a-z]+$')
+    re_is_a_number = re.compile(r'^-?[0-9\.]')
+    re_remove_dots = re.compile(r'\.+$')
+
+    valid_tags = set(["min", "max", "group"])
+
+    # Order of parameter groups
+    priority = {
+        # All other groups = 0 (sort alphabetically)
+        "Miscellaneous": -10
+    }
+
+    def __init__(self):
+        self.param_groups = {}
+
+    def GetSupportedExtensions(self):
+        """
+        Returns list of supported file extensions that can be parsed by this
+        parser.
+        """
+        return ["cpp", "c"]
+
+    def Parse(self, contents):
+        """
+        Incrementally parse program contents and append all found parameters
+        to the list.
+        """
+        # This code is essentially a comment-parsing grammar. "state"
+        # represents parser state. It contains human-readable state
+        # names.
+        state = None
+        for line in self.re_split_lines.split(contents):
+            line = line.strip()
+            # Ignore empty lines
+            if line == "":
+                continue
+            if self.re_comment_start.match(line):
+                state = "wait-short"
+                short_desc = None
+                long_desc = None
+                tags = {}
+            elif state is not None and state != "comment-processed":
+                m = self.re_comment_end.search(line)
+                if m:
+                    line = m.group(1)
+                    last_comment_line = True
+                else:
+                    last_comment_line = False
+                m = self.re_comment_content.match(line)
+                if m:
+                    comment_content = m.group(1)
+                    if comment_content == "":
+                        # When short comment ends with empty comment line,
+                        # start waiting for the next part - long comment.
+                        if state == "wait-short-end":
+                            state = "wait-long"
+                    else:
+                        m = self.re_comment_tag.match(comment_content)
+                        if m:
+                            tag, desc = m.group(1, 2)
+                            tags[tag] = desc
+                            current_tag = tag
+                            state = "wait-tag-end"
+                        elif state == "wait-short":
+                            # Store first line of the short description
+                            short_desc = comment_content
+                            state = "wait-short-end"
+                        elif state == "wait-short-end":
+                            # Append comment line to the short description
+                            short_desc += "\n" + comment_content
+                        elif state == "wait-long":
+                            # Store first line of the long description
+                            long_desc = comment_content
+                            state = "wait-long-end"
+                        elif state == "wait-long-end":
+                            # Append comment line to the long description
+                            long_desc += "\n" + comment_content
+                        elif state == "wait-tag-end":
+                            # Append comment line to the tag text
+                            tags[current_tag] += "\n" + comment_content
+                        else:
+                            raise AssertionError(
+                                    "Invalid parser state: %s" % state)
+                elif not last_comment_line:
+                    # Invalid comment line (inside comment, but not starting with
+                    # "*" or "*/". Reset parsed content.
+                    state = None
+                if last_comment_line:
+                    state = "comment-processed"
+            else:
+                # Non-empty line outside the comment
+                m = self.re_parameter_definition.match(line)
+                if m:
+                    tp, code, defval = m.group(1, 2, 3)
+                    # Remove trailing type specifier from numbers: 0.1f => 0.1
+                    if self.re_is_a_number.match(defval):
+                        defval = self.re_cut_type_specifier.sub('', defval)
+                    param = Parameter()
+                    param.SetField("code", code)
+                    param.SetField("short_desc", code)
+                    param.SetField("type", tp)
+                    param.SetField("default", defval)
+                    # If comment was found before the parameter declaration,
+                    # inject its data into the newly created parameter.
+                    group = "Miscellaneous"
+                    if state == "comment-processed":
+                        if short_desc is not None:
+                            param.SetField("short_desc",
+                                    self.re_remove_dots.sub('', short_desc))
+                        if long_desc is not None:
+                            param.SetField("long_desc", long_desc)
+                        for tag in tags:
+                            if tag == "group":
+                                group = tags[tag]
+                            elif tag not in self.valid_tags:
+                                sys.stderr.write("Skipping invalid"
+                                        "documentation tag: '%s'\n" % tag)
+                            else:
+                                param.SetField(tag, tags[tag])
+                    # Store the parameter
+                    if group not in self.param_groups:
+                        self.param_groups[group] = ParameterGroup(group)
+                    self.param_groups[group].AddParameter(param)
+                # Reset parsed comment.
+                state = None
+
+    def GetParamGroups(self):
+        """
+        Returns the parsed list of parameters. Every parameter is a Parameter
+        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()))
diff --git a/Tools/px4params/px_process_params.py b/Tools/px4params/px_process_params.py
new file mode 100755
index 000000000..cdf5aba7c
--- /dev/null
+++ b/Tools/px4params/px_process_params.py
@@ -0,0 +1,61 @@
+#!/usr/bin/env python
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# 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.
+#
+
+import scanner
+import parser
+import xmlout
+import dokuwikiout
+
+# Initialize parser
+prs = parser.Parser()
+
+# Scan directories, and parse the files
+sc = scanner.Scanner()
+sc.ScanDir("../../src", prs)
+output = prs.GetParamGroups()
+
+# Output into XML
+out = xmlout.XMLOutput()
+out.Save(output, "parameters.xml")
+
+# Output into DokuWiki
+out = dokuwikiout.DokuWikiOutput()
+out.Save(output, "parameters.wiki")
diff --git a/Tools/px4params/scanner.py b/Tools/px4params/scanner.py
new file mode 100644
index 000000000..b5a1af47c
--- /dev/null
+++ b/Tools/px4params/scanner.py
@@ -0,0 +1,34 @@
+import os
+import re
+
+class Scanner(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())
+        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:
+                        path = os.path.join(dirname, filename)
+                        self.ScanFile(path, parser)
+
+    def ScanFile(self, path, parser):
+        """
+        Scans provided file and passes its contents to the parser using
+        parser.Parse method.
+        """
+        with open(path, 'r') as f:
+            contents = f.read()
+        parser.Parse(contents)
diff --git a/Tools/px4params/xmlout.py b/Tools/px4params/xmlout.py
new file mode 100644
index 000000000..d56802b15
--- /dev/null
+++ b/Tools/px4params/xmlout.py
@@ -0,0 +1,22 @@
+import output
+from xml.dom.minidom import getDOMImplementation
+
+class XMLOutput(output.Output):
+    def Generate(self, groups):
+        impl = getDOMImplementation()
+        xml_document = impl.createDocument(None, "parameters", None)
+        xml_parameters = xml_document.documentElement
+        for group in groups:
+            xml_group = xml_document.createElement("group")
+            xml_group.setAttribute("name", group.GetName())
+            xml_parameters.appendChild(xml_group)
+            for param in group.GetParams():
+                xml_param = xml_document.createElement("parameter")
+                xml_group.appendChild(xml_param)
+                for code in param.GetFieldCodes():
+                    value = param.GetFieldValue(code)
+                    xml_field = xml_document.createElement(code)
+                    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")
diff --git a/Tools/px4params/xmlrpc.sh b/Tools/px4params/xmlrpc.sh
new file mode 100644
index 000000000..36c52ff71
--- /dev/null
+++ b/Tools/px4params/xmlrpc.sh
@@ -0,0 +1,5 @@
+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_mkfw.py b/Tools/px_mkfw.py
index faef9ca33..b598a65a1 100755
--- a/Tools/px_mkfw.py
+++ b/Tools/px_mkfw.py
@@ -1,7 +1,7 @@
 #!/usr/bin/env python
 ############################################################################
 #
-#   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
@@ -60,7 +60,7 @@ def mkdesc():
 	proto['description']	= ""
 	proto['git_identity']	= ""
 	proto['build_time']	= 0
-	proto['image']		= base64.b64encode(bytearray())
+	proto['image']		= bytes()
 	proto['image_size']	= 0
 	return proto
 
@@ -99,12 +99,12 @@ if args.description != None:
 if args.git_identity != None:
 	cmd = " ".join(["git", "--git-dir", args.git_identity + "/.git", "describe", "--always", "--dirty"])
 	p = subprocess.Popen(cmd, shell=True, stdout=subprocess.PIPE).stdout
-	desc['git_identity']	= p.read().strip()
+	desc['git_identity']	= str(p.read().strip())
 	p.close()
 if args.image != None:
 	f = open(args.image, "rb")
 	bytes = f.read()
 	desc['image_size'] = len(bytes)
-	desc['image'] = base64.b64encode(zlib.compress(bytes,9))
+	desc['image'] = base64.b64encode(zlib.compress(bytes,9)).decode('utf-8')
 
 print(json.dumps(desc, indent=4))
diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py
index d2ebf1698..e4a8b3c05 100755
--- a/Tools/px_uploader.py
+++ b/Tools/px_uploader.py
@@ -1,7 +1,7 @@
 #!/usr/bin/env python
 ############################################################################
 #
-#   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
@@ -50,6 +50,9 @@
 #       Currently only used for informational purposes.
 #
 
+# for python2.7 compatibility
+from __future__ import print_function
+
 import sys
 import argparse
 import binascii
@@ -102,7 +105,7 @@ class firmware(object):
             0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
             0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
             0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d])
-        crcpad = bytearray('\xff\xff\xff\xff')
+        crcpad = bytearray(b'\xff\xff\xff\xff')
 
         def __init__(self, path):
 
@@ -137,38 +140,48 @@ class uploader(object):
         '''Uploads a firmware file to the PX FMU bootloader'''
 
         # protocol bytes
-        INSYNC          = chr(0x12)
-        EOC             = chr(0x20)
+        INSYNC          = b'\x12'
+        EOC             = b'\x20'
 
         # reply bytes
-        OK              = chr(0x10)
-        FAILED          = chr(0x11)
-        INVALID         = chr(0x13)     # rev3+
+        OK              = b'\x10'
+        FAILED          = b'\x11'
+        INVALID         = b'\x13'     # rev3+
 
         # command bytes
-        NOP             = chr(0x00)     # guaranteed to be discarded by the bootloader
-        GET_SYNC        = chr(0x21)
-        GET_DEVICE      = chr(0x22)
-        CHIP_ERASE      = chr(0x23)
-        CHIP_VERIFY     = chr(0x24)     # rev2 only
-        PROG_MULTI      = chr(0x27)
-        READ_MULTI      = chr(0x28)     # rev2 only
-        GET_CRC         = chr(0x29)     # rev3+
-        REBOOT          = chr(0x30)
+        NOP             = b'\x00'     # guaranteed to be discarded by the bootloader
+        GET_SYNC        = b'\x21'
+        GET_DEVICE      = b'\x22'
+        CHIP_ERASE      = b'\x23'
+        CHIP_VERIFY     = b'\x24'     # rev2 only
+        PROG_MULTI      = b'\x27'
+        READ_MULTI      = b'\x28'     # rev2 only
+        GET_CRC         = b'\x29'     # rev3+
+        GET_OTP         = b'\x2a'     # rev4+  , get a word from OTP area
+        GET_SN          = b'\x2b'     # rev4+  , get a word from SN area
+        REBOOT          = b'\x30'
         
-        INFO_BL_REV     = chr(1)        # bootloader protocol revision
+        INFO_BL_REV     = b'\x01'        # bootloader protocol revision
         BL_REV_MIN      = 2             # minimum supported bootloader protocol 
-        BL_REV_MAX      = 3             # maximum supported bootloader protocol 
-        INFO_BOARD_ID   = chr(2)        # board type
-        INFO_BOARD_REV  = chr(3)        # board revision
-        INFO_FLASH_SIZE = chr(4)        # max firmware size in bytes
+        BL_REV_MAX      = 4             # maximum supported bootloader protocol 
+        INFO_BOARD_ID   = b'\x02'        # board type
+        INFO_BOARD_REV  = b'\x03'        # board revision
+        INFO_FLASH_SIZE = b'\x04'        # max firmware size in bytes
 
         PROG_MULTI_MAX  = 60            # protocol max is 255, must be multiple of 4
         READ_MULTI_MAX  = 60            # protocol max is 255, something overflows with >= 64
+        
+        NSH_INIT        = bytearray(b'\x0d\x0d\x0d')
+        NSH_REBOOT_BL   = b"reboot -b\n"
+        NSH_REBOOT      = b"reboot\n"
+        MAVLINK_REBOOT_ID1 = bytearray(b'\xfe\x21\x72\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x01\x00\x00\x48\xf0')
+        MAVLINK_REBOOT_ID0 = bytearray(b'\xfe\x21\x45\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x00\x00\x00\xd7\xac')
 
         def __init__(self, portname, baudrate):
                 # open the port, keep the default timeout short so we can poll quickly
                 self.port = serial.Serial(portname, baudrate, timeout=0.5)
+                self.otp = b''
+                self.sn = b''
 
         def close(self):
                 if self.port is not None:
@@ -176,7 +189,7 @@ class uploader(object):
 
         def __send(self, c):
 #               print("send " + binascii.hexlify(c))
-                self.port.write(str(c))
+                self.port.write(c)
 
         def __recv(self, count=1):
                 c = self.port.read(count)
@@ -192,9 +205,9 @@ class uploader(object):
 
         def __getSync(self):
                 self.port.flush()
-                c = self.__recv()
-                if c is not self.INSYNC:
-                        raise RuntimeError("unexpected 0x%x instead of INSYNC" % ord(c))
+                c = bytes(self.__recv())
+                if c != self.INSYNC:
+                        raise RuntimeError("unexpected %s instead of INSYNC" % c)
                 c = self.__recv()
                 if c == self.INVALID:
                         raise RuntimeError("bootloader reports INVALID OPERATION")
@@ -231,6 +244,22 @@ class uploader(object):
                 self.__getSync()
                 return value
 
+        # send the GET_OTP command and wait for an info parameter
+        def __getOTP(self, param):
+                t = struct.pack("I", param) # int param as 32bit ( 4 byte ) char array.
+                self.__send(uploader.GET_OTP + t + uploader.EOC)
+                value = self.__recv(4)
+                self.__getSync()
+                return value
+
+        # send the GET_OTP command and wait for an info parameter
+        def __getSN(self, param):
+                t = struct.pack("I", param) # int param as 32bit ( 4 byte ) char array.
+                self.__send(uploader.GET_SN + t + uploader.EOC)
+                value = self.__recv(4)
+                self.__getSync()
+                return value
+
         # send the CHIP_ERASE command and wait for the bootloader to become ready
         def __erase(self):
                 self.__send(uploader.CHIP_ERASE
@@ -249,17 +278,29 @@ class uploader(object):
 
         # send a PROG_MULTI command to write a collection of bytes
         def __program_multi(self, data):
-                self.__send(uploader.PROG_MULTI
-                            + chr(len(data)))
+                
+                if runningPython3 == True:
+                    length = len(data).to_bytes(1, byteorder='big')
+                else:
+                    length = chr(len(data))
+            
+                self.__send(uploader.PROG_MULTI)
+                self.__send(length)
                 self.__send(data)
                 self.__send(uploader.EOC)
                 self.__getSync()
 
         # verify multiple bytes in flash
         def __verify_multi(self, data):
-                self.__send(uploader.READ_MULTI
-                            + chr(len(data))
-                            + uploader.EOC)
+            
+                if runningPython3 == True:
+                    length = len(data).to_bytes(1, byteorder='big')
+                else:
+                    length = chr(len(data))
+                
+                self.__send(uploader.READ_MULTI)
+                self.__send(length)
+                self.__send(uploader.EOC)
                 self.port.flush()
                 programmed = self.__recv(len(data))
                 if programmed != data:
@@ -335,6 +376,31 @@ class uploader(object):
                 if self.fw_maxsize < fw.property('image_size'):
                         raise RuntimeError("Firmware image is too large for this board")
 
+                # OTP added in v4:
+                if self.bl_rev > 3:
+                    for byte in range(0,32*6,4):
+                        x = self.__getOTP(byte)
+                        self.otp  = self.otp + x
+                        print(binascii.hexlify(x).decode('Latin-1') + ' ', end='')
+                    # see src/modules/systemlib/otp.h in px4 code:
+                    self.otp_id = self.otp[0:4]
+                    self.otp_idtype = self.otp[4:5]
+                    self.otp_vid = self.otp[8:4:-1]
+                    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('')
                 print("erase...")
                 self.__erase()
 
@@ -350,7 +416,27 @@ class uploader(object):
                 print("done, rebooting.")
                 self.__reboot()
                 self.port.close()
+                
+        def send_reboot(self):
+                try:
+                    # try reboot via NSH first
+                    self.__send(uploader.NSH_INIT)
+                    self.__send(uploader.NSH_REBOOT_BL)
+                    self.__send(uploader.NSH_INIT)
+                    self.__send(uploader.NSH_REBOOT)
+                    # then try MAVLINK command
+                    self.__send(uploader.MAVLINK_REBOOT_ID1)
+                    self.__send(uploader.MAVLINK_REBOOT_ID0)
+                except:
+                    return
+                
+                
 
+# Detect python version
+if sys.version_info[0] < 3:
+    runningPython3 = False
+else:
+    runningPython3 = True
 
 # Parse commandline arguments
 parser = argparse.ArgumentParser(description="Firmware uploader for the PX autopilot system.")
@@ -365,7 +451,19 @@ print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property(
 
 # Spin waiting for a device to show up
 while True:
-        for port in args.port.split(","):
+        portlist = []
+        patterns = args.port.split(",")
+        # on unix-like platforms use glob to support wildcard ports. This allows
+        # the use of /dev/serial/by-id/usb-3D_Robotics on Linux, which prevents the upload from
+        # causing modem hangups etc
+        if "linux" in _platform or "darwin" in _platform:
+                import glob
+                for pattern in patterns:
+                        portlist += glob.glob(pattern)
+        else:
+                portlist = patterns
+
+        for port in portlist:
 
                 #print("Trying %s" % port)
 
@@ -383,7 +481,7 @@ while True:
                                 # Windows, don't open POSIX ports
                                 if not "/" in port:
                                         up = uploader(port, args.baud)
-                except:
+                except Exception:
                         # open failed, rate-limit our attempts
                         time.sleep(0.05)
 
@@ -396,8 +494,12 @@ while True:
                         up.identify()
                         print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port))
 
-                except:
-                        # most probably a timeout talking to the port, no bootloader
+                except Exception:
+                        # most probably a timeout talking to the port, no bootloader, try to reboot the board
+                        print("attempting reboot on %s..." % port)
+                        up.send_reboot()
+                        # wait for the reboot, without we might run into Serial I/O Error 5 
+                        time.sleep(0.5)
                         continue
 
                 try:
diff --git a/Tools/sdlog2/README.txt b/Tools/sdlog2/README.txt
new file mode 100644
index 000000000..abeb9a4c7
--- /dev/null
+++ b/Tools/sdlog2/README.txt
@@ -0,0 +1,13 @@
+====== PX4 LOG CONVERSION ======
+
+On each log session (commonly started and stopped by arming and disarming the vehicle) a new file logxxx.bin is created. In many cases there will be only one logfile named log001.bin (only one flight).
+
+There are two conversion scripts in this ZIP file:
+
+logconv.m: This is a MATLAB script which will automatically convert and display the flight data with a GUI. If running this script, the second script can be ignored.
+
+sdlog2_dump.py: This is a Python script (compatible with v2 and v3) which converts the self-describing binary log format to a CSV file. To export a CSV file from within a shell (Windows CMD or BASH on Linux / Mac OS), run:
+
+python sdlog2_dump.py log001.bin -f "export.csv" -t "TIME" -d "," -n ""
+
+Python can be downloaded from http://python.org, but is available as default on Mac OS and Linux.
\ No newline at end of file
diff --git a/ROMFS/px4fmu_common/logging/logconv.m b/Tools/sdlog2/logconv.m
index 3750ddae2..e19c97fa3 100644
--- a/ROMFS/px4fmu_common/logging/logconv.m
+++ b/Tools/sdlog2/logconv.m
@@ -1,586 +1,535 @@
-% This Matlab Script can be used to import the binary logged values of the

-% PX4FMU into data that can be plotted and analyzed.

-

-%% ************************************************************************

-% PX4LOG_PLOTSCRIPT: Main function

-% ************************************************************************

-function PX4Log_Plotscript

-

-% Clear everything

-clc

-clear all

-close all

-

-% ************************************************************************

-% SETTINGS

-% ************************************************************************

-

-% Set the path to your sysvector.bin file here

-filePath = 'sysvector.bin';

-

-% Set the minimum and maximum times to plot here [in seconds]

-mintime=0;        %The minimum time/timestamp to display, as set by the user [0 for first element / start]

-maxtime=0;        %The maximum time/timestamp to display, as set by the user [0 for last element / end]

-

-%Determine which data to plot. Not completely implemented yet.

-bDisplayGPS=true;

-

-%conversion factors

-fconv_gpsalt=1E-3; %[mm] to [m]

-fconv_gpslatlong=1E-7; %[gps_raw_position_unit] to [deg]

-fconv_timestamp=1E-6; % [microseconds] to [seconds]

-

-% ************************************************************************

-% Import the PX4 logs

-% ************************************************************************

-ImportPX4LogData();

-

-%Translate min and max plot times to indices

-time=double(sysvector.timestamp) .*fconv_timestamp;

-mintime_log=time(1);        %The minimum time/timestamp found in the log

-maxtime_log=time(end);      %The maximum time/timestamp found in the log

-CurTime=mintime_log;        %The current time at which to draw the aircraft position

-

-[imintime,imaxtime]=FindMinMaxTimeIndices();

-

-% ************************************************************************

-% PLOT & GUI SETUP

-% ************************************************************************

-NrFigures=5;

-NrAxes=10;

-h.figures(1:NrFigures)=0.0;  % Temporary initialization of figure handle array - these are numbered consecutively

-h.axes(1:NrAxes)=0.0;        % Temporary initialization of axes handle array - these are numbered consecutively

-h.pathpoints=[];             % Temporary initiliazation of path points

-

-% Setup the GUI to control the plots

-InitControlGUI();

-% Setup the plotting-GUI (figures, axes) itself.

-InitPlotGUI();

-

-% ************************************************************************

-% DRAW EVERYTHING

-% ************************************************************************

-DrawRawData();

-DrawCurrentAircraftState();

-

-%% ************************************************************************

-%  *** END OF MAIN SCRIPT ***

-%  NESTED FUNCTION DEFINTIONS FROM HERE ON

-%  ************************************************************************

-

-

-%% ************************************************************************

-%  IMPORTPX4LOGDATA (nested function)

-%  ************************************************************************

-%  Attention:  This is the import routine for firmware from ca. 03/2013.

-%              Other firmware versions might require different import 

-%              routines.

-

-function ImportPX4LogData()

-    % Work around a Matlab bug (not related to PX4)

-    % where timestamps from 1.1.1970 do not allow to

-    % read the file's size

-    if ismac

-        system('touch -t 201212121212.12 sysvector.bin');

-    end

-

-    % ************************************************************************

-    % RETRIEVE SYSTEM VECTOR

-    % *************************************************************************

-    % //All measurements in NED frame

-    %

-    % uint64_t timestamp; //[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 - current drawn from battery at this time instant

-    % float bat_discharged - discharged energy in mAh

-    % float adc[4]; //remaining auxiliary 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]; //pitch, roll, yaw [rad]

-    % float rotMatrix[9]; //unitvectors

-    % float actuator_control[4]; //unitvector

-    % float optical_flow[4]; //roll, pitch, yaw [-1..1], thrust [0..1]

-    % float diff_pressure; - pressure difference in millibar

-    % float ind_airspeed;

-    % float true_airspeed;

-

-    % Definition of the logged values

-    logFormat{1} = struct('name', 'timestamp',             'bytes', 8, 'array', 1, 'precision', 'uint64',  'machineformat', 'ieee-le.l64');

-    logFormat{2} = struct('name', 'gyro',                  'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{3} = struct('name', 'accel',                 'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{4} = struct('name', 'mag',                   'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{5} = struct('name', 'baro',                  'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{6} = struct('name', 'baro_alt',              'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{7} = struct('name', 'baro_temp',             'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{8} = struct('name', 'control',               'bytes', 4, 'array', 4, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{9} = struct('name', 'actuators',             'bytes', 4, 'array', 8, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{10} = struct('name', 'vbat',                 'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{11} = struct('name', 'bat_current',          'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{12} = struct('name', 'bat_discharged',       'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{13} = struct('name', 'adc',                  'bytes', 4, 'array', 4, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{14} = struct('name', 'local_position',       'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{15} = struct('name', 'gps_raw_position',     'bytes', 4, 'array', 3, 'precision', 'uint32',  'machineformat', 'ieee-le');

-    logFormat{16} = struct('name', 'attitude',             'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{17} = struct('name', 'rot_matrix',           'bytes', 4, 'array', 9, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{18} = struct('name', 'vicon_position',       'bytes', 4, 'array', 6, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{19} = struct('name', 'actuator_control',     'bytes', 4, 'array', 4, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{20} = struct('name', 'optical_flow',         'bytes', 4, 'array', 6, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{21} = struct('name', 'diff_pressure',        'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{22} = struct('name', 'ind_airspeed',         'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{23} = struct('name', 'true_airspeed',        'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');

-

-    % First get length of one line

-    columns = length(logFormat);

-    lineLength = 0;

-

-    for i=1:columns

-        lineLength = lineLength + logFormat{i}.bytes * logFormat{i}.array;

-    end

-

-

-    if exist(filePath, 'file')

-

-        fileInfo = dir(filePath);

-        fileSize = fileInfo.bytes;

-

-        elements = int64(fileSize./(lineLength));

-

-        fid = fopen(filePath, 'r');

-        offset = 0;

-        for i=1:columns

-            % using fread with a skip speeds up the import drastically, do not

-            % import the values one after the other

-            sysvector.(genvarname(logFormat{i}.name)) = transpose(fread(...

-                fid, ...

-                [logFormat{i}.array, elements], [num2str(logFormat{i}.array),'*',logFormat{i}.precision,'=>',logFormat{i}.precision], ...

-                lineLength - logFormat{i}.bytes*logFormat{i}.array, ...

-                logFormat{i}.machineformat) ...

-            );

-            offset = offset + logFormat{i}.bytes*logFormat{i}.array;

-            fseek(fid, offset,'bof');

-        end

-

-        % shot the flight time

-        time_us = sysvector.timestamp(end) - sysvector.timestamp(1);

-        time_s = time_us*1e-6;

-        time_m = time_s/60;

-

-        % close the logfile

-        fclose(fid);

-

-        disp(['end log2matlab conversion' char(10)]);

-    else

-        disp(['file: ' filePath ' does not exist' char(10)]);

-    end

-end

-

-%% ************************************************************************

-%  INITCONTROLGUI (nested function)

-%  ************************************************************************

-%Setup central control GUI components to control current time where data is shown

-function InitControlGUI()

-    %**********************************************************************

-    % GUI size definitions

-    %**********************************************************************

-    dxy=5; %margins

-    %Panel: Plotctrl

-    dlabels=120;

-    dsliders=200;

-    dedits=80;

-    hslider=20;

-        

-    hpanel1=40; %panel1

-    hpanel2=220;%panel2

-    hpanel3=3*hslider+4*dxy+3*dxy;%panel3.

-    

-    width=dlabels+dsliders+dedits+4*dxy+2*dxy;  %figure width

-    height=hpanel1+hpanel2+hpanel3+4*dxy;       %figure height

-        

-    %**********************************************************************

-    % Create GUI

-    %**********************************************************************

-    h.figures(1)=figure('Units','pixels','position',[200 200 width height],'Name','Control GUI');

-    h.guistatepanel=uipanel('Title','Current GUI state','Units','pixels','Position',[dxy dxy width-2*dxy hpanel1],'parent',h.figures(1));

-    h.aircraftstatepanel=uipanel('Title','Current aircraft state','Units','pixels','Position',[dxy hpanel1+2*dxy width-2*dxy hpanel2],'parent',h.figures(1));

-    h.plotctrlpanel=uipanel('Title','Plot Control','Units','pixels','Position',[dxy hpanel1+hpanel2+3*dxy width-2*dxy hpanel3],'parent',h.figures(1));

-    

-    %%Control GUI-elements

-    %Slider: Current time

-    h.labels.CurTime=uicontrol(gcf,'style','text','Position',[dxy dxy dlabels hslider],'String','Current time t[s]:','parent',h.plotctrlpanel,'HorizontalAlignment','left');

-    h.sliders.CurTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels dxy dsliders hslider],...

-        'min',mintime,'max',maxtime,'value',mintime,'callback',@curtime_callback,'parent',h.plotctrlpanel);

-    temp=get(h.sliders.CurTime,'Max')-get(h.sliders.CurTime,'Min');

-    set(h.sliders.CurTime,'SliderStep',[1.0/temp 5.0/temp]);

-    h.edits.CurTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders dxy dedits hslider],'String',get(h.sliders.CurTime,'value'),...

-        'BackgroundColor','white','callback',@curtime_callback,'parent',h.plotctrlpanel);

-

-    %Slider: MaxTime

-    h.labels.MaxTime=uicontrol(gcf,'style','text','position',[dxy 2*dxy+hslider dlabels hslider],'String','Max. time t[s] to display:','parent',h.plotctrlpanel,'HorizontalAlignment','left');

-    h.sliders.MaxTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 2*dxy+hslider dsliders hslider],...

-        'min',mintime_log,'max',maxtime_log,'value',maxtime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel);

-    h.edits.MaxTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 2*dxy+hslider dedits hslider],'String',get(h.sliders.MaxTime,'value'),...

-        'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel);

-    

-    %Slider: MinTime

-    h.labels.MinTime=uicontrol(gcf,'style','text','position',[dxy 3*dxy+2*hslider dlabels hslider],'String','Min. time t[s] to dispay :','parent',h.plotctrlpanel,'HorizontalAlignment','left');

-    h.sliders.MinTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 3*dxy+2*hslider dsliders hslider],...

-        'min',mintime_log,'max',maxtime_log,'value',mintime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel);

-    h.edits.MinTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 3*dxy+2*hslider dedits hslider],'String',get(h.sliders.MinTime,'value'),...

-        'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel);

-

-    %%Current data/state GUI-elements (Multiline-edit-box)

-    h.edits.AircraftState=uicontrol(gcf,'style','edit','Units','normalized','position',[.02 .02 0.96 0.96],'Min',1,'Max',10,'String','This shows the current aircraft state',...

-                                    'HorizontalAlignment','left','parent',h.aircraftstatepanel);

-    

-    h.labels.GUIState=uicontrol(gcf,'style','text','Units','pixels','position',[dxy dxy width-4*dxy hslider],'String','Current state of this GUI',...

-                                'HorizontalAlignment','left','parent',h.guistatepanel);

-    

-end

-

-%% ************************************************************************

-%  INITPLOTGUI (nested function)

-%  ************************************************************************

-function InitPlotGUI()

-    

-    % Setup handles to lines and text

-    h.markertext=[];

-    templinehandle=0.0;%line([0 1],[0 5]);   % Just a temporary handle to init array

-    h.markerline(1:NrAxes)=templinehandle;   % the actual handle-array to the lines  - these are numbered consecutively

-    h.markerline(1:NrAxes)=0.0;

-        

-    % Setup all other figures and axes for plotting

-    %  PLOT WINDOW 1: GPS POSITION

-    h.figures(2)=figure('units','normalized','Toolbar','figure', 'Name', 'GPS Position');

-    h.axes(1)=axes();

-    set(h.axes(1),'Parent',h.figures(2));

-

-    %  PLOT WINDOW 2: IMU, baro altitude

-    h.figures(3)=figure('Name', 'IMU / Baro Altitude');

-    h.axes(2)=subplot(4,1,1);

-    h.axes(3)=subplot(4,1,2);

-    h.axes(4)=subplot(4,1,3);

-    h.axes(5)=subplot(4,1,4);

-    set(h.axes(2:5),'Parent',h.figures(3));

-    

-    %  PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,...

-    h.figures(4)=figure('Name', 'Attitude Estimate / Actuators / Airspeeds');

-    h.axes(6)=subplot(4,1,1);

-    h.axes(7)=subplot(4,1,2);

-    h.axes(8)=subplot(4,1,3);

-    h.axes(9)=subplot(4,1,4);

-    set(h.axes(6:9),'Parent',h.figures(4));

-    

-    %  PLOT WINDOW 4: LOG STATS

-    h.figures(5) = figure('Name', 'Log Statistics');

-    h.axes(10)=subplot(1,1,1);

-    set(h.axes(10:10),'Parent',h.figures(5));

-    

-end

-

-%% ************************************************************************

-%  DRAWRAWDATA (nested function)

-%  ************************************************************************

-%Draws the raw data from the sysvector, but does not add any

-%marker-lines or so

-function DrawRawData() 

-    % ************************************************************************

-    %  PLOT WINDOW 1: GPS POSITION & GUI

-    %  ************************************************************************

-    figure(h.figures(2));

-    % Only plot GPS data if available

-    if (sum(double(sysvector.gps_raw_position(imintime:imaxtime,1)))>0) && (bDisplayGPS)

-        %Draw data

-        plot3(h.axes(1),double(sysvector.gps_raw_position(imintime:imaxtime,1))*fconv_gpslatlong, ...

-                        double(sysvector.gps_raw_position(imintime:imaxtime,2))*fconv_gpslatlong, ...

-                        double(sysvector.gps_raw_position(imintime:imaxtime,3))*fconv_gpsalt,'r.');

-        title(h.axes(1),'GPS Position Data(if available)');

-        xlabel(h.axes(1),'Latitude [deg]');

-        ylabel(h.axes(1),'Longitude [deg]');

-        zlabel(h.axes(1),'Altitude above MSL [m]');

-        grid on

-        

-        %Reset path

-        h.pathpoints=0;

-    end

-

-    % ************************************************************************

-    %  PLOT WINDOW 2: IMU, baro altitude

-    %  ************************************************************************

-    figure(h.figures(3));

-    plot(h.axes(2),time(imintime:imaxtime),sysvector.mag(imintime:imaxtime,:));

-    title(h.axes(2),'Magnetometers [Gauss]');

-    legend(h.axes(2),'x','y','z');

-    plot(h.axes(3),time(imintime:imaxtime),sysvector.accel(imintime:imaxtime,:));

-    title(h.axes(3),'Accelerometers [m/s²]');

-    legend(h.axes(3),'x','y','z');

-    plot(h.axes(4),time(imintime:imaxtime),sysvector.gyro(imintime:imaxtime,:));

-    title(h.axes(4),'Gyroscopes [rad/s]');

-    legend(h.axes(4),'x','y','z');

-    plot(h.axes(5),time(imintime:imaxtime),sysvector.baro_alt(imintime:imaxtime),'color','blue');

-    if(bDisplayGPS)

-        hold on;

-        plot(h.axes(5),time(imintime:imaxtime),double(sysvector.gps_raw_position(imintime:imaxtime,3)).*fconv_gpsalt,'color','red');

-        hold off

-        legend('Barometric Altitude [m]','GPS Altitude [m]');

-    else

-        legend('Barometric Altitude [m]');

-    end

-    title(h.axes(5),'Altitude above MSL [m]');

-

-    % ************************************************************************

-    %  PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,...

-    % ************************************************************************

-    figure(h.figures(4));

-    %Attitude Estimate

-    plot(h.axes(6),time(imintime:imaxtime), sysvector.attitude(imintime:imaxtime,:).*180./3.14159);

-    title(h.axes(6),'Estimated attitude [deg]');

-    legend(h.axes(6),'roll','pitch','yaw');

-    %Actuator Controls

-    plot(h.axes(7),time(imintime:imaxtime), sysvector.actuator_control(imintime:imaxtime,:));

-    title(h.axes(7),'Actuator control [-]');

-    legend(h.axes(7),'0','1','2','3');

-    %Actuator Controls

-    plot(h.axes(8),time(imintime:imaxtime), sysvector.actuators(imintime:imaxtime,1:8));

-    title(h.axes(8),'Actuator PWM (raw-)outputs [µs]');

-    legend(h.axes(8),'CH1','CH2','CH3','CH4','CH5','CH6','CH7','CH8');

-    set(h.axes(8), 'YLim',[800 2200]);

-    %Airspeeds

-    plot(h.axes(9),time(imintime:imaxtime), sysvector.ind_airspeed(imintime:imaxtime));

-    hold on

-    plot(h.axes(9),time(imintime:imaxtime), sysvector.true_airspeed(imintime:imaxtime));

-    hold off

-    %add GPS total airspeed here

-    title(h.axes(9),'Airspeed [m/s]');

-    legend(h.axes(9),'Indicated Airspeed (IAS)','True Airspeed (TAS)','GPS Airspeed');

-    %calculate time differences and plot them

-    intervals = zeros(0,imaxtime - imintime);

-    for k = imintime+1:imaxtime

-        intervals(k) = time(k) - time(k-1);

-    end

-    plot(h.axes(10), time(imintime:imaxtime), intervals);

-

-    %Set same timescale for all plots

-    for i=2:NrAxes

-        set(h.axes(i),'XLim',[mintime maxtime]);

-    end

-    

-    set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]);

-end

-

-%% ************************************************************************

-%  DRAWCURRENTAIRCRAFTSTATE(nested function)

-%  ************************************************************************

-function DrawCurrentAircraftState()

-    %find current data index        

-    i=find(time>=CurTime,1,'first');

-

-    %**********************************************************************

-    % Current aircraft state label update

-    %**********************************************************************

-    acstate{1,:}=[sprintf('%s \t\t','GPS Pos:'),'[lat=',num2str(double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong),'°, ',...

-                        'lon=',num2str(double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong),'°, ',...

-                        'alt=',num2str(double(sysvector.gps_raw_position(i,3))*fconv_gpsalt),'m]'];

-    acstate{2,:}=[sprintf('%s \t\t','Mags[gauss]'),'[x=',num2str(sysvector.mag(i,1)),...

-                               ', y=',num2str(sysvector.mag(i,2)),...

-                               ', z=',num2str(sysvector.mag(i,3)),']'];

-    acstate{3,:}=[sprintf('%s \t\t','Accels[m/s²]'),'[x=',num2str(sysvector.accel(i,1)),...

-                               ', y=',num2str(sysvector.accel(i,2)),...

-                               ', z=',num2str(sysvector.accel(i,3)),']'];

-    acstate{4,:}=[sprintf('%s \t\t','Gyros[rad/s]'),'[x=',num2str(sysvector.gyro(i,1)),...

-                               ', y=',num2str(sysvector.gyro(i,2)),...

-                               ', z=',num2str(sysvector.gyro(i,3)),']'];

-    acstate{5,:}=[sprintf('%s \t\t','Altitude[m]'),'[Barometric: ',num2str(sysvector.baro_alt(i)),'m, GPS: ',num2str(double(sysvector.gps_raw_position(i,3))*fconv_gpsalt),'m]'];

-    acstate{6,:}=[sprintf('%s \t','Est. attitude[deg]:'),'[Roll=',num2str(sysvector.attitude(i,1).*180./3.14159),...

-                               ', Pitch=',num2str(sysvector.attitude(i,2).*180./3.14159),...

-                               ', Yaw=',num2str(sysvector.attitude(i,3).*180./3.14159),']'];

-    acstate{7,:}=sprintf('%s \t[','Actuator Ctrls [-]:');

-    for j=1:4

-        acstate{7,:}=[acstate{7,:},num2str(sysvector.actuator_control(i,j)),','];

-    end

-    acstate{7,:}=[acstate{7,:},']'];

-    acstate{8,:}=sprintf('%s \t[','Actuator Outputs [PWM/µs]:');

-    for j=1:8

-        acstate{8,:}=[acstate{8,:},num2str(sysvector.actuators(i,j)),','];

-    end

-    acstate{8,:}=[acstate{8,:},']'];

-    acstate{9,:}=[sprintf('%s \t','Airspeed[m/s]:'),'[IAS: ',num2str(sysvector.ind_airspeed(i)),', TAS: ',num2str(sysvector.true_airspeed(i)),']'];

-    

-    set(h.edits.AircraftState,'String',acstate);

-    

-    %**********************************************************************

-    % GPS Plot Update

-    %**********************************************************************

-    %Plot traveled path, and  and time.

-    figure(h.figures(2));

-    hold on;

-    if(CurTime>mintime+1) %the +1 is only a small bugfix

-        h.pathline=plot3(h.axes(1),double(sysvector.gps_raw_position(imintime:i,1))*fconv_gpslatlong, ...

-                                    double(sysvector.gps_raw_position(imintime:i,2))*fconv_gpslatlong, ...

-                                    double(sysvector.gps_raw_position(imintime:i,3))*fconv_gpsalt,'b','LineWidth',2); 

-    end;

-    hold off

-    %Plot current position

-    newpoint=[double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong double(sysvector.gps_raw_position(i,3))*fconv_gpsalt];

-    if(numel(h.pathpoints)<=3) %empty path

-        h.pathpoints(1,1:3)=newpoint;

-    else %Not empty, append new point 

-        h.pathpoints(size(h.pathpoints,1)+1,:)=newpoint;

-    end

-    axes(h.axes(1));    

-    line(h.pathpoints(:,1),h.pathpoints(:,2),h.pathpoints(:,3),'LineStyle','none','Marker','.','MarkerEdge','black','MarkerSize',20);

-    

-    

-    % Plot current time (small label next to current gps position) 

-    textdesc=strcat('  t=',num2str(time(i)),'s');

-    if(isvalidhandle(h.markertext))

-            delete(h.markertext); %delete old text

-    end 

-    h.markertext=text(double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong,double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong,...

-                        double(sysvector.gps_raw_position(i,3))*fconv_gpsalt,textdesc);

-    set(h.edits.CurTime,'String',CurTime);

-        

-    %**********************************************************************

-    % Plot the lines showing the current time in the 2-d plots

-    %**********************************************************************

-    for i=2:NrAxes

-        if(isvalidhandle(h.markerline(i))) delete(h.markerline(i)); end

-        ylims=get(h.axes(i),'YLim');

-        h.markerline(i)=line([CurTime CurTime] ,get(h.axes(i),'YLim'),'Color','black');

-        set(h.markerline(i),'parent',h.axes(i));

-    end

-    

-    set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]);

-end

-

-%% ************************************************************************

-%  MINMAXTIME CALLBACK (nested function)

-%  ************************************************************************

-function minmaxtime_callback(hObj,event) %#ok<INUSL>

-    new_mintime=get(h.sliders.MinTime,'Value');

-    new_maxtime=get(h.sliders.MaxTime,'Value');

-

-    %Safety checks:

-    bErr=false;

-    %1: mintime must be < maxtime

-    if((new_mintime>maxtime) || (new_maxtime<mintime))

-        set(h.labels.GUIState,'String','Error: Mintime cannot be bigger than maxtime! Values were not changed.','BackgroundColor','red');

-        bErr=true;

-    else

-        %2: MinTime must be <=CurTime

-        if(new_mintime>CurTime)

-            set(h.labels.GUIState,'String','Error: Mintime cannot be bigger than CurTime! CurTime set to new mintime.','BackgroundColor','red');

-            mintime=new_mintime;

-            CurTime=mintime;

-            bErr=true;

-        end

-        %3: MaxTime must be >CurTime

-        if(new_maxtime<CurTime)

-            set(h.labels.GUIState,'String','Error: Maxtime cannot be smaller than CurTime! CurTime set to new maxtime.','BackgroundColor','red');

-            maxtime=new_maxtime;

-            CurTime=maxtime;

-            bErr=true;

-        end

-    end

-

-    if(bErr==false)

-        maxtime=new_maxtime;

-        mintime=new_mintime;

-    end

-

-    %Needs to be done in case values were reset above

-    set(h.sliders.MinTime,'Value',mintime);

-    set(h.sliders.MaxTime,'Value',maxtime);

-

-    %Update curtime-slider

-    set(h.sliders.CurTime,'Value',CurTime);

-    set(h.sliders.CurTime,'Max',maxtime);

-    set(h.sliders.CurTime,'Min',mintime);

-    temp=get(h.sliders.CurTime,'Max')-get(h.sliders.CurTime,'Min'); 

-    set(h.sliders.CurTime,'SliderStep',[1.0/temp 5.0/temp]);  %Set Stepsize to constant [in seconds]

-

-    %update edit fields

-    set(h.edits.CurTime,'String',get(h.sliders.CurTime,'Value'));

-    set(h.edits.MinTime,'String',get(h.sliders.MinTime,'Value'));

-    set(h.edits.MaxTime,'String',get(h.sliders.MaxTime,'Value'));

-

-    %Finally, we have to redraw. Update time indices first.

-    [imintime,imaxtime]=FindMinMaxTimeIndices();

-    DrawRawData();                  %Rawdata only

-    DrawCurrentAircraftState();     %path info & markers

-end

-

-

-%% ************************************************************************

-%  CURTIME CALLBACK (nested function)

-%  ************************************************************************

-function curtime_callback(hObj,event) %#ok<INUSL>

-    %find current time

-    if(hObj==h.sliders.CurTime)

-        CurTime=get(h.sliders.CurTime,'Value');

-    elseif (hObj==h.edits.CurTime)

-        temp=str2num(get(h.edits.CurTime,'String'));

-        if(temp<maxtime && temp>mintime)

-            CurTime=temp;

-        else

-            %Error

-            set(h.labels.GUIState,'String','Error: You tried to set an invalid current time! Previous value restored.','BackgroundColor','red');

-        end

-    else

-        %Error

-        set(h.labels.GUIState,'String','Error: curtime_callback','BackgroundColor','red');        

-    end

-    

-    set(h.sliders.CurTime,'Value',CurTime);

-    set(h.edits.CurTime,'String',num2str(CurTime));

-    

-    %Redraw time markers, but don't have to redraw the whole raw data

-    DrawCurrentAircraftState();

-end

-

-%% ************************************************************************

-%  FINDMINMAXINDICES (nested function)

-%  ************************************************************************

-function [idxmin,idxmax] = FindMinMaxTimeIndices()

-    for i=1:size(sysvector.timestamp,1)

-        if time(i)>=mintime; idxmin=i; break; end

-    end

-    for i=1:size(sysvector.timestamp,1)

-        if maxtime==0; idxmax=size(sysvector.timestamp,1); break; end

-        if time(i)>=maxtime; idxmax=i; break; end

-    end

-    mintime=time(idxmin); 

-    maxtime=time(idxmax);

-end

-

-%% ************************************************************************

-%  ISVALIDHANDLE (nested function)

-%  ************************************************************************

-function isvalid = isvalidhandle(handle)

-   if(exist(varname(handle))>0 && length(ishandle(handle))>0) 

-        if(ishandle(handle)>0)

-            if(handle>0.0)

-                isvalid=true;

-                return;

-            end

-        end

-   end

-   isvalid=false;

-end

-

-%% ************************************************************************

-%  JUST SOME SMALL HELPER FUNCTIONS (nested function)

-%  ************************************************************************

-function out = varname(var)

-   out = inputname(1);

-end

-

-%This is the end of the matlab file / the main function

-end

+% This Matlab Script can be used to import the binary logged values of the
+% PX4FMU into data that can be plotted and analyzed.
+
+%% ************************************************************************
+% PX4LOG_PLOTSCRIPT: Main function
+% ************************************************************************
+function PX4Log_Plotscript
+
+% Clear everything
+clc
+clear all
+close all
+
+% ************************************************************************
+% SETTINGS
+% ************************************************************************
+
+% Set the path to your sysvector.bin file here
+filePath = 'log001.bin';
+
+% Set the minimum and maximum times to plot here [in seconds]
+mintime=0;        %The minimum time/timestamp to display, as set by the user [0 for first element / start]
+maxtime=0;        %The maximum time/timestamp to display, as set by the user [0 for last element / end]
+
+%Determine which data to plot. Not completely implemented yet.
+bDisplayGPS=true;
+
+%conversion factors
+fconv_gpsalt=1; %[mm] to [m]
+fconv_gpslatlong=1; %[gps_raw_position_unit] to [deg]
+fconv_timestamp=1E-6; % [microseconds] to [seconds]
+
+% ************************************************************************
+% Import the PX4 logs
+% ************************************************************************
+ImportPX4LogData();
+
+%Translate min and max plot times to indices
+time=double(sysvector.TIME_StartTime) .*fconv_timestamp;
+mintime_log=time(1);        %The minimum time/timestamp found in the log
+maxtime_log=time(end);      %The maximum time/timestamp found in the log
+CurTime=mintime_log;        %The current time at which to draw the aircraft position
+
+[imintime,imaxtime]=FindMinMaxTimeIndices();
+
+% ************************************************************************
+% PLOT & GUI SETUP
+% ************************************************************************
+NrFigures=5;
+NrAxes=10;
+h.figures(1:NrFigures)=0.0;  % Temporary initialization of figure handle array - these are numbered consecutively
+h.axes(1:NrAxes)=0.0;        % Temporary initialization of axes handle array - these are numbered consecutively
+h.pathpoints=[];             % Temporary initiliazation of path points
+
+% Setup the GUI to control the plots
+InitControlGUI();
+% Setup the plotting-GUI (figures, axes) itself.
+InitPlotGUI();
+
+% ************************************************************************
+% DRAW EVERYTHING
+% ************************************************************************
+DrawRawData();
+DrawCurrentAircraftState();
+
+%% ************************************************************************
+%  *** END OF MAIN SCRIPT ***
+%  NESTED FUNCTION DEFINTIONS FROM HERE ON
+%  ************************************************************************
+
+
+%% ************************************************************************
+%  IMPORTPX4LOGDATA (nested function)
+%  ************************************************************************
+%  Attention:  This is the import routine for firmware from ca. 03/2013.
+%              Other firmware versions might require different import 
+%              routines.
+
+%% ************************************************************************
+%  IMPORTPX4LOGDATA (nested function)
+%  ************************************************************************
+%  Attention:  This is the import routine for firmware from ca. 03/2013.
+%              Other firmware versions might require different import 
+%              routines.
+
+function ImportPX4LogData()
+
+    % ************************************************************************
+    % RETRIEVE SYSTEM VECTOR
+    % *************************************************************************
+    % //All measurements in NED frame
+    
+    % Convert to CSV
+    %arg1 = 'log-fx61-20130721-2.bin';
+    arg1 = filePath;
+    delim = ',';
+    time_field = 'TIME';
+    data_file = 'data.csv';
+    csv_null = '';
+    
+    if not(exist(data_file, 'file'))
+        s = system( sprintf('python sdlog2_dump.py "%s" -f "%s" -t"%s" -d"%s" -n"%s"', arg1, data_file, time_field, delim, csv_null) );
+    end
+
+    if exist(data_file, 'file')
+
+        %data = csvread(data_file);
+        sysvector = tdfread(data_file, ',');
+
+        % shot the flight time
+        time_us = sysvector.TIME_StartTime(end) - sysvector.TIME_StartTime(1);
+        time_s = uint64(time_us*1e-6);
+        time_m = uint64(time_s/60);
+        time_s = time_s - time_m * 60;
+        
+        disp([sprintf('Flight log duration: %d:%d (minutes:seconds)', time_m, time_s) char(10)]);
+
+        disp(['logfile conversion finished.' char(10)]);
+    else
+        disp(['file: ' data_file ' does not exist' char(10)]);
+    end
+end
+
+%% ************************************************************************
+%  INITCONTROLGUI (nested function)
+%  ************************************************************************
+%Setup central control GUI components to control current time where data is shown
+function InitControlGUI()
+    %**********************************************************************
+    % GUI size definitions
+    %**********************************************************************
+    dxy=5; %margins
+    %Panel: Plotctrl
+    dlabels=120;
+    dsliders=200;
+    dedits=80;
+    hslider=20;
+        
+    hpanel1=40; %panel1
+    hpanel2=220;%panel2
+    hpanel3=3*hslider+4*dxy+3*dxy;%panel3.
+    
+    width=dlabels+dsliders+dedits+4*dxy+2*dxy;  %figure width
+    height=hpanel1+hpanel2+hpanel3+4*dxy;       %figure height
+        
+    %**********************************************************************
+    % Create GUI
+    %**********************************************************************
+    h.figures(1)=figure('Units','pixels','position',[200 200 width height],'Name','Control GUI');
+    h.guistatepanel=uipanel('Title','Current GUI state','Units','pixels','Position',[dxy dxy width-2*dxy hpanel1],'parent',h.figures(1));
+    h.aircraftstatepanel=uipanel('Title','Current aircraft state','Units','pixels','Position',[dxy hpanel1+2*dxy width-2*dxy hpanel2],'parent',h.figures(1));
+    h.plotctrlpanel=uipanel('Title','Plot Control','Units','pixels','Position',[dxy hpanel1+hpanel2+3*dxy width-2*dxy hpanel3],'parent',h.figures(1));
+    
+    %%Control GUI-elements
+    %Slider: Current time
+    h.labels.CurTime=uicontrol(gcf,'style','text','Position',[dxy dxy dlabels hslider],'String','Current time t[s]:','parent',h.plotctrlpanel,'HorizontalAlignment','left');
+    h.sliders.CurTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels dxy dsliders hslider],...
+        'min',mintime,'max',maxtime,'value',mintime,'callback',@curtime_callback,'parent',h.plotctrlpanel);
+    temp=get(h.sliders.CurTime,'Max')-get(h.sliders.CurTime,'Min');
+    set(h.sliders.CurTime,'SliderStep',[1.0/temp 5.0/temp]);
+    h.edits.CurTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders dxy dedits hslider],'String',get(h.sliders.CurTime,'value'),...
+        'BackgroundColor','white','callback',@curtime_callback,'parent',h.plotctrlpanel);
+
+    %Slider: MaxTime
+    h.labels.MaxTime=uicontrol(gcf,'style','text','position',[dxy 2*dxy+hslider dlabels hslider],'String','Max. time t[s] to display:','parent',h.plotctrlpanel,'HorizontalAlignment','left');
+    h.sliders.MaxTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 2*dxy+hslider dsliders hslider],...
+        'min',mintime_log,'max',maxtime_log,'value',maxtime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel);
+    h.edits.MaxTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 2*dxy+hslider dedits hslider],'String',get(h.sliders.MaxTime,'value'),...
+        'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel);
+    
+    %Slider: MinTime
+    h.labels.MinTime=uicontrol(gcf,'style','text','position',[dxy 3*dxy+2*hslider dlabels hslider],'String','Min. time t[s] to dispay :','parent',h.plotctrlpanel,'HorizontalAlignment','left');
+    h.sliders.MinTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 3*dxy+2*hslider dsliders hslider],...
+        'min',mintime_log,'max',maxtime_log,'value',mintime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel);
+    h.edits.MinTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 3*dxy+2*hslider dedits hslider],'String',get(h.sliders.MinTime,'value'),...
+        'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel);
+
+    %%Current data/state GUI-elements (Multiline-edit-box)
+    h.edits.AircraftState=uicontrol(gcf,'style','edit','Units','normalized','position',[.02 .02 0.96 0.96],'Min',1,'Max',10,'String','This shows the current aircraft state',...
+                                    'HorizontalAlignment','left','parent',h.aircraftstatepanel);
+    
+    h.labels.GUIState=uicontrol(gcf,'style','text','Units','pixels','position',[dxy dxy width-4*dxy hslider],'String','Current state of this GUI',...
+                                'HorizontalAlignment','left','parent',h.guistatepanel);
+    
+end
+
+%% ************************************************************************
+%  INITPLOTGUI (nested function)
+%  ************************************************************************
+function InitPlotGUI()
+    
+    % Setup handles to lines and text
+    h.markertext=[];
+    templinehandle=0.0;%line([0 1],[0 5]);   % Just a temporary handle to init array
+    h.markerline(1:NrAxes)=templinehandle;   % the actual handle-array to the lines  - these are numbered consecutively
+    h.markerline(1:NrAxes)=0.0;
+        
+    % Setup all other figures and axes for plotting
+    %  PLOT WINDOW 1: GPS POSITION
+    h.figures(2)=figure('units','normalized','Toolbar','figure', 'Name', 'GPS Position');
+    h.axes(1)=axes();
+    set(h.axes(1),'Parent',h.figures(2));
+
+    %  PLOT WINDOW 2: IMU, baro altitude
+    h.figures(3)=figure('Name', 'IMU / Baro Altitude');
+    h.axes(2)=subplot(4,1,1);
+    h.axes(3)=subplot(4,1,2);
+    h.axes(4)=subplot(4,1,3);
+    h.axes(5)=subplot(4,1,4);
+    set(h.axes(2:5),'Parent',h.figures(3));
+    
+    %  PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,...
+    h.figures(4)=figure('Name', 'Attitude Estimate / Actuators / Airspeeds');
+    h.axes(6)=subplot(4,1,1);
+    h.axes(7)=subplot(4,1,2);
+    h.axes(8)=subplot(4,1,3);
+    h.axes(9)=subplot(4,1,4);
+    set(h.axes(6:9),'Parent',h.figures(4));
+    
+    %  PLOT WINDOW 4: LOG STATS
+    h.figures(5) = figure('Name', 'Log Statistics');
+    h.axes(10)=subplot(1,1,1);
+    set(h.axes(10:10),'Parent',h.figures(5));
+    
+end
+
+%% ************************************************************************
+%  DRAWRAWDATA (nested function)
+%  ************************************************************************
+%Draws the raw data from the sysvector, but does not add any
+%marker-lines or so
+function DrawRawData() 
+    % ************************************************************************
+    %  PLOT WINDOW 1: GPS POSITION & GUI
+    %  ************************************************************************
+    figure(h.figures(2));
+    % Only plot GPS data if available
+    if (sum(double(sysvector.GPS_Lat(imintime:imaxtime)))>0) && (bDisplayGPS)
+        %Draw data
+        plot3(h.axes(1),double(sysvector.GPS_Lat(imintime:imaxtime))*fconv_gpslatlong, ...
+                        double(sysvector.GPS_Lon(imintime:imaxtime))*fconv_gpslatlong, ...
+                        double(sysvector.GPS_Alt(imintime:imaxtime))*fconv_gpsalt,'r.');
+        title(h.axes(1),'GPS Position Data(if available)');
+        xlabel(h.axes(1),'Latitude [deg]');
+        ylabel(h.axes(1),'Longitude [deg]');
+        zlabel(h.axes(1),'Altitude above MSL [m]');
+        grid on
+        
+        %Reset path
+        h.pathpoints=0;
+    end
+
+    % ************************************************************************
+    %  PLOT WINDOW 2: IMU, baro altitude
+    %  ************************************************************************
+    figure(h.figures(3));
+    plot(h.axes(2),time(imintime:imaxtime),[sysvector.IMU_MagX(imintime:imaxtime), sysvector.IMU_MagY(imintime:imaxtime), sysvector.IMU_MagZ(imintime:imaxtime)]);
+    title(h.axes(2),'Magnetometers [Gauss]');
+    legend(h.axes(2),'x','y','z');
+    plot(h.axes(3),time(imintime:imaxtime),[sysvector.IMU_AccX(imintime:imaxtime), sysvector.IMU_AccY(imintime:imaxtime), sysvector.IMU_AccZ(imintime:imaxtime)]);
+    title(h.axes(3),'Accelerometers [m/s²]');
+    legend(h.axes(3),'x','y','z');
+    plot(h.axes(4),time(imintime:imaxtime),[sysvector.IMU_GyroX(imintime:imaxtime), sysvector.IMU_GyroY(imintime:imaxtime), sysvector.IMU_GyroZ(imintime:imaxtime)]);
+    title(h.axes(4),'Gyroscopes [rad/s]');
+    legend(h.axes(4),'x','y','z');
+    plot(h.axes(5),time(imintime:imaxtime),sysvector.SENS_BaroAlt(imintime:imaxtime),'color','blue');
+    if(bDisplayGPS)
+        hold on;
+        plot(h.axes(5),time(imintime:imaxtime),double(sysvector.GPS_Alt(imintime:imaxtime)).*fconv_gpsalt,'color','red');
+        hold off
+        legend('Barometric Altitude [m]','GPS Altitude [m]');
+    else
+        legend('Barometric Altitude [m]');
+    end
+    title(h.axes(5),'Altitude above MSL [m]');
+
+    % ************************************************************************
+    %  PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,...
+    % ************************************************************************
+    figure(h.figures(4));
+    %Attitude Estimate
+    plot(h.axes(6),time(imintime:imaxtime), [sysvector.ATT_Roll(imintime:imaxtime), sysvector.ATT_Pitch(imintime:imaxtime), sysvector.ATT_Yaw(imintime:imaxtime)] .*180./3.14159);
+    title(h.axes(6),'Estimated attitude [deg]');
+    legend(h.axes(6),'roll','pitch','yaw');
+    %Actuator Controls
+    plot(h.axes(7),time(imintime:imaxtime), [sysvector.ATTC_Roll(imintime:imaxtime), sysvector.ATTC_Pitch(imintime:imaxtime), sysvector.ATTC_Yaw(imintime:imaxtime), sysvector.ATTC_Thrust(imintime:imaxtime)]);
+    title(h.axes(7),'Actuator control [-]');
+    legend(h.axes(7),'ATT CTRL Roll [-1..+1]','ATT CTRL Pitch [-1..+1]','ATT CTRL Yaw [-1..+1]','ATT CTRL Thrust [0..+1]');
+    %Actuator Controls
+    plot(h.axes(8),time(imintime:imaxtime), [sysvector.OUT0_Out0(imintime:imaxtime), sysvector.OUT0_Out1(imintime:imaxtime), sysvector.OUT0_Out2(imintime:imaxtime), sysvector.OUT0_Out3(imintime:imaxtime), sysvector.OUT0_Out4(imintime:imaxtime), sysvector.OUT0_Out5(imintime:imaxtime), sysvector.OUT0_Out6(imintime:imaxtime), sysvector.OUT0_Out7(imintime:imaxtime)]);
+    title(h.axes(8),'Actuator PWM (raw-)outputs [µs]');
+    legend(h.axes(8),'CH1','CH2','CH3','CH4','CH5','CH6','CH7','CH8');
+    set(h.axes(8), 'YLim',[800 2200]);
+    %Airspeeds
+    plot(h.axes(9),time(imintime:imaxtime), sysvector.AIRS_IndSpeed(imintime:imaxtime));
+    hold on
+    plot(h.axes(9),time(imintime:imaxtime), sysvector.AIRS_TrueSpeed(imintime:imaxtime));
+    hold off
+    %add GPS total airspeed here
+    title(h.axes(9),'Airspeed [m/s]');
+    legend(h.axes(9),'Indicated Airspeed (IAS)','True Airspeed (TAS)','GPS Airspeed');
+    %calculate time differences and plot them
+    intervals = zeros(0,imaxtime - imintime);
+    for k = imintime+1:imaxtime
+        intervals(k) = time(k) - time(k-1);
+    end
+    plot(h.axes(10), time(imintime:imaxtime), intervals);
+
+    %Set same timescale for all plots
+    for i=2:NrAxes
+        set(h.axes(i),'XLim',[mintime maxtime]);
+    end
+    
+    set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]);
+end
+
+%% ************************************************************************
+%  DRAWCURRENTAIRCRAFTSTATE(nested function)
+%  ************************************************************************
+function DrawCurrentAircraftState()
+    %find current data index        
+    i=find(time>=CurTime,1,'first');
+
+    %**********************************************************************
+    % Current aircraft state label update
+    %**********************************************************************
+    acstate{1,:}=[sprintf('%s \t\t','GPS Pos:'),'[lat=',num2str(double(sysvector.GPS_Lat(i))*fconv_gpslatlong),'°, ',...
+                        'lon=',num2str(double(sysvector.GPS_Lon(i))*fconv_gpslatlong),'°, ',...
+                        'alt=',num2str(double(sysvector.GPS_Alt(i))*fconv_gpsalt),'m]'];
+    acstate{2,:}=[sprintf('%s \t\t','Mags[gauss]'),'[x=',num2str(sysvector.IMU_MagX(i)),...
+                               ', y=',num2str(sysvector.IMU_MagY(i)),...
+                               ', z=',num2str(sysvector.IMU_MagZ(i)),']'];
+    acstate{3,:}=[sprintf('%s \t\t','Accels[m/s²]'),'[x=',num2str(sysvector.IMU_AccX(i)),...
+                               ', y=',num2str(sysvector.IMU_AccY(i)),...
+                               ', z=',num2str(sysvector.IMU_AccZ(i)),']'];
+    acstate{4,:}=[sprintf('%s \t\t','Gyros[rad/s]'),'[x=',num2str(sysvector.IMU_GyroX(i)),...
+                               ', y=',num2str(sysvector.IMU_GyroY(i)),...
+                               ', z=',num2str(sysvector.IMU_GyroZ(i)),']'];
+    acstate{5,:}=[sprintf('%s \t\t','Altitude[m]'),'[Barometric: ',num2str(sysvector.SENS_BaroAlt(i)),'m, GPS: ',num2str(double(sysvector.GPS_Alt(i))*fconv_gpsalt),'m]'];
+    acstate{6,:}=[sprintf('%s \t','Est. attitude[deg]:'),'[Roll=',num2str(sysvector.ATT_Roll(i).*180./3.14159),...
+                               ', Pitch=',num2str(sysvector.ATT_Pitch(i).*180./3.14159),...
+                               ', Yaw=',num2str(sysvector.ATT_Yaw(i).*180./3.14159),']'];
+    acstate{7,:}=sprintf('%s \t[','Actuator Ctrls [-]:');
+    %for j=1:4
+    acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Roll(i)),','];
+    acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Pitch(i)),','];
+    acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Yaw(i)),','];
+    acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Thrust(i)),','];
+    %end
+    acstate{7,:}=[acstate{7,:},']'];
+    acstate{8,:}=sprintf('%s \t[','Actuator Outputs [PWM/µs]:');
+    %for j=1:8
+    acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out0(i)),','];
+    acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out1(i)),','];
+    acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out2(i)),','];
+    acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out3(i)),','];
+    acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out4(i)),','];
+    acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out5(i)),','];
+    acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out6(i)),','];
+    acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out7(i)),','];
+    %end
+    acstate{8,:}=[acstate{8,:},']'];
+    acstate{9,:}=[sprintf('%s \t','Airspeed[m/s]:'),'[IAS: ',num2str(sysvector.AIRS_IndSpeed(i)),', TAS: ',num2str(sysvector.AIRS_TrueSpeed(i)),']'];
+    
+    set(h.edits.AircraftState,'String',acstate);
+    
+    %**********************************************************************
+    % GPS Plot Update
+    %**********************************************************************
+    %Plot traveled path, and  and time.
+    figure(h.figures(2));
+    hold on;
+    if(CurTime>mintime+1) %the +1 is only a small bugfix
+        h.pathline=plot3(h.axes(1),double(sysvector.GPS_Lat(imintime:i))*fconv_gpslatlong, ...
+                                    double(sysvector.GPS_Lon(imintime:i))*fconv_gpslatlong, ...
+                                    double(sysvector.GPS_Alt(imintime:i))*fconv_gpsalt,'b','LineWidth',2); 
+    end;
+    hold off
+    %Plot current position
+    newpoint=[double(sysvector.GPS_Lat(i))*fconv_gpslatlong double(sysvector.GPS_Lat(i))*fconv_gpslatlong double(sysvector.GPS_Alt(i))*fconv_gpsalt];
+    if(numel(h.pathpoints)<=3) %empty path
+        h.pathpoints(1,1:3)=newpoint;
+    else %Not empty, append new point 
+        h.pathpoints(size(h.pathpoints,1)+1,:)=newpoint;
+    end
+    axes(h.axes(1));    
+    line(h.pathpoints(:,1),h.pathpoints(:,2),h.pathpoints(:,3),'LineStyle','none','Marker','.','MarkerEdge','black','MarkerSize',20);
+    
+    
+    % Plot current time (small label next to current gps position) 
+    textdesc=strcat('  t=',num2str(time(i)),'s');
+    if(isvalidhandle(h.markertext))
+            delete(h.markertext); %delete old text
+    end 
+    h.markertext=text(double(sysvector.GPS_Lat(i))*fconv_gpslatlong,double(sysvector.GPS_Lon(i))*fconv_gpslatlong,...
+                        double(sysvector.GPS_Alt(i))*fconv_gpsalt,textdesc);
+    set(h.edits.CurTime,'String',CurTime);
+        
+    %**********************************************************************
+    % Plot the lines showing the current time in the 2-d plots
+    %**********************************************************************
+    for i=2:NrAxes
+        if(isvalidhandle(h.markerline(i))) delete(h.markerline(i)); end
+        ylims=get(h.axes(i),'YLim');
+        h.markerline(i)=line([CurTime CurTime] ,get(h.axes(i),'YLim'),'Color','black');
+        set(h.markerline(i),'parent',h.axes(i));
+    end
+    
+    set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]);
+end
+
+%% ************************************************************************
+%  MINMAXTIME CALLBACK (nested function)
+%  ************************************************************************
+function minmaxtime_callback(hObj,event) %#ok<INUSL>
+    new_mintime=get(h.sliders.MinTime,'Value');
+    new_maxtime=get(h.sliders.MaxTime,'Value');
+
+    %Safety checks:
+    bErr=false;
+    %1: mintime must be < maxtime
+    if((new_mintime>maxtime) || (new_maxtime<mintime))
+        set(h.labels.GUIState,'String','Error: Mintime cannot be bigger than maxtime! Values were not changed.','BackgroundColor','red');
+        bErr=true;
+    else
+        %2: MinTime must be <=CurTime
+        if(new_mintime>CurTime)
+            set(h.labels.GUIState,'String','Error: Mintime cannot be bigger than CurTime! CurTime set to new mintime.','BackgroundColor','red');
+            mintime=new_mintime;
+            CurTime=mintime;
+            bErr=true;
+        end
+        %3: MaxTime must be >CurTime
+        if(new_maxtime<CurTime)
+            set(h.labels.GUIState,'String','Error: Maxtime cannot be smaller than CurTime! CurTime set to new maxtime.','BackgroundColor','red');
+            maxtime=new_maxtime;
+            CurTime=maxtime;
+            bErr=true;
+        end
+    end
+
+    if(bErr==false)
+        maxtime=new_maxtime;
+        mintime=new_mintime;
+    end
+
+    %Needs to be done in case values were reset above
+    set(h.sliders.MinTime,'Value',mintime);
+    set(h.sliders.MaxTime,'Value',maxtime);
+
+    %Update curtime-slider
+    set(h.sliders.CurTime,'Value',CurTime);
+    set(h.sliders.CurTime,'Max',maxtime);
+    set(h.sliders.CurTime,'Min',mintime);
+    temp=get(h.sliders.CurTime,'Max')-get(h.sliders.CurTime,'Min'); 
+    set(h.sliders.CurTime,'SliderStep',[1.0/temp 5.0/temp]);  %Set Stepsize to constant [in seconds]
+
+    %update edit fields
+    set(h.edits.CurTime,'String',get(h.sliders.CurTime,'Value'));
+    set(h.edits.MinTime,'String',get(h.sliders.MinTime,'Value'));
+    set(h.edits.MaxTime,'String',get(h.sliders.MaxTime,'Value'));
+
+    %Finally, we have to redraw. Update time indices first.
+    [imintime,imaxtime]=FindMinMaxTimeIndices();
+    DrawRawData();                  %Rawdata only
+    DrawCurrentAircraftState();     %path info & markers
+end
+
+
+%% ************************************************************************
+%  CURTIME CALLBACK (nested function)
+%  ************************************************************************
+function curtime_callback(hObj,event) %#ok<INUSL>
+    %find current time
+    if(hObj==h.sliders.CurTime)
+        CurTime=get(h.sliders.CurTime,'Value');
+    elseif (hObj==h.edits.CurTime)
+        temp=str2num(get(h.edits.CurTime,'String'));
+        if(temp<maxtime && temp>mintime)
+            CurTime=temp;
+        else
+            %Error
+            set(h.labels.GUIState,'String','Error: You tried to set an invalid current time! Previous value restored.','BackgroundColor','red');
+        end
+    else
+        %Error
+        set(h.labels.GUIState,'String','Error: curtime_callback','BackgroundColor','red');        
+    end
+    
+    set(h.sliders.CurTime,'Value',CurTime);
+    set(h.edits.CurTime,'String',num2str(CurTime));
+    
+    %Redraw time markers, but don't have to redraw the whole raw data
+    DrawCurrentAircraftState();
+end
+
+%% ************************************************************************
+%  FINDMINMAXINDICES (nested function)
+%  ************************************************************************
+function [idxmin,idxmax] = FindMinMaxTimeIndices()
+    for i=1:size(sysvector.TIME_StartTime,1)
+        if time(i)>=mintime; idxmin=i; break; end
+    end
+    for i=1:size(sysvector.TIME_StartTime,1)
+        if maxtime==0; idxmax=size(sysvector.TIME_StartTime,1); break; end
+        if time(i)>=maxtime; idxmax=i; break; end
+    end
+    mintime=time(idxmin); 
+    maxtime=time(idxmax);
+end
+
+%% ************************************************************************
+%  ISVALIDHANDLE (nested function)
+%  ************************************************************************
+function isvalid = isvalidhandle(handle)
+   if(exist(varname(handle))>0 && length(ishandle(handle))>0) 
+        if(ishandle(handle)>0)
+            if(handle>0.0)
+                isvalid=true;
+                return;
+            end
+        end
+   end
+   isvalid=false;
+end
+
+%% ************************************************************************
+%  JUST SOME SMALL HELPER FUNCTIONS (nested function)
+%  ************************************************************************
+function out = varname(var)
+   out = inputname(1);
+end
+
+%This is the end of the matlab file / the main function
+end
diff --git a/Tools/sdlog2/sdlog2_dump.py b/Tools/sdlog2/sdlog2_dump.py
new file mode 100644
index 000000000..5b1e55e22
--- /dev/null
+++ b/Tools/sdlog2/sdlog2_dump.py
@@ -0,0 +1,334 @@
+#!/usr/bin/env python
+
+from __future__ import print_function
+
+"""Dump binary log generated by PX4's sdlog2 or APM as CSV
+    
+Usage: python sdlog2_dump.py <log.bin> [-v] [-e] [-d delimiter] [-n null] [-m MSG[.field1,field2,...]]
+    
+    -v  Use plain debug output instead of CSV.
+    
+	-e	Recover from errors.
+    
+    -d  Use "delimiter" in CSV. Default is ",".
+    
+    -n  Use "null" as placeholder for empty values in CSV. Default is empty.
+    
+    -m MSG[.field1,field2,...]
+        Dump only messages of specified type, and only specified fields.
+        Multiple -m options allowed."""
+
+__author__  = "Anton Babushkin"
+__version__ = "1.2"
+
+import struct, sys
+
+if sys.hexversion >= 0x030000F0:
+    runningPython3 = True
+    def _parseCString(cstr):
+        return str(cstr, 'ascii').split('\0')[0]
+else:
+    runningPython3 = False
+    def _parseCString(cstr):
+        return str(cstr).split('\0')[0]
+
+class SDLog2Parser:
+    BLOCK_SIZE = 8192
+    MSG_HEADER_LEN = 3
+    MSG_HEAD1 = 0xA3
+    MSG_HEAD2 = 0x95
+    MSG_FORMAT_PACKET_LEN = 89
+    MSG_FORMAT_STRUCT = "BB4s16s64s"
+    MSG_TYPE_FORMAT = 0x80
+    FORMAT_TO_STRUCT = {
+        "b": ("b", None),
+        "B": ("B", None),
+        "h": ("h", None),
+        "H": ("H", None),
+        "i": ("i", None),
+        "I": ("I", None),
+        "f": ("f", None),
+        "n": ("4s", None),
+        "N": ("16s", None),
+        "Z": ("64s", None),
+        "c": ("h", 0.01),
+        "C": ("H", 0.01),
+        "e": ("i", 0.01),
+        "E": ("I", 0.01),
+        "L": ("i", 0.0000001),
+        "M": ("b", None),
+        "q": ("q", None),
+        "Q": ("Q", None),
+    }
+    __csv_delim = ","
+    __csv_null = ""
+    __msg_filter = []
+    __time_msg = None
+    __debug_out = False
+    __correct_errors = False
+    __file_name = None
+    __file = None
+    
+    def __init__(self):
+        return
+    
+    def reset(self):
+        self.__msg_descrs = {}      # message descriptions by message type map
+        self.__msg_labels = {}      # message labels by message name map
+        self.__msg_names = []       # message names in the same order as FORMAT messages
+        self.__buffer = bytearray() # buffer for input binary data
+        self.__ptr = 0              # read pointer in buffer
+        self.__csv_columns = []     # CSV file columns in correct order in format "MSG.label"
+        self.__csv_data = {}        # current values for all columns
+        self.__csv_updated = False
+        self.__msg_filter_map = {}  # filter in form of map, with '*" expanded to full list of fields
+    
+    def setCSVDelimiter(self, csv_delim):
+        self.__csv_delim = csv_delim
+    
+    def setCSVNull(self, csv_null):
+        self.__csv_null = csv_null
+    
+    def setMsgFilter(self, msg_filter):
+        self.__msg_filter = msg_filter
+    
+    def setTimeMsg(self, time_msg):
+        self.__time_msg = time_msg
+    
+    def setDebugOut(self, debug_out):
+        self.__debug_out = debug_out
+
+    def setCorrectErrors(self, correct_errors):
+        self.__correct_errors = correct_errors
+
+    def setFileName(self, file_name):
+    	self.__file_name = file_name
+    	if file_name != None:
+    		self.__file = open(file_name, 'w+')
+    	else:
+    		self.__file = None
+
+    
+    def process(self, fn):
+        self.reset()
+        if self.__debug_out:
+            # init __msg_filter_map
+            for msg_name, show_fields in self.__msg_filter:
+                self.__msg_filter_map[msg_name] = show_fields
+        first_data_msg = True
+        f = open(fn, "rb")
+        bytes_read = 0
+        while True:
+            chunk = f.read(self.BLOCK_SIZE)
+            if len(chunk) == 0:
+                break
+            self.__buffer = self.__buffer[self.__ptr:] + chunk
+            self.__ptr = 0
+            while self.__bytesLeft() >= self.MSG_HEADER_LEN:
+                head1 = self.__buffer[self.__ptr]
+                head2 = self.__buffer[self.__ptr+1]
+                if (head1 != self.MSG_HEAD1 or head2 != self.MSG_HEAD2):
+                    if self.__correct_errors:
+                        self.__ptr += 1
+                        continue
+                    else:
+                        raise Exception("Invalid header at %i (0x%X): %02X %02X, must be %02X %02X" % (bytes_read + self.__ptr, bytes_read + self.__ptr, head1, head2, self.MSG_HEAD1, self.MSG_HEAD2))
+                msg_type = self.__buffer[self.__ptr+2]
+                if msg_type == self.MSG_TYPE_FORMAT:
+                    # parse FORMAT message
+                    if self.__bytesLeft() < self.MSG_FORMAT_PACKET_LEN:
+                        break
+                    self.__parseMsgDescr()
+                else:
+                    # parse data message
+                    msg_descr = self.__msg_descrs[msg_type]
+                    if msg_descr == None:
+                        raise Exception("Unknown msg type: %i" % msg_type)
+                    msg_length = msg_descr[0]
+                    if self.__bytesLeft() < msg_length:
+                        break
+                    if first_data_msg:
+                        # build CSV columns and init data map
+                        self.__initCSV()
+                        first_data_msg = False
+                    self.__parseMsg(msg_descr)
+            bytes_read += self.__ptr
+        if not self.__debug_out and self.__time_msg != None and self.__csv_updated:
+            self.__printCSVRow()
+        f.close()
+    
+    def __bytesLeft(self):
+        return len(self.__buffer) - self.__ptr
+    
+    def __filterMsg(self, msg_name):
+        show_fields = "*"
+        if len(self.__msg_filter_map) > 0:
+            show_fields = self.__msg_filter_map.get(msg_name)
+        return show_fields
+    
+    def __initCSV(self):
+        if len(self.__msg_filter) == 0:
+            for msg_name in self.__msg_names:
+                self.__msg_filter.append((msg_name, "*"))
+        for msg_name, show_fields in self.__msg_filter:
+            if show_fields == "*":
+                show_fields = self.__msg_labels.get(msg_name, [])
+            self.__msg_filter_map[msg_name] = show_fields
+            for field in show_fields:
+                full_label = msg_name + "_" + field
+                self.__csv_columns.append(full_label)
+                self.__csv_data[full_label] = None
+        if self.__file != None:
+            print(self.__csv_delim.join(self.__csv_columns), file=self.__file)
+        else:
+            print(self.__csv_delim.join(self.__csv_columns))
+
+    def __printCSVRow(self):
+        s = []
+        for full_label in self.__csv_columns:
+            v = self.__csv_data[full_label]
+            if v == None:
+                v = self.__csv_null
+            else:
+                v = str(v)
+            s.append(v)
+
+        if self.__file != None:
+            print(self.__csv_delim.join(s), file=self.__file)
+        else:
+            print(self.__csv_delim.join(s))
+
+    def __parseMsgDescr(self):
+        if runningPython3:
+            data = struct.unpack(self.MSG_FORMAT_STRUCT, self.__buffer[self.__ptr + 3 : self.__ptr + self.MSG_FORMAT_PACKET_LEN])
+        else:
+            data = struct.unpack(self.MSG_FORMAT_STRUCT, str(self.__buffer[self.__ptr + 3 : self.__ptr + self.MSG_FORMAT_PACKET_LEN]))
+        msg_type = data[0]
+        if msg_type != self.MSG_TYPE_FORMAT:
+            msg_length = data[1]
+            msg_name = _parseCString(data[2])
+            msg_format = _parseCString(data[3])
+            msg_labels = _parseCString(data[4]).split(",")
+            # Convert msg_format to struct.unpack format string
+            msg_struct = ""
+            msg_mults = []
+            for c in msg_format:
+                try:
+                    f = self.FORMAT_TO_STRUCT[c]
+                    msg_struct += f[0]
+                    msg_mults.append(f[1])
+                except KeyError as e:
+                    raise Exception("Unsupported format char: %s in message %s (%i)" % (c, msg_name, msg_type))
+            msg_struct = "<" + msg_struct   # force little-endian
+            self.__msg_descrs[msg_type] = (msg_length, msg_name, msg_format, msg_labels, msg_struct, msg_mults)
+            self.__msg_labels[msg_name] = msg_labels
+            self.__msg_names.append(msg_name)
+            if self.__debug_out:
+                if self.__filterMsg(msg_name) != None:
+                    print("MSG FORMAT: type = %i, length = %i, name = %s, format = %s, labels = %s, struct = %s, mults = %s" % (
+                                msg_type, msg_length, msg_name, msg_format, str(msg_labels), msg_struct, msg_mults))
+        self.__ptr += self.MSG_FORMAT_PACKET_LEN
+    
+    def __parseMsg(self, msg_descr):
+        msg_length, msg_name, msg_format, msg_labels, msg_struct, msg_mults = msg_descr
+        if not self.__debug_out and self.__time_msg != None and msg_name == self.__time_msg and self.__csv_updated:
+            self.__printCSVRow()
+            self.__csv_updated = False
+        show_fields = self.__filterMsg(msg_name)
+        if (show_fields != None):
+            if runningPython3:
+                data = list(struct.unpack(msg_struct, self.__buffer[self.__ptr+self.MSG_HEADER_LEN:self.__ptr+msg_length]))
+            else:
+                data = list(struct.unpack(msg_struct, str(self.__buffer[self.__ptr+self.MSG_HEADER_LEN:self.__ptr+msg_length])))
+            for i in range(len(data)):
+                if type(data[i]) is str:
+                    data[i] = _parseCString(data[i])
+                m = msg_mults[i]
+                if m != None:
+                    data[i] = data[i] * m
+            if self.__debug_out:
+                s = []
+                for i in range(len(data)):
+                    label = msg_labels[i]
+                    if show_fields == "*" or label in show_fields:
+                        s.append(label + "=" + str(data[i]))
+                print("MSG %s: %s" % (msg_name, ", ".join(s)))
+            else:
+                # update CSV data buffer
+                for i in range(len(data)):
+                    label = msg_labels[i]
+                    if label in show_fields:
+                        self.__csv_data[msg_name + "_" + label] = data[i]
+                        if self.__time_msg != None and msg_name != self.__time_msg:
+                            self.__csv_updated = True
+                if self.__time_msg == None:
+                    self.__printCSVRow()
+        self.__ptr += msg_length
+
+def _main():
+    if len(sys.argv) < 2:
+        print("Usage: python sdlog2_dump.py <log.bin> [-v] [-e] [-d delimiter] [-n null] [-m MSG[.field1,field2,...]] [-t TIME_MSG_NAME]\n")
+        print("\t-v\tUse plain debug output instead of CSV.\n")
+        print("\t-e\tRecover from errors.\n")
+        print("\t-d\tUse \"delimiter\" in CSV. Default is \",\".\n")
+        print("\t-n\tUse \"null\" as placeholder for empty values in CSV. Default is empty.\n")
+        print("\t-m MSG[.field1,field2,...]\n\t\tDump only messages of specified type, and only specified fields.\n\t\tMultiple -m options allowed.")
+        print("\t-t\tSpecify TIME message name to group data messages by time and significantly reduce duplicate output.\n")
+        print("\t-fPrint to file instead of stdout")
+        return
+    fn = sys.argv[1]
+    debug_out = False
+    correct_errors = False
+    msg_filter = []
+    csv_null = ""
+    csv_delim = ","
+    time_msg = "TIME"
+    file_name = None
+    opt = None
+    for arg in sys.argv[2:]:
+        if opt != None:
+            if opt == "d":
+                csv_delim = arg
+            elif opt == "n":
+                csv_null = arg
+            elif opt == "t":
+                time_msg = arg
+            elif opt == "f":
+            	file_name = arg
+            elif opt == "m":
+                show_fields = "*"
+                a = arg.split("_")
+                if len(a) > 1:
+                    show_fields = a[1].split(",")
+                msg_filter.append((a[0], show_fields))
+            opt = None
+        else:
+            if arg == "-v":
+                debug_out = True
+            elif arg == "-e":
+                correct_errors = True
+            elif arg == "-d":
+                opt = "d"
+            elif arg == "-n":
+                opt = "n"
+            elif arg == "-m":
+                opt = "m"
+            elif arg == "-t":
+                opt = "t"
+            elif arg == "-f":
+                opt = "f"
+
+    if csv_delim == "\\t":
+        csv_delim = "\t"
+    parser = SDLog2Parser()
+    parser.setCSVDelimiter(csv_delim)
+    parser.setCSVNull(csv_null)
+    parser.setMsgFilter(msg_filter)
+    parser.setTimeMsg(time_msg)
+    parser.setFileName(file_name)
+    parser.setDebugOut(debug_out)
+    parser.setCorrectErrors(correct_errors)
+    parser.process(fn)
+
+if __name__ == "__main__":
+    _main()
diff --git a/Tools/tests-host/.gitignore b/Tools/tests-host/.gitignore
new file mode 100644
index 000000000..61e091551
--- /dev/null
+++ b/Tools/tests-host/.gitignore
@@ -0,0 +1,2 @@
+./obj/*
+mixer_test
diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile
new file mode 100644
index 000000000..7ab1454f0
--- /dev/null
+++ b/Tools/tests-host/Makefile
@@ -0,0 +1,47 @@
+
+CC=g++
+CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers -I../../src -D__EXPORT="" -Dnullptr="0"
+
+ODIR=obj
+LDIR =../lib
+
+LIBS=-lm
+
+#_DEPS = test.h
+#DEPS = $(patsubst %,$(IDIR)/%,$(_DEPS))
+
+_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o \
+	mixer.o mixer_group.o mixer_load.o test_conv.o pwm_limit.o hrt.o
+OBJ = $(patsubst %,$(ODIR)/%,$(_OBJ))
+
+#$(DEPS)
+$(ODIR)/%.o: %.cpp
+	mkdir -p obj
+	$(CC) -c -o $@ $< $(CFLAGS)
+
+$(ODIR)/%.o: ../../src/systemcmds/tests/%.cpp
+	$(CC) -c -o $@ $< $(CFLAGS)
+
+$(ODIR)/%.o: ../../src/modules/systemlib/%.cpp
+	$(CC) -c -o $@ $< $(CFLAGS)
+
+$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.cpp
+	$(CC) -c -o $@ $< $(CFLAGS)
+
+$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.cpp
+	$(CC) -c -o $@ $< $(CFLAGS)
+
+$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.c
+	$(CC) -c -o $@ $< $(CFLAGS)
+
+$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.c
+	$(CC) -c -o $@ $< $(CFLAGS)
+
+#
+mixer_test: $(OBJ)
+	g++ -o $@ $^ $(CFLAGS) $(LIBS)
+
+.PHONY: clean
+
+clean:
+	rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~
\ No newline at end of file
diff --git a/Tools/tests-host/arch/board/board.h b/Tools/tests-host/arch/board/board.h
new file mode 100644
index 000000000..e69de29bb
--- /dev/null
+++ b/Tools/tests-host/arch/board/board.h
diff --git a/Tools/tests-host/hrt.cpp b/Tools/tests-host/hrt.cpp
new file mode 100644
index 000000000..01b5958b7
--- /dev/null
+++ b/Tools/tests-host/hrt.cpp
@@ -0,0 +1,16 @@
+#include <sys/time.h>
+#include <inttypes.h>
+#include <drivers/drv_hrt.h>
+#include <stdio.h>
+
+hrt_abstime hrt_absolute_time() {
+    struct timeval te; 
+    gettimeofday(&te, NULL); // get current time
+    hrt_abstime us = static_cast<uint64_t>(te.tv_sec) * 1e6 + te.tv_usec; // caculate us
+    return us;
+}
+
+hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then) {
+	// not thread safe
+    return hrt_absolute_time() - *then;
+}
diff --git a/Tools/tests-host/mixer_test.cpp b/Tools/tests-host/mixer_test.cpp
new file mode 100644
index 000000000..e311617f9
--- /dev/null
+++ b/Tools/tests-host/mixer_test.cpp
@@ -0,0 +1,14 @@
+#include <systemlib/mixer/mixer.h>
+#include <systemlib/err.h>
+#include "../../src/systemcmds/tests/tests.h"
+
+int main(int argc, char *argv[]) {
+	warnx("Host execution started");
+
+	char* args[] = {argv[0], "../../ROMFS/px4fmu_common/mixers/IO_pass.mix",
+				 "../../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"};
+
+	test_mixer(3, args);
+
+	test_conv(1, args);
+}
\ No newline at end of file
diff --git a/Tools/tests-host/nuttx/config.h b/Tools/tests-host/nuttx/config.h
new file mode 100644
index 000000000..e69de29bb
--- /dev/null
+++ b/Tools/tests-host/nuttx/config.h
diff --git a/Tools/tests-host/queue.h b/Tools/tests-host/queue.h
new file mode 100644
index 000000000..0fdb170db
--- /dev/null
+++ b/Tools/tests-host/queue.h
@@ -0,0 +1,133 @@
+/************************************************************************
+ * include/queue.h
+ *
+ *   Copyright (C) 2007-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 __INCLUDE_QUEUE_H
+#define __INCLUDE_QUEUE_H
+
+#ifndef FAR
+#define FAR
+#endif
+
+/************************************************************************
+ * Included Files
+ ************************************************************************/
+
+#include <sys/types.h>
+
+/************************************************************************
+ * Pre-processor Definitions
+ ************************************************************************/
+
+#define sq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0)
+#define dq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0)
+
+#define sq_next(p) ((p)->flink)
+#define dq_next(p) ((p)->flink)
+#define dq_prev(p) ((p)->blink)
+
+#define sq_empty(q) ((q)->head == NULL)
+#define dq_empty(q) ((q)->head == NULL)
+
+#define sq_peek(q)  ((q)->head)
+#define dq_peek(q)  ((q)->head)
+
+/************************************************************************
+ * Global Type Declarations
+ ************************************************************************/
+
+struct sq_entry_s
+{
+  FAR struct sq_entry_s *flink;
+};
+typedef struct sq_entry_s sq_entry_t;
+
+struct dq_entry_s
+{
+  FAR struct dq_entry_s *flink;
+  FAR struct dq_entry_s *blink;
+};
+typedef struct dq_entry_s dq_entry_t;
+
+struct sq_queue_s
+{
+  FAR sq_entry_t *head;
+  FAR sq_entry_t *tail;
+};
+typedef struct sq_queue_s  sq_queue_t;
+
+struct dq_queue_s
+{
+  FAR dq_entry_t *head;
+  FAR dq_entry_t *tail;
+};
+typedef struct dq_queue_s dq_queue_t;
+
+/************************************************************************
+ * Global Function Prototypes
+ ************************************************************************/
+
+#ifdef __cplusplus
+#define EXTERN extern "C"
+extern "C" {
+#else
+#define EXTERN extern
+#endif
+
+EXTERN void  sq_addfirst(FAR sq_entry_t *node, sq_queue_t *queue);
+EXTERN void  dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue);
+EXTERN void  sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue);
+EXTERN void  dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue);
+EXTERN void  sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node,
+                         sq_queue_t *queue);
+EXTERN void  dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node,
+                         dq_queue_t *queue);
+EXTERN void  dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node,
+                          dq_queue_t *queue);
+
+EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue);
+EXTERN void            sq_rem(FAR sq_entry_t *node, sq_queue_t *queue);
+EXTERN void            dq_rem(FAR dq_entry_t *node, dq_queue_t *queue);
+EXTERN FAR sq_entry_t *sq_remlast(sq_queue_t *queue);
+EXTERN FAR dq_entry_t *dq_remlast(dq_queue_t *queue);
+EXTERN FAR sq_entry_t *sq_remfirst(sq_queue_t *queue);
+EXTERN FAR dq_entry_t *dq_remfirst(dq_queue_t *queue);
+
+#undef EXTERN
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __INCLUDE_QUEUE_H_ */
+
diff --git a/makefiles/board_px4fmu-v1.mk b/makefiles/board_px4fmu-v1.mk
index 837069644..4d692e31a 100644
--- a/makefiles/board_px4fmu-v1.mk
+++ b/makefiles/board_px4fmu-v1.mk
@@ -6,5 +6,6 @@
 # Configure the toolchain
 #
 CONFIG_ARCH			 = CORTEXM4F
+CONFIG_BOARD			 = PX4FMU_V1
 
 include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk
diff --git a/makefiles/board_px4fmu-v2.mk b/makefiles/board_px4fmu-v2.mk
new file mode 100644
index 000000000..e9a2985b7
--- /dev/null
+++ b/makefiles/board_px4fmu-v2.mk
@@ -0,0 +1,11 @@
+#
+# Board-specific definitions for the PX4FMUv2
+#
+
+#
+# Configure the toolchain
+#
+CONFIG_ARCH			 = CORTEXM4F
+CONFIG_BOARD			 = PX4FMU_V2
+
+include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk
diff --git a/makefiles/board_px4io-v1.mk b/makefiles/board_px4io-v1.mk
index b0eb2dae7..1872a4124 100644
--- a/makefiles/board_px4io-v1.mk
+++ b/makefiles/board_px4io-v1.mk
@@ -6,5 +6,6 @@
 # Configure the toolchain
 #
 CONFIG_ARCH			 = CORTEXM3
+CONFIG_BOARD			 = PX4IO_V1
 
 include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk
diff --git a/makefiles/board_px4io-v2.mk b/makefiles/board_px4io-v2.mk
new file mode 100644
index 000000000..50a4068fb
--- /dev/null
+++ b/makefiles/board_px4io-v2.mk
@@ -0,0 +1,11 @@
+#
+# Board-specific definitions for the PX4IOv2
+#
+
+#
+# Configure the toolchain
+#
+CONFIG_ARCH			 = CORTEXM3
+CONFIG_BOARD			 = PX4IO_V2
+
+include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk
diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index 4fc61e97d..43a03f4f7 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -6,6 +6,7 @@
 # Use the configuration's ROMFS.
 #
 ROMFS_ROOT	 = $(PX4_BASE)/ROMFS/px4fmu_common
+ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v1_default.bin
 
 #
 # Board support modules
@@ -17,27 +18,31 @@ MODULES		+= drivers/stm32/tone_alarm
 MODULES		+= drivers/led
 MODULES		+= drivers/px4io
 MODULES		+= drivers/px4fmu
-MODULES		+= drivers/boards/px4fmu
+MODULES		+= drivers/boards/px4fmu-v1
 MODULES		+= drivers/ardrone_interface
 MODULES		+= drivers/l3gd20
-MODULES		+= drivers/bma180
+#MODULES		+= drivers/bma180
 MODULES		+= drivers/mpu6000
 MODULES		+= drivers/hmc5883
 MODULES		+= drivers/ms5611
 MODULES		+= drivers/mb12xx
 MODULES		+= drivers/gps
 MODULES		+= drivers/hil
-MODULES		+= drivers/hott_telemetry
+MODULES		+= drivers/hott/hott_telemetry
+MODULES		+= drivers/hott/hott_sensors
 MODULES		+= drivers/blinkm
+MODULES		+= drivers/rgbled
 MODULES		+= drivers/mkblctrl
-MODULES		+= drivers/md25
+MODULES		+= drivers/airspeed
 MODULES		+= drivers/ets_airspeed
+MODULES		+= drivers/meas_airspeed
+MODULES		+= drivers/frsky_telemetry
 MODULES		+= modules/sensors
 
 #
 # System commands
 #
-MODULES		+= systemcmds/eeprom
+MODULES		+= systemcmds/mtd
 MODULES		+= systemcmds/bl_update
 MODULES		+= systemcmds/boardinfo
 MODULES		+= systemcmds/i2c
@@ -46,53 +51,71 @@ 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/tests
+MODULES		+= systemcmds/config
+MODULES		+= systemcmds/nshterm
+MODULES		+= systemcmds/hw_ver
 
 #
 # General system control
 #
 MODULES		+= modules/commander
+MODULES		+= modules/navigator
 MODULES		+= modules/mavlink
 MODULES		+= modules/mavlink_onboard
 MODULES		+= modules/gpio_led
 
 #
-# Estimation modules (EKF / other filters)
+# Estimation modules (EKF/ SO3 / other filters)
 #
 MODULES		+= modules/attitude_estimator_ekf
-MODULES		+= modules/position_estimator_mc
-MODULES		+= modules/position_estimator
+MODULES		+= modules/attitude_estimator_so3
 MODULES		+= modules/att_pos_estimator_ekf
+MODULES		+= modules/position_estimator_inav
+#MODULES		+= examples/flow_position_estimator
 
 #
 # Vehicle Control
 #
-MODULES		+= modules/fixedwing_backside
-MODULES		+= modules/fixedwing_att_control
-MODULES		+= modules/fixedwing_pos_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
 
 #
 # Logging
 #
-MODULES		+= modules/sdlog
+MODULES		+= modules/sdlog2
+
+#
+# Unit tests
+#
+#MODULES 	+= modules/unit_test
+#MODULES 	+= modules/commander/commander_tests
 
 #
 # Library modules
 #
 MODULES		+= modules/systemlib
 MODULES		+= modules/systemlib/mixer
-MODULES		+= modules/mathlib
 MODULES		+= modules/controllib
 MODULES		+= modules/uORB
 
 #
 # Libraries
 #
-LIBRARIES	+= modules/mathlib/CMSIS
+LIBRARIES	+= lib/mathlib/CMSIS
+MODULES		+= lib/mathlib
+MODULES		+= lib/mathlib/math/filter
+MODULES		+= lib/ecl
+MODULES		+= lib/external_lgpl
+MODULES		+= lib/geo
+MODULES		+= lib/conversion
 
 #
 # Demo apps
@@ -112,7 +135,10 @@ LIBRARIES	+= modules/mathlib/CMSIS
 
 # Tutorial code from
 # https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control
-MODULES			+= examples/fixedwing_control
+#MODULES			+= examples/fixedwing_control
+
+# Hardware test
+#MODULES			+= examples/hwtest
 
 #
 # Transitional support - add commands from the NuttX export archive.
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
new file mode 100644
index 000000000..dc9208339
--- /dev/null
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -0,0 +1,161 @@
+#
+# Makefile for the px4fmu_default configuration
+#
+
+#
+# Use the configuration's ROMFS, copy the px4iov2 firmware into
+# the ROMFS if it's available
+#
+ROMFS_ROOT	 = $(PX4_BASE)/ROMFS/px4fmu_common
+ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_default.bin
+
+#
+# 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/px4io
+MODULES		+= drivers/boards/px4fmu-v2
+MODULES		+= drivers/rgbled
+MODULES		+= drivers/mpu6000
+MODULES		+= drivers/lsm303d
+MODULES		+= drivers/l3gd20
+MODULES		+= drivers/hmc5883
+MODULES		+= drivers/ms5611
+MODULES		+= drivers/mb12xx
+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
+
+
+# Needs to be burned to the ground and re-written; for now,
+# just don't build it.
+#MODULES		+= drivers/mkblctrl
+
+#
+# System commands
+#
+MODULES		+= systemcmds/bl_update
+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/tests
+MODULES		+= systemcmds/config
+MODULES		+= systemcmds/nshterm
+MODULES		+= systemcmds/mtd
+MODULES		+= systemcmds/hw_ver
+
+#
+# General system control
+#
+MODULES		+= modules/commander
+MODULES		+= modules/navigator
+MODULES		+= modules/mavlink
+MODULES		+= modules/mavlink_onboard
+
+#
+# Estimation modules (EKF/ SO3 / other filters)
+#
+MODULES		+= modules/attitude_estimator_ekf
+MODULES		+= modules/attitude_estimator_so3
+MODULES		+= modules/att_pos_estimator_ekf
+MODULES		+= modules/position_estimator_inav
+MODULES		+= examples/flow_position_estimator
+
+#
+# Vehicle Control
+#
+#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
+
+#
+# Logging
+#
+MODULES		+= modules/sdlog2
+
+#
+# Unit tests
+#
+#MODULES 	+= modules/unit_test
+#MODULES 	+= modules/commander/commander_tests
+
+#
+# Library modules
+#
+MODULES		+= modules/systemlib
+MODULES		+= modules/systemlib/mixer
+MODULES		+= modules/controllib
+MODULES		+= modules/uORB
+
+#
+# Libraries
+#
+LIBRARIES	+= lib/mathlib/CMSIS
+MODULES		+= lib/mathlib
+MODULES		+= lib/mathlib/math/filter
+MODULES		+= lib/ecl
+MODULES		+= lib/external_lgpl
+MODULES		+= lib/geo
+MODULES		+= lib/conversion
+
+#
+# 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
+
+#                  command                 priority                   stack  entrypoint
+BUILTIN_COMMANDS := \
+	$(call _B, sercon,                 ,                          2048,  sercon_main                ) \
+	$(call _B, serdis,                 ,                          2048,  serdis_main                )
diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk
new file mode 100644
index 000000000..4f997e96b
--- /dev/null
+++ b/makefiles/config_px4fmu-v2_test.mk
@@ -0,0 +1,59 @@
+#
+# Makefile for the px4fmu_default configuration
+#
+
+#
+# Use the configuration's ROMFS.
+#
+ROMFS_ROOT	 = $(PX4_BASE)/ROMFS/px4fmu_test
+ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_default.bin
+
+#
+# Board support modules
+#
+MODULES		+= drivers/device
+MODULES		+= drivers/stm32
+MODULES		+= drivers/stm32/adc
+MODULES		+= drivers/stm32/tone_alarm
+MODULES		+= drivers/led
+MODULES		+= drivers/boards/px4fmu-v2
+MODULES		+= drivers/px4io
+MODULES		+= drivers/rgbled
+MODULES		+= drivers/mpu6000
+MODULES		+= drivers/lsm303d
+MODULES		+= drivers/l3gd20
+MODULES		+= drivers/hmc5883
+MODULES		+= drivers/ms5611
+MODULES		+= systemcmds/perf
+MODULES		+= systemcmds/reboot
+MODULES		+= systemcmds/tests
+MODULES		+= systemcmds/nshterm
+MODULES		+= systemcmds/mtd
+MODULES		+= systemcmds/hw_ver
+
+#
+# Library modules
+#
+MODULES		+= modules/systemlib
+MODULES		+= modules/systemlib/mixer
+MODULES		+= modules/uORB
+LIBRARIES	+= lib/mathlib/CMSIS
+MODULES		+= lib/mathlib
+MODULES		+= lib/mathlib/math/filter
+
+#
+# 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
+
+#                  command                 priority                   stack  entrypoint
+BUILTIN_COMMANDS := \
+	$(call _B, sercon,                 ,                          2048,  sercon_main                ) \
+	$(call _B, serdis,                 ,                          2048,  serdis_main                )
diff --git a/makefiles/config_px4io-v1_default.mk b/makefiles/config_px4io-v1_default.mk
index cf70391bc..73f8adf20 100644
--- a/makefiles/config_px4io-v1_default.mk
+++ b/makefiles/config_px4io-v1_default.mk
@@ -6,5 +6,5 @@
 # Board support modules
 #
 MODULES		+= drivers/stm32
-MODULES		+= drivers/boards/px4io
+MODULES		+= drivers/boards/px4io-v1
 MODULES		+= modules/px4iofirmware
\ No newline at end of file
diff --git a/makefiles/config_px4io-v2_default.mk b/makefiles/config_px4io-v2_default.mk
new file mode 100644
index 000000000..dbeaba3d3
--- /dev/null
+++ b/makefiles/config_px4io-v2_default.mk
@@ -0,0 +1,10 @@
+#
+# Makefile for the px4iov2_default configuration
+#
+
+#
+# Board support modules
+#
+MODULES		+= drivers/stm32
+MODULES		+= drivers/boards/px4io-v2
+MODULES		+= modules/px4iofirmware
\ No newline at end of file
diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk
index 6b09e6ec3..cb20d9cd1 100644
--- a/makefiles/firmware.mk
+++ b/makefiles/firmware.mk
@@ -110,6 +110,8 @@ ifneq ($(words $(PX4_BASE)),1)
 $(error Cannot build when the PX4_BASE path contains one or more space characters.)
 endif
 
+$(info %  GIT_DESC            = $(GIT_DESC))
+
 #
 # Set a default target so that included makefiles or errors here don't 
 # cause confusion.
@@ -153,6 +155,7 @@ ifeq ($(BOARD_FILE),)
 $(error Config $(CONFIG) references board $(BOARD), but no board definition file found)
 endif
 export BOARD
+export BOARD_FILE
 include $(BOARD_FILE)
 $(info %  BOARD               = $(BOARD))
 
@@ -176,6 +179,23 @@ GLOBAL_DEPS		+= $(MAKEFILE_LIST)
 #
 EXTRA_CLEANS		 = 
 
+
+#
+# Extra defines for compilation
+#
+export EXTRADEFINES := -DGIT_VERSION=$(GIT_DESC)
+
+#
+# Append the per-board driver directory to the header search path.
+#
+INCLUDE_DIRS		+= $(PX4_MODULE_SRC)drivers/boards/$(BOARD)
+
+################################################################################
+# NuttX libraries and paths
+################################################################################
+
+include $(PX4_MK_DIR)/nuttx.mk
+
 ################################################################################
 # Modules
 ################################################################################
@@ -297,12 +317,6 @@ $(LIBRARY_CLEANS):
 	clean
 
 ################################################################################
-# NuttX libraries and paths
-################################################################################
-
-include $(PX4_MK_DIR)/nuttx.mk
-
-################################################################################
 # ROMFS generation
 ################################################################################
 
@@ -316,7 +330,7 @@ endif
 # a root from several templates. That would be a nice feature.
 #
 
-# Add dependencies on anything in the ROMFS root
+# Add dependencies on anything in the ROMFS root directory
 ROMFS_FILES		+= $(wildcard \
 			     $(ROMFS_ROOT)/* \
 			     $(ROMFS_ROOT)/*/* \
@@ -328,7 +342,14 @@ ifeq ($(ROMFS_FILES),)
 $(error ROMFS_ROOT $(ROMFS_ROOT) specifies a directory containing no files)
 endif
 ROMFS_DEPS		+= $(ROMFS_FILES)
+
+# Extra files that may be copied into the ROMFS /extras directory
+# ROMFS_EXTRA_FILES are required, ROMFS_OPTIONAL_FILES are optional
+ROMFS_EXTRA_FILES	+= $(wildcard $(ROMFS_OPTIONAL_FILES))
+ROMFS_DEPS		+= $(ROMFS_EXTRA_FILES)
+
 ROMFS_IMG		 = romfs.img
+ROMFS_SCRATCH		 = romfs_scratch
 ROMFS_CSRC		 = $(ROMFS_IMG:.img=.c)
 ROMFS_OBJ		 = $(ROMFS_CSRC:.c=.o)
 LIBS			+= $(ROMFS_OBJ)
@@ -339,9 +360,18 @@ $(ROMFS_OBJ): $(ROMFS_IMG) $(GLOBAL_DEPS)
 	$(call BIN_TO_OBJ,$<,$@,romfs_img)
 
 # Generate the ROMFS image from the root
-$(ROMFS_IMG): $(ROMFS_DEPS) $(GLOBAL_DEPS)
+$(ROMFS_IMG): $(ROMFS_SCRATCH) $(ROMFS_DEPS) $(GLOBAL_DEPS)
 	@$(ECHO) "ROMFS:   $@"
-	$(Q) $(GENROMFS) -f $@ -d $(ROMFS_ROOT) -V "NSHInitVol"
+	$(Q) $(GENROMFS) -f $@ -d $(ROMFS_SCRATCH) -V "NSHInitVol"
+
+# Construct the ROMFS scratch root from the canonical root
+$(ROMFS_SCRATCH): $(ROMFS_DEPS) $(GLOBAL_DEPS)
+	$(Q) $(MKDIR) -p $(ROMFS_SCRATCH)
+	$(Q) $(COPYDIR) $(ROMFS_ROOT)/* $(ROMFS_SCRATCH)
+ifneq ($(ROMFS_EXTRA_FILES),)
+	$(Q) $(MKDIR) -p $(ROMFS_SCRATCH)/extras
+	$(Q) $(COPY) $(ROMFS_EXTRA_FILES) $(ROMFS_SCRATCH)/extras
+endif
 
 EXTRA_CLEANS		+= $(ROMGS_OBJ) $(ROMFS_IMG)
 
@@ -385,7 +415,7 @@ define BUILTIN_DEF
 endef
 
 # Don't generate until modules have updated their command files
-$(BUILTIN_CSRC):	$(GLOBAL_DEPS) $(MODULE_OBJS) $(BUILTIN_COMMAND_FILES)
+$(BUILTIN_CSRC):	$(GLOBAL_DEPS) $(MODULE_OBJS) $(MODULE_MKFILES) $(BUILTIN_COMMAND_FILES)
 	@$(ECHO) "CMDS:    $@"
 	$(Q) $(ECHO) '/* builtin command list - automatically generated, do not edit */' > $@
 	$(Q) $(ECHO) '#include <nuttx/config.h>' >> $@
diff --git a/makefiles/module.mk b/makefiles/module.mk
index 9e4cbafc9..9c1a828cc 100644
--- a/makefiles/module.mk
+++ b/makefiles/module.mk
@@ -98,6 +98,7 @@
 #
 # CONFIG
 # BOARD
+# BOARD_FILE
 # MODULE_WORK_DIR
 # MODULE_OBJ
 # MODULE_MK
@@ -117,7 +118,7 @@ $(info %% MODULE_MK           = $(MODULE_MK))
 #
 # Get the board/toolchain config
 #
-include $(PX4_MK_DIR)/board_$(BOARD).mk
+include $(BOARD_FILE)
 
 #
 # Get the module's config
diff --git a/makefiles/nuttx.mk b/makefiles/nuttx.mk
index 346735a02..d283096b2 100644
--- a/makefiles/nuttx.mk
+++ b/makefiles/nuttx.mk
@@ -69,10 +69,14 @@ INCLUDE_DIRS		+= $(NUTTX_EXPORT_DIR)include \
 
 LIB_DIRS		+= $(NUTTX_EXPORT_DIR)libs
 LIBS			+= -lapps -lnuttx
-LINK_DEPS		+= $(NUTTX_EXPORT_DIR)libs/libapps.a \
+NUTTX_LIBS		 = $(NUTTX_EXPORT_DIR)libs/libapps.a \
 			   $(NUTTX_EXPORT_DIR)libs/libnuttx.a
+LINK_DEPS		+= $(NUTTX_LIBS)
 
 $(NUTTX_CONFIG_HEADER):	$(NUTTX_ARCHIVE)
 	@$(ECHO) %% Unpacking $(NUTTX_ARCHIVE)
 	$(Q) $(UNZIP_CMD) -q -o -d $(WORK_DIR) $(NUTTX_ARCHIVE)
 	$(Q) $(TOUCH) $@
+
+ $(LDSCRIPT): $(NUTTX_CONFIG_HEADER)
+ $(NUTTX_LIBS): $(NUTTX_CONFIG_HEADER)
diff --git a/makefiles/setup.mk b/makefiles/setup.mk
index 92461fafb..183b143d6 100644
--- a/makefiles/setup.mk
+++ b/makefiles/setup.mk
@@ -43,6 +43,7 @@
 #
 export PX4_INCLUDE_DIR	 = $(abspath $(PX4_BASE)/src/include)/
 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 NUTTX_APP_SRC	 = $(abspath $(PX4_BASE)/NuttX/apps)/
@@ -57,7 +58,8 @@ export ARCHIVE_DIR	 = $(abspath $(PX4_BASE)/Archives)/
 #
 export INCLUDE_DIRS	:= $(PX4_MODULE_SRC) \
 			   $(PX4_MODULE_SRC)/modules/ \
-			   $(PX4_INCLUDE_DIR)
+			   $(PX4_INCLUDE_DIR) \
+			   $(PX4_LIB_DIR)
 
 #
 # Tools
@@ -65,11 +67,13 @@ export INCLUDE_DIRS	:= $(PX4_MODULE_SRC) \
 export MKFW		 = $(PX4_BASE)/Tools/px_mkfw.py
 export UPLOADER		 = $(PX4_BASE)/Tools/px_uploader.py
 export COPY		 = cp
+export COPYDIR		 = cp -Rf
 export REMOVE		 = rm -f
 export RMDIR		 = rm -rf
 export GENROMFS		 = genromfs
 export TOUCH		 = touch
 export MKDIR		 = mkdir
+export FIND		 = find
 export ECHO		 = echo
 export UNZIP_CMD	 = unzip
 export PYTHON		 = python
diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk
index c75a08bd1..bb729e103 100644
--- a/makefiles/toolchain_gnu-arm-eabi.mk
+++ b/makefiles/toolchain_gnu-arm-eabi.mk
@@ -50,7 +50,7 @@ OBJDUMP			 = $(CROSSDEV)objdump
 
 # XXX this is pulled pretty directly from the fmu Make.defs - needs cleanup
 
-MAXOPTIMIZATION		 = -O3
+MAXOPTIMIZATION		 ?= -O3
 
 # Base CPU flags for each of the supported architectures.
 #
@@ -70,6 +70,14 @@ ARCHCPUFLAGS_CORTEXM3	 = -mcpu=cortex-m3 \
 			   -march=armv7-m \
 			   -mfloat-abi=soft
 
+ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions \
+			   -ffixed-r10
+
+ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions \
+			   -ffixed-r10
+
+ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = 
+
 # Pick the right set of flags for the architecture.
 #
 ARCHCPUFLAGS		 = $(ARCHCPUFLAGS_$(CONFIG_ARCH))
@@ -77,6 +85,13 @@ ifeq ($(ARCHCPUFLAGS),)
 $(error Must set CONFIG_ARCH to one of CORTEXM4F, CORTEXM4 or CORTEXM3)
 endif
 
+# Set the board flags
+#
+ifeq ($(CONFIG_BOARD),)
+$(error Board config does not define CONFIG_BOARD)
+endif
+ARCHDEFINES		+= -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD)
+
 # optimisation flags
 #
 ARCHOPTIMIZATION	 = $(MAXOPTIMIZATION) \
@@ -91,8 +106,8 @@ ARCHOPTIMIZATION	 = $(MAXOPTIMIZATION) \
 
 # enable precise stack overflow tracking
 # note - requires corresponding support in NuttX
-INSTRUMENTATIONDEFINES	 = -finstrument-functions \
-			   -ffixed-r10
+INSTRUMENTATIONDEFINES	 = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH))
+
 # Language-specific flags
 #
 ARCHCFLAGS		 = -std=gnu99
@@ -123,8 +138,7 @@ ARCHCWARNINGS		 = $(ARCHWARNINGS) \
 			   -Wold-style-declaration \
 			   -Wmissing-parameter-type \
 			   -Wmissing-prototypes \
-			   -Wnested-externs \
-			   -Wunsuffixed-float-constants
+			   -Wnested-externs
 
 # C++-specific warnings
 #
diff --git a/makefiles/upload.mk b/makefiles/upload.mk
index 4b01b447d..bc26d743d 100644
--- a/makefiles/upload.mk
+++ b/makefiles/upload.mk
@@ -12,13 +12,13 @@ SYSTYPE			:= $(shell uname -s)
 # XXX The uploader should be smarter than this.
 #
 ifeq ($(SYSTYPE),Darwin)
-SERIAL_PORTS		?= "/dev/tty.usbmodemPX1,/dev/tty.usbmodemPX2,/dev/tty.usbmodemPX3,/dev/tty.usbmodemPX4,/dev/tty.usbmodem1,/dev/tty.usbmodem2,/dev/tty.usbmodem3,/dev/tty.usbmodem4"
+SERIAL_PORTS		?= "/dev/tty.usbmodemPX*,/dev/tty.usbmodem*"
 endif
 ifeq ($(SYSTYPE),Linux)
-SERIAL_PORTS		?= "/dev/ttyACM5,/dev/ttyACM4,/dev/ttyACM3,/dev/ttyACM2,/dev/ttyACM1,/dev/ttyACM0"
+SERIAL_PORTS		?= "/dev/serial/by-id/usb-3D_Robotics*"
 endif
 ifeq ($(SERIAL_PORTS),)
-SERIAL_PORTS		 = "\\\\.\\COM32,\\\\.\\COM31,\\\\.\\COM30,\\\\.\\COM29,\\\\.\\COM28,\\\\.\\COM27,\\\\.\\COM26,\\\\.\\COM25,\\\\.\\COM24,\\\\.\\COM23,\\\\.\\COM22,\\\\.\\COM21,\\\\.\\COM20,\\\\.\\COM19,\\\\.\\COM18,\\\\.\\COM17,\\\\.\\COM16,\\\\.\\COM15,\\\\.\\COM14,\\\\.\\COM13,\\\\.\\COM12,\\\\.\\COM11,\\\\.\\COM10,\\\\.\\COM9,\\\\.\\COM8,\\\\.\\COM7,\\\\.\\COM6,\\\\.\\COM5,\\\\.\\COM4,\\\\.\\COM3,\\\\.\\COM2,\\\\.\\COM1,\\\\.\\COM0"
+SERIAL_PORTS		 = "COM32,COM31,COM30,COM29,COM28,COM27,COM26,COM25,COM24,COM23,COM22,COM21,COM20,COM19,COM18,COM17,COM16,COM15,COM14,COM13,COM12,COM11,COM10,COM9,COM8,COM7,COM6,COM5,COM4,COM3,COM2,COM1,COM0"
 endif
 
 .PHONY:	all upload-$(METHOD)-$(BOARD)
@@ -27,7 +27,7 @@ all:	upload-$(METHOD)-$(BOARD)
 upload-serial-px4fmu-v1:	$(BUNDLE) $(UPLOADER)
 	$(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE)
 
-upload-serial-px4fmuv2:	$(BUNDLE) $(UPLOADER)
+upload-serial-px4fmu-v2:	$(BUNDLE) $(UPLOADER)
 	$(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE)
 
 #
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h b/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h
index afc57c443..2acb7965c 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h
@@ -12,15 +12,15 @@ extern "C" {
 // MESSAGE LENGTHS AND CRCS
 
 #ifndef MAVLINK_MESSAGE_LENGTHS
-#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 0, 0, 0, 254, 249, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 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, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0}
+#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 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, 0, 0, 0, 235, 93, 124, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 111, 21, 21, 144, 1, 234, 73, 181, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}
+#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 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, {"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_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}}
+#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"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"
@@ -74,10 +74,12 @@ enum MAV_CMD
 	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|  */
@@ -86,7 +88,8 @@ enum MAV_CMD
 	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_ENUM_END=401, /*  | */
+	MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX|  */
+	MAV_CMD_ENUM_END=501, /*  | */
 };
 #endif
 
@@ -98,7 +101,8 @@ 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_ENUM_END=3, /*  | */
+	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
 
@@ -109,7 +113,7 @@ enum 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_MAXALT=2, /* Breached maximum altitude | */
 	FENCE_BREACH_BOUNDARY=3, /* Breached fence boundary | */
 	FENCE_BREACH_ENUM_END=4, /*  | */
 };
@@ -142,6 +146,17 @@ enum LIMIT_MODULE
 };
 #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
@@ -178,6 +193,11 @@ enum LIMIT_MODULE
 #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
 }
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h
index a59f89aee..c3ead1140 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h
@@ -16,6 +16,9 @@ typedef struct __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 { \
@@ -51,7 +54,7 @@ static inline uint16_t mavlink_msg_ahrs_pack(uint8_t system_id, uint8_t componen
 						       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[28];
+	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);
@@ -60,7 +63,7 @@ static inline uint16_t mavlink_msg_ahrs_pack(uint8_t system_id, uint8_t componen
 	_mav_put_float(buf, 20, error_rp);
 	_mav_put_float(buf, 24, error_yaw);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 28);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS_LEN);
 #else
 	mavlink_ahrs_t packet;
 	packet.omegaIx = omegaIx;
@@ -71,18 +74,22 @@ static inline uint16_t mavlink_msg_ahrs_pack(uint8_t system_id, uint8_t componen
 	packet.error_rp = error_rp;
 	packet.error_yaw = error_yaw;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 28);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_AHRS;
-	return mavlink_finalize_message(msg, system_id, component_id, 28, 127);
+#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 was sent over
+ * @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
@@ -98,7 +105,7 @@ static inline uint16_t mavlink_msg_ahrs_pack_chan(uint8_t system_id, uint8_t com
 						           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[28];
+	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);
@@ -107,7 +114,7 @@ static inline uint16_t mavlink_msg_ahrs_pack_chan(uint8_t system_id, uint8_t com
 	_mav_put_float(buf, 20, error_rp);
 	_mav_put_float(buf, 24, error_yaw);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 28);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS_LEN);
 #else
 	mavlink_ahrs_t packet;
 	packet.omegaIx = omegaIx;
@@ -118,15 +125,19 @@ static inline uint16_t mavlink_msg_ahrs_pack_chan(uint8_t system_id, uint8_t com
 	packet.error_rp = error_rp;
 	packet.error_yaw = error_yaw;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 28);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_AHRS;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 28, 127);
+#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 into a message
+ * @brief Encode a ahrs struct
  *
  * @param system_id ID of this system
  * @param component_id ID of this component (e.g. 200 for IMU)
@@ -139,6 +150,20 @@ static inline uint16_t mavlink_msg_ahrs_encode(uint8_t system_id, uint8_t compon
 }
 
 /**
+ * @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
  *
@@ -155,7 +180,7 @@ static inline uint16_t mavlink_msg_ahrs_encode(uint8_t system_id, uint8_t compon
 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[28];
+	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);
@@ -164,7 +189,11 @@ static inline void mavlink_msg_ahrs_send(mavlink_channel_t chan, float omegaIx,
 	_mav_put_float(buf, 20, error_rp);
 	_mav_put_float(buf, 24, error_yaw);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, 28, 127);
+#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;
@@ -175,7 +204,11 @@ static inline void mavlink_msg_ahrs_send(mavlink_channel_t chan, float omegaIx,
 	packet.error_rp = error_rp;
 	packet.error_yaw = error_yaw;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)&packet, 28, 127);
+#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
 }
 
@@ -271,6 +304,6 @@ static inline void mavlink_msg_ahrs_decode(const mavlink_message_t* msg, mavlink
 	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), 28);
+	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
new file mode 100644
index 000000000..f6fde9590
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h
@@ -0,0 +1,287 @@
+// 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
new file mode 100644
index 000000000..d046f2ad0
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h
@@ -0,0 +1,419 @@
+// 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
index ea640c4fb..821ce73e4 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h
@@ -15,6 +15,9 @@ typedef struct __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 { \
@@ -48,7 +51,7 @@ static inline uint16_t mavlink_msg_ap_adc_pack(uint8_t system_id, uint8_t compon
 						       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[12];
+	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);
@@ -56,7 +59,7 @@ static inline uint16_t mavlink_msg_ap_adc_pack(uint8_t system_id, uint8_t compon
 	_mav_put_uint16_t(buf, 8, adc5);
 	_mav_put_uint16_t(buf, 10, adc6);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AP_ADC_LEN);
 #else
 	mavlink_ap_adc_t packet;
 	packet.adc1 = adc1;
@@ -66,18 +69,22 @@ static inline uint16_t mavlink_msg_ap_adc_pack(uint8_t system_id, uint8_t compon
 	packet.adc5 = adc5;
 	packet.adc6 = adc6;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AP_ADC_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_AP_ADC;
-	return mavlink_finalize_message(msg, system_id, component_id, 12, 188);
+#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 was sent over
+ * @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
@@ -92,7 +99,7 @@ static inline uint16_t mavlink_msg_ap_adc_pack_chan(uint8_t system_id, uint8_t c
 						           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[12];
+	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);
@@ -100,7 +107,7 @@ static inline uint16_t mavlink_msg_ap_adc_pack_chan(uint8_t system_id, uint8_t c
 	_mav_put_uint16_t(buf, 8, adc5);
 	_mav_put_uint16_t(buf, 10, adc6);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AP_ADC_LEN);
 #else
 	mavlink_ap_adc_t packet;
 	packet.adc1 = adc1;
@@ -110,15 +117,19 @@ static inline uint16_t mavlink_msg_ap_adc_pack_chan(uint8_t system_id, uint8_t c
 	packet.adc5 = adc5;
 	packet.adc6 = adc6;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AP_ADC_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_AP_ADC;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 188);
+#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 into a message
+ * @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)
@@ -131,6 +142,20 @@ static inline uint16_t mavlink_msg_ap_adc_encode(uint8_t system_id, uint8_t comp
 }
 
 /**
+ * @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
  *
@@ -146,7 +171,7 @@ static inline uint16_t mavlink_msg_ap_adc_encode(uint8_t system_id, uint8_t comp
 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[12];
+	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);
@@ -154,7 +179,11 @@ static inline void mavlink_msg_ap_adc_send(mavlink_channel_t chan, uint16_t adc1
 	_mav_put_uint16_t(buf, 8, adc5);
 	_mav_put_uint16_t(buf, 10, adc6);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, buf, 12, 188);
+#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;
@@ -164,7 +193,11 @@ static inline void mavlink_msg_ap_adc_send(mavlink_channel_t chan, uint16_t adc1
 	packet.adc5 = adc5;
 	packet.adc6 = adc6;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, (const char *)&packet, 12, 188);
+#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
 }
 
@@ -249,6 +282,6 @@ static inline void mavlink_msg_ap_adc_decode(const mavlink_message_t* msg, mavli
 	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), 12);
+	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
index 359201c8c..9200eefa0 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h
@@ -12,6 +12,9 @@ typedef struct __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 { \
@@ -39,28 +42,32 @@ static inline uint16_t mavlink_msg_data16_pack(uint8_t system_id, uint8_t compon
 						       uint8_t type, uint8_t len, const uint8_t *data)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[18];
+	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, 18);
+        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, 18);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA16_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_DATA16;
-	return mavlink_finalize_message(msg, system_id, component_id, 18, 234);
+#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 was sent over
+ * @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
@@ -72,25 +79,29 @@ static inline uint16_t mavlink_msg_data16_pack_chan(uint8_t system_id, uint8_t c
 						           uint8_t type,uint8_t len,const uint8_t *data)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[18];
+	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, 18);
+        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, 18);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA16_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_DATA16;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 234);
+#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 into a message
+ * @brief Encode a data16 struct
  *
  * @param system_id ID of this system
  * @param component_id ID of this component (e.g. 200 for IMU)
@@ -103,6 +114,20 @@ static inline uint16_t mavlink_msg_data16_encode(uint8_t system_id, uint8_t comp
 }
 
 /**
+ * @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
  *
@@ -115,17 +140,25 @@ static inline uint16_t mavlink_msg_data16_encode(uint8_t system_id, uint8_t comp
 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[18];
+	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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, buf, 18, 234);
+#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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, (const char *)&packet, 18, 234);
+#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
 }
 
@@ -177,6 +210,6 @@ static inline void mavlink_msg_data16_decode(const mavlink_message_t* msg, mavli
 	data16->len = mavlink_msg_data16_get_len(msg);
 	mavlink_msg_data16_get_data(msg, data16->data);
 #else
-	memcpy(data16, _MAV_PAYLOAD(msg), 18);
+	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
index a9b3fe28d..3afedb787 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h
@@ -12,6 +12,9 @@ typedef struct __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 { \
@@ -39,28 +42,32 @@ static inline uint16_t mavlink_msg_data32_pack(uint8_t system_id, uint8_t compon
 						       uint8_t type, uint8_t len, const uint8_t *data)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[34];
+	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, 34);
+        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, 34);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA32_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_DATA32;
-	return mavlink_finalize_message(msg, system_id, component_id, 34, 73);
+#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 was sent over
+ * @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
@@ -72,25 +79,29 @@ static inline uint16_t mavlink_msg_data32_pack_chan(uint8_t system_id, uint8_t c
 						           uint8_t type,uint8_t len,const uint8_t *data)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[34];
+	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, 34);
+        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, 34);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA32_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_DATA32;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 34, 73);
+#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 into a message
+ * @brief Encode a data32 struct
  *
  * @param system_id ID of this system
  * @param component_id ID of this component (e.g. 200 for IMU)
@@ -103,6 +114,20 @@ static inline uint16_t mavlink_msg_data32_encode(uint8_t system_id, uint8_t comp
 }
 
 /**
+ * @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
  *
@@ -115,17 +140,25 @@ static inline uint16_t mavlink_msg_data32_encode(uint8_t system_id, uint8_t comp
 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[34];
+	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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, buf, 34, 73);
+#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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, (const char *)&packet, 34, 73);
+#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
 }
 
@@ -177,6 +210,6 @@ static inline void mavlink_msg_data32_decode(const mavlink_message_t* msg, mavli
 	data32->len = mavlink_msg_data32_get_len(msg);
 	mavlink_msg_data32_get_data(msg, data32->data);
 #else
-	memcpy(data32, _MAV_PAYLOAD(msg), 34);
+	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
index 29dcaa6d8..6931ada16 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h
@@ -12,6 +12,9 @@ typedef struct __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 { \
@@ -39,28 +42,32 @@ static inline uint16_t mavlink_msg_data64_pack(uint8_t system_id, uint8_t compon
 						       uint8_t type, uint8_t len, const uint8_t *data)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[66];
+	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, 66);
+        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, 66);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA64_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_DATA64;
-	return mavlink_finalize_message(msg, system_id, component_id, 66, 181);
+#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 was sent over
+ * @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
@@ -72,25 +79,29 @@ static inline uint16_t mavlink_msg_data64_pack_chan(uint8_t system_id, uint8_t c
 						           uint8_t type,uint8_t len,const uint8_t *data)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[66];
+	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, 66);
+        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, 66);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA64_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_DATA64;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 66, 181);
+#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 into a message
+ * @brief Encode a data64 struct
  *
  * @param system_id ID of this system
  * @param component_id ID of this component (e.g. 200 for IMU)
@@ -103,6 +114,20 @@ static inline uint16_t mavlink_msg_data64_encode(uint8_t system_id, uint8_t comp
 }
 
 /**
+ * @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
  *
@@ -115,17 +140,25 @@ static inline uint16_t mavlink_msg_data64_encode(uint8_t system_id, uint8_t comp
 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[66];
+	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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, buf, 66, 181);
+#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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, (const char *)&packet, 66, 181);
+#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
 }
 
@@ -177,6 +210,6 @@ static inline void mavlink_msg_data64_decode(const mavlink_message_t* msg, mavli
 	data64->len = mavlink_msg_data64_get_len(msg);
 	mavlink_msg_data64_get_data(msg, data64->data);
 #else
-	memcpy(data64, _MAV_PAYLOAD(msg), 66);
+	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
index df0821c87..cffc7d7e7 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h
@@ -12,6 +12,9 @@ typedef struct __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 { \
@@ -39,28 +42,32 @@ static inline uint16_t mavlink_msg_data96_pack(uint8_t system_id, uint8_t compon
 						       uint8_t type, uint8_t len, const uint8_t *data)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[98];
+	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, 98);
+        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, 98);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA96_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_DATA96;
-	return mavlink_finalize_message(msg, system_id, component_id, 98, 22);
+#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 was sent over
+ * @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
@@ -72,25 +79,29 @@ static inline uint16_t mavlink_msg_data96_pack_chan(uint8_t system_id, uint8_t c
 						           uint8_t type,uint8_t len,const uint8_t *data)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[98];
+	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, 98);
+        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, 98);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA96_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_DATA96;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 98, 22);
+#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 into a message
+ * @brief Encode a data96 struct
  *
  * @param system_id ID of this system
  * @param component_id ID of this component (e.g. 200 for IMU)
@@ -103,6 +114,20 @@ static inline uint16_t mavlink_msg_data96_encode(uint8_t system_id, uint8_t comp
 }
 
 /**
+ * @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
  *
@@ -115,17 +140,25 @@ static inline uint16_t mavlink_msg_data96_encode(uint8_t system_id, uint8_t comp
 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[98];
+	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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, buf, 98, 22);
+#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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, (const char *)&packet, 98, 22);
+#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
 }
 
@@ -177,6 +210,6 @@ static inline void mavlink_msg_data96_decode(const mavlink_message_t* msg, mavli
 	data96->len = mavlink_msg_data96_get_len(msg);
 	mavlink_msg_data96_get_data(msg, data96->data);
 #else
-	memcpy(data96, _MAV_PAYLOAD(msg), 98);
+	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
index cc49c5025..c6518c419 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h
@@ -20,6 +20,9 @@ typedef struct __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 { \
@@ -63,7 +66,7 @@ static inline uint16_t mavlink_msg_digicam_configure_pack(uint8_t system_id, uin
 						       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[15];
+	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);
@@ -76,7 +79,7 @@ static inline uint16_t mavlink_msg_digicam_configure_pack(uint8_t system_id, uin
 	_mav_put_uint8_t(buf, 13, engine_cut_off);
 	_mav_put_uint8_t(buf, 14, extra_param);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 15);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN);
 #else
 	mavlink_digicam_configure_t packet;
 	packet.extra_value = extra_value;
@@ -91,18 +94,22 @@ static inline uint16_t mavlink_msg_digicam_configure_pack(uint8_t system_id, uin
 	packet.engine_cut_off = engine_cut_off;
 	packet.extra_param = extra_param;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 15);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONFIGURE;
-	return mavlink_finalize_message(msg, system_id, component_id, 15, 84);
+#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 was sent over
+ * @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
@@ -122,7 +129,7 @@ static inline uint16_t mavlink_msg_digicam_configure_pack_chan(uint8_t system_id
 						           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[15];
+	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);
@@ -135,7 +142,7 @@ static inline uint16_t mavlink_msg_digicam_configure_pack_chan(uint8_t system_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, 15);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN);
 #else
 	mavlink_digicam_configure_t packet;
 	packet.extra_value = extra_value;
@@ -150,15 +157,19 @@ static inline uint16_t mavlink_msg_digicam_configure_pack_chan(uint8_t system_id
 	packet.engine_cut_off = engine_cut_off;
 	packet.extra_param = extra_param;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 15);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONFIGURE;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 15, 84);
+#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 into a message
+ * @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)
@@ -171,6 +182,20 @@ static inline uint16_t mavlink_msg_digicam_configure_encode(uint8_t system_id, u
 }
 
 /**
+ * @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
  *
@@ -191,7 +216,7 @@ static inline uint16_t mavlink_msg_digicam_configure_encode(uint8_t system_id, u
 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[15];
+	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);
@@ -204,7 +229,11 @@ static inline void mavlink_msg_digicam_configure_send(mavlink_channel_t chan, ui
 	_mav_put_uint8_t(buf, 13, engine_cut_off);
 	_mav_put_uint8_t(buf, 14, extra_param);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, buf, 15, 84);
+#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;
@@ -219,7 +248,11 @@ static inline void mavlink_msg_digicam_configure_send(mavlink_channel_t chan, ui
 	packet.engine_cut_off = engine_cut_off;
 	packet.extra_param = extra_param;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, (const char *)&packet, 15, 84);
+#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
 }
 
@@ -359,6 +392,6 @@ static inline void mavlink_msg_digicam_configure_decode(const mavlink_message_t*
 	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), 15);
+	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
index a3b4878c4..bfa5414a3 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h
@@ -19,6 +19,9 @@ typedef struct __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 { \
@@ -60,7 +63,7 @@ static inline uint16_t mavlink_msg_digicam_control_pack(uint8_t system_id, uint8
 						       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[13];
+	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);
@@ -72,7 +75,7 @@ static inline uint16_t mavlink_msg_digicam_control_pack(uint8_t system_id, uint8
 	_mav_put_uint8_t(buf, 11, command_id);
 	_mav_put_uint8_t(buf, 12, extra_param);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 13);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN);
 #else
 	mavlink_digicam_control_t packet;
 	packet.extra_value = extra_value;
@@ -86,18 +89,22 @@ static inline uint16_t mavlink_msg_digicam_control_pack(uint8_t system_id, uint8
 	packet.command_id = command_id;
 	packet.extra_param = extra_param;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 13);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONTROL;
-	return mavlink_finalize_message(msg, system_id, component_id, 13, 22);
+#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 was sent over
+ * @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
@@ -116,7 +123,7 @@ static inline uint16_t mavlink_msg_digicam_control_pack_chan(uint8_t system_id,
 						           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[13];
+	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);
@@ -128,7 +135,7 @@ static inline uint16_t mavlink_msg_digicam_control_pack_chan(uint8_t system_id,
 	_mav_put_uint8_t(buf, 11, command_id);
 	_mav_put_uint8_t(buf, 12, extra_param);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 13);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN);
 #else
 	mavlink_digicam_control_t packet;
 	packet.extra_value = extra_value;
@@ -142,15 +149,19 @@ static inline uint16_t mavlink_msg_digicam_control_pack_chan(uint8_t system_id,
 	packet.command_id = command_id;
 	packet.extra_param = extra_param;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 13);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONTROL;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 13, 22);
+#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 into a message
+ * @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)
@@ -163,6 +174,20 @@ static inline uint16_t mavlink_msg_digicam_control_encode(uint8_t system_id, uin
 }
 
 /**
+ * @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
  *
@@ -182,7 +207,7 @@ static inline uint16_t mavlink_msg_digicam_control_encode(uint8_t system_id, uin
 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[13];
+	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);
@@ -194,7 +219,11 @@ static inline void mavlink_msg_digicam_control_send(mavlink_channel_t chan, uint
 	_mav_put_uint8_t(buf, 11, command_id);
 	_mav_put_uint8_t(buf, 12, extra_param);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, buf, 13, 22);
+#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;
@@ -208,7 +237,11 @@ static inline void mavlink_msg_digicam_control_send(mavlink_channel_t chan, uint
 	packet.command_id = command_id;
 	packet.extra_param = extra_param;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, (const char *)&packet, 13, 22);
+#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
 }
 
@@ -337,6 +370,6 @@ static inline void mavlink_msg_digicam_control_decode(const mavlink_message_t* m
 	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), 13);
+	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
index c1e405b0a..fe3677d53 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h
@@ -12,6 +12,9 @@ typedef struct __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 { \
@@ -39,30 +42,34 @@ static inline uint16_t mavlink_msg_fence_fetch_point_pack(uint8_t system_id, uin
 						       uint8_t target_system, uint8_t target_component, uint8_t idx)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[3];
+	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, 3);
+        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, 3);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_FENCE_FETCH_POINT;
-	return mavlink_finalize_message(msg, system_id, component_id, 3, 68);
+#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 was sent over
+ * @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
@@ -74,27 +81,31 @@ static inline uint16_t mavlink_msg_fence_fetch_point_pack_chan(uint8_t system_id
 						           uint8_t target_system,uint8_t target_component,uint8_t idx)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[3];
+	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, 3);
+        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, 3);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_FENCE_FETCH_POINT;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3, 68);
+#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 into a message
+ * @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)
@@ -107,6 +118,20 @@ static inline uint16_t mavlink_msg_fence_fetch_point_encode(uint8_t system_id, u
 }
 
 /**
+ * @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
  *
@@ -119,19 +144,27 @@ static inline uint16_t mavlink_msg_fence_fetch_point_encode(uint8_t system_id, u
 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[3];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, buf, 3, 68);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, (const char *)&packet, 3, 68);
+#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
 }
 
@@ -183,6 +216,6 @@ static inline void mavlink_msg_fence_fetch_point_decode(const mavlink_message_t*
 	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), 3);
+	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
index b31319c74..febda6cdc 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h
@@ -15,6 +15,9 @@ typedef struct __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 { \
@@ -48,7 +51,7 @@ static inline uint16_t mavlink_msg_fence_point_pack(uint8_t system_id, uint8_t c
 						       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[12];
+	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);
@@ -56,7 +59,7 @@ static inline uint16_t mavlink_msg_fence_point_pack(uint8_t system_id, uint8_t c
 	_mav_put_uint8_t(buf, 10, idx);
 	_mav_put_uint8_t(buf, 11, count);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_POINT_LEN);
 #else
 	mavlink_fence_point_t packet;
 	packet.lat = lat;
@@ -66,18 +69,22 @@ static inline uint16_t mavlink_msg_fence_point_pack(uint8_t system_id, uint8_t c
 	packet.idx = idx;
 	packet.count = count;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_POINT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_FENCE_POINT;
-	return mavlink_finalize_message(msg, system_id, component_id, 12, 78);
+#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 was sent over
+ * @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
@@ -92,7 +99,7 @@ static inline uint16_t mavlink_msg_fence_point_pack_chan(uint8_t system_id, uint
 						           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[12];
+	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);
@@ -100,7 +107,7 @@ static inline uint16_t mavlink_msg_fence_point_pack_chan(uint8_t system_id, uint
 	_mav_put_uint8_t(buf, 10, idx);
 	_mav_put_uint8_t(buf, 11, count);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_POINT_LEN);
 #else
 	mavlink_fence_point_t packet;
 	packet.lat = lat;
@@ -110,15 +117,19 @@ static inline uint16_t mavlink_msg_fence_point_pack_chan(uint8_t system_id, uint
 	packet.idx = idx;
 	packet.count = count;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_POINT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_FENCE_POINT;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 78);
+#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 into a message
+ * @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)
@@ -131,6 +142,20 @@ static inline uint16_t mavlink_msg_fence_point_encode(uint8_t system_id, uint8_t
 }
 
 /**
+ * @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
  *
@@ -146,7 +171,7 @@ static inline uint16_t mavlink_msg_fence_point_encode(uint8_t system_id, uint8_t
 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[12];
+	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);
@@ -154,7 +179,11 @@ static inline void mavlink_msg_fence_point_send(mavlink_channel_t chan, uint8_t
 	_mav_put_uint8_t(buf, 10, idx);
 	_mav_put_uint8_t(buf, 11, count);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, buf, 12, 78);
+#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;
@@ -164,7 +193,11 @@ static inline void mavlink_msg_fence_point_send(mavlink_channel_t chan, uint8_t
 	packet.idx = idx;
 	packet.count = count;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, (const char *)&packet, 12, 78);
+#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
 }
 
@@ -249,6 +282,6 @@ static inline void mavlink_msg_fence_point_decode(const mavlink_message_t* 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), 12);
+	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
index ce3e7ee67..612090406 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h
@@ -13,6 +13,9 @@ typedef struct __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 { \
@@ -42,13 +45,13 @@ static inline uint16_t mavlink_msg_fence_status_pack(uint8_t system_id, uint8_t
 						       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[8];
+	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, 8);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN);
 #else
 	mavlink_fence_status_t packet;
 	packet.breach_time = breach_time;
@@ -56,18 +59,22 @@ static inline uint16_t mavlink_msg_fence_status_pack(uint8_t system_id, uint8_t
 	packet.breach_status = breach_status;
 	packet.breach_type = breach_type;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 8);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_FENCE_STATUS;
-	return mavlink_finalize_message(msg, system_id, component_id, 8, 189);
+#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 was sent over
+ * @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
@@ -80,13 +87,13 @@ static inline uint16_t mavlink_msg_fence_status_pack_chan(uint8_t system_id, uin
 						           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[8];
+	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, 8);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN);
 #else
 	mavlink_fence_status_t packet;
 	packet.breach_time = breach_time;
@@ -94,15 +101,19 @@ static inline uint16_t mavlink_msg_fence_status_pack_chan(uint8_t system_id, uin
 	packet.breach_status = breach_status;
 	packet.breach_type = breach_type;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 8);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_FENCE_STATUS;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8, 189);
+#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 into a message
+ * @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)
@@ -115,6 +126,20 @@ static inline uint16_t mavlink_msg_fence_status_encode(uint8_t system_id, uint8_
 }
 
 /**
+ * @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
  *
@@ -128,13 +153,17 @@ static inline uint16_t mavlink_msg_fence_status_encode(uint8_t system_id, uint8_
 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[8];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, buf, 8, 189);
+#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;
@@ -142,7 +171,11 @@ static inline void mavlink_msg_fence_status_send(mavlink_channel_t chan, uint8_t
 	packet.breach_status = breach_status;
 	packet.breach_type = breach_type;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, (const char *)&packet, 8, 189);
+#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
 }
 
@@ -205,6 +238,6 @@ static inline void mavlink_msg_fence_status_decode(const mavlink_message_t* 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), 8);
+	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
index 952e65951..2f5dea513 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h
@@ -11,6 +11,9 @@ typedef struct __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 { \
@@ -36,28 +39,32 @@ static inline uint16_t mavlink_msg_hwstatus_pack(uint8_t system_id, uint8_t comp
 						       uint16_t Vcc, uint8_t I2Cerr)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[3];
+	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, 3);
+        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, 3);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HWSTATUS_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_HWSTATUS;
-	return mavlink_finalize_message(msg, system_id, component_id, 3, 21);
+#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 was sent over
+ * @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
@@ -68,25 +75,29 @@ static inline uint16_t mavlink_msg_hwstatus_pack_chan(uint8_t system_id, uint8_t
 						           uint16_t Vcc,uint8_t I2Cerr)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[3];
+	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, 3);
+        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, 3);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HWSTATUS_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_HWSTATUS;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3, 21);
+#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 into a message
+ * @brief Encode a hwstatus struct
  *
  * @param system_id ID of this system
  * @param component_id ID of this component (e.g. 200 for IMU)
@@ -99,6 +110,20 @@ static inline uint16_t mavlink_msg_hwstatus_encode(uint8_t system_id, uint8_t co
 }
 
 /**
+ * @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
  *
@@ -110,17 +135,25 @@ static inline uint16_t mavlink_msg_hwstatus_encode(uint8_t system_id, uint8_t co
 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[3];
+	char buf[MAVLINK_MSG_ID_HWSTATUS_LEN];
 	_mav_put_uint16_t(buf, 0, Vcc);
 	_mav_put_uint8_t(buf, 2, I2Cerr);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, buf, 3, 21);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, (const char *)&packet, 3, 21);
+#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
 }
 
@@ -161,6 +194,6 @@ static inline void mavlink_msg_hwstatus_decode(const mavlink_message_t* msg, mav
 	hwstatus->Vcc = mavlink_msg_hwstatus_get_Vcc(msg);
 	hwstatus->I2Cerr = mavlink_msg_hwstatus_get_I2Cerr(msg);
 #else
-	memcpy(hwstatus, _MAV_PAYLOAD(msg), 3);
+	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
index 7caac1c5a..34743fd02 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h
@@ -18,6 +18,9 @@ typedef struct __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 { \
@@ -57,7 +60,7 @@ static inline uint16_t mavlink_msg_limits_status_pack(uint8_t system_id, uint8_t
 						       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[22];
+	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);
@@ -68,7 +71,7 @@ static inline uint16_t mavlink_msg_limits_status_pack(uint8_t system_id, uint8_t
 	_mav_put_uint8_t(buf, 20, mods_required);
 	_mav_put_uint8_t(buf, 21, mods_triggered);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 22);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN);
 #else
 	mavlink_limits_status_t packet;
 	packet.last_trigger = last_trigger;
@@ -81,18 +84,22 @@ static inline uint16_t mavlink_msg_limits_status_pack(uint8_t system_id, uint8_t
 	packet.mods_required = mods_required;
 	packet.mods_triggered = mods_triggered;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 22);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_LIMITS_STATUS;
-	return mavlink_finalize_message(msg, system_id, component_id, 22, 144);
+#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 was sent over
+ * @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
@@ -110,7 +117,7 @@ static inline uint16_t mavlink_msg_limits_status_pack_chan(uint8_t system_id, ui
 						           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[22];
+	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);
@@ -121,7 +128,7 @@ static inline uint16_t mavlink_msg_limits_status_pack_chan(uint8_t system_id, ui
 	_mav_put_uint8_t(buf, 20, mods_required);
 	_mav_put_uint8_t(buf, 21, mods_triggered);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 22);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN);
 #else
 	mavlink_limits_status_t packet;
 	packet.last_trigger = last_trigger;
@@ -134,15 +141,19 @@ static inline uint16_t mavlink_msg_limits_status_pack_chan(uint8_t system_id, ui
 	packet.mods_required = mods_required;
 	packet.mods_triggered = mods_triggered;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 22);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_LIMITS_STATUS;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 22, 144);
+#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 into a message
+ * @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)
@@ -155,6 +166,20 @@ static inline uint16_t mavlink_msg_limits_status_encode(uint8_t system_id, uint8
 }
 
 /**
+ * @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
  *
@@ -173,7 +198,7 @@ static inline uint16_t mavlink_msg_limits_status_encode(uint8_t system_id, uint8
 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[22];
+	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);
@@ -184,7 +209,11 @@ static inline void mavlink_msg_limits_status_send(mavlink_channel_t chan, uint8_
 	_mav_put_uint8_t(buf, 20, mods_required);
 	_mav_put_uint8_t(buf, 21, mods_triggered);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, buf, 22, 144);
+#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;
@@ -197,7 +226,11 @@ static inline void mavlink_msg_limits_status_send(mavlink_channel_t chan, uint8_
 	packet.mods_required = mods_required;
 	packet.mods_triggered = mods_triggered;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, (const char *)&packet, 22, 144);
+#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
 }
 
@@ -315,6 +348,6 @@ static inline void mavlink_msg_limits_status_decode(const mavlink_message_t* 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), 22);
+	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
index a095a804f..55f772bbc 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h
@@ -11,6 +11,9 @@ typedef struct __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 { \
@@ -36,28 +39,32 @@ static inline uint16_t mavlink_msg_meminfo_pack(uint8_t system_id, uint8_t compo
 						       uint16_t brkval, uint16_t freemem)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[4];
+	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, 4);
+        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, 4);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MEMINFO_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MEMINFO;
-	return mavlink_finalize_message(msg, system_id, component_id, 4, 208);
+#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 was sent over
+ * @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
@@ -68,25 +75,29 @@ static inline uint16_t mavlink_msg_meminfo_pack_chan(uint8_t system_id, uint8_t
 						           uint16_t brkval,uint16_t freemem)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[4];
+	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, 4);
+        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, 4);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MEMINFO_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MEMINFO;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 208);
+#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 into a message
+ * @brief Encode a meminfo struct
  *
  * @param system_id ID of this system
  * @param component_id ID of this component (e.g. 200 for IMU)
@@ -99,6 +110,20 @@ static inline uint16_t mavlink_msg_meminfo_encode(uint8_t system_id, uint8_t com
 }
 
 /**
+ * @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
  *
@@ -110,17 +135,25 @@ static inline uint16_t mavlink_msg_meminfo_encode(uint8_t system_id, uint8_t com
 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[4];
+	char buf[MAVLINK_MSG_ID_MEMINFO_LEN];
 	_mav_put_uint16_t(buf, 0, brkval);
 	_mav_put_uint16_t(buf, 2, freemem);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, buf, 4, 208);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, (const char *)&packet, 4, 208);
+#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
 }
 
@@ -161,6 +194,6 @@ static inline void mavlink_msg_meminfo_decode(const mavlink_message_t* msg, mavl
 	meminfo->brkval = mavlink_msg_meminfo_get_brkval(msg);
 	meminfo->freemem = mavlink_msg_meminfo_get_freemem(msg);
 #else
-	memcpy(meminfo, _MAV_PAYLOAD(msg), 4);
+	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
index 051a71837..de717dfa4 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h
@@ -15,6 +15,9 @@ typedef struct __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 { \
@@ -48,7 +51,7 @@ static inline uint16_t mavlink_msg_mount_configure_pack(uint8_t system_id, uint8
 						       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[6];
+	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);
@@ -56,7 +59,7 @@ static inline uint16_t mavlink_msg_mount_configure_pack(uint8_t system_id, uint8
 	_mav_put_uint8_t(buf, 4, stab_pitch);
 	_mav_put_uint8_t(buf, 5, stab_yaw);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 6);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN);
 #else
 	mavlink_mount_configure_t packet;
 	packet.target_system = target_system;
@@ -66,18 +69,22 @@ static inline uint16_t mavlink_msg_mount_configure_pack(uint8_t system_id, uint8
 	packet.stab_pitch = stab_pitch;
 	packet.stab_yaw = stab_yaw;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 6);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MOUNT_CONFIGURE;
-	return mavlink_finalize_message(msg, system_id, component_id, 6, 19);
+#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 was sent over
+ * @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
@@ -92,7 +99,7 @@ static inline uint16_t mavlink_msg_mount_configure_pack_chan(uint8_t system_id,
 						           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[6];
+	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);
@@ -100,7 +107,7 @@ static inline uint16_t mavlink_msg_mount_configure_pack_chan(uint8_t system_id,
 	_mav_put_uint8_t(buf, 4, stab_pitch);
 	_mav_put_uint8_t(buf, 5, stab_yaw);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 6);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN);
 #else
 	mavlink_mount_configure_t packet;
 	packet.target_system = target_system;
@@ -110,15 +117,19 @@ static inline uint16_t mavlink_msg_mount_configure_pack_chan(uint8_t system_id,
 	packet.stab_pitch = stab_pitch;
 	packet.stab_yaw = stab_yaw;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 6);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MOUNT_CONFIGURE;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 6, 19);
+#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 into a message
+ * @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)
@@ -131,6 +142,20 @@ static inline uint16_t mavlink_msg_mount_configure_encode(uint8_t system_id, uin
 }
 
 /**
+ * @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
  *
@@ -146,7 +171,7 @@ static inline uint16_t mavlink_msg_mount_configure_encode(uint8_t system_id, uin
 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[6];
+	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);
@@ -154,7 +179,11 @@ static inline void mavlink_msg_mount_configure_send(mavlink_channel_t chan, uint
 	_mav_put_uint8_t(buf, 4, stab_pitch);
 	_mav_put_uint8_t(buf, 5, stab_yaw);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, buf, 6, 19);
+#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;
@@ -164,7 +193,11 @@ static inline void mavlink_msg_mount_configure_send(mavlink_channel_t chan, uint
 	packet.stab_pitch = stab_pitch;
 	packet.stab_yaw = stab_yaw;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, (const char *)&packet, 6, 19);
+#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
 }
 
@@ -249,6 +282,6 @@ static inline void mavlink_msg_mount_configure_decode(const mavlink_message_t* m
 	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), 6);
+	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
index a51992299..44416353e 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h
@@ -15,6 +15,9 @@ typedef struct __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 { \
@@ -48,7 +51,7 @@ static inline uint16_t mavlink_msg_mount_control_pack(uint8_t system_id, uint8_t
 						       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[15];
+	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);
@@ -56,7 +59,7 @@ static inline uint16_t mavlink_msg_mount_control_pack(uint8_t system_id, uint8_t
 	_mav_put_uint8_t(buf, 13, target_component);
 	_mav_put_uint8_t(buf, 14, save_position);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 15);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN);
 #else
 	mavlink_mount_control_t packet;
 	packet.input_a = input_a;
@@ -66,18 +69,22 @@ static inline uint16_t mavlink_msg_mount_control_pack(uint8_t system_id, uint8_t
 	packet.target_component = target_component;
 	packet.save_position = save_position;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 15);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MOUNT_CONTROL;
-	return mavlink_finalize_message(msg, system_id, component_id, 15, 21);
+#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 was sent over
+ * @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
@@ -92,7 +99,7 @@ static inline uint16_t mavlink_msg_mount_control_pack_chan(uint8_t system_id, ui
 						           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[15];
+	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);
@@ -100,7 +107,7 @@ static inline uint16_t mavlink_msg_mount_control_pack_chan(uint8_t system_id, ui
 	_mav_put_uint8_t(buf, 13, target_component);
 	_mav_put_uint8_t(buf, 14, save_position);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 15);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN);
 #else
 	mavlink_mount_control_t packet;
 	packet.input_a = input_a;
@@ -110,15 +117,19 @@ static inline uint16_t mavlink_msg_mount_control_pack_chan(uint8_t system_id, ui
 	packet.target_component = target_component;
 	packet.save_position = save_position;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 15);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MOUNT_CONTROL;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 15, 21);
+#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 into a message
+ * @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)
@@ -131,6 +142,20 @@ static inline uint16_t mavlink_msg_mount_control_encode(uint8_t system_id, uint8
 }
 
 /**
+ * @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
  *
@@ -146,7 +171,7 @@ static inline uint16_t mavlink_msg_mount_control_encode(uint8_t system_id, uint8
 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[15];
+	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);
@@ -154,7 +179,11 @@ static inline void mavlink_msg_mount_control_send(mavlink_channel_t chan, uint8_
 	_mav_put_uint8_t(buf, 13, target_component);
 	_mav_put_uint8_t(buf, 14, save_position);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, buf, 15, 21);
+#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;
@@ -164,7 +193,11 @@ static inline void mavlink_msg_mount_control_send(mavlink_channel_t chan, uint8_
 	packet.target_component = target_component;
 	packet.save_position = save_position;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, (const char *)&packet, 15, 21);
+#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
 }
 
@@ -249,6 +282,6 @@ static inline void mavlink_msg_mount_control_decode(const mavlink_message_t* 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), 15);
+	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
index edc188ebd..4905905dc 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h
@@ -14,6 +14,9 @@ typedef struct __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 { \
@@ -45,14 +48,14 @@ static inline uint16_t mavlink_msg_mount_status_pack(uint8_t system_id, uint8_t
 						       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[14];
+	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, 14);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN);
 #else
 	mavlink_mount_status_t packet;
 	packet.pointing_a = pointing_a;
@@ -61,18 +64,22 @@ static inline uint16_t mavlink_msg_mount_status_pack(uint8_t system_id, uint8_t
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 14);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MOUNT_STATUS;
-	return mavlink_finalize_message(msg, system_id, component_id, 14, 134);
+#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 was sent over
+ * @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
@@ -86,14 +93,14 @@ static inline uint16_t mavlink_msg_mount_status_pack_chan(uint8_t system_id, uin
 						           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[14];
+	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, 14);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN);
 #else
 	mavlink_mount_status_t packet;
 	packet.pointing_a = pointing_a;
@@ -102,15 +109,19 @@ static inline uint16_t mavlink_msg_mount_status_pack_chan(uint8_t system_id, uin
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 14);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MOUNT_STATUS;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14, 134);
+#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 into a message
+ * @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)
@@ -123,6 +134,20 @@ static inline uint16_t mavlink_msg_mount_status_encode(uint8_t system_id, uint8_
 }
 
 /**
+ * @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
  *
@@ -137,14 +162,18 @@ static inline uint16_t mavlink_msg_mount_status_encode(uint8_t system_id, uint8_
 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[14];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, buf, 14, 134);
+#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;
@@ -153,7 +182,11 @@ static inline void mavlink_msg_mount_status_send(mavlink_channel_t chan, uint8_t
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, (const char *)&packet, 14, 134);
+#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
 }
 
@@ -227,6 +260,6 @@ static inline void mavlink_msg_mount_status_decode(const mavlink_message_t* 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), 14);
+	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
index 0f3d03c0a..8e9740e82 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h
@@ -16,6 +16,9 @@ typedef struct __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 { \
@@ -51,7 +54,7 @@ static inline uint16_t mavlink_msg_radio_pack(uint8_t system_id, uint8_t compone
 						       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[9];
+	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);
@@ -60,7 +63,7 @@ static inline uint16_t mavlink_msg_radio_pack(uint8_t system_id, uint8_t compone
 	_mav_put_uint8_t(buf, 7, noise);
 	_mav_put_uint8_t(buf, 8, remnoise);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 9);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RADIO_LEN);
 #else
 	mavlink_radio_t packet;
 	packet.rxerrors = rxerrors;
@@ -71,18 +74,22 @@ static inline uint16_t mavlink_msg_radio_pack(uint8_t system_id, uint8_t compone
 	packet.noise = noise;
 	packet.remnoise = remnoise;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 9);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RADIO_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_RADIO;
-	return mavlink_finalize_message(msg, system_id, component_id, 9, 21);
+#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 was sent over
+ * @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
@@ -98,7 +105,7 @@ static inline uint16_t mavlink_msg_radio_pack_chan(uint8_t system_id, uint8_t co
 						           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[9];
+	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);
@@ -107,7 +114,7 @@ static inline uint16_t mavlink_msg_radio_pack_chan(uint8_t system_id, uint8_t co
 	_mav_put_uint8_t(buf, 7, noise);
 	_mav_put_uint8_t(buf, 8, remnoise);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 9);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RADIO_LEN);
 #else
 	mavlink_radio_t packet;
 	packet.rxerrors = rxerrors;
@@ -118,15 +125,19 @@ static inline uint16_t mavlink_msg_radio_pack_chan(uint8_t system_id, uint8_t co
 	packet.noise = noise;
 	packet.remnoise = remnoise;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 9);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RADIO_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_RADIO;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 9, 21);
+#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 into a message
+ * @brief Encode a radio struct
  *
  * @param system_id ID of this system
  * @param component_id ID of this component (e.g. 200 for IMU)
@@ -139,6 +150,20 @@ static inline uint16_t mavlink_msg_radio_encode(uint8_t system_id, uint8_t compo
 }
 
 /**
+ * @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
  *
@@ -155,7 +180,7 @@ static inline uint16_t mavlink_msg_radio_encode(uint8_t system_id, uint8_t compo
 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[9];
+	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);
@@ -164,7 +189,11 @@ static inline void mavlink_msg_radio_send(mavlink_channel_t chan, uint8_t rssi,
 	_mav_put_uint8_t(buf, 7, noise);
 	_mav_put_uint8_t(buf, 8, remnoise);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, buf, 9, 21);
+#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;
@@ -175,7 +204,11 @@ static inline void mavlink_msg_radio_send(mavlink_channel_t chan, uint8_t rssi,
 	packet.noise = noise;
 	packet.remnoise = remnoise;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, (const char *)&packet, 9, 21);
+#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
 }
 
@@ -271,6 +304,6 @@ static inline void mavlink_msg_radio_decode(const mavlink_message_t* msg, mavlin
 	radio->noise = mavlink_msg_radio_get_noise(msg);
 	radio->remnoise = mavlink_msg_radio_get_remnoise(msg);
 #else
-	memcpy(radio, _MAV_PAYLOAD(msg), 9);
+	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
new file mode 100644
index 000000000..f057e3c33
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_fetch_point.h
@@ -0,0 +1,221 @@
+// 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
new file mode 100644
index 000000000..2c21db4c0
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_point.h
@@ -0,0 +1,375 @@
+// 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
new file mode 100644
index 000000000..c476447a8
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h
@@ -0,0 +1,199 @@
+// 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
index 56fb52d96..31b7d989d 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h
@@ -21,6 +21,9 @@ typedef struct __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 { \
@@ -66,7 +69,7 @@ static inline uint16_t mavlink_msg_sensor_offsets_pack(uint8_t system_id, uint8_
 						       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[42];
+	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);
@@ -80,7 +83,7 @@ static inline uint16_t mavlink_msg_sensor_offsets_pack(uint8_t system_id, uint8_
 	_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, 42);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN);
 #else
 	mavlink_sensor_offsets_t packet;
 	packet.mag_declination = mag_declination;
@@ -96,18 +99,22 @@ static inline uint16_t mavlink_msg_sensor_offsets_pack(uint8_t system_id, uint8_
 	packet.mag_ofs_y = mag_ofs_y;
 	packet.mag_ofs_z = mag_ofs_z;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 42);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_SENSOR_OFFSETS;
-	return mavlink_finalize_message(msg, system_id, component_id, 42, 134);
+#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 was sent over
+ * @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
@@ -128,7 +135,7 @@ static inline uint16_t mavlink_msg_sensor_offsets_pack_chan(uint8_t system_id, u
 						           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[42];
+	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);
@@ -142,7 +149,7 @@ static inline uint16_t mavlink_msg_sensor_offsets_pack_chan(uint8_t system_id, u
 	_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, 42);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN);
 #else
 	mavlink_sensor_offsets_t packet;
 	packet.mag_declination = mag_declination;
@@ -158,15 +165,19 @@ static inline uint16_t mavlink_msg_sensor_offsets_pack_chan(uint8_t system_id, u
 	packet.mag_ofs_y = mag_ofs_y;
 	packet.mag_ofs_z = mag_ofs_z;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 42);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_SENSOR_OFFSETS;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 42, 134);
+#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 into a message
+ * @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)
@@ -179,6 +190,20 @@ static inline uint16_t mavlink_msg_sensor_offsets_encode(uint8_t system_id, uint
 }
 
 /**
+ * @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
  *
@@ -200,7 +225,7 @@ static inline uint16_t mavlink_msg_sensor_offsets_encode(uint8_t system_id, uint
 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[42];
+	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);
@@ -214,7 +239,11 @@ static inline void mavlink_msg_sensor_offsets_send(mavlink_channel_t chan, int16
 	_mav_put_int16_t(buf, 38, mag_ofs_y);
 	_mav_put_int16_t(buf, 40, mag_ofs_z);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, buf, 42, 134);
+#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;
@@ -230,7 +259,11 @@ static inline void mavlink_msg_sensor_offsets_send(mavlink_channel_t chan, int16
 	packet.mag_ofs_y = mag_ofs_y;
 	packet.mag_ofs_z = mag_ofs_z;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, (const char *)&packet, 42, 134);
+#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
 }
 
@@ -381,6 +414,6 @@ static inline void mavlink_msg_sensor_offsets_decode(const mavlink_message_t* ms
 	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), 42);
+	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
index 4c13fd186..b2c629c39 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h
@@ -14,6 +14,9 @@ typedef struct __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 { \
@@ -45,14 +48,14 @@ static inline uint16_t mavlink_msg_set_mag_offsets_pack(uint8_t system_id, uint8
 						       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[8];
+	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, 8);
+        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;
@@ -61,18 +64,22 @@ static inline uint16_t mavlink_msg_set_mag_offsets_pack(uint8_t system_id, uint8
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 8);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_SET_MAG_OFFSETS;
-	return mavlink_finalize_message(msg, system_id, component_id, 8, 219);
+#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 was sent over
+ * @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
@@ -86,14 +93,14 @@ static inline uint16_t mavlink_msg_set_mag_offsets_pack_chan(uint8_t system_id,
 						           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[8];
+	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, 8);
+        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;
@@ -102,15 +109,19 @@ static inline uint16_t mavlink_msg_set_mag_offsets_pack_chan(uint8_t system_id,
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 8);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_SET_MAG_OFFSETS;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8, 219);
+#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 into a message
+ * @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)
@@ -123,6 +134,20 @@ static inline uint16_t mavlink_msg_set_mag_offsets_encode(uint8_t system_id, uin
 }
 
 /**
+ * @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
  *
@@ -137,14 +162,18 @@ static inline uint16_t mavlink_msg_set_mag_offsets_encode(uint8_t system_id, uin
 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[8];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, buf, 8, 219);
+#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;
@@ -153,7 +182,11 @@ static inline void mavlink_msg_set_mag_offsets_send(mavlink_channel_t chan, uint
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, (const char *)&packet, 8, 219);
+#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
 }
 
@@ -227,6 +260,6 @@ static inline void mavlink_msg_set_mag_offsets_decode(const mavlink_message_t* m
 	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), 8);
+	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
index 8ee447c77..8ef44f76d 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h
@@ -13,13 +13,16 @@ typedef struct __mavlink_simstate_t
  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 lng; ///< Longitude in degrees
+ 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 { \
@@ -34,8 +37,8 @@ typedef struct __mavlink_simstate_t
          { "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_FLOAT, 0, 36, offsetof(mavlink_simstate_t, lat) }, \
-         { "lng", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_simstate_t, lng) }, \
+         { "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) }, \
          } \
 }
 
@@ -55,15 +58,15 @@ typedef struct __mavlink_simstate_t
  * @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 lng Longitude in degrees
+ * @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, float lat, float lng)
+						       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[44];
+	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);
@@ -73,10 +76,10 @@ static inline uint16_t mavlink_msg_simstate_pack(uint8_t system_id, uint8_t comp
 	_mav_put_float(buf, 24, xgyro);
 	_mav_put_float(buf, 28, ygyro);
 	_mav_put_float(buf, 32, zgyro);
-	_mav_put_float(buf, 36, lat);
-	_mav_put_float(buf, 40, lng);
+	_mav_put_int32_t(buf, 36, lat);
+	_mav_put_int32_t(buf, 40, lng);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 44);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIMSTATE_LEN);
 #else
 	mavlink_simstate_t packet;
 	packet.roll = roll;
@@ -91,18 +94,22 @@ static inline uint16_t mavlink_msg_simstate_pack(uint8_t system_id, uint8_t comp
 	packet.lat = lat;
 	packet.lng = lng;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 44);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SIMSTATE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_SIMSTATE;
-	return mavlink_finalize_message(msg, system_id, component_id, 44, 111);
+#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 was sent over
+ * @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)
@@ -113,16 +120,16 @@ static inline uint16_t mavlink_msg_simstate_pack(uint8_t system_id, uint8_t comp
  * @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 lng Longitude in degrees
+ * @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,float lat,float lng)
+						           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[44];
+	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);
@@ -132,10 +139,10 @@ static inline uint16_t mavlink_msg_simstate_pack_chan(uint8_t system_id, uint8_t
 	_mav_put_float(buf, 24, xgyro);
 	_mav_put_float(buf, 28, ygyro);
 	_mav_put_float(buf, 32, zgyro);
-	_mav_put_float(buf, 36, lat);
-	_mav_put_float(buf, 40, lng);
+	_mav_put_int32_t(buf, 36, lat);
+	_mav_put_int32_t(buf, 40, lng);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 44);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIMSTATE_LEN);
 #else
 	mavlink_simstate_t packet;
 	packet.roll = roll;
@@ -150,15 +157,19 @@ static inline uint16_t mavlink_msg_simstate_pack_chan(uint8_t system_id, uint8_t
 	packet.lat = lat;
 	packet.lng = lng;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 44);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SIMSTATE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_SIMSTATE;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 44, 111);
+#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 into a message
+ * @brief Encode a simstate struct
  *
  * @param system_id ID of this system
  * @param component_id ID of this component (e.g. 200 for IMU)
@@ -171,6 +182,20 @@ static inline uint16_t mavlink_msg_simstate_encode(uint8_t system_id, uint8_t co
 }
 
 /**
+ * @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
  *
@@ -183,15 +208,15 @@ static inline uint16_t mavlink_msg_simstate_encode(uint8_t system_id, uint8_t co
  * @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 lng Longitude in degrees
+ * @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, float lat, float lng)
+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[44];
+	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);
@@ -201,10 +226,14 @@ static inline void mavlink_msg_simstate_send(mavlink_channel_t chan, float roll,
 	_mav_put_float(buf, 24, xgyro);
 	_mav_put_float(buf, 28, ygyro);
 	_mav_put_float(buf, 32, zgyro);
-	_mav_put_float(buf, 36, lat);
-	_mav_put_float(buf, 40, lng);
+	_mav_put_int32_t(buf, 36, lat);
+	_mav_put_int32_t(buf, 40, lng);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, buf, 44, 111);
+#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;
@@ -219,7 +248,11 @@ static inline void mavlink_msg_simstate_send(mavlink_channel_t chan, float roll,
 	packet.lat = lat;
 	packet.lng = lng;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, (const char *)&packet, 44, 111);
+#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
 }
 
@@ -321,21 +354,21 @@ static inline float mavlink_msg_simstate_get_zgyro(const mavlink_message_t* msg)
 /**
  * @brief Get field lat from simstate message
  *
- * @return Latitude in degrees
+ * @return Latitude in degrees * 1E7
  */
-static inline float mavlink_msg_simstate_get_lat(const mavlink_message_t* msg)
+static inline int32_t mavlink_msg_simstate_get_lat(const mavlink_message_t* msg)
 {
-	return _MAV_RETURN_float(msg,  36);
+	return _MAV_RETURN_int32_t(msg,  36);
 }
 
 /**
  * @brief Get field lng from simstate message
  *
- * @return Longitude in degrees
+ * @return Longitude in degrees * 1E7
  */
-static inline float mavlink_msg_simstate_get_lng(const mavlink_message_t* msg)
+static inline int32_t mavlink_msg_simstate_get_lng(const mavlink_message_t* msg)
 {
-	return _MAV_RETURN_float(msg,  40);
+	return _MAV_RETURN_int32_t(msg,  40);
 }
 
 /**
@@ -359,6 +392,6 @@ static inline void mavlink_msg_simstate_decode(const mavlink_message_t* msg, mav
 	simstate->lat = mavlink_msg_simstate_get_lat(msg);
 	simstate->lng = mavlink_msg_simstate_get_lng(msg);
 #else
-	memcpy(simstate, _MAV_PAYLOAD(msg), 44);
+	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
index e7fa7d1ea..7608a7bd1 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h
@@ -12,6 +12,9 @@ typedef struct __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 { \
@@ -39,30 +42,34 @@ static inline uint16_t mavlink_msg_wind_pack(uint8_t system_id, uint8_t componen
 						       float direction, float speed, float speed_z)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[12];
+	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, 12);
+        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, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WIND_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_WIND;
-	return mavlink_finalize_message(msg, system_id, component_id, 12, 1);
+#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 was sent over
+ * @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)
@@ -74,27 +81,31 @@ static inline uint16_t mavlink_msg_wind_pack_chan(uint8_t system_id, uint8_t com
 						           float direction,float speed,float speed_z)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[12];
+	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, 12);
+        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, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WIND_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_WIND;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 1);
+#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 into a message
+ * @brief Encode a wind struct
  *
  * @param system_id ID of this system
  * @param component_id ID of this component (e.g. 200 for IMU)
@@ -107,6 +118,20 @@ static inline uint16_t mavlink_msg_wind_encode(uint8_t system_id, uint8_t compon
 }
 
 /**
+ * @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
  *
@@ -119,19 +144,27 @@ static inline uint16_t mavlink_msg_wind_encode(uint8_t system_id, uint8_t compon
 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[12];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, buf, 12, 1);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, (const char *)&packet, 12, 1);
+#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
 }
 
@@ -183,6 +216,6 @@ static inline void mavlink_msg_wind_decode(const mavlink_message_t* msg, mavlink
 	wind->speed = mavlink_msg_wind_get_speed(msg);
 	wind->speed_z = mavlink_msg_wind_get_speed_z(msg);
 #else
-	memcpy(wind, _MAV_PAYLOAD(msg), 12);
+	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
index 0442ab787..b2496334f 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h
@@ -31,17 +31,17 @@ static void mavlink_test_sensor_offsets(uint8_t system_id, uint8_t component_id,
         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,
+	}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));
@@ -96,10 +96,10 @@ static void mavlink_test_set_mag_offsets(uint8_t system_id, uint8_t component_id
         uint16_t i;
 	mavlink_set_mag_offsets_t packet_in = {
 		17235,
-	17339,
-	17443,
-	151,
-	218,
+	}17339,
+	}17443,
+	}151,
+	}218,
 	};
 	mavlink_set_mag_offsets_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -147,7 +147,7 @@ static void mavlink_test_meminfo(uint8_t system_id, uint8_t component_id, mavlin
         uint16_t i;
 	mavlink_meminfo_t packet_in = {
 		17235,
-	17339,
+	}17339,
 	};
 	mavlink_meminfo_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -192,11 +192,11 @@ static void mavlink_test_ap_adc(uint8_t system_id, uint8_t component_id, mavlink
         uint16_t i;
 	mavlink_ap_adc_t packet_in = {
 		17235,
-	17339,
-	17443,
-	17547,
-	17651,
-	17755,
+	}17339,
+	}17443,
+	}17547,
+	}17651,
+	}17755,
 	};
 	mavlink_ap_adc_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -245,16 +245,16 @@ static void mavlink_test_digicam_configure(uint8_t system_id, uint8_t component_
         uint16_t i;
 	mavlink_digicam_configure_t packet_in = {
 		17.0,
-	17443,
-	151,
-	218,
-	29,
-	96,
-	163,
-	230,
-	41,
-	108,
-	175,
+	}17443,
+	}151,
+	}218,
+	}29,
+	}96,
+	}163,
+	}230,
+	}41,
+	}108,
+	}175,
 	};
 	mavlink_digicam_configure_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -308,15 +308,15 @@ static void mavlink_test_digicam_control(uint8_t system_id, uint8_t component_id
         uint16_t i;
 	mavlink_digicam_control_t packet_in = {
 		17.0,
-	17,
-	84,
-	151,
-	218,
-	29,
-	96,
-	163,
-	230,
-	41,
+	}17,
+	}84,
+	}151,
+	}218,
+	}29,
+	}96,
+	}163,
+	}230,
+	}41,
 	};
 	mavlink_digicam_control_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -369,11 +369,11 @@ static void mavlink_test_mount_configure(uint8_t system_id, uint8_t component_id
         uint16_t i;
 	mavlink_mount_configure_t packet_in = {
 		5,
-	72,
-	139,
-	206,
-	17,
-	84,
+	}72,
+	}139,
+	}206,
+	}17,
+	}84,
 	};
 	mavlink_mount_configure_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -422,11 +422,11 @@ static void mavlink_test_mount_control(uint8_t system_id, uint8_t component_id,
         uint16_t i;
 	mavlink_mount_control_t packet_in = {
 		963497464,
-	963497672,
-	963497880,
-	41,
-	108,
-	175,
+	}963497672,
+	}963497880,
+	}41,
+	}108,
+	}175,
 	};
 	mavlink_mount_control_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -475,10 +475,10 @@ static void mavlink_test_mount_status(uint8_t system_id, uint8_t component_id, m
         uint16_t i;
 	mavlink_mount_status_t packet_in = {
 		963497464,
-	963497672,
-	963497880,
-	41,
-	108,
+	}963497672,
+	}963497880,
+	}41,
+	}108,
 	};
 	mavlink_mount_status_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -526,11 +526,11 @@ static void mavlink_test_fence_point(uint8_t system_id, uint8_t component_id, ma
         uint16_t i;
 	mavlink_fence_point_t packet_in = {
 		17.0,
-	45.0,
-	29,
-	96,
-	163,
-	230,
+	}45.0,
+	}29,
+	}96,
+	}163,
+	}230,
 	};
 	mavlink_fence_point_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -579,8 +579,8 @@ static void mavlink_test_fence_fetch_point(uint8_t system_id, uint8_t component_
         uint16_t i;
 	mavlink_fence_fetch_point_t packet_in = {
 		5,
-	72,
-	139,
+	}72,
+	}139,
 	};
 	mavlink_fence_fetch_point_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -626,9 +626,9 @@ static void mavlink_test_fence_status(uint8_t system_id, uint8_t component_id, m
         uint16_t i;
 	mavlink_fence_status_t packet_in = {
 		963497464,
-	17443,
-	151,
-	218,
+	}17443,
+	}151,
+	}218,
 	};
 	mavlink_fence_status_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -675,12 +675,12 @@ static void mavlink_test_ahrs(uint8_t system_id, uint8_t component_id, mavlink_m
         uint16_t i;
 	mavlink_ahrs_t packet_in = {
 		17.0,
-	45.0,
-	73.0,
-	101.0,
-	129.0,
-	157.0,
-	185.0,
+	}45.0,
+	}73.0,
+	}101.0,
+	}129.0,
+	}157.0,
+	}185.0,
 	};
 	mavlink_ahrs_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -730,16 +730,16 @@ static void mavlink_test_simstate(uint8_t system_id, uint8_t component_id, mavli
         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,
-	269.0,
-	297.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));
@@ -793,7 +793,7 @@ static void mavlink_test_hwstatus(uint8_t system_id, uint8_t component_id, mavli
         uint16_t i;
 	mavlink_hwstatus_t packet_in = {
 		17235,
-	139,
+	}139,
 	};
 	mavlink_hwstatus_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -838,12 +838,12 @@ static void mavlink_test_radio(uint8_t system_id, uint8_t component_id, mavlink_
         uint16_t i;
 	mavlink_radio_t packet_in = {
 		17235,
-	17339,
-	17,
-	84,
-	151,
-	218,
-	29,
+	}17339,
+	}17,
+	}84,
+	}151,
+	}218,
+	}29,
 	};
 	mavlink_radio_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -893,14 +893,14 @@ static void mavlink_test_limits_status(uint8_t system_id, uint8_t component_id,
         uint16_t i;
 	mavlink_limits_status_t packet_in = {
 		963497464,
-	963497672,
-	963497880,
-	963498088,
-	18067,
-	187,
-	254,
-	65,
-	132,
+	}963497672,
+	}963497880,
+	}963498088,
+	}18067,
+	}187,
+	}254,
+	}65,
+	}132,
 	};
 	mavlink_limits_status_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -952,8 +952,8 @@ static void mavlink_test_wind(uint8_t system_id, uint8_t component_id, mavlink_m
         uint16_t i;
 	mavlink_wind_t packet_in = {
 		17.0,
-	45.0,
-	73.0,
+	}45.0,
+	}73.0,
 	};
 	mavlink_wind_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -999,8 +999,8 @@ static void mavlink_test_data16(uint8_t system_id, uint8_t component_id, mavlink
         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 },
+	}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));
@@ -1046,8 +1046,8 @@ static void mavlink_test_data32(uint8_t system_id, uint8_t component_id, mavlink
         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 },
+	}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));
@@ -1093,8 +1093,8 @@ static void mavlink_test_data64(uint8_t system_id, uint8_t component_id, mavlink
         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 },
+	}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));
@@ -1140,8 +1140,8 @@ static void mavlink_test_data96(uint8_t system_id, uint8_t component_id, mavlink
         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 },
+	}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));
@@ -1180,6 +1180,277 @@ static void mavlink_test_data96(uint8_t system_id, uint8_t component_id, mavlink
         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);
@@ -1204,6 +1475,11 @@ static void mavlink_test_ardupilotmega(uint8_t system_id, uint8_t component_id,
 	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
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/version.h b/mavlink/include/mavlink/v1.0/ardupilotmega/version.h
index ff24a2daa..983237eb5 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/version.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/version.h
@@ -5,8 +5,8 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Wed Jan  9 16:17:57 2013"
+#define MAVLINK_BUILD_DATE "Tue Feb  4 15:27:33 2014"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
-#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254
+#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
new file mode 100644
index 000000000..b4dde669f
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/autoquad/autoquad.h
@@ -0,0 +1,123 @@
+/** @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
new file mode 100644
index 000000000..3f80c9a41
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/autoquad/mavlink.h
@@ -0,0 +1,27 @@
+/** @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
new file mode 100644
index 000000000..ed7c86bcb
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h
@@ -0,0 +1,617 @@
+// 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
new file mode 100644
index 000000000..4eafe7be5
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/autoquad/testsuite.h
@@ -0,0 +1,118 @@
+/** @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
new file mode 100644
index 000000000..d125e92b1
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/autoquad/version.h
@@ -0,0 +1,12 @@
+/** @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/common/common.h b/mavlink/include/mavlink/v1.0/common/common.h
index d223fc6f5..7e1cf765b 100644
--- a/mavlink/include/mavlink/v1.0/common/common.h
+++ b/mavlink/include/mavlink/v1.0/common/common.h
@@ -12,15 +12,15 @@ extern "C" {
 // MESSAGE LENGTHS AND CRCS
 
 #ifndef MAVLINK_MESSAGE_LENGTHS
-#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 0, 0, 0, 254, 249, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 33, 25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0}
+#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 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, 0, 0, 0, 235, 93, 124, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 241, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}
+#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 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, {"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_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}}
+#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"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"
@@ -48,7 +48,11 @@ enum MAV_AUTOPILOT
 	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_ENUM_END=13, /*  | */
+	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
 
@@ -199,6 +203,35 @@ enum MAV_COMPONENT
 };
 #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
@@ -258,6 +291,7 @@ enum MAV_CMD
 	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|  */
@@ -266,7 +300,8 @@ enum MAV_CMD
 	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_ENUM_END=401, /*  | */
+	MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX|  */
+	MAV_CMD_ENUM_END=501, /*  | */
 };
 #endif
 
@@ -485,9 +520,26 @@ enum MAV_SEVERITY
 #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"
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_6dof_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_6dof_setpoint.h
deleted file mode 100644
index 0c4ab8c7d..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_6dof_setpoint.h
+++ /dev/null
@@ -1,276 +0,0 @@
-// MESSAGE 6DOF_SETPOINT PACKING
-
-#define MAVLINK_MSG_ID_6DOF_SETPOINT 149
-
-typedef struct __mavlink_6dof_setpoint_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_6dof_setpoint_t;
-
-#define MAVLINK_MSG_ID_6DOF_SETPOINT_LEN 25
-#define MAVLINK_MSG_ID_149_LEN 25
-
-
-
-#define MAVLINK_MESSAGE_INFO_6DOF_SETPOINT { \
-	"6DOF_SETPOINT", \
-	7, \
-	{  { "trans_x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_6dof_setpoint_t, trans_x) }, \
-         { "trans_y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_6dof_setpoint_t, trans_y) }, \
-         { "trans_z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_6dof_setpoint_t, trans_z) }, \
-         { "rot_x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_6dof_setpoint_t, rot_x) }, \
-         { "rot_y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_6dof_setpoint_t, rot_y) }, \
-         { "rot_z", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_6dof_setpoint_t, rot_z) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_6dof_setpoint_t, target_system) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a 6dof_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 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_6dof_setpoint_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[25];
-	_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, 25);
-#else
-	mavlink_6dof_setpoint_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, 25);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_6DOF_SETPOINT;
-	return mavlink_finalize_message(msg, system_id, component_id, 25, 144);
-}
-
-/**
- * @brief Pack a 6dof_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 was 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_6dof_setpoint_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[25];
-	_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, 25);
-#else
-	mavlink_6dof_setpoint_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, 25);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_6DOF_SETPOINT;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 25, 144);
-}
-
-/**
- * @brief Encode a 6dof_setpoint 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 6dof_setpoint C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_6dof_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_6dof_setpoint_t* 6dof_setpoint)
-{
-	return mavlink_msg_6dof_setpoint_pack(system_id, component_id, msg, 6dof_setpoint->target_system, 6dof_setpoint->trans_x, 6dof_setpoint->trans_y, 6dof_setpoint->trans_z, 6dof_setpoint->rot_x, 6dof_setpoint->rot_y, 6dof_setpoint->rot_z);
-}
-
-/**
- * @brief Send a 6dof_setpoint 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_6dof_setpoint_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[25];
-	_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);
-
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_6DOF_SETPOINT, buf, 25, 144);
-#else
-	mavlink_6dof_setpoint_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;
-
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_6DOF_SETPOINT, (const char *)&packet, 25, 144);
-#endif
-}
-
-#endif
-
-// MESSAGE 6DOF_SETPOINT UNPACKING
-
-
-/**
- * @brief Get field target_system from 6dof_setpoint message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_6dof_setpoint_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  24);
-}
-
-/**
- * @brief Get field trans_x from 6dof_setpoint message
- *
- * @return Translational Component in x
- */
-static inline float mavlink_msg_6dof_setpoint_get_trans_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field trans_y from 6dof_setpoint message
- *
- * @return Translational Component in y
- */
-static inline float mavlink_msg_6dof_setpoint_get_trans_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field trans_z from 6dof_setpoint message
- *
- * @return Translational Component in z
- */
-static inline float mavlink_msg_6dof_setpoint_get_trans_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field rot_x from 6dof_setpoint message
- *
- * @return Rotational Component in x
- */
-static inline float mavlink_msg_6dof_setpoint_get_rot_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field rot_y from 6dof_setpoint message
- *
- * @return Rotational Component in y
- */
-static inline float mavlink_msg_6dof_setpoint_get_rot_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field rot_z from 6dof_setpoint message
- *
- * @return Rotational Component in z
- */
-static inline float mavlink_msg_6dof_setpoint_get_rot_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Decode a 6dof_setpoint message into a struct
- *
- * @param msg The message to decode
- * @param 6dof_setpoint C-struct to decode the message contents into
- */
-static inline void mavlink_msg_6dof_setpoint_decode(const mavlink_message_t* msg, mavlink_6dof_setpoint_t* 6dof_setpoint)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	6dof_setpoint->trans_x = mavlink_msg_6dof_setpoint_get_trans_x(msg);
-	6dof_setpoint->trans_y = mavlink_msg_6dof_setpoint_get_trans_y(msg);
-	6dof_setpoint->trans_z = mavlink_msg_6dof_setpoint_get_trans_z(msg);
-	6dof_setpoint->rot_x = mavlink_msg_6dof_setpoint_get_rot_x(msg);
-	6dof_setpoint->rot_y = mavlink_msg_6dof_setpoint_get_rot_y(msg);
-	6dof_setpoint->rot_z = mavlink_msg_6dof_setpoint_get_rot_z(msg);
-	6dof_setpoint->target_system = mavlink_msg_6dof_setpoint_get_target_system(msg);
-#else
-	memcpy(6dof_setpoint, _MAV_PAYLOAD(msg), 25);
-#endif
-}
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_8dof_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_8dof_setpoint.h
deleted file mode 100644
index 521d2fb66..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_8dof_setpoint.h
+++ /dev/null
@@ -1,320 +0,0 @@
-// MESSAGE 8DOF_SETPOINT PACKING
-
-#define MAVLINK_MSG_ID_8DOF_SETPOINT 148
-
-typedef struct __mavlink_8dof_setpoint_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_8dof_setpoint_t;
-
-#define MAVLINK_MSG_ID_8DOF_SETPOINT_LEN 33
-#define MAVLINK_MSG_ID_148_LEN 33
-
-
-
-#define MAVLINK_MESSAGE_INFO_8DOF_SETPOINT { \
-	"8DOF_SETPOINT", \
-	9, \
-	{  { "val1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_8dof_setpoint_t, val1) }, \
-         { "val2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_8dof_setpoint_t, val2) }, \
-         { "val3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_8dof_setpoint_t, val3) }, \
-         { "val4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_8dof_setpoint_t, val4) }, \
-         { "val5", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_8dof_setpoint_t, val5) }, \
-         { "val6", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_8dof_setpoint_t, val6) }, \
-         { "val7", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_8dof_setpoint_t, val7) }, \
-         { "val8", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_8dof_setpoint_t, val8) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_8dof_setpoint_t, target_system) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a 8dof_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 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_8dof_setpoint_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[33];
-	_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, 33);
-#else
-	mavlink_8dof_setpoint_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, 33);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_8DOF_SETPOINT;
-	return mavlink_finalize_message(msg, system_id, component_id, 33, 42);
-}
-
-/**
- * @brief Pack a 8dof_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 was 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_8dof_setpoint_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[33];
-	_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, 33);
-#else
-	mavlink_8dof_setpoint_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, 33);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_8DOF_SETPOINT;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 33, 42);
-}
-
-/**
- * @brief Encode a 8dof_setpoint 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 8dof_setpoint C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_8dof_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_8dof_setpoint_t* 8dof_setpoint)
-{
-	return mavlink_msg_8dof_setpoint_pack(system_id, component_id, msg, 8dof_setpoint->target_system, 8dof_setpoint->val1, 8dof_setpoint->val2, 8dof_setpoint->val3, 8dof_setpoint->val4, 8dof_setpoint->val5, 8dof_setpoint->val6, 8dof_setpoint->val7, 8dof_setpoint->val8);
-}
-
-/**
- * @brief Send a 8dof_setpoint 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_8dof_setpoint_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[33];
-	_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);
-
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_8DOF_SETPOINT, buf, 33, 42);
-#else
-	mavlink_8dof_setpoint_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;
-
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_8DOF_SETPOINT, (const char *)&packet, 33, 42);
-#endif
-}
-
-#endif
-
-// MESSAGE 8DOF_SETPOINT UNPACKING
-
-
-/**
- * @brief Get field target_system from 8dof_setpoint message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_8dof_setpoint_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  32);
-}
-
-/**
- * @brief Get field val1 from 8dof_setpoint message
- *
- * @return Value 1
- */
-static inline float mavlink_msg_8dof_setpoint_get_val1(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field val2 from 8dof_setpoint message
- *
- * @return Value 2
- */
-static inline float mavlink_msg_8dof_setpoint_get_val2(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field val3 from 8dof_setpoint message
- *
- * @return Value 3
- */
-static inline float mavlink_msg_8dof_setpoint_get_val3(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field val4 from 8dof_setpoint message
- *
- * @return Value 4
- */
-static inline float mavlink_msg_8dof_setpoint_get_val4(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field val5 from 8dof_setpoint message
- *
- * @return Value 5
- */
-static inline float mavlink_msg_8dof_setpoint_get_val5(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field val6 from 8dof_setpoint message
- *
- * @return Value 6
- */
-static inline float mavlink_msg_8dof_setpoint_get_val6(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field val7 from 8dof_setpoint message
- *
- * @return Value 7
- */
-static inline float mavlink_msg_8dof_setpoint_get_val7(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Get field val8 from 8dof_setpoint message
- *
- * @return Value 8
- */
-static inline float mavlink_msg_8dof_setpoint_get_val8(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  28);
-}
-
-/**
- * @brief Decode a 8dof_setpoint message into a struct
- *
- * @param msg The message to decode
- * @param 8dof_setpoint C-struct to decode the message contents into
- */
-static inline void mavlink_msg_8dof_setpoint_decode(const mavlink_message_t* msg, mavlink_8dof_setpoint_t* 8dof_setpoint)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	8dof_setpoint->val1 = mavlink_msg_8dof_setpoint_get_val1(msg);
-	8dof_setpoint->val2 = mavlink_msg_8dof_setpoint_get_val2(msg);
-	8dof_setpoint->val3 = mavlink_msg_8dof_setpoint_get_val3(msg);
-	8dof_setpoint->val4 = mavlink_msg_8dof_setpoint_get_val4(msg);
-	8dof_setpoint->val5 = mavlink_msg_8dof_setpoint_get_val5(msg);
-	8dof_setpoint->val6 = mavlink_msg_8dof_setpoint_get_val6(msg);
-	8dof_setpoint->val7 = mavlink_msg_8dof_setpoint_get_val7(msg);
-	8dof_setpoint->val8 = mavlink_msg_8dof_setpoint_get_val8(msg);
-	8dof_setpoint->target_system = mavlink_msg_8dof_setpoint_get_target_system(msg);
-#else
-	memcpy(8dof_setpoint, _MAV_PAYLOAD(msg), 33);
-#endif
-}
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h
index 6db0592f2..8ddf5bf09 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h
@@ -16,6 +16,9 @@ typedef struct __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 { \
@@ -51,7 +54,7 @@ static inline uint16_t mavlink_msg_attitude_pack(uint8_t system_id, uint8_t comp
 						       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[28];
+	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);
@@ -60,7 +63,7 @@ static inline uint16_t mavlink_msg_attitude_pack(uint8_t system_id, uint8_t comp
 	_mav_put_float(buf, 20, pitchspeed);
 	_mav_put_float(buf, 24, yawspeed);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 28);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_LEN);
 #else
 	mavlink_attitude_t packet;
 	packet.time_boot_ms = time_boot_ms;
@@ -71,18 +74,22 @@ static inline uint16_t mavlink_msg_attitude_pack(uint8_t system_id, uint8_t comp
 	packet.pitchspeed = pitchspeed;
 	packet.yawspeed = yawspeed;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 28);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_ATTITUDE;
-	return mavlink_finalize_message(msg, system_id, component_id, 28, 39);
+#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 was sent over
+ * @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)
@@ -98,7 +105,7 @@ static inline uint16_t mavlink_msg_attitude_pack_chan(uint8_t system_id, uint8_t
 						           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[28];
+	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);
@@ -107,7 +114,7 @@ static inline uint16_t mavlink_msg_attitude_pack_chan(uint8_t system_id, uint8_t
 	_mav_put_float(buf, 20, pitchspeed);
 	_mav_put_float(buf, 24, yawspeed);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 28);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_LEN);
 #else
 	mavlink_attitude_t packet;
 	packet.time_boot_ms = time_boot_ms;
@@ -118,15 +125,19 @@ static inline uint16_t mavlink_msg_attitude_pack_chan(uint8_t system_id, uint8_t
 	packet.pitchspeed = pitchspeed;
 	packet.yawspeed = yawspeed;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 28);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_ATTITUDE;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 28, 39);
+#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 into a message
+ * @brief Encode a attitude struct
  *
  * @param system_id ID of this system
  * @param component_id ID of this component (e.g. 200 for IMU)
@@ -139,6 +150,20 @@ static inline uint16_t mavlink_msg_attitude_encode(uint8_t system_id, uint8_t co
 }
 
 /**
+ * @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
  *
@@ -155,7 +180,7 @@ static inline uint16_t mavlink_msg_attitude_encode(uint8_t system_id, uint8_t co
 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[28];
+	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);
@@ -164,7 +189,11 @@ static inline void mavlink_msg_attitude_send(mavlink_channel_t chan, uint32_t ti
 	_mav_put_float(buf, 20, pitchspeed);
 	_mav_put_float(buf, 24, yawspeed);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, buf, 28, 39);
+#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;
@@ -175,7 +204,11 @@ static inline void mavlink_msg_attitude_send(mavlink_channel_t chan, uint32_t ti
 	packet.pitchspeed = pitchspeed;
 	packet.yawspeed = yawspeed;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, (const char *)&packet, 28, 39);
+#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
 }
 
@@ -271,6 +304,6 @@ static inline void mavlink_msg_attitude_decode(const mavlink_message_t* msg, mav
 	attitude->pitchspeed = mavlink_msg_attitude_get_pitchspeed(msg);
 	attitude->yawspeed = mavlink_msg_attitude_get_yawspeed(msg);
 #else
-	memcpy(attitude, _MAV_PAYLOAD(msg), 28);
+	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
index 556048865..9f8d58759 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h
@@ -17,6 +17,9 @@ typedef struct __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 { \
@@ -54,7 +57,7 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack(uint8_t system_id, u
 						       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[32];
+	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);
@@ -64,7 +67,7 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack(uint8_t system_id, u
 	_mav_put_float(buf, 24, pitchspeed);
 	_mav_put_float(buf, 28, yawspeed);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 32);
+        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;
@@ -76,18 +79,22 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack(uint8_t system_id, u
 	packet.pitchspeed = pitchspeed;
 	packet.yawspeed = yawspeed;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 32);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_ATTITUDE_QUATERNION;
-	return mavlink_finalize_message(msg, system_id, component_id, 32, 246);
+#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 was sent over
+ * @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
@@ -104,7 +111,7 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack_chan(uint8_t system_
 						           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[32];
+	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);
@@ -114,7 +121,7 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack_chan(uint8_t system_
 	_mav_put_float(buf, 24, pitchspeed);
 	_mav_put_float(buf, 28, yawspeed);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 32);
+        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;
@@ -126,15 +133,19 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack_chan(uint8_t system_
 	packet.pitchspeed = pitchspeed;
 	packet.yawspeed = yawspeed;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 32);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_ATTITUDE_QUATERNION;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 246);
+#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 into a message
+ * @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)
@@ -147,6 +158,20 @@ static inline uint16_t mavlink_msg_attitude_quaternion_encode(uint8_t system_id,
 }
 
 /**
+ * @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
  *
@@ -164,7 +189,7 @@ static inline uint16_t mavlink_msg_attitude_quaternion_encode(uint8_t system_id,
 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[32];
+	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);
@@ -174,7 +199,11 @@ static inline void mavlink_msg_attitude_quaternion_send(mavlink_channel_t chan,
 	_mav_put_float(buf, 24, pitchspeed);
 	_mav_put_float(buf, 28, yawspeed);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, buf, 32, 246);
+#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;
@@ -186,7 +215,11 @@ static inline void mavlink_msg_attitude_quaternion_send(mavlink_channel_t chan,
 	packet.pitchspeed = pitchspeed;
 	packet.yawspeed = yawspeed;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)&packet, 32, 246);
+#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
 }
 
@@ -293,6 +326,6 @@ static inline void mavlink_msg_attitude_quaternion_decode(const mavlink_message_
 	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), 32);
+	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
index baa119fde..5703a5987 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h
@@ -10,6 +10,9 @@ typedef struct __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 { \
@@ -33,26 +36,30 @@ static inline uint16_t mavlink_msg_auth_key_pack(uint8_t system_id, uint8_t comp
 						       const char *key)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[32];
+	char buf[MAVLINK_MSG_ID_AUTH_KEY_LEN];
 
 	_mav_put_char_array(buf, 0, key, 32);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 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, 32);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTH_KEY_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_AUTH_KEY;
-	return mavlink_finalize_message(msg, system_id, component_id, 32, 119);
+#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 was sent over
+ * @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)
@@ -62,23 +69,27 @@ static inline uint16_t mavlink_msg_auth_key_pack_chan(uint8_t system_id, uint8_t
 						           const char *key)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[32];
+	char buf[MAVLINK_MSG_ID_AUTH_KEY_LEN];
 
 	_mav_put_char_array(buf, 0, key, 32);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 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, 32);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTH_KEY_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_AUTH_KEY;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 119);
+#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 into a message
+ * @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)
@@ -91,6 +102,20 @@ static inline uint16_t mavlink_msg_auth_key_encode(uint8_t system_id, uint8_t co
 }
 
 /**
+ * @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
  *
@@ -101,15 +126,23 @@ static inline uint16_t mavlink_msg_auth_key_encode(uint8_t system_id, uint8_t co
 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[32];
+	char buf[MAVLINK_MSG_ID_AUTH_KEY_LEN];
 
 	_mav_put_char_array(buf, 0, key, 32);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, buf, 32, 119);
+#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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, (const char *)&packet, 32, 119);
+#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
 }
 
@@ -139,6 +172,6 @@ static inline void mavlink_msg_auth_key_decode(const mavlink_message_t* msg, mav
 #if MAVLINK_NEED_BYTE_SWAP
 	mavlink_msg_auth_key_get_key(msg, auth_key->key);
 #else
-	memcpy(auth_key, _MAV_PAYLOAD(msg), 32);
+	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
index c78fb4f31..03e4d569e 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h
@@ -4,6 +4,8 @@
 
 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
@@ -15,23 +17,28 @@ typedef struct __mavlink_battery_status_t
  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 16
-#define MAVLINK_MSG_ID_147_LEN 16
+#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", \
-	9, \
-	{  { "voltage_cell_1", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_battery_status_t, voltage_cell_1) }, \
-         { "voltage_cell_2", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_battery_status_t, voltage_cell_2) }, \
-         { "voltage_cell_3", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_battery_status_t, voltage_cell_3) }, \
-         { "voltage_cell_4", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_battery_status_t, voltage_cell_4) }, \
-         { "voltage_cell_5", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_battery_status_t, voltage_cell_5) }, \
-         { "voltage_cell_6", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_battery_status_t, voltage_cell_6) }, \
-         { "current_battery", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_battery_status_t, current_battery) }, \
-         { "accu_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_battery_status_t, accu_id) }, \
-         { "battery_remaining", NULL, MAVLINK_TYPE_INT8_T, 0, 15, offsetof(mavlink_battery_status_t, battery_remaining) }, \
+	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) }, \
          } \
 }
 
@@ -50,27 +57,33 @@ typedef struct __mavlink_battery_status_t
  * @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, int8_t battery_remaining)
+						       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[16];
-	_mav_put_uint16_t(buf, 0, voltage_cell_1);
-	_mav_put_uint16_t(buf, 2, voltage_cell_2);
-	_mav_put_uint16_t(buf, 4, voltage_cell_3);
-	_mav_put_uint16_t(buf, 6, voltage_cell_4);
-	_mav_put_uint16_t(buf, 8, voltage_cell_5);
-	_mav_put_uint16_t(buf, 10, voltage_cell_6);
-	_mav_put_int16_t(buf, 12, current_battery);
-	_mav_put_uint8_t(buf, 14, accu_id);
-	_mav_put_int8_t(buf, 15, battery_remaining);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 16);
+	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;
@@ -81,18 +94,22 @@ static inline uint16_t mavlink_msg_battery_status_pack(uint8_t system_id, uint8_
 	packet.accu_id = accu_id;
 	packet.battery_remaining = battery_remaining;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 16);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_BATTERY_STATUS;
-	return mavlink_finalize_message(msg, system_id, component_id, 16, 42);
+#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 was sent over
+ * @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)
@@ -102,28 +119,34 @@ static inline uint16_t mavlink_msg_battery_status_pack(uint8_t system_id, uint8_
  * @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,int8_t battery_remaining)
+						           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[16];
-	_mav_put_uint16_t(buf, 0, voltage_cell_1);
-	_mav_put_uint16_t(buf, 2, voltage_cell_2);
-	_mav_put_uint16_t(buf, 4, voltage_cell_3);
-	_mav_put_uint16_t(buf, 6, voltage_cell_4);
-	_mav_put_uint16_t(buf, 8, voltage_cell_5);
-	_mav_put_uint16_t(buf, 10, voltage_cell_6);
-	_mav_put_int16_t(buf, 12, current_battery);
-	_mav_put_uint8_t(buf, 14, accu_id);
-	_mav_put_int8_t(buf, 15, battery_remaining);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 16);
+	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;
@@ -134,15 +157,19 @@ static inline uint16_t mavlink_msg_battery_status_pack_chan(uint8_t system_id, u
 	packet.accu_id = accu_id;
 	packet.battery_remaining = battery_remaining;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 16);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_BATTERY_STATUS;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16, 42);
+#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 into a message
+ * @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)
@@ -151,7 +178,21 @@ static inline uint16_t mavlink_msg_battery_status_pack_chan(uint8_t system_id, u
  */
 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->battery_remaining);
+	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);
 }
 
 /**
@@ -166,27 +207,37 @@ static inline uint16_t mavlink_msg_battery_status_encode(uint8_t system_id, uint
  * @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, int8_t battery_remaining)
+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[16];
-	_mav_put_uint16_t(buf, 0, voltage_cell_1);
-	_mav_put_uint16_t(buf, 2, voltage_cell_2);
-	_mav_put_uint16_t(buf, 4, voltage_cell_3);
-	_mav_put_uint16_t(buf, 6, voltage_cell_4);
-	_mav_put_uint16_t(buf, 8, voltage_cell_5);
-	_mav_put_uint16_t(buf, 10, voltage_cell_6);
-	_mav_put_int16_t(buf, 12, current_battery);
-	_mav_put_uint8_t(buf, 14, accu_id);
-	_mav_put_int8_t(buf, 15, battery_remaining);
-
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, buf, 16, 42);
+	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;
@@ -197,7 +248,11 @@ static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8
 	packet.accu_id = accu_id;
 	packet.battery_remaining = battery_remaining;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, (const char *)&packet, 16, 42);
+#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
 }
 
@@ -213,7 +268,7 @@ static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8
  */
 static inline uint8_t mavlink_msg_battery_status_get_accu_id(const mavlink_message_t* msg)
 {
-	return _MAV_RETURN_uint8_t(msg,  14);
+	return _MAV_RETURN_uint8_t(msg,  22);
 }
 
 /**
@@ -223,7 +278,7 @@ static inline uint8_t mavlink_msg_battery_status_get_accu_id(const mavlink_messa
  */
 static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_1(const mavlink_message_t* msg)
 {
-	return _MAV_RETURN_uint16_t(msg,  0);
+	return _MAV_RETURN_uint16_t(msg,  8);
 }
 
 /**
@@ -233,7 +288,7 @@ static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_1(const mavli
  */
 static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_2(const mavlink_message_t* msg)
 {
-	return _MAV_RETURN_uint16_t(msg,  2);
+	return _MAV_RETURN_uint16_t(msg,  10);
 }
 
 /**
@@ -243,7 +298,7 @@ static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_2(const mavli
  */
 static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_3(const mavlink_message_t* msg)
 {
-	return _MAV_RETURN_uint16_t(msg,  4);
+	return _MAV_RETURN_uint16_t(msg,  12);
 }
 
 /**
@@ -253,7 +308,7 @@ static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_3(const mavli
  */
 static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_4(const mavlink_message_t* msg)
 {
-	return _MAV_RETURN_uint16_t(msg,  6);
+	return _MAV_RETURN_uint16_t(msg,  14);
 }
 
 /**
@@ -263,7 +318,7 @@ static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_4(const mavli
  */
 static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_5(const mavlink_message_t* msg)
 {
-	return _MAV_RETURN_uint16_t(msg,  8);
+	return _MAV_RETURN_uint16_t(msg,  16);
 }
 
 /**
@@ -273,7 +328,7 @@ static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_5(const mavli
  */
 static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_6(const mavlink_message_t* msg)
 {
-	return _MAV_RETURN_uint16_t(msg,  10);
+	return _MAV_RETURN_uint16_t(msg,  18);
 }
 
 /**
@@ -283,7 +338,27 @@ static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_6(const mavli
  */
 static inline int16_t mavlink_msg_battery_status_get_current_battery(const mavlink_message_t* msg)
 {
-	return _MAV_RETURN_int16_t(msg,  12);
+	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);
 }
 
 /**
@@ -293,7 +368,7 @@ static inline int16_t mavlink_msg_battery_status_get_current_battery(const mavli
  */
 static inline int8_t mavlink_msg_battery_status_get_battery_remaining(const mavlink_message_t* msg)
 {
-	return _MAV_RETURN_int8_t(msg,  15);
+	return _MAV_RETURN_int8_t(msg,  23);
 }
 
 /**
@@ -305,6 +380,8 @@ static inline int8_t mavlink_msg_battery_status_get_battery_remaining(const mavl
 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);
@@ -315,6 +392,6 @@ static inline void mavlink_msg_battery_status_decode(const mavlink_message_t* ms
 	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), 16);
+	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
index a55851008..0b6de930d 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h
@@ -13,6 +13,9 @@ typedef struct __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 { \
@@ -42,30 +45,34 @@ static inline uint16_t mavlink_msg_change_operator_control_pack(uint8_t system_i
 						       uint8_t target_system, uint8_t control_request, uint8_t version, const char *passkey)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[28];
+	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, 28);
+        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, 28);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL;
-	return mavlink_finalize_message(msg, system_id, component_id, 28, 217);
+#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 was sent over
+ * @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
@@ -78,27 +85,31 @@ static inline uint16_t mavlink_msg_change_operator_control_pack_chan(uint8_t sys
 						           uint8_t target_system,uint8_t control_request,uint8_t version,const char *passkey)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[28];
+	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, 28);
+        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, 28);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 28, 217);
+#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 into a message
+ * @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)
@@ -111,6 +122,20 @@ static inline uint16_t mavlink_msg_change_operator_control_encode(uint8_t system
 }
 
 /**
+ * @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
  *
@@ -124,19 +149,27 @@ static inline uint16_t mavlink_msg_change_operator_control_encode(uint8_t system
 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[28];
+	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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, buf, 28, 217);
+#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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, (const char *)&packet, 28, 217);
+#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
 }
 
@@ -199,6 +232,6 @@ static inline void mavlink_msg_change_operator_control_decode(const mavlink_mess
 	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), 28);
+	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
index 1d89a0f78..c6f6a28e4 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h
@@ -12,6 +12,9 @@ typedef struct __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 { \
@@ -39,30 +42,34 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_pack(uint8_t syst
 						       uint8_t gcs_system_id, uint8_t control_request, uint8_t ack)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[3];
+	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, 3);
+        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, 3);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 3, 104);
+#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 was sent over
+ * @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
@@ -74,27 +81,31 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_pack_chan(uint8_t
 						           uint8_t gcs_system_id,uint8_t control_request,uint8_t ack)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[3];
+	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, 3);
+        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, 3);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3, 104);
+#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 into a message
+ * @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)
@@ -107,6 +118,20 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_encode(uint8_t sy
 }
 
 /**
+ * @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
  *
@@ -119,19 +144,27 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_encode(uint8_t sy
 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[3];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, buf, 3, 104);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, (const char *)&packet, 3, 104);
+#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
 }
 
@@ -183,6 +216,6 @@ static inline void mavlink_msg_change_operator_control_ack_decode(const mavlink_
 	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), 3);
+	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
index df6e9b9e3..dca2fe681 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h
@@ -11,6 +11,9 @@ typedef struct __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 { \
@@ -36,28 +39,32 @@ static inline uint16_t mavlink_msg_command_ack_pack(uint8_t system_id, uint8_t c
 						       uint16_t command, uint8_t result)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[3];
+	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, 3);
+        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, 3);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK;
-	return mavlink_finalize_message(msg, system_id, component_id, 3, 143);
+#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 was sent over
+ * @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
@@ -68,25 +75,29 @@ static inline uint16_t mavlink_msg_command_ack_pack_chan(uint8_t system_id, uint
 						           uint16_t command,uint8_t result)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[3];
+	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, 3);
+        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, 3);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3, 143);
+#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 into a message
+ * @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)
@@ -99,6 +110,20 @@ static inline uint16_t mavlink_msg_command_ack_encode(uint8_t system_id, uint8_t
 }
 
 /**
+ * @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
  *
@@ -110,17 +135,25 @@ static inline uint16_t mavlink_msg_command_ack_encode(uint8_t system_id, uint8_t
 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[3];
+	char buf[MAVLINK_MSG_ID_COMMAND_ACK_LEN];
 	_mav_put_uint16_t(buf, 0, command);
 	_mav_put_uint8_t(buf, 2, result);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, buf, 3, 143);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, (const char *)&packet, 3, 143);
+#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
 }
 
@@ -161,6 +194,6 @@ static inline void mavlink_msg_command_ack_decode(const mavlink_message_t* msg,
 	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), 3);
+	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
index 54ca77eaa..8f705c0dd 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h
@@ -20,6 +20,9 @@ typedef struct __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 { \
@@ -63,7 +66,7 @@ static inline uint16_t mavlink_msg_command_long_pack(uint8_t system_id, uint8_t
 						       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[33];
+	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);
@@ -76,7 +79,7 @@ static inline uint16_t mavlink_msg_command_long_pack(uint8_t system_id, uint8_t
 	_mav_put_uint8_t(buf, 31, target_component);
 	_mav_put_uint8_t(buf, 32, confirmation);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 33);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMMAND_LONG_LEN);
 #else
 	mavlink_command_long_t packet;
 	packet.param1 = param1;
@@ -91,18 +94,22 @@ static inline uint16_t mavlink_msg_command_long_pack(uint8_t system_id, uint8_t
 	packet.target_component = target_component;
 	packet.confirmation = confirmation;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 33);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_LONG_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_COMMAND_LONG;
-	return mavlink_finalize_message(msg, system_id, component_id, 33, 152);
+#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 was sent over
+ * @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
@@ -122,7 +129,7 @@ static inline uint16_t mavlink_msg_command_long_pack_chan(uint8_t system_id, uin
 						           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[33];
+	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);
@@ -135,7 +142,7 @@ static inline uint16_t mavlink_msg_command_long_pack_chan(uint8_t system_id, uin
 	_mav_put_uint8_t(buf, 31, target_component);
 	_mav_put_uint8_t(buf, 32, confirmation);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 33);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMMAND_LONG_LEN);
 #else
 	mavlink_command_long_t packet;
 	packet.param1 = param1;
@@ -150,15 +157,19 @@ static inline uint16_t mavlink_msg_command_long_pack_chan(uint8_t system_id, uin
 	packet.target_component = target_component;
 	packet.confirmation = confirmation;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 33);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_LONG_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_COMMAND_LONG;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 33, 152);
+#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 into a message
+ * @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)
@@ -171,6 +182,20 @@ static inline uint16_t mavlink_msg_command_long_encode(uint8_t system_id, uint8_
 }
 
 /**
+ * @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
  *
@@ -191,7 +216,7 @@ static inline uint16_t mavlink_msg_command_long_encode(uint8_t system_id, uint8_
 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[33];
+	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);
@@ -204,7 +229,11 @@ static inline void mavlink_msg_command_long_send(mavlink_channel_t chan, uint8_t
 	_mav_put_uint8_t(buf, 31, target_component);
 	_mav_put_uint8_t(buf, 32, confirmation);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, buf, 33, 152);
+#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;
@@ -219,7 +248,11 @@ static inline void mavlink_msg_command_long_send(mavlink_channel_t chan, uint8_t
 	packet.target_component = target_component;
 	packet.confirmation = confirmation;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, (const char *)&packet, 33, 152);
+#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
 }
 
@@ -359,6 +392,6 @@ static inline void mavlink_msg_command_long_decode(const mavlink_message_t* 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), 33);
+	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
index e5ec29045..dc0768e12 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h
@@ -12,6 +12,9 @@ typedef struct __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 { \
@@ -39,30 +42,34 @@ static inline uint16_t mavlink_msg_data_stream_pack(uint8_t system_id, uint8_t c
 						       uint8_t stream_id, uint16_t message_rate, uint8_t on_off)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[4];
+	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, 4);
+        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, 4);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA_STREAM_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_DATA_STREAM;
-	return mavlink_finalize_message(msg, system_id, component_id, 4, 21);
+#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 was sent over
+ * @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
@@ -74,27 +81,31 @@ static inline uint16_t mavlink_msg_data_stream_pack_chan(uint8_t system_id, uint
 						           uint8_t stream_id,uint16_t message_rate,uint8_t on_off)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[4];
+	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, 4);
+        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, 4);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA_STREAM_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_DATA_STREAM;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 21);
+#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 into a message
+ * @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)
@@ -107,6 +118,20 @@ static inline uint16_t mavlink_msg_data_stream_encode(uint8_t system_id, uint8_t
 }
 
 /**
+ * @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
  *
@@ -119,19 +144,27 @@ static inline uint16_t mavlink_msg_data_stream_encode(uint8_t system_id, uint8_t
 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[4];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, buf, 4, 21);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, (const char *)&packet, 4, 21);
+#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
 }
 
@@ -183,6 +216,6 @@ static inline void mavlink_msg_data_stream_decode(const mavlink_message_t* 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), 4);
+	memcpy(data_stream, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DATA_STREAM_LEN);
 #endif
 }
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_data_transmission_handshake.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_transmission_handshake.h
index d3ca73f12..fdd8fddd8 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_data_transmission_handshake.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_transmission_handshake.h
@@ -1,20 +1,23 @@
 // MESSAGE DATA_TRANSMISSION_HANDSHAKE PACKING
 
-#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE 193
+#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 packets; ///< number of packets beeing sent (set on ACK only)
  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 12
-#define MAVLINK_MSG_ID_193_LEN 12
+#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
 
 
 
@@ -24,10 +27,10 @@ typedef struct __mavlink_data_transmission_handshake_t
 	{  { "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) }, \
-         { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_data_transmission_handshake_t, type) }, \
-         { "packets", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_data_transmission_handshake_t, packets) }, \
-         { "payload", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_data_transmission_handshake_t, payload) }, \
-         { "jpg_quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_data_transmission_handshake_t, jpg_quality) }, \
+         { "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) }, \
          } \
 }
 
@@ -48,41 +51,45 @@ typedef struct __mavlink_data_transmission_handshake_t
  * @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, uint8_t packets, uint8_t payload, uint8_t jpg_quality)
+						       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[12];
+	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_uint8_t(buf, 8, type);
-	_mav_put_uint8_t(buf, 9, packets);
-	_mav_put_uint8_t(buf, 10, payload);
-	_mav_put_uint8_t(buf, 11, jpg_quality);
+	_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, 12);
+        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.type = type;
 	packet.packets = packets;
+	packet.type = type;
 	packet.payload = payload;
 	packet.jpg_quality = jpg_quality;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE;
-	return mavlink_finalize_message(msg, system_id, component_id, 12, 23);
+#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 was sent over
+ * @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)
@@ -95,38 +102,42 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_pack(uint8_t syst
  */
 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,uint8_t packets,uint8_t payload,uint8_t jpg_quality)
+						           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[12];
+	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_uint8_t(buf, 8, type);
-	_mav_put_uint8_t(buf, 9, packets);
-	_mav_put_uint8_t(buf, 10, payload);
-	_mav_put_uint8_t(buf, 11, jpg_quality);
+	_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, 12);
+        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.type = type;
 	packet.packets = packets;
+	packet.type = type;
 	packet.payload = payload;
 	packet.jpg_quality = jpg_quality;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 23);
+#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 into a message
+ * @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)
@@ -139,6 +150,20 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_encode(uint8_t sy
 }
 
 /**
+ * @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
  *
@@ -152,30 +177,38 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_encode(uint8_t sy
  */
 #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, uint8_t packets, uint8_t payload, uint8_t jpg_quality)
+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[12];
+	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_uint8_t(buf, 8, type);
-	_mav_put_uint8_t(buf, 9, packets);
-	_mav_put_uint8_t(buf, 10, payload);
-	_mav_put_uint8_t(buf, 11, jpg_quality);
+	_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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, buf, 12, 23);
+#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.type = type;
 	packet.packets = packets;
+	packet.type = type;
 	packet.payload = payload;
 	packet.jpg_quality = jpg_quality;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, (const char *)&packet, 12, 23);
+#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
 }
 
@@ -191,7 +224,7 @@ static inline void mavlink_msg_data_transmission_handshake_send(mavlink_channel_
  */
 static inline uint8_t mavlink_msg_data_transmission_handshake_get_type(const mavlink_message_t* msg)
 {
-	return _MAV_RETURN_uint8_t(msg,  8);
+	return _MAV_RETURN_uint8_t(msg,  10);
 }
 
 /**
@@ -229,9 +262,9 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_get_height(const
  *
  * @return number of packets beeing sent (set on ACK only)
  */
-static inline uint8_t mavlink_msg_data_transmission_handshake_get_packets(const mavlink_message_t* msg)
+static inline uint16_t mavlink_msg_data_transmission_handshake_get_packets(const mavlink_message_t* msg)
 {
-	return _MAV_RETURN_uint8_t(msg,  9);
+	return _MAV_RETURN_uint16_t(msg,  8);
 }
 
 /**
@@ -241,7 +274,7 @@ static inline uint8_t mavlink_msg_data_transmission_handshake_get_packets(const
  */
 static inline uint8_t mavlink_msg_data_transmission_handshake_get_payload(const mavlink_message_t* msg)
 {
-	return _MAV_RETURN_uint8_t(msg,  10);
+	return _MAV_RETURN_uint8_t(msg,  11);
 }
 
 /**
@@ -251,7 +284,7 @@ static inline uint8_t mavlink_msg_data_transmission_handshake_get_payload(const
  */
 static inline uint8_t mavlink_msg_data_transmission_handshake_get_jpg_quality(const mavlink_message_t* msg)
 {
-	return _MAV_RETURN_uint8_t(msg,  11);
+	return _MAV_RETURN_uint8_t(msg,  12);
 }
 
 /**
@@ -266,11 +299,11 @@ static inline void mavlink_msg_data_transmission_handshake_decode(const mavlink_
 	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->type = mavlink_msg_data_transmission_handshake_get_type(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), 12);
+	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
index 5ff88e6a8..9a6ed87ee 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h
@@ -12,6 +12,9 @@ typedef struct __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 { \
@@ -39,30 +42,34 @@ static inline uint16_t mavlink_msg_debug_pack(uint8_t system_id, uint8_t compone
 						       uint32_t time_boot_ms, uint8_t ind, float value)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[9];
+	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, 9);
+        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, 9);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEBUG_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_DEBUG;
-	return mavlink_finalize_message(msg, system_id, component_id, 9, 46);
+#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 was sent over
+ * @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
@@ -74,27 +81,31 @@ static inline uint16_t mavlink_msg_debug_pack_chan(uint8_t system_id, uint8_t co
 						           uint32_t time_boot_ms,uint8_t ind,float value)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[9];
+	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, 9);
+        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, 9);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEBUG_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_DEBUG;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 9, 46);
+#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 into a message
+ * @brief Encode a debug struct
  *
  * @param system_id ID of this system
  * @param component_id ID of this component (e.g. 200 for IMU)
@@ -107,6 +118,20 @@ static inline uint16_t mavlink_msg_debug_encode(uint8_t system_id, uint8_t compo
 }
 
 /**
+ * @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
  *
@@ -119,19 +144,27 @@ static inline uint16_t mavlink_msg_debug_encode(uint8_t system_id, uint8_t compo
 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[9];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, buf, 9, 46);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, (const char *)&packet, 9, 46);
+#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
 }
 
@@ -183,6 +216,6 @@ static inline void mavlink_msg_debug_decode(const mavlink_message_t* msg, mavlin
 	debug->value = mavlink_msg_debug_get_value(msg);
 	debug->ind = mavlink_msg_debug_get_ind(msg);
 #else
-	memcpy(debug, _MAV_PAYLOAD(msg), 9);
+	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
index 0b443a061..6cfc75212 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h
@@ -14,6 +14,9 @@ typedef struct __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 { \
@@ -45,13 +48,13 @@ static inline uint16_t mavlink_msg_debug_vect_pack(uint8_t system_id, uint8_t co
 						       const char *name, uint64_t time_usec, float x, float y, float z)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[30];
+	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, 30);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DEBUG_VECT_LEN);
 #else
 	mavlink_debug_vect_t packet;
 	packet.time_usec = time_usec;
@@ -59,18 +62,22 @@ static inline uint16_t mavlink_msg_debug_vect_pack(uint8_t system_id, uint8_t co
 	packet.y = y;
 	packet.z = z;
 	mav_array_memcpy(packet.name, name, sizeof(char)*10);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 30);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEBUG_VECT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT;
-	return mavlink_finalize_message(msg, system_id, component_id, 30, 49);
+#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 was sent over
+ * @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
@@ -84,13 +91,13 @@ static inline uint16_t mavlink_msg_debug_vect_pack_chan(uint8_t system_id, uint8
 						           const char *name,uint64_t time_usec,float x,float y,float z)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[30];
+	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, 30);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DEBUG_VECT_LEN);
 #else
 	mavlink_debug_vect_t packet;
 	packet.time_usec = time_usec;
@@ -98,15 +105,19 @@ static inline uint16_t mavlink_msg_debug_vect_pack_chan(uint8_t system_id, uint8
 	packet.y = y;
 	packet.z = z;
 	mav_array_memcpy(packet.name, name, sizeof(char)*10);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 30);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEBUG_VECT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 30, 49);
+#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 into a message
+ * @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)
@@ -119,6 +130,20 @@ static inline uint16_t mavlink_msg_debug_vect_encode(uint8_t system_id, uint8_t
 }
 
 /**
+ * @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
  *
@@ -133,13 +158,17 @@ static inline uint16_t mavlink_msg_debug_vect_encode(uint8_t system_id, uint8_t
 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[30];
+	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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, buf, 30, 49);
+#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;
@@ -147,7 +176,11 @@ static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const cha
 	packet.y = y;
 	packet.z = z;
 	mav_array_memcpy(packet.name, name, sizeof(char)*10);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, (const char *)&packet, 30, 49);
+#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
 }
 
@@ -221,6 +254,6 @@ static inline void mavlink_msg_debug_vect_decode(const mavlink_message_t* msg, m
 	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), 30);
+	memcpy(debug_vect, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DEBUG_VECT_LEN);
 #endif
 }
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_encapsulated_data.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h
index e07be2952..5900ea838 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_encapsulated_data.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h
@@ -1,6 +1,6 @@
 // MESSAGE ENCAPSULATED_DATA PACKING
 
-#define MAVLINK_MSG_ID_ENCAPSULATED_DATA 194
+#define MAVLINK_MSG_ID_ENCAPSULATED_DATA 131
 
 typedef struct __mavlink_encapsulated_data_t
 {
@@ -9,7 +9,10 @@ typedef struct __mavlink_encapsulated_data_t
 } mavlink_encapsulated_data_t;
 
 #define MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN 255
-#define MAVLINK_MSG_ID_194_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
 
@@ -36,26 +39,30 @@ static inline uint16_t mavlink_msg_encapsulated_data_pack(uint8_t system_id, uin
 						       uint16_t seqnr, const uint8_t *data)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[255];
+	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, 255);
+        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, 255);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA;
-	return mavlink_finalize_message(msg, system_id, component_id, 255, 223);
+#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 was sent over
+ * @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
@@ -66,23 +73,27 @@ static inline uint16_t mavlink_msg_encapsulated_data_pack_chan(uint8_t system_id
 						           uint16_t seqnr,const uint8_t *data)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[255];
+	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, 255);
+        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, 255);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 255, 223);
+#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 into a message
+ * @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)
@@ -95,6 +106,20 @@ static inline uint16_t mavlink_msg_encapsulated_data_encode(uint8_t system_id, u
 }
 
 /**
+ * @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
  *
@@ -106,15 +131,23 @@ static inline uint16_t mavlink_msg_encapsulated_data_encode(uint8_t system_id, u
 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[255];
+	char buf[MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN];
 	_mav_put_uint16_t(buf, 0, seqnr);
 	_mav_put_uint8_t_array(buf, 2, data, 253);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, buf, 255, 223);
+#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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, (const char *)&packet, 255, 223);
+#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
 }
 
@@ -155,6 +188,6 @@ static inline void mavlink_msg_encapsulated_data_decode(const mavlink_message_t*
 	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), 255);
+	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
index 27f5a91db..4f31698d5 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h
@@ -12,6 +12,9 @@ typedef struct __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 { \
@@ -39,28 +42,32 @@ static inline uint16_t mavlink_msg_file_transfer_dir_list_pack(uint8_t system_id
 						       uint64_t transfer_uid, const char *dir_path, uint8_t flags)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[249];
+	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, 249);
+        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, 249);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 249, 93);
+#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 was sent over
+ * @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
@@ -72,25 +79,29 @@ static inline uint16_t mavlink_msg_file_transfer_dir_list_pack_chan(uint8_t syst
 						           uint64_t transfer_uid,const char *dir_path,uint8_t flags)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[249];
+	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, 249);
+        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, 249);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 249, 93);
+#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 into a message
+ * @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)
@@ -103,6 +114,20 @@ static inline uint16_t mavlink_msg_file_transfer_dir_list_encode(uint8_t system_
 }
 
 /**
+ * @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
  *
@@ -115,17 +140,25 @@ static inline uint16_t mavlink_msg_file_transfer_dir_list_encode(uint8_t system_
 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[249];
+	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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST, buf, 249, 93);
+#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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST, (const char *)&packet, 249, 93);
+#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
 }
 
@@ -177,6 +210,6 @@ static inline void mavlink_msg_file_transfer_dir_list_decode(const mavlink_messa
 	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), 249);
+	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
index f7035ec9e..fc6247fac 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h
@@ -11,6 +11,9 @@ typedef struct __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 { \
@@ -36,28 +39,32 @@ static inline uint16_t mavlink_msg_file_transfer_res_pack(uint8_t system_id, uin
 						       uint64_t transfer_uid, uint8_t result)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[9];
+	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, 9);
+        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, 9);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_FILE_TRANSFER_RES;
-	return mavlink_finalize_message(msg, system_id, component_id, 9, 124);
+#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 was sent over
+ * @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
@@ -68,25 +75,29 @@ static inline uint16_t mavlink_msg_file_transfer_res_pack_chan(uint8_t system_id
 						           uint64_t transfer_uid,uint8_t result)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[9];
+	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, 9);
+        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, 9);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_FILE_TRANSFER_RES;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 9, 124);
+#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 into a message
+ * @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)
@@ -99,6 +110,20 @@ static inline uint16_t mavlink_msg_file_transfer_res_encode(uint8_t system_id, u
 }
 
 /**
+ * @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
  *
@@ -110,17 +135,25 @@ static inline uint16_t mavlink_msg_file_transfer_res_encode(uint8_t system_id, u
 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[9];
+	char buf[MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN];
 	_mav_put_uint64_t(buf, 0, transfer_uid);
 	_mav_put_uint8_t(buf, 8, result);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES, buf, 9, 124);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES, (const char *)&packet, 9, 124);
+#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
 }
 
@@ -161,6 +194,6 @@ static inline void mavlink_msg_file_transfer_res_decode(const mavlink_message_t*
 	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), 9);
+	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
index 5eaa9b220..05be77339 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h
@@ -14,6 +14,9 @@ typedef struct __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 { \
@@ -45,13 +48,13 @@ static inline uint16_t mavlink_msg_file_transfer_start_pack(uint8_t system_id, u
 						       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[254];
+	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, 254);
+        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;
@@ -59,18 +62,22 @@ static inline uint16_t mavlink_msg_file_transfer_start_pack(uint8_t system_id, u
 	packet.direction = direction;
 	packet.flags = flags;
 	mav_array_memcpy(packet.dest_path, dest_path, sizeof(char)*240);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 254);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_FILE_TRANSFER_START;
-	return mavlink_finalize_message(msg, system_id, component_id, 254, 235);
+#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 was sent over
+ * @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
@@ -84,13 +91,13 @@ static inline uint16_t mavlink_msg_file_transfer_start_pack_chan(uint8_t system_
 						           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[254];
+	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, 254);
+        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;
@@ -98,15 +105,19 @@ static inline uint16_t mavlink_msg_file_transfer_start_pack_chan(uint8_t system_
 	packet.direction = direction;
 	packet.flags = flags;
 	mav_array_memcpy(packet.dest_path, dest_path, sizeof(char)*240);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 254);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_FILE_TRANSFER_START;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 254, 235);
+#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 into a message
+ * @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)
@@ -119,6 +130,20 @@ static inline uint16_t mavlink_msg_file_transfer_start_encode(uint8_t system_id,
 }
 
 /**
+ * @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
  *
@@ -133,13 +158,17 @@ static inline uint16_t mavlink_msg_file_transfer_start_encode(uint8_t system_id,
 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[254];
+	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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_START, buf, 254, 235);
+#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;
@@ -147,7 +176,11 @@ static inline void mavlink_msg_file_transfer_start_send(mavlink_channel_t chan,
 	packet.direction = direction;
 	packet.flags = flags;
 	mav_array_memcpy(packet.dest_path, dest_path, sizeof(char)*240);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_START, (const char *)&packet, 254, 235);
+#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
 }
 
@@ -221,6 +254,6 @@ static inline void mavlink_msg_file_transfer_start_decode(const mavlink_message_
 	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), 254);
+	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
index 780f562c5..7ed3d2a63 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h
@@ -12,12 +12,15 @@ typedef struct __mavlink_global_position_int_t
  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: 65535
+ 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 { \
@@ -50,14 +53,14 @@ typedef struct __mavlink_global_position_int_t
  * @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: 65535
+ * @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[28];
+	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);
@@ -68,7 +71,7 @@ static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, u
 	_mav_put_int16_t(buf, 24, vz);
 	_mav_put_uint16_t(buf, 26, hdg);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 28);
+        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;
@@ -81,18 +84,22 @@ static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, u
 	packet.vz = vz;
 	packet.hdg = hdg;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 28);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT;
-	return mavlink_finalize_message(msg, system_id, component_id, 28, 104);
+#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 was sent over
+ * @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
@@ -102,7 +109,7 @@ static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, u
  * @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: 65535
+ * @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,
@@ -110,7 +117,7 @@ static inline uint16_t mavlink_msg_global_position_int_pack_chan(uint8_t system_
 						           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[28];
+	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);
@@ -121,7 +128,7 @@ static inline uint16_t mavlink_msg_global_position_int_pack_chan(uint8_t system_
 	_mav_put_int16_t(buf, 24, vz);
 	_mav_put_uint16_t(buf, 26, hdg);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 28);
+        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;
@@ -134,15 +141,19 @@ static inline uint16_t mavlink_msg_global_position_int_pack_chan(uint8_t system_
 	packet.vz = vz;
 	packet.hdg = hdg;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 28);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 28, 104);
+#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 into a message
+ * @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)
@@ -155,6 +166,20 @@ static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id,
 }
 
 /**
+ * @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
  *
@@ -166,14 +191,14 @@ static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id,
  * @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: 65535
+ * @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[28];
+	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);
@@ -184,7 +209,11 @@ static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan,
 	_mav_put_int16_t(buf, 24, vz);
 	_mav_put_uint16_t(buf, 26, hdg);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, buf, 28, 104);
+#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;
@@ -197,7 +226,11 @@ static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan,
 	packet.vz = vz;
 	packet.hdg = hdg;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, (const char *)&packet, 28, 104);
+#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
 }
 
@@ -289,7 +322,7 @@ static inline int16_t mavlink_msg_global_position_int_get_vz(const mavlink_messa
 /**
  * @brief Get field hdg from global_position_int message
  *
- * @return Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535
+ * @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)
 {
@@ -315,6 +348,6 @@ static inline void mavlink_msg_global_position_int_decode(const mavlink_message_
 	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), 28);
+	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
index 853b85dae..1a1c97199 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h
@@ -4,9 +4,9 @@
 
 typedef struct __mavlink_global_position_setpoint_int_t
 {
- int32_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 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;
@@ -14,6 +14,9 @@ typedef struct __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 { \
@@ -35,9 +38,9 @@ typedef struct __mavlink_global_position_setpoint_int_t
  * @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 WGS84 Latitude position in degrees * 1E7
- * @param longitude WGS84 Longitude position in degrees * 1E7
- * @param altitude WGS84 Altitude in meters * 1000 (positive for up)
+ * @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)
  */
@@ -45,14 +48,14 @@ static inline uint16_t mavlink_msg_global_position_setpoint_int_pack(uint8_t sys
 						       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[15];
+	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, 15);
+        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;
@@ -61,23 +64,27 @@ static inline uint16_t mavlink_msg_global_position_setpoint_int_pack(uint8_t sys
 	packet.yaw = yaw;
 	packet.coordinate_frame = coordinate_frame;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 15);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 15, 141);
+#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 was sent over
+ * @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 WGS84 Latitude position in degrees * 1E7
- * @param longitude WGS84 Longitude position in degrees * 1E7
- * @param altitude WGS84 Altitude in meters * 1000 (positive for up)
+ * @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)
  */
@@ -86,14 +93,14 @@ static inline uint16_t mavlink_msg_global_position_setpoint_int_pack_chan(uint8_
 						           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[15];
+	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, 15);
+        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;
@@ -102,15 +109,19 @@ static inline uint16_t mavlink_msg_global_position_setpoint_int_pack_chan(uint8_
 	packet.yaw = yaw;
 	packet.coordinate_frame = coordinate_frame;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 15);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 15, 141);
+#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 into a message
+ * @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)
@@ -123,13 +134,27 @@ static inline uint16_t mavlink_msg_global_position_setpoint_int_encode(uint8_t s
 }
 
 /**
+ * @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 WGS84 Latitude position in degrees * 1E7
- * @param longitude WGS84 Longitude position in degrees * 1E7
- * @param altitude WGS84 Altitude in meters * 1000 (positive for up)
+ * @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
@@ -137,14 +162,18 @@ static inline uint16_t mavlink_msg_global_position_setpoint_int_encode(uint8_t s
 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[15];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, buf, 15, 141);
+#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;
@@ -153,7 +182,11 @@ static inline void mavlink_msg_global_position_setpoint_int_send(mavlink_channel
 	packet.yaw = yaw;
 	packet.coordinate_frame = coordinate_frame;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, (const char *)&packet, 15, 141);
+#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
 }
 
@@ -175,7 +208,7 @@ static inline uint8_t mavlink_msg_global_position_setpoint_int_get_coordinate_fr
 /**
  * @brief Get field latitude from global_position_setpoint_int message
  *
- * @return WGS84 Latitude position in degrees * 1E7
+ * @return Latitude (WGS84), in degrees * 1E7
  */
 static inline int32_t mavlink_msg_global_position_setpoint_int_get_latitude(const mavlink_message_t* msg)
 {
@@ -185,7 +218,7 @@ static inline int32_t mavlink_msg_global_position_setpoint_int_get_latitude(cons
 /**
  * @brief Get field longitude from global_position_setpoint_int message
  *
- * @return WGS84 Longitude position in degrees * 1E7
+ * @return Longitude (WGS84), in degrees * 1E7
  */
 static inline int32_t mavlink_msg_global_position_setpoint_int_get_longitude(const mavlink_message_t* msg)
 {
@@ -195,7 +228,7 @@ static inline int32_t mavlink_msg_global_position_setpoint_int_get_longitude(con
 /**
  * @brief Get field altitude from global_position_setpoint_int message
  *
- * @return WGS84 Altitude in meters * 1000 (positive for up)
+ * @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)
 {
@@ -227,6 +260,6 @@ static inline void mavlink_msg_global_position_setpoint_int_decode(const mavlink
 	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), 15);
+	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
index a911fe25e..f7be74c91 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h
@@ -16,6 +16,9 @@ typedef struct __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 { \
@@ -51,7 +54,7 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack(uint8_t
 						       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[32];
+	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);
@@ -60,7 +63,7 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack(uint8_t
 	_mav_put_float(buf, 24, pitch);
 	_mav_put_float(buf, 28, yaw);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 32);
+        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;
@@ -71,18 +74,22 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack(uint8_t
 	packet.pitch = pitch;
 	packet.yaw = yaw;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 32);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 32, 102);
+#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 was sent over
+ * @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
@@ -98,7 +105,7 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack_chan(uin
 						           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[32];
+	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);
@@ -107,7 +114,7 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack_chan(uin
 	_mav_put_float(buf, 24, pitch);
 	_mav_put_float(buf, 28, yaw);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 32);
+        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;
@@ -118,15 +125,19 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack_chan(uin
 	packet.pitch = pitch;
 	packet.yaw = yaw;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 32);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 102);
+#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 into a message
+ * @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)
@@ -139,6 +150,20 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_encode(uint8_
 }
 
 /**
+ * @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
  *
@@ -155,7 +180,7 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_encode(uint8_
 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[32];
+	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);
@@ -164,7 +189,11 @@ static inline void mavlink_msg_global_vision_position_estimate_send(mavlink_chan
 	_mav_put_float(buf, 24, pitch);
 	_mav_put_float(buf, 28, yaw);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, buf, 32, 102);
+#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;
@@ -175,7 +204,11 @@ static inline void mavlink_msg_global_vision_position_estimate_send(mavlink_chan
 	packet.pitch = pitch;
 	packet.yaw = yaw;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, (const char *)&packet, 32, 102);
+#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
 }
 
@@ -271,6 +304,6 @@ static inline void mavlink_msg_global_vision_position_estimate_decode(const mavl
 	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), 32);
+	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
new file mode 100644
index 000000000..17e5bd002
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h
@@ -0,0 +1,419 @@
+// 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
index 2084718b5..016e9cb0e 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h
@@ -4,14 +4,17 @@
 
 typedef struct __mavlink_gps_global_origin_t
 {
- int32_t latitude; ///< Latitude (WGS84), expressed as * 1E7
- int32_t longitude; ///< Longitude (WGS84), expressed as * 1E7
- int32_t altitude; ///< Altitude(WGS84), expressed as * 1000
+ 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 { \
@@ -30,43 +33,47 @@ typedef struct __mavlink_gps_global_origin_t
  * @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), expressed as * 1E7
- * @param longitude Longitude (WGS84), expressed as * 1E7
- * @param altitude Altitude(WGS84), expressed as * 1000
+ * @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[12];
+	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, 12);
+        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, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN;
-	return mavlink_finalize_message(msg, system_id, component_id, 12, 39);
+#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 was sent over
+ * @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), expressed as * 1E7
- * @param longitude Longitude (WGS84), expressed as * 1E7
- * @param altitude Altitude(WGS84), expressed as * 1000
+ * @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,
@@ -74,27 +81,31 @@ static inline uint16_t mavlink_msg_gps_global_origin_pack_chan(uint8_t system_id
 						           int32_t latitude,int32_t longitude,int32_t altitude)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[12];
+	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, 12);
+        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, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 39);
+#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 into a message
+ * @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)
@@ -107,31 +118,53 @@ static inline uint16_t mavlink_msg_gps_global_origin_encode(uint8_t system_id, u
 }
 
 /**
+ * @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), expressed as * 1E7
- * @param longitude Longitude (WGS84), expressed as * 1E7
- * @param altitude Altitude(WGS84), expressed as * 1000
+ * @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[12];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, buf, 12, 39);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, (const char *)&packet, 12, 39);
+#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
 }
 
@@ -143,7 +176,7 @@ static inline void mavlink_msg_gps_global_origin_send(mavlink_channel_t chan, in
 /**
  * @brief Get field latitude from gps_global_origin message
  *
- * @return Latitude (WGS84), expressed as * 1E7
+ * @return Latitude (WGS84), in degrees * 1E7
  */
 static inline int32_t mavlink_msg_gps_global_origin_get_latitude(const mavlink_message_t* msg)
 {
@@ -153,7 +186,7 @@ static inline int32_t mavlink_msg_gps_global_origin_get_latitude(const mavlink_m
 /**
  * @brief Get field longitude from gps_global_origin message
  *
- * @return Longitude (WGS84), expressed as * 1E7
+ * @return Longitude (WGS84), in degrees * 1E7
  */
 static inline int32_t mavlink_msg_gps_global_origin_get_longitude(const mavlink_message_t* msg)
 {
@@ -163,7 +196,7 @@ static inline int32_t mavlink_msg_gps_global_origin_get_longitude(const mavlink_
 /**
  * @brief Get field altitude from gps_global_origin message
  *
- * @return Altitude(WGS84), expressed as * 1000
+ * @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)
 {
@@ -183,6 +216,6 @@ static inline void mavlink_msg_gps_global_origin_decode(const mavlink_message_t*
 	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), 12);
+	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
new file mode 100644
index 000000000..485d8a4af
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h
@@ -0,0 +1,237 @@
+// 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
index 57ec97376..a105f8cda 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h
@@ -5,13 +5,13 @@
 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 in 1E7 degrees
- int32_t lon; ///< Longitude in 1E7 degrees
- int32_t alt; ///< Altitude in 1E3 meters (millimeters) above MSL
- uint16_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
+ 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;
@@ -19,6 +19,9 @@ typedef struct __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 { \
@@ -46,13 +49,13 @@ typedef struct __mavlink_gps_raw_int_t
  *
  * @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 in 1E7 degrees
- * @param lon Longitude in 1E7 degrees
- * @param alt Altitude in 1E3 meters (millimeters) above MSL
- * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
- * @param epv GPS VDOP horizontal 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 cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535
+ * @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)
  */
@@ -60,7 +63,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t c
 						       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[30];
+	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);
@@ -72,7 +75,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t c
 	_mav_put_uint8_t(buf, 28, fix_type);
 	_mav_put_uint8_t(buf, 29, satellites_visible);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 30);
+        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;
@@ -86,28 +89,32 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t c
 	packet.fix_type = fix_type;
 	packet.satellites_visible = satellites_visible;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 30);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT;
-	return mavlink_finalize_message(msg, system_id, component_id, 30, 24);
+#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 was sent over
+ * @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 in 1E7 degrees
- * @param lon Longitude in 1E7 degrees
- * @param alt Altitude in 1E3 meters (millimeters) above MSL
- * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
- * @param epv GPS VDOP horizontal 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 cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535
+ * @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)
  */
@@ -116,7 +123,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint
 						           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[30];
+	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);
@@ -128,7 +135,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint
 	_mav_put_uint8_t(buf, 28, fix_type);
 	_mav_put_uint8_t(buf, 29, satellites_visible);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 30);
+        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;
@@ -142,15 +149,19 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint
 	packet.fix_type = fix_type;
 	packet.satellites_visible = satellites_visible;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 30);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 30, 24);
+#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 into a message
+ * @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)
@@ -163,18 +174,32 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t
 }
 
 /**
+ * @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 in 1E7 degrees
- * @param lon Longitude in 1E7 degrees
- * @param alt Altitude in 1E3 meters (millimeters) above MSL
- * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
- * @param epv GPS VDOP horizontal 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 cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535
+ * @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
@@ -182,7 +207,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t
 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[30];
+	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);
@@ -194,7 +219,11 @@ static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t
 	_mav_put_uint8_t(buf, 28, fix_type);
 	_mav_put_uint8_t(buf, 29, satellites_visible);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, buf, 30, 24);
+#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;
@@ -208,7 +237,11 @@ static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t
 	packet.fix_type = fix_type;
 	packet.satellites_visible = satellites_visible;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, (const char *)&packet, 30, 24);
+#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
 }
 
@@ -240,7 +273,7 @@ static inline uint8_t mavlink_msg_gps_raw_int_get_fix_type(const mavlink_message
 /**
  * @brief Get field lat from gps_raw_int message
  *
- * @return Latitude in 1E7 degrees
+ * @return Latitude (WGS84), in degrees * 1E7
  */
 static inline int32_t mavlink_msg_gps_raw_int_get_lat(const mavlink_message_t* msg)
 {
@@ -250,7 +283,7 @@ static inline int32_t mavlink_msg_gps_raw_int_get_lat(const mavlink_message_t* m
 /**
  * @brief Get field lon from gps_raw_int message
  *
- * @return Longitude in 1E7 degrees
+ * @return Longitude (WGS84), in degrees * 1E7
  */
 static inline int32_t mavlink_msg_gps_raw_int_get_lon(const mavlink_message_t* msg)
 {
@@ -260,7 +293,7 @@ static inline int32_t mavlink_msg_gps_raw_int_get_lon(const mavlink_message_t* m
 /**
  * @brief Get field alt from gps_raw_int message
  *
- * @return Altitude in 1E3 meters (millimeters) above MSL
+ * @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)
 {
@@ -270,7 +303,7 @@ static inline int32_t mavlink_msg_gps_raw_int_get_alt(const mavlink_message_t* m
 /**
  * @brief Get field eph from gps_raw_int message
  *
- * @return GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
+ * @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)
 {
@@ -280,7 +313,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_get_eph(const mavlink_message_t*
 /**
  * @brief Get field epv from gps_raw_int message
  *
- * @return GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
+ * @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)
 {
@@ -290,7 +323,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_get_epv(const mavlink_message_t*
 /**
  * @brief Get field vel from gps_raw_int message
  *
- * @return GPS ground speed (m/s * 100). If unknown, set to: 65535
+ * @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)
 {
@@ -300,7 +333,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_get_vel(const mavlink_message_t*
 /**
  * @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: 65535
+ * @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)
 {
@@ -337,6 +370,6 @@ static inline void mavlink_msg_gps_raw_int_decode(const mavlink_message_t* 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), 30);
+	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
index bd3257f88..719abcce8 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h
@@ -10,11 +10,23 @@ typedef struct __mavlink_gps_status_t
  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
+ uint8_t _field_len_satellite_prn;
+ uint8_t _field_len_satellite_used;
+ uint8_t _field_len_satellite_elevation;
+ uint8_t _field_len_satellite_azimuth;
+ uint8_t _field_len_satellite_snr;
 } mavlink_gps_status_t;
 
+#define MAVLINK_MSG_STATUSTEXT_VARLEN_BYTES 5
+
 #define MAVLINK_MSG_ID_GPS_STATUS_LEN 101
 #define MAVLINK_MSG_ID_25_LEN 101
 
+#define MAVLINK_MSG_ID_GPS_STATUS_FIXED_LEN 1
+
+#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
@@ -25,11 +37,16 @@ typedef struct __mavlink_gps_status_t
 	"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) }, \
+         { "satellite_prn", NULL, MAVLINK_TYPE_UINT8_T, -1, -1, -1 }, \
+         { "satellite_used", NULL, MAVLINK_TYPE_UINT8_T, -2, -2, -2 }, \
+         { "satellite_elevation", NULL, MAVLINK_TYPE_UINT8_T, -3, -3, -3 }, \
+         { "satellite_azimuth", NULL, MAVLINK_TYPE_UINT8_T, -4, -4, -4 }, \
+         { "satellite_snr", NULL, MAVLINK_TYPE_UINT8_T, -5, -5, -5 }, \
+         { "_field_len_satellite_prn", NULL, MAVLINK_TYPE_UINT8_T, 0, -1, 1 } \
+         { "_field_len_satellite_used", NULL, MAVLINK_TYPE_UINT8_T, 0, -2, 1 } \
+         { "_field_len_satellite_elevation", NULL, MAVLINK_TYPE_UINT8_T, 0, -3, 1 } \
+         { "_field_len_satellite_azimuth", NULL, MAVLINK_TYPE_UINT8_T, 0, -4, 1 } \
+         { "_field_len_satellite_snr", NULL, MAVLINK_TYPE_UINT8_T, 0, -5, 1 } \
          } \
 }
 
@@ -52,14 +69,14 @@ static inline uint16_t mavlink_msg_gps_status_pack(uint8_t system_id, uint8_t co
 						       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[101];
+	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, 101);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_STATUS_LEN);
 #else
 	mavlink_gps_status_t packet;
 	packet.satellites_visible = satellites_visible;
@@ -68,18 +85,22 @@ static inline uint16_t mavlink_msg_gps_status_pack(uint8_t system_id, uint8_t co
 	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, 101);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_STATUS_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_GPS_STATUS;
-	return mavlink_finalize_message(msg, system_id, component_id, 101, 23);
+#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 was sent over
+ * @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
@@ -94,14 +115,14 @@ static inline uint16_t mavlink_msg_gps_status_pack_chan(uint8_t system_id, uint8
 						           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[101];
+	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, 101);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_STATUS_LEN);
 #else
 	mavlink_gps_status_t packet;
 	packet.satellites_visible = satellites_visible;
@@ -110,15 +131,19 @@ static inline uint16_t mavlink_msg_gps_status_pack_chan(uint8_t system_id, uint8
 	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, 101);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_STATUS_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_GPS_STATUS;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 101, 23);
+#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 into a message
+ * @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)
@@ -131,6 +156,20 @@ static inline uint16_t mavlink_msg_gps_status_encode(uint8_t system_id, uint8_t
 }
 
 /**
+ * @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
  *
@@ -146,14 +185,18 @@ static inline uint16_t mavlink_msg_gps_status_encode(uint8_t system_id, uint8_t
 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[101];
+	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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, buf, 101, 23);
+#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;
@@ -162,7 +205,11 @@ static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t s
 	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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, (const char *)&packet, 101, 23);
+#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
 }
 
@@ -239,6 +286,19 @@ static inline uint16_t mavlink_msg_gps_status_get_satellite_snr(const mavlink_me
  */
 static inline void mavlink_msg_gps_status_decode(const mavlink_message_t* msg, mavlink_gps_status_t* gps_status)
 {
+	/* first get the length fields */
+	gps_status->_field_len_satellite_prn = _MAV_RETURN_uint8_t(msg, msg->len - MAVLINK_MSG_STATUSTEXT_VARLEN_BYTES + 0);
+	gps_status->_field_len_satellite_used = _MAV_RETURN_uint8_t(msg, msg->len - MAVLINK_MSG_STATUSTEXT_VARLEN_BYTES + 1);
+	gps_status->_field_len_satellite_elevation = _MAV_RETURN_uint8_t(msg, msg->len - MAVLINK_MSG_STATUSTEXT_VARLEN_BYTES + 2);
+	gps_status->_field_len_satellite_azimuth = _MAV_RETURN_uint8_t(msg, msg->len - MAVLINK_MSG_STATUSTEXT_VARLEN_BYTES + 3);
+	gps_status->_field_len_satellite_snr = _MAV_RETURN_uint8_t(msg, msg->len - MAVLINK_MSG_STATUSTEXT_VARLEN_BYTES + 4);
+
+	uint16_t varlengths = gps_status->_field_len_satellite_prn + 
+				gps_status->_field_len_satellite_used +
+				gps_status->_field_len_satellite_elevation +
+				gps_status->_field_len_satellite_azimuth +
+				gps_status->_field_len_satellite_snr;
+
 #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);
@@ -247,6 +307,9 @@ static inline void mavlink_msg_gps_status_decode(const mavlink_message_t* msg, m
 	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), 101);
+	memcpy(gps_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS_STATUS_FIXED_LEN);
 #endif
+
+	// xxx decode rest
+
 }
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h
index 599ea0bc5..826138fad 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h
@@ -15,6 +15,9 @@ typedef struct __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 { \
@@ -47,7 +50,7 @@ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t com
 						       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[9];
+	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);
@@ -55,7 +58,7 @@ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t com
 	_mav_put_uint8_t(buf, 7, system_status);
 	_mav_put_uint8_t(buf, 8, 3);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 9);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HEARTBEAT_LEN);
 #else
 	mavlink_heartbeat_t packet;
 	packet.custom_mode = custom_mode;
@@ -65,18 +68,22 @@ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t com
 	packet.system_status = system_status;
 	packet.mavlink_version = 3;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 9);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HEARTBEAT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_HEARTBEAT;
-	return mavlink_finalize_message(msg, system_id, component_id, 9, 50);
+#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 was sent over
+ * @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
@@ -90,7 +97,7 @@ static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_
 						           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[9];
+	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);
@@ -98,7 +105,7 @@ static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_
 	_mav_put_uint8_t(buf, 7, system_status);
 	_mav_put_uint8_t(buf, 8, 3);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 9);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HEARTBEAT_LEN);
 #else
 	mavlink_heartbeat_t packet;
 	packet.custom_mode = custom_mode;
@@ -108,15 +115,19 @@ static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_
 	packet.system_status = system_status;
 	packet.mavlink_version = 3;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 9);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HEARTBEAT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_HEARTBEAT;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 9, 50);
+#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 into a message
+ * @brief Encode a heartbeat struct
  *
  * @param system_id ID of this system
  * @param component_id ID of this component (e.g. 200 for IMU)
@@ -129,6 +140,20 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c
 }
 
 /**
+ * @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
  *
@@ -143,7 +168,7 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c
 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[9];
+	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);
@@ -151,7 +176,11 @@ static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t ty
 	_mav_put_uint8_t(buf, 7, system_status);
 	_mav_put_uint8_t(buf, 8, 3);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, 9, 50);
+#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;
@@ -161,7 +190,11 @@ static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t ty
 	packet.system_status = system_status;
 	packet.mavlink_version = 3;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)&packet, 9, 50);
+#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
 }
 
@@ -246,6 +279,6 @@ static inline void mavlink_msg_heartbeat_decode(const mavlink_message_t* msg, ma
 	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), 9);
+	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
index 9714c698f..0dcd95ed3 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h
@@ -24,6 +24,9 @@ typedef struct __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 { \
@@ -75,7 +78,7 @@ static inline uint16_t mavlink_msg_highres_imu_pack(uint8_t system_id, uint8_t c
 						       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[62];
+	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);
@@ -92,7 +95,7 @@ static inline uint16_t mavlink_msg_highres_imu_pack(uint8_t system_id, uint8_t c
 	_mav_put_float(buf, 56, temperature);
 	_mav_put_uint16_t(buf, 60, fields_updated);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 62);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN);
 #else
 	mavlink_highres_imu_t packet;
 	packet.time_usec = time_usec;
@@ -111,18 +114,22 @@ static inline uint16_t mavlink_msg_highres_imu_pack(uint8_t system_id, uint8_t c
 	packet.temperature = temperature;
 	packet.fields_updated = fields_updated;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 62);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_HIGHRES_IMU;
-	return mavlink_finalize_message(msg, system_id, component_id, 62, 93);
+#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 was sent over
+ * @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)
@@ -146,7 +153,7 @@ static inline uint16_t mavlink_msg_highres_imu_pack_chan(uint8_t system_id, uint
 						           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[62];
+	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);
@@ -163,7 +170,7 @@ static inline uint16_t mavlink_msg_highres_imu_pack_chan(uint8_t system_id, uint
 	_mav_put_float(buf, 56, temperature);
 	_mav_put_uint16_t(buf, 60, fields_updated);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 62);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN);
 #else
 	mavlink_highres_imu_t packet;
 	packet.time_usec = time_usec;
@@ -182,15 +189,19 @@ static inline uint16_t mavlink_msg_highres_imu_pack_chan(uint8_t system_id, uint
 	packet.temperature = temperature;
 	packet.fields_updated = fields_updated;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 62);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_HIGHRES_IMU;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 62, 93);
+#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 into a message
+ * @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)
@@ -203,6 +214,20 @@ static inline uint16_t mavlink_msg_highres_imu_encode(uint8_t system_id, uint8_t
 }
 
 /**
+ * @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
  *
@@ -227,7 +252,7 @@ static inline uint16_t mavlink_msg_highres_imu_encode(uint8_t system_id, uint8_t
 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[62];
+	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);
@@ -244,7 +269,11 @@ static inline void mavlink_msg_highres_imu_send(mavlink_channel_t chan, uint64_t
 	_mav_put_float(buf, 56, temperature);
 	_mav_put_uint16_t(buf, 60, fields_updated);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, buf, 62, 93);
+#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;
@@ -263,7 +292,11 @@ static inline void mavlink_msg_highres_imu_send(mavlink_channel_t chan, uint64_t
 	packet.temperature = temperature;
 	packet.fields_updated = fields_updated;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, (const char *)&packet, 62, 93);
+#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
 }
 
@@ -447,6 +480,6 @@ static inline void mavlink_msg_highres_imu_decode(const mavlink_message_t* 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), 62);
+	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
index 41a9bc949..aed5108d0 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h
@@ -20,6 +20,9 @@ typedef struct __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 { \
@@ -63,7 +66,7 @@ static inline uint16_t mavlink_msg_hil_controls_pack(uint8_t system_id, uint8_t
 						       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[42];
+	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);
@@ -76,7 +79,7 @@ static inline uint16_t mavlink_msg_hil_controls_pack(uint8_t system_id, uint8_t
 	_mav_put_uint8_t(buf, 40, mode);
 	_mav_put_uint8_t(buf, 41, nav_mode);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 42);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_CONTROLS_LEN);
 #else
 	mavlink_hil_controls_t packet;
 	packet.time_usec = time_usec;
@@ -91,18 +94,22 @@ static inline uint16_t mavlink_msg_hil_controls_pack(uint8_t system_id, uint8_t
 	packet.mode = mode;
 	packet.nav_mode = nav_mode;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 42);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_CONTROLS_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_HIL_CONTROLS;
-	return mavlink_finalize_message(msg, system_id, component_id, 42, 63);
+#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 was sent over
+ * @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
@@ -122,7 +129,7 @@ static inline uint16_t mavlink_msg_hil_controls_pack_chan(uint8_t system_id, uin
 						           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[42];
+	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);
@@ -135,7 +142,7 @@ static inline uint16_t mavlink_msg_hil_controls_pack_chan(uint8_t system_id, uin
 	_mav_put_uint8_t(buf, 40, mode);
 	_mav_put_uint8_t(buf, 41, nav_mode);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 42);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_CONTROLS_LEN);
 #else
 	mavlink_hil_controls_t packet;
 	packet.time_usec = time_usec;
@@ -150,15 +157,19 @@ static inline uint16_t mavlink_msg_hil_controls_pack_chan(uint8_t system_id, uin
 	packet.mode = mode;
 	packet.nav_mode = nav_mode;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 42);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_CONTROLS_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_HIL_CONTROLS;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 42, 63);
+#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 into a message
+ * @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)
@@ -171,6 +182,20 @@ static inline uint16_t mavlink_msg_hil_controls_encode(uint8_t system_id, uint8_
 }
 
 /**
+ * @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
  *
@@ -191,7 +216,7 @@ static inline uint16_t mavlink_msg_hil_controls_encode(uint8_t system_id, uint8_
 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[42];
+	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);
@@ -204,7 +229,11 @@ static inline void mavlink_msg_hil_controls_send(mavlink_channel_t chan, uint64_
 	_mav_put_uint8_t(buf, 40, mode);
 	_mav_put_uint8_t(buf, 41, nav_mode);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, buf, 42, 63);
+#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;
@@ -219,7 +248,11 @@ static inline void mavlink_msg_hil_controls_send(mavlink_channel_t chan, uint64_
 	packet.mode = mode;
 	packet.nav_mode = nav_mode;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, (const char *)&packet, 42, 63);
+#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
 }
 
@@ -359,6 +392,6 @@ static inline void mavlink_msg_hil_controls_decode(const mavlink_message_t* 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), 42);
+	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
new file mode 100644
index 000000000..91aec5b08
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h
@@ -0,0 +1,441 @@
+// 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
new file mode 100644
index 000000000..acb1392e1
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h
@@ -0,0 +1,331 @@
+// 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
index 7ac0853d3..a42bde50b 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h
@@ -23,6 +23,9 @@ typedef struct __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 { \
@@ -72,7 +75,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack(uint8_t system_id, uin
 						       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[33];
+	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);
@@ -88,7 +91,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack(uint8_t system_id, uin
 	_mav_put_uint16_t(buf, 30, chan12_raw);
 	_mav_put_uint8_t(buf, 32, rssi);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 33);
+        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;
@@ -106,18 +109,22 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack(uint8_t system_id, uin
 	packet.chan12_raw = chan12_raw;
 	packet.rssi = rssi;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 33);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 33, 54);
+#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 was sent over
+ * @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
@@ -140,7 +147,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack_chan(uint8_t system_id
 						           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[33];
+	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);
@@ -156,7 +163,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack_chan(uint8_t system_id
 	_mav_put_uint16_t(buf, 30, chan12_raw);
 	_mav_put_uint8_t(buf, 32, rssi);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 33);
+        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;
@@ -174,15 +181,19 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack_chan(uint8_t system_id
 	packet.chan12_raw = chan12_raw;
 	packet.rssi = rssi;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 33);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 33, 54);
+#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 into a message
+ * @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)
@@ -195,6 +206,20 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_encode(uint8_t system_id, u
 }
 
 /**
+ * @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
  *
@@ -218,7 +243,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_encode(uint8_t system_id, u
 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[33];
+	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);
@@ -234,7 +259,11 @@ static inline void mavlink_msg_hil_rc_inputs_raw_send(mavlink_channel_t chan, ui
 	_mav_put_uint16_t(buf, 30, chan12_raw);
 	_mav_put_uint8_t(buf, 32, rssi);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, buf, 33, 54);
+#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;
@@ -252,7 +281,11 @@ static inline void mavlink_msg_hil_rc_inputs_raw_send(mavlink_channel_t chan, ui
 	packet.chan12_raw = chan12_raw;
 	packet.rssi = rssi;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, (const char *)&packet, 33, 54);
+#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
 }
 
@@ -425,6 +458,6 @@ static inline void mavlink_msg_hil_rc_inputs_raw_decode(const mavlink_message_t*
 	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), 33);
+	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
new file mode 100644
index 000000000..6c2667473
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h
@@ -0,0 +1,485 @@
+// 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
index 144781295..bcc857767 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h
@@ -8,9 +8,9 @@ typedef struct __mavlink_hil_state_t
  float 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 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)
@@ -25,6 +25,9 @@ typedef struct __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 { \
@@ -60,9 +63,9 @@ typedef struct __mavlink_hil_state_t
  * @param roll Roll angle (rad)
  * @param pitch Pitch angle (rad)
  * @param yaw Yaw angle (rad)
- * @param rollspeed Roll angular speed (rad/s)
- * @param pitchspeed Pitch angular speed (rad/s)
- * @param yawspeed Yaw angular speed (rad/s)
+ * @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)
@@ -78,7 +81,7 @@ static inline uint16_t mavlink_msg_hil_state_pack(uint8_t system_id, uint8_t com
 						       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[56];
+	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);
@@ -96,7 +99,7 @@ static inline uint16_t mavlink_msg_hil_state_pack(uint8_t system_id, uint8_t com
 	_mav_put_int16_t(buf, 52, yacc);
 	_mav_put_int16_t(buf, 54, zacc);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 56);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_STATE_LEN);
 #else
 	mavlink_hil_state_t packet;
 	packet.time_usec = time_usec;
@@ -116,26 +119,30 @@ static inline uint16_t mavlink_msg_hil_state_pack(uint8_t system_id, uint8_t com
 	packet.yacc = yacc;
 	packet.zacc = zacc;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 56);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_STATE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_HIL_STATE;
-	return mavlink_finalize_message(msg, system_id, component_id, 56, 183);
+#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 was sent over
+ * @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 Roll angular speed (rad/s)
- * @param pitchspeed Pitch angular speed (rad/s)
- * @param yawspeed Yaw angular speed (rad/s)
+ * @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)
@@ -152,7 +159,7 @@ static inline uint16_t mavlink_msg_hil_state_pack_chan(uint8_t system_id, uint8_
 						           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[56];
+	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);
@@ -170,7 +177,7 @@ static inline uint16_t mavlink_msg_hil_state_pack_chan(uint8_t system_id, uint8_
 	_mav_put_int16_t(buf, 52, yacc);
 	_mav_put_int16_t(buf, 54, zacc);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 56);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_STATE_LEN);
 #else
 	mavlink_hil_state_t packet;
 	packet.time_usec = time_usec;
@@ -190,15 +197,19 @@ static inline uint16_t mavlink_msg_hil_state_pack_chan(uint8_t system_id, uint8_
 	packet.yacc = yacc;
 	packet.zacc = zacc;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 56);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_STATE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_HIL_STATE;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 56, 183);
+#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 into a message
+ * @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)
@@ -211,6 +222,20 @@ static inline uint16_t mavlink_msg_hil_state_encode(uint8_t system_id, uint8_t c
 }
 
 /**
+ * @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
  *
@@ -218,9 +243,9 @@ static inline uint16_t mavlink_msg_hil_state_encode(uint8_t system_id, uint8_t c
  * @param roll Roll angle (rad)
  * @param pitch Pitch angle (rad)
  * @param yaw Yaw angle (rad)
- * @param rollspeed Roll angular speed (rad/s)
- * @param pitchspeed Pitch angular speed (rad/s)
- * @param yawspeed Yaw angular speed (rad/s)
+ * @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)
@@ -236,7 +261,7 @@ static inline uint16_t mavlink_msg_hil_state_encode(uint8_t system_id, uint8_t c
 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[56];
+	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);
@@ -254,7 +279,11 @@ static inline void mavlink_msg_hil_state_send(mavlink_channel_t chan, uint64_t t
 	_mav_put_int16_t(buf, 52, yacc);
 	_mav_put_int16_t(buf, 54, zacc);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, buf, 56, 183);
+#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;
@@ -274,7 +303,11 @@ static inline void mavlink_msg_hil_state_send(mavlink_channel_t chan, uint64_t t
 	packet.yacc = yacc;
 	packet.zacc = zacc;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, (const char *)&packet, 56, 183);
+#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
 }
 
@@ -326,7 +359,7 @@ static inline float mavlink_msg_hil_state_get_yaw(const mavlink_message_t* msg)
 /**
  * @brief Get field rollspeed from hil_state message
  *
- * @return Roll angular speed (rad/s)
+ * @return Body frame roll / phi angular speed (rad/s)
  */
 static inline float mavlink_msg_hil_state_get_rollspeed(const mavlink_message_t* msg)
 {
@@ -336,7 +369,7 @@ static inline float mavlink_msg_hil_state_get_rollspeed(const mavlink_message_t*
 /**
  * @brief Get field pitchspeed from hil_state message
  *
- * @return Pitch angular speed (rad/s)
+ * @return Body frame pitch / theta angular speed (rad/s)
  */
 static inline float mavlink_msg_hil_state_get_pitchspeed(const mavlink_message_t* msg)
 {
@@ -346,7 +379,7 @@ static inline float mavlink_msg_hil_state_get_pitchspeed(const mavlink_message_t
 /**
  * @brief Get field yawspeed from hil_state message
  *
- * @return Yaw angular speed (rad/s)
+ * @return Body frame yaw / psi angular speed (rad/s)
  */
 static inline float mavlink_msg_hil_state_get_yawspeed(const mavlink_message_t* msg)
 {
@@ -469,6 +502,6 @@ static inline void mavlink_msg_hil_state_decode(const mavlink_message_t* msg, ma
 	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), 56);
+	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
new file mode 100644
index 000000000..732176193
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h
@@ -0,0 +1,501 @@
+// 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
index fe0a791fc..a0b72c0e1 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h
@@ -16,6 +16,9 @@ typedef struct __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 { \
@@ -51,7 +54,7 @@ static inline uint16_t mavlink_msg_local_position_ned_pack(uint8_t system_id, ui
 						       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[28];
+	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);
@@ -60,7 +63,7 @@ static inline uint16_t mavlink_msg_local_position_ned_pack(uint8_t system_id, ui
 	_mav_put_float(buf, 20, vy);
 	_mav_put_float(buf, 24, vz);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 28);
+        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;
@@ -71,18 +74,22 @@ static inline uint16_t mavlink_msg_local_position_ned_pack(uint8_t system_id, ui
 	packet.vy = vy;
 	packet.vz = vz;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 28);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_NED;
-	return mavlink_finalize_message(msg, system_id, component_id, 28, 185);
+#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 was sent over
+ * @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
@@ -98,7 +105,7 @@ static inline uint16_t mavlink_msg_local_position_ned_pack_chan(uint8_t system_i
 						           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[28];
+	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);
@@ -107,7 +114,7 @@ static inline uint16_t mavlink_msg_local_position_ned_pack_chan(uint8_t system_i
 	_mav_put_float(buf, 20, vy);
 	_mav_put_float(buf, 24, vz);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 28);
+        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;
@@ -118,15 +125,19 @@ static inline uint16_t mavlink_msg_local_position_ned_pack_chan(uint8_t system_i
 	packet.vy = vy;
 	packet.vz = vz;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 28);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_NED;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 28, 185);
+#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 into a message
+ * @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)
@@ -139,6 +150,20 @@ static inline uint16_t mavlink_msg_local_position_ned_encode(uint8_t system_id,
 }
 
 /**
+ * @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
  *
@@ -155,7 +180,7 @@ static inline uint16_t mavlink_msg_local_position_ned_encode(uint8_t system_id,
 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[28];
+	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);
@@ -164,7 +189,11 @@ static inline void mavlink_msg_local_position_ned_send(mavlink_channel_t chan, u
 	_mav_put_float(buf, 20, vy);
 	_mav_put_float(buf, 24, vz);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, buf, 28, 185);
+#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;
@@ -175,7 +204,11 @@ static inline void mavlink_msg_local_position_ned_send(mavlink_channel_t chan, u
 	packet.vy = vy;
 	packet.vz = vz;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, (const char *)&packet, 28, 185);
+#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
 }
 
@@ -271,6 +304,6 @@ static inline void mavlink_msg_local_position_ned_decode(const mavlink_message_t
 	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), 28);
+	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
index ac1941db0..8c4686202 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h
@@ -16,6 +16,9 @@ typedef struct __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 { \
@@ -51,7 +54,7 @@ static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_pack(
 						       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[28];
+	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);
@@ -60,7 +63,7 @@ static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_pack(
 	_mav_put_float(buf, 20, pitch);
 	_mav_put_float(buf, 24, yaw);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 28);
+        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;
@@ -71,18 +74,22 @@ static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_pack(
 	packet.pitch = pitch;
 	packet.yaw = yaw;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 28);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 28, 231);
+#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 was sent over
+ * @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
@@ -98,7 +105,7 @@ static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_pack_
 						           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[28];
+	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);
@@ -107,7 +114,7 @@ static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_pack_
 	_mav_put_float(buf, 20, pitch);
 	_mav_put_float(buf, 24, yaw);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 28);
+        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;
@@ -118,15 +125,19 @@ static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_pack_
 	packet.pitch = pitch;
 	packet.yaw = yaw;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 28);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 28, 231);
+#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 into a message
+ * @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)
@@ -139,6 +150,20 @@ static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_encod
 }
 
 /**
+ * @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
  *
@@ -155,7 +180,7 @@ static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_encod
 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[28];
+	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);
@@ -164,7 +189,11 @@ static inline void mavlink_msg_local_position_ned_system_global_offset_send(mavl
 	_mav_put_float(buf, 20, pitch);
 	_mav_put_float(buf, 24, yaw);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, buf, 28, 231);
+#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;
@@ -175,7 +204,11 @@ static inline void mavlink_msg_local_position_ned_system_global_offset_send(mavl
 	packet.pitch = pitch;
 	packet.yaw = yaw;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, (const char *)&packet, 28, 231);
+#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
 }
 
@@ -271,6 +304,6 @@ static inline void mavlink_msg_local_position_ned_system_global_offset_decode(co
 	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), 28);
+	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
index 9ab87d0da..1794815f8 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h
@@ -14,6 +14,9 @@ typedef struct __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 { \
@@ -45,14 +48,14 @@ static inline uint16_t mavlink_msg_local_position_setpoint_pack(uint8_t system_i
 						       uint8_t coordinate_frame, float x, float y, float z, float yaw)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[17];
+	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, 17);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN);
 #else
 	mavlink_local_position_setpoint_t packet;
 	packet.x = x;
@@ -61,18 +64,22 @@ static inline uint16_t mavlink_msg_local_position_setpoint_pack(uint8_t system_i
 	packet.yaw = yaw;
 	packet.coordinate_frame = coordinate_frame;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 17);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT;
-	return mavlink_finalize_message(msg, system_id, component_id, 17, 223);
+#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 was sent over
+ * @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
@@ -86,14 +93,14 @@ static inline uint16_t mavlink_msg_local_position_setpoint_pack_chan(uint8_t sys
 						           uint8_t coordinate_frame,float x,float y,float z,float yaw)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[17];
+	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, 17);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN);
 #else
 	mavlink_local_position_setpoint_t packet;
 	packet.x = x;
@@ -102,15 +109,19 @@ static inline uint16_t mavlink_msg_local_position_setpoint_pack_chan(uint8_t sys
 	packet.yaw = yaw;
 	packet.coordinate_frame = coordinate_frame;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 17);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 17, 223);
+#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 into a message
+ * @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)
@@ -123,6 +134,20 @@ static inline uint16_t mavlink_msg_local_position_setpoint_encode(uint8_t system
 }
 
 /**
+ * @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
  *
@@ -137,14 +162,18 @@ static inline uint16_t mavlink_msg_local_position_setpoint_encode(uint8_t system
 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[17];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, buf, 17, 223);
+#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;
@@ -153,7 +182,11 @@ static inline void mavlink_msg_local_position_setpoint_send(mavlink_channel_t ch
 	packet.yaw = yaw;
 	packet.coordinate_frame = coordinate_frame;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, (const char *)&packet, 17, 223);
+#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
 }
 
@@ -227,6 +260,6 @@ static inline void mavlink_msg_local_position_setpoint_decode(const mavlink_mess
 	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), 17);
+	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
new file mode 100644
index 000000000..1cf5d15e4
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_data.h
@@ -0,0 +1,237 @@
+// 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
new file mode 100644
index 000000000..681d8f07c
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_entry.h
@@ -0,0 +1,265 @@
+// 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
new file mode 100644
index 000000000..feafeaf16
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_erase.h
@@ -0,0 +1,199 @@
+// 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
new file mode 100644
index 000000000..5be9eea47
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_data.h
@@ -0,0 +1,265 @@
+// 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
new file mode 100644
index 000000000..48a5a03b4
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_end.h
@@ -0,0 +1,199 @@
+// 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
new file mode 100644
index 000000000..7a5b25c17
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_list.h
@@ -0,0 +1,243 @@
+// 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
index 96b3d3040..6b6e9e148 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h
@@ -15,6 +15,9 @@ typedef struct __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 { \
@@ -48,7 +51,7 @@ static inline uint16_t mavlink_msg_manual_control_pack(uint8_t system_id, uint8_
 						       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[11];
+	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);
@@ -56,7 +59,7 @@ static inline uint16_t mavlink_msg_manual_control_pack(uint8_t system_id, uint8_
 	_mav_put_uint16_t(buf, 8, buttons);
 	_mav_put_uint8_t(buf, 10, target);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 11);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN);
 #else
 	mavlink_manual_control_t packet;
 	packet.x = x;
@@ -66,18 +69,22 @@ static inline uint16_t mavlink_msg_manual_control_pack(uint8_t system_id, uint8_
 	packet.buttons = buttons;
 	packet.target = target;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 11);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL;
-	return mavlink_finalize_message(msg, system_id, component_id, 11, 243);
+#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 was sent over
+ * @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.
@@ -92,7 +99,7 @@ static inline uint16_t mavlink_msg_manual_control_pack_chan(uint8_t system_id, u
 						           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[11];
+	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);
@@ -100,7 +107,7 @@ static inline uint16_t mavlink_msg_manual_control_pack_chan(uint8_t system_id, u
 	_mav_put_uint16_t(buf, 8, buttons);
 	_mav_put_uint8_t(buf, 10, target);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 11);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN);
 #else
 	mavlink_manual_control_t packet;
 	packet.x = x;
@@ -110,15 +117,19 @@ static inline uint16_t mavlink_msg_manual_control_pack_chan(uint8_t system_id, u
 	packet.buttons = buttons;
 	packet.target = target;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 11);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 11, 243);
+#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 into a message
+ * @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)
@@ -131,6 +142,20 @@ static inline uint16_t mavlink_msg_manual_control_encode(uint8_t system_id, uint
 }
 
 /**
+ * @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
  *
@@ -146,7 +171,7 @@ static inline uint16_t mavlink_msg_manual_control_encode(uint8_t system_id, uint
 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[11];
+	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);
@@ -154,7 +179,11 @@ static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8
 	_mav_put_uint16_t(buf, 8, buttons);
 	_mav_put_uint8_t(buf, 10, target);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, buf, 11, 243);
+#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;
@@ -164,7 +193,11 @@ static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8
 	packet.buttons = buttons;
 	packet.target = target;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, (const char *)&packet, 11, 243);
+#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
 }
 
@@ -249,6 +282,6 @@ static inline void mavlink_msg_manual_control_decode(const mavlink_message_t* ms
 	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), 11);
+	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
index 71db380a1..a694947c1 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h
@@ -16,6 +16,9 @@ typedef struct __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 { \
@@ -51,7 +54,7 @@ static inline uint16_t mavlink_msg_manual_setpoint_pack(uint8_t system_id, uint8
 						       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[22];
+	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);
@@ -60,7 +63,7 @@ static inline uint16_t mavlink_msg_manual_setpoint_pack(uint8_t system_id, uint8
 	_mav_put_uint8_t(buf, 20, mode_switch);
 	_mav_put_uint8_t(buf, 21, manual_override_switch);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 22);
+        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;
@@ -71,18 +74,22 @@ static inline uint16_t mavlink_msg_manual_setpoint_pack(uint8_t system_id, uint8
 	packet.mode_switch = mode_switch;
 	packet.manual_override_switch = manual_override_switch;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 22);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MANUAL_SETPOINT;
-	return mavlink_finalize_message(msg, system_id, component_id, 22, 106);
+#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 was sent over
+ * @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
@@ -98,7 +105,7 @@ static inline uint16_t mavlink_msg_manual_setpoint_pack_chan(uint8_t system_id,
 						           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[22];
+	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);
@@ -107,7 +114,7 @@ static inline uint16_t mavlink_msg_manual_setpoint_pack_chan(uint8_t system_id,
 	_mav_put_uint8_t(buf, 20, mode_switch);
 	_mav_put_uint8_t(buf, 21, manual_override_switch);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 22);
+        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;
@@ -118,15 +125,19 @@ static inline uint16_t mavlink_msg_manual_setpoint_pack_chan(uint8_t system_id,
 	packet.mode_switch = mode_switch;
 	packet.manual_override_switch = manual_override_switch;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 22);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MANUAL_SETPOINT;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 22, 106);
+#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 into a message
+ * @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)
@@ -139,6 +150,20 @@ static inline uint16_t mavlink_msg_manual_setpoint_encode(uint8_t system_id, uin
 }
 
 /**
+ * @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
  *
@@ -155,7 +180,7 @@ static inline uint16_t mavlink_msg_manual_setpoint_encode(uint8_t system_id, uin
 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[22];
+	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);
@@ -164,7 +189,11 @@ static inline void mavlink_msg_manual_setpoint_send(mavlink_channel_t chan, uint
 	_mav_put_uint8_t(buf, 20, mode_switch);
 	_mav_put_uint8_t(buf, 21, manual_override_switch);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, buf, 22, 106);
+#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;
@@ -175,7 +204,11 @@ static inline void mavlink_msg_manual_setpoint_send(mavlink_channel_t chan, uint
 	packet.mode_switch = mode_switch;
 	packet.manual_override_switch = manual_override_switch;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, (const char *)&packet, 22, 106);
+#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
 }
 
@@ -271,6 +304,6 @@ static inline void mavlink_msg_manual_setpoint_decode(const mavlink_message_t* m
 	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), 22);
+	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
index a61c13019..5f79329c2 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h
@@ -13,6 +13,9 @@ typedef struct __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 { \
@@ -42,30 +45,34 @@ static inline uint16_t mavlink_msg_memory_vect_pack(uint8_t system_id, uint8_t c
 						       uint16_t address, uint8_t ver, uint8_t type, const int8_t *value)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[36];
+	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, 36);
+        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, 36);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MEMORY_VECT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MEMORY_VECT;
-	return mavlink_finalize_message(msg, system_id, component_id, 36, 204);
+#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 was sent over
+ * @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
@@ -78,27 +85,31 @@ static inline uint16_t mavlink_msg_memory_vect_pack_chan(uint8_t system_id, uint
 						           uint16_t address,uint8_t ver,uint8_t type,const int8_t *value)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[36];
+	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, 36);
+        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, 36);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MEMORY_VECT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MEMORY_VECT;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 36, 204);
+#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 into a message
+ * @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)
@@ -111,6 +122,20 @@ static inline uint16_t mavlink_msg_memory_vect_encode(uint8_t system_id, uint8_t
 }
 
 /**
+ * @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
  *
@@ -124,19 +149,27 @@ static inline uint16_t mavlink_msg_memory_vect_encode(uint8_t system_id, uint8_t
 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[36];
+	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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, buf, 36, 204);
+#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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, (const char *)&packet, 36, 204);
+#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
 }
 
@@ -199,6 +232,6 @@ static inline void mavlink_msg_memory_vect_decode(const mavlink_message_t* 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), 36);
+	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
index 92eca79d1..7421d8394 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h
@@ -12,6 +12,9 @@ typedef struct __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 { \
@@ -39,30 +42,34 @@ static inline uint16_t mavlink_msg_mission_ack_pack(uint8_t system_id, uint8_t c
 						       uint8_t target_system, uint8_t target_component, uint8_t type)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[3];
+	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, 3);
+        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, 3);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ACK_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MISSION_ACK;
-	return mavlink_finalize_message(msg, system_id, component_id, 3, 153);
+#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 was sent over
+ * @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
@@ -74,27 +81,31 @@ static inline uint16_t mavlink_msg_mission_ack_pack_chan(uint8_t system_id, uint
 						           uint8_t target_system,uint8_t target_component,uint8_t type)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[3];
+	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, 3);
+        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, 3);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ACK_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MISSION_ACK;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3, 153);
+#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 into a message
+ * @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)
@@ -107,6 +118,20 @@ static inline uint16_t mavlink_msg_mission_ack_encode(uint8_t system_id, uint8_t
 }
 
 /**
+ * @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
  *
@@ -119,19 +144,27 @@ static inline uint16_t mavlink_msg_mission_ack_encode(uint8_t system_id, uint8_t
 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[3];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, buf, 3, 153);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, (const char *)&packet, 3, 153);
+#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
 }
 
@@ -183,6 +216,6 @@ static inline void mavlink_msg_mission_ack_decode(const mavlink_message_t* 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), 3);
+	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
index 602852f7e..8f441c8e5 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h
@@ -11,6 +11,9 @@ typedef struct __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 { \
@@ -36,28 +39,32 @@ static inline uint16_t mavlink_msg_mission_clear_all_pack(uint8_t system_id, uin
 						       uint8_t target_system, uint8_t target_component)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[2];
+	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, 2);
+        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, 2);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MISSION_CLEAR_ALL;
-	return mavlink_finalize_message(msg, system_id, component_id, 2, 232);
+#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 was sent over
+ * @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
@@ -68,25 +75,29 @@ static inline uint16_t mavlink_msg_mission_clear_all_pack_chan(uint8_t system_id
 						           uint8_t target_system,uint8_t target_component)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[2];
+	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, 2);
+        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, 2);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MISSION_CLEAR_ALL;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 232);
+#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 into a message
+ * @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)
@@ -99,6 +110,20 @@ static inline uint16_t mavlink_msg_mission_clear_all_encode(uint8_t system_id, u
 }
 
 /**
+ * @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
  *
@@ -110,17 +135,25 @@ static inline uint16_t mavlink_msg_mission_clear_all_encode(uint8_t system_id, u
 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[2];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, buf, 2, 232);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, (const char *)&packet, 2, 232);
+#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
 }
 
@@ -161,6 +194,6 @@ static inline void mavlink_msg_mission_clear_all_decode(const mavlink_message_t*
 	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), 2);
+	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
index 61d8b221c..eac779306 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h
@@ -12,6 +12,9 @@ typedef struct __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 { \
@@ -39,30 +42,34 @@ static inline uint16_t mavlink_msg_mission_count_pack(uint8_t system_id, uint8_t
 						       uint8_t target_system, uint8_t target_component, uint16_t count)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[4];
+	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, 4);
+        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, 4);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MISSION_COUNT;
-	return mavlink_finalize_message(msg, system_id, component_id, 4, 221);
+#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 was sent over
+ * @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
@@ -74,27 +81,31 @@ static inline uint16_t mavlink_msg_mission_count_pack_chan(uint8_t system_id, ui
 						           uint8_t target_system,uint8_t target_component,uint16_t count)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[4];
+	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, 4);
+        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, 4);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MISSION_COUNT;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 221);
+#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 into a message
+ * @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)
@@ -107,6 +118,20 @@ static inline uint16_t mavlink_msg_mission_count_encode(uint8_t system_id, uint8
 }
 
 /**
+ * @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
  *
@@ -119,19 +144,27 @@ static inline uint16_t mavlink_msg_mission_count_encode(uint8_t system_id, uint8
 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[4];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, buf, 4, 221);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, (const char *)&packet, 4, 221);
+#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
 }
 
@@ -183,6 +216,6 @@ static inline void mavlink_msg_mission_count_decode(const mavlink_message_t* 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), 4);
+	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
index 99370f835..dbcdbd3f9 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h
@@ -10,6 +10,9 @@ typedef struct __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 { \
@@ -33,26 +36,30 @@ static inline uint16_t mavlink_msg_mission_current_pack(uint8_t system_id, uint8
 						       uint16_t seq)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[2];
+	char buf[MAVLINK_MSG_ID_MISSION_CURRENT_LEN];
 	_mav_put_uint16_t(buf, 0, seq);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 2);
+        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, 2);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MISSION_CURRENT;
-	return mavlink_finalize_message(msg, system_id, component_id, 2, 28);
+#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 was sent over
+ * @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)
@@ -62,23 +69,27 @@ static inline uint16_t mavlink_msg_mission_current_pack_chan(uint8_t system_id,
 						           uint16_t seq)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[2];
+	char buf[MAVLINK_MSG_ID_MISSION_CURRENT_LEN];
 	_mav_put_uint16_t(buf, 0, seq);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 2);
+        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, 2);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MISSION_CURRENT;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 28);
+#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 into a message
+ * @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)
@@ -91,6 +102,20 @@ static inline uint16_t mavlink_msg_mission_current_encode(uint8_t system_id, uin
 }
 
 /**
+ * @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
  *
@@ -101,15 +126,23 @@ static inline uint16_t mavlink_msg_mission_current_encode(uint8_t system_id, uin
 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[2];
+	char buf[MAVLINK_MSG_ID_MISSION_CURRENT_LEN];
 	_mav_put_uint16_t(buf, 0, seq);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, buf, 2, 28);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, (const char *)&packet, 2, 28);
+#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
 }
 
@@ -139,6 +172,6 @@ static inline void mavlink_msg_mission_current_decode(const mavlink_message_t* m
 #if MAVLINK_NEED_BYTE_SWAP
 	mission_current->seq = mavlink_msg_mission_current_get_seq(msg);
 #else
-	memcpy(mission_current, _MAV_PAYLOAD(msg), 2);
+	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
index d2c66d4da..a8d60eca7 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h
@@ -4,13 +4,13 @@
 
 typedef struct __mavlink_mission_item_t
 {
- float 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 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
+ 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
@@ -23,6 +23,9 @@ typedef struct __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 { \
@@ -59,20 +62,20 @@ typedef struct __mavlink_mission_item_t
  * @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 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters
- * @param param2 PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds
- * @param 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.
- * @param param4 PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH
+ * @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
+ * @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[37];
+	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);
@@ -88,7 +91,7 @@ static inline uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t
 	_mav_put_uint8_t(buf, 35, current);
 	_mav_put_uint8_t(buf, 36, autocontinue);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 37);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN);
 #else
 	mavlink_mission_item_t packet;
 	packet.param1 = param1;
@@ -106,18 +109,22 @@ static inline uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t
 	packet.current = current;
 	packet.autocontinue = autocontinue;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 37);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM;
-	return mavlink_finalize_message(msg, system_id, component_id, 37, 254);
+#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 was sent over
+ * @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
@@ -126,13 +133,13 @@ static inline uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t
  * @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 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters
- * @param param2 PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds
- * @param 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.
- * @param param4 PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH
+ * @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
+ * @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,
@@ -140,7 +147,7 @@ static inline uint16_t mavlink_msg_mission_item_pack_chan(uint8_t system_id, uin
 						           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[37];
+	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);
@@ -156,7 +163,7 @@ static inline uint16_t mavlink_msg_mission_item_pack_chan(uint8_t system_id, uin
 	_mav_put_uint8_t(buf, 35, current);
 	_mav_put_uint8_t(buf, 36, autocontinue);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 37);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN);
 #else
 	mavlink_mission_item_t packet;
 	packet.param1 = param1;
@@ -174,15 +181,19 @@ static inline uint16_t mavlink_msg_mission_item_pack_chan(uint8_t system_id, uin
 	packet.current = current;
 	packet.autocontinue = autocontinue;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 37);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 37, 254);
+#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 into a message
+ * @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)
@@ -195,6 +206,20 @@ static inline uint16_t mavlink_msg_mission_item_encode(uint8_t system_id, uint8_
 }
 
 /**
+ * @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
  *
@@ -205,20 +230,20 @@ static inline uint16_t mavlink_msg_mission_item_encode(uint8_t system_id, uint8_
  * @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 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters
- * @param param2 PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds
- * @param 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.
- * @param param4 PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH
+ * @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
+ * @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[37];
+	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);
@@ -234,7 +259,11 @@ static inline void mavlink_msg_mission_item_send(mavlink_channel_t chan, uint8_t
 	_mav_put_uint8_t(buf, 35, current);
 	_mav_put_uint8_t(buf, 36, autocontinue);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, buf, 37, 254);
+#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;
@@ -252,7 +281,11 @@ static inline void mavlink_msg_mission_item_send(mavlink_channel_t chan, uint8_t
 	packet.current = current;
 	packet.autocontinue = autocontinue;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, (const char *)&packet, 37, 254);
+#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
 }
 
@@ -334,7 +367,7 @@ static inline uint8_t mavlink_msg_mission_item_get_autocontinue(const mavlink_me
 /**
  * @brief Get field param1 from mission_item message
  *
- * @return PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters
+ * @return PARAM1, see MAV_CMD enum
  */
 static inline float mavlink_msg_mission_item_get_param1(const mavlink_message_t* msg)
 {
@@ -344,7 +377,7 @@ static inline float mavlink_msg_mission_item_get_param1(const mavlink_message_t*
 /**
  * @brief Get field param2 from mission_item message
  *
- * @return PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds
+ * @return PARAM2, see MAV_CMD enum
  */
 static inline float mavlink_msg_mission_item_get_param2(const mavlink_message_t* msg)
 {
@@ -354,7 +387,7 @@ static inline float mavlink_msg_mission_item_get_param2(const mavlink_message_t*
 /**
  * @brief Get field param3 from mission_item message
  *
- * @return 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.
+ * @return PARAM3, see MAV_CMD enum
  */
 static inline float mavlink_msg_mission_item_get_param3(const mavlink_message_t* msg)
 {
@@ -364,7 +397,7 @@ static inline float mavlink_msg_mission_item_get_param3(const mavlink_message_t*
 /**
  * @brief Get field param4 from mission_item message
  *
- * @return PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH
+ * @return PARAM4, see MAV_CMD enum
  */
 static inline float mavlink_msg_mission_item_get_param4(const mavlink_message_t* msg)
 {
@@ -394,7 +427,7 @@ static inline float mavlink_msg_mission_item_get_y(const mavlink_message_t* msg)
 /**
  * @brief Get field z from mission_item message
  *
- * @return PARAM7 / z position: global: altitude
+ * @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)
 {
@@ -425,6 +458,6 @@ static inline void mavlink_msg_mission_item_decode(const mavlink_message_t* 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), 37);
+	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
index 171f9857e..f3744fde6 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h
@@ -10,6 +10,9 @@ typedef struct __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 { \
@@ -33,26 +36,30 @@ static inline uint16_t mavlink_msg_mission_item_reached_pack(uint8_t system_id,
 						       uint16_t seq)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[2];
+	char buf[MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN];
 	_mav_put_uint16_t(buf, 0, seq);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 2);
+        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, 2);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM_REACHED;
-	return mavlink_finalize_message(msg, system_id, component_id, 2, 11);
+#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 was sent over
+ * @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)
@@ -62,23 +69,27 @@ static inline uint16_t mavlink_msg_mission_item_reached_pack_chan(uint8_t system
 						           uint16_t seq)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[2];
+	char buf[MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN];
 	_mav_put_uint16_t(buf, 0, seq);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 2);
+        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, 2);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM_REACHED;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 11);
+#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 into a message
+ * @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)
@@ -91,6 +102,20 @@ static inline uint16_t mavlink_msg_mission_item_reached_encode(uint8_t system_id
 }
 
 /**
+ * @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
  *
@@ -101,15 +126,23 @@ static inline uint16_t mavlink_msg_mission_item_reached_encode(uint8_t system_id
 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[2];
+	char buf[MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN];
 	_mav_put_uint16_t(buf, 0, seq);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, buf, 2, 11);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, (const char *)&packet, 2, 11);
+#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
 }
 
@@ -139,6 +172,6 @@ static inline void mavlink_msg_mission_item_reached_decode(const mavlink_message
 #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), 2);
+	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
index cde0a0cfb..ac84e3554 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h
@@ -12,6 +12,9 @@ typedef struct __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 { \
@@ -39,30 +42,34 @@ static inline uint16_t mavlink_msg_mission_request_pack(uint8_t system_id, uint8
 						       uint8_t target_system, uint8_t target_component, uint16_t seq)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[4];
+	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, 4);
+        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, 4);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST;
-	return mavlink_finalize_message(msg, system_id, component_id, 4, 230);
+#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 was sent over
+ * @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
@@ -74,27 +81,31 @@ static inline uint16_t mavlink_msg_mission_request_pack_chan(uint8_t system_id,
 						           uint8_t target_system,uint8_t target_component,uint16_t seq)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[4];
+	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, 4);
+        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, 4);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 230);
+#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 into a message
+ * @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)
@@ -107,6 +118,20 @@ static inline uint16_t mavlink_msg_mission_request_encode(uint8_t system_id, uin
 }
 
 /**
+ * @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
  *
@@ -119,19 +144,27 @@ static inline uint16_t mavlink_msg_mission_request_encode(uint8_t system_id, uin
 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[4];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, buf, 4, 230);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, (const char *)&packet, 4, 230);
+#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
 }
 
@@ -183,6 +216,6 @@ static inline void mavlink_msg_mission_request_decode(const mavlink_message_t* m
 	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), 4);
+	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
index 1ada635b5..d999babdb 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h
@@ -11,6 +11,9 @@ typedef struct __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 { \
@@ -36,28 +39,32 @@ static inline uint16_t mavlink_msg_mission_request_list_pack(uint8_t system_id,
 						       uint8_t target_system, uint8_t target_component)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[2];
+	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, 2);
+        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, 2);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST_LIST;
-	return mavlink_finalize_message(msg, system_id, component_id, 2, 132);
+#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 was sent over
+ * @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
@@ -68,25 +75,29 @@ static inline uint16_t mavlink_msg_mission_request_list_pack_chan(uint8_t system
 						           uint8_t target_system,uint8_t target_component)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[2];
+	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, 2);
+        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, 2);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST_LIST;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 132);
+#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 into a message
+ * @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)
@@ -99,6 +110,20 @@ static inline uint16_t mavlink_msg_mission_request_list_encode(uint8_t system_id
 }
 
 /**
+ * @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
  *
@@ -110,17 +135,25 @@ static inline uint16_t mavlink_msg_mission_request_list_encode(uint8_t system_id
 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[2];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, buf, 2, 132);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, (const char *)&packet, 2, 132);
+#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
 }
 
@@ -161,6 +194,6 @@ static inline void mavlink_msg_mission_request_list_decode(const mavlink_message
 	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), 2);
+	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
index 76fd43bef..35c7e1285 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h
@@ -13,6 +13,9 @@ typedef struct __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 { \
@@ -42,13 +45,13 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_pack(uint8_t sys
 						       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[6];
+	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, 6);
+        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;
@@ -56,18 +59,22 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_pack(uint8_t sys
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 6);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 6, 212);
+#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 was sent over
+ * @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
@@ -80,13 +87,13 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_pack_chan(uint8_
 						           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[6];
+	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, 6);
+        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;
@@ -94,15 +101,19 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_pack_chan(uint8_
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 6);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 6, 212);
+#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 into a message
+ * @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)
@@ -115,6 +126,20 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_encode(uint8_t s
 }
 
 /**
+ * @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
  *
@@ -128,13 +153,17 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_encode(uint8_t s
 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[6];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, buf, 6, 212);
+#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;
@@ -142,7 +171,11 @@ static inline void mavlink_msg_mission_request_partial_list_send(mavlink_channel
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, (const char *)&packet, 6, 212);
+#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
 }
 
@@ -205,6 +238,6 @@ static inline void mavlink_msg_mission_request_partial_list_decode(const mavlink
 	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), 6);
+	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
index de0dbcd75..63b37cf8c 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h
@@ -12,6 +12,9 @@ typedef struct __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 { \
@@ -39,30 +42,34 @@ static inline uint16_t mavlink_msg_mission_set_current_pack(uint8_t system_id, u
 						       uint8_t target_system, uint8_t target_component, uint16_t seq)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[4];
+	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, 4);
+        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, 4);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MISSION_SET_CURRENT;
-	return mavlink_finalize_message(msg, system_id, component_id, 4, 28);
+#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 was sent over
+ * @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
@@ -74,27 +81,31 @@ static inline uint16_t mavlink_msg_mission_set_current_pack_chan(uint8_t system_
 						           uint8_t target_system,uint8_t target_component,uint16_t seq)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[4];
+	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, 4);
+        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, 4);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MISSION_SET_CURRENT;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 28);
+#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 into a message
+ * @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)
@@ -107,6 +118,20 @@ static inline uint16_t mavlink_msg_mission_set_current_encode(uint8_t system_id,
 }
 
 /**
+ * @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
  *
@@ -119,19 +144,27 @@ static inline uint16_t mavlink_msg_mission_set_current_encode(uint8_t system_id,
 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[4];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, buf, 4, 28);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, (const char *)&packet, 4, 28);
+#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
 }
 
@@ -183,6 +216,6 @@ static inline void mavlink_msg_mission_set_current_decode(const mavlink_message_
 	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), 4);
+	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
index 0e77569cf..7de38bd7b 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h
@@ -13,6 +13,9 @@ typedef struct __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 { \
@@ -42,13 +45,13 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_pack(uint8_t syste
 						       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[6];
+	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, 6);
+        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;
@@ -56,18 +59,22 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_pack(uint8_t syste
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 6);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 6, 9);
+#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 was sent over
+ * @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
@@ -80,13 +87,13 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_pack_chan(uint8_t
 						           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[6];
+	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, 6);
+        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;
@@ -94,15 +101,19 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_pack_chan(uint8_t
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 6);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 6, 9);
+#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 into a message
+ * @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)
@@ -115,6 +126,20 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_encode(uint8_t sys
 }
 
 /**
+ * @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
  *
@@ -128,13 +153,17 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_encode(uint8_t sys
 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[6];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, buf, 6, 9);
+#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;
@@ -142,7 +171,11 @@ static inline void mavlink_msg_mission_write_partial_list_send(mavlink_channel_t
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, (const char *)&packet, 6, 9);
+#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
 }
 
@@ -205,6 +238,6 @@ static inline void mavlink_msg_mission_write_partial_list_decode(const mavlink_m
 	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), 6);
+	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
index 23a819e2c..fbf4f75c9 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h
@@ -12,6 +12,9 @@ typedef struct __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 { \
@@ -39,28 +42,32 @@ static inline uint16_t mavlink_msg_named_value_float_pack(uint8_t system_id, uin
 						       uint32_t time_boot_ms, const char *name, float value)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[18];
+	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, 18);
+        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, 18);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT;
-	return mavlink_finalize_message(msg, system_id, component_id, 18, 170);
+#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 was sent over
+ * @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
@@ -72,25 +79,29 @@ static inline uint16_t mavlink_msg_named_value_float_pack_chan(uint8_t system_id
 						           uint32_t time_boot_ms,const char *name,float value)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[18];
+	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, 18);
+        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, 18);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 170);
+#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 into a message
+ * @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)
@@ -103,6 +114,20 @@ static inline uint16_t mavlink_msg_named_value_float_encode(uint8_t system_id, u
 }
 
 /**
+ * @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
  *
@@ -115,17 +140,25 @@ static inline uint16_t mavlink_msg_named_value_float_encode(uint8_t system_id, u
 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[18];
+	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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, buf, 18, 170);
+#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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, (const char *)&packet, 18, 170);
+#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
 }
 
@@ -177,6 +210,6 @@ static inline void mavlink_msg_named_value_float_decode(const mavlink_message_t*
 	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), 18);
+	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
index 3c67dff03..052f24793 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h
@@ -12,6 +12,9 @@ typedef struct __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 { \
@@ -39,28 +42,32 @@ static inline uint16_t mavlink_msg_named_value_int_pack(uint8_t system_id, uint8
 						       uint32_t time_boot_ms, const char *name, int32_t value)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[18];
+	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, 18);
+        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, 18);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT;
-	return mavlink_finalize_message(msg, system_id, component_id, 18, 44);
+#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 was sent over
+ * @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
@@ -72,25 +79,29 @@ static inline uint16_t mavlink_msg_named_value_int_pack_chan(uint8_t system_id,
 						           uint32_t time_boot_ms,const char *name,int32_t value)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[18];
+	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, 18);
+        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, 18);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 44);
+#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 into a message
+ * @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)
@@ -103,6 +114,20 @@ static inline uint16_t mavlink_msg_named_value_int_encode(uint8_t system_id, uin
 }
 
 /**
+ * @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
  *
@@ -115,17 +140,25 @@ static inline uint16_t mavlink_msg_named_value_int_encode(uint8_t system_id, uin
 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[18];
+	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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, buf, 18, 44);
+#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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, (const char *)&packet, 18, 44);
+#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
 }
 
@@ -177,6 +210,6 @@ static inline void mavlink_msg_named_value_int_decode(const mavlink_message_t* m
 	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), 18);
+	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
index 028afdabc..e95c144de 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h
@@ -17,6 +17,9 @@ typedef struct __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 { \
@@ -54,7 +57,7 @@ static inline uint16_t mavlink_msg_nav_controller_output_pack(uint8_t system_id,
 						       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[26];
+	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);
@@ -64,7 +67,7 @@ static inline uint16_t mavlink_msg_nav_controller_output_pack(uint8_t system_id,
 	_mav_put_int16_t(buf, 22, target_bearing);
 	_mav_put_uint16_t(buf, 24, wp_dist);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 26);
+        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;
@@ -76,18 +79,22 @@ static inline uint16_t mavlink_msg_nav_controller_output_pack(uint8_t system_id,
 	packet.target_bearing = target_bearing;
 	packet.wp_dist = wp_dist;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 26);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT;
-	return mavlink_finalize_message(msg, system_id, component_id, 26, 183);
+#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 was sent over
+ * @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
@@ -104,7 +111,7 @@ static inline uint16_t mavlink_msg_nav_controller_output_pack_chan(uint8_t syste
 						           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[26];
+	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);
@@ -114,7 +121,7 @@ static inline uint16_t mavlink_msg_nav_controller_output_pack_chan(uint8_t syste
 	_mav_put_int16_t(buf, 22, target_bearing);
 	_mav_put_uint16_t(buf, 24, wp_dist);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 26);
+        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;
@@ -126,15 +133,19 @@ static inline uint16_t mavlink_msg_nav_controller_output_pack_chan(uint8_t syste
 	packet.target_bearing = target_bearing;
 	packet.wp_dist = wp_dist;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 26);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 26, 183);
+#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 into a message
+ * @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)
@@ -147,6 +158,20 @@ static inline uint16_t mavlink_msg_nav_controller_output_encode(uint8_t system_i
 }
 
 /**
+ * @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
  *
@@ -164,7 +189,7 @@ static inline uint16_t mavlink_msg_nav_controller_output_encode(uint8_t system_i
 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[26];
+	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);
@@ -174,7 +199,11 @@ static inline void mavlink_msg_nav_controller_output_send(mavlink_channel_t chan
 	_mav_put_int16_t(buf, 22, target_bearing);
 	_mav_put_uint16_t(buf, 24, wp_dist);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, buf, 26, 183);
+#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;
@@ -186,7 +215,11 @@ static inline void mavlink_msg_nav_controller_output_send(mavlink_channel_t chan
 	packet.target_bearing = target_bearing;
 	packet.wp_dist = wp_dist;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, (const char *)&packet, 26, 183);
+#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
 }
 
@@ -293,6 +326,6 @@ static inline void mavlink_msg_nav_controller_output_decode(const mavlink_messag
 	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), 26);
+	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
index 9bb1e3369..4debb6e66 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h
@@ -15,6 +15,9 @@ typedef struct __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
 
@@ -49,14 +52,14 @@ static inline uint16_t mavlink_msg_omnidirectional_flow_pack(uint8_t system_id,
 						       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[54];
+	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, 54);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN);
 #else
 	mavlink_omnidirectional_flow_t packet;
 	packet.time_usec = time_usec;
@@ -65,18 +68,22 @@ static inline uint16_t mavlink_msg_omnidirectional_flow_pack(uint8_t system_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, 54);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW;
-	return mavlink_finalize_message(msg, system_id, component_id, 54, 211);
+#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 was sent over
+ * @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
@@ -91,14 +98,14 @@ static inline uint16_t mavlink_msg_omnidirectional_flow_pack_chan(uint8_t system
 						           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[54];
+	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, 54);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN);
 #else
 	mavlink_omnidirectional_flow_t packet;
 	packet.time_usec = time_usec;
@@ -107,15 +114,19 @@ static inline uint16_t mavlink_msg_omnidirectional_flow_pack_chan(uint8_t system
 	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, 54);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 54, 211);
+#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 into a message
+ * @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)
@@ -128,6 +139,20 @@ static inline uint16_t mavlink_msg_omnidirectional_flow_encode(uint8_t system_id
 }
 
 /**
+ * @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
  *
@@ -143,14 +168,18 @@ static inline uint16_t mavlink_msg_omnidirectional_flow_encode(uint8_t system_id
 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[54];
+	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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW, buf, 54, 211);
+#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;
@@ -159,7 +188,11 @@ static inline void mavlink_msg_omnidirectional_flow_send(mavlink_channel_t chan,
 	packet.quality = quality;
 	mav_array_memcpy(packet.left, left, sizeof(int16_t)*10);
 	mav_array_memcpy(packet.right, right, sizeof(int16_t)*10);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW, (const char *)&packet, 54, 211);
+#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
 }
 
@@ -244,6 +277,6 @@ static inline void mavlink_msg_omnidirectional_flow_decode(const mavlink_message
 	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), 54);
+	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
index b277cab51..cf6db9147 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h
@@ -8,8 +8,8 @@ typedef struct __mavlink_optical_flow_t
  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
+ 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;
@@ -17,6 +17,9 @@ typedef struct __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 { \
@@ -42,8 +45,8 @@ typedef struct __mavlink_optical_flow_t
  *
  * @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_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
@@ -54,7 +57,7 @@ static inline uint16_t mavlink_msg_optical_flow_pack(uint8_t system_id, uint8_t
 						       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[26];
+	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);
@@ -64,7 +67,7 @@ static inline uint16_t mavlink_msg_optical_flow_pack(uint8_t system_id, uint8_t
 	_mav_put_uint8_t(buf, 24, sensor_id);
 	_mav_put_uint8_t(buf, 25, quality);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 26);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN);
 #else
 	mavlink_optical_flow_t packet;
 	packet.time_usec = time_usec;
@@ -76,23 +79,27 @@ static inline uint16_t mavlink_msg_optical_flow_pack(uint8_t system_id, uint8_t
 	packet.sensor_id = sensor_id;
 	packet.quality = quality;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 26);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW;
-	return mavlink_finalize_message(msg, system_id, component_id, 26, 175);
+#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 was sent over
+ * @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_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
@@ -104,7 +111,7 @@ static inline uint16_t mavlink_msg_optical_flow_pack_chan(uint8_t system_id, uin
 						           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[26];
+	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);
@@ -114,7 +121,7 @@ static inline uint16_t mavlink_msg_optical_flow_pack_chan(uint8_t system_id, uin
 	_mav_put_uint8_t(buf, 24, sensor_id);
 	_mav_put_uint8_t(buf, 25, quality);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 26);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN);
 #else
 	mavlink_optical_flow_t packet;
 	packet.time_usec = time_usec;
@@ -126,15 +133,19 @@ static inline uint16_t mavlink_msg_optical_flow_pack_chan(uint8_t system_id, uin
 	packet.sensor_id = sensor_id;
 	packet.quality = quality;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 26);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 26, 175);
+#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 into a message
+ * @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)
@@ -147,13 +158,27 @@ static inline uint16_t mavlink_msg_optical_flow_encode(uint8_t system_id, uint8_
 }
 
 /**
+ * @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 in x-sensor direction
- * @param flow_y Flow in pixels in y-sensor direction
+ * @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
@@ -164,7 +189,7 @@ static inline uint16_t mavlink_msg_optical_flow_encode(uint8_t system_id, uint8_
 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[26];
+	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);
@@ -174,7 +199,11 @@ static inline void mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_
 	_mav_put_uint8_t(buf, 24, sensor_id);
 	_mav_put_uint8_t(buf, 25, quality);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, buf, 26, 175);
+#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;
@@ -186,7 +215,11 @@ static inline void mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_
 	packet.sensor_id = sensor_id;
 	packet.quality = quality;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)&packet, 26, 175);
+#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
 }
 
@@ -218,7 +251,7 @@ static inline uint8_t mavlink_msg_optical_flow_get_sensor_id(const mavlink_messa
 /**
  * @brief Get field flow_x from optical_flow message
  *
- * @return Flow in pixels in x-sensor direction
+ * @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)
 {
@@ -228,7 +261,7 @@ static inline int16_t mavlink_msg_optical_flow_get_flow_x(const mavlink_message_
 /**
  * @brief Get field flow_y from optical_flow message
  *
- * @return Flow in pixels in y-sensor direction
+ * @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)
 {
@@ -293,6 +326,6 @@ static inline void mavlink_msg_optical_flow_decode(const mavlink_message_t* 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), 26);
+	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
index 125df80c8..39e007274 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h
@@ -11,6 +11,9 @@ typedef struct __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 { \
@@ -36,28 +39,32 @@ static inline uint16_t mavlink_msg_param_request_list_pack(uint8_t system_id, ui
 						       uint8_t target_system, uint8_t target_component)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[2];
+	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, 2);
+        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, 2);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST;
-	return mavlink_finalize_message(msg, system_id, component_id, 2, 159);
+#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 was sent over
+ * @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
@@ -68,25 +75,29 @@ static inline uint16_t mavlink_msg_param_request_list_pack_chan(uint8_t system_i
 						           uint8_t target_system,uint8_t target_component)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[2];
+	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, 2);
+        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, 2);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 159);
+#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 into a message
+ * @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)
@@ -99,6 +110,20 @@ static inline uint16_t mavlink_msg_param_request_list_encode(uint8_t system_id,
 }
 
 /**
+ * @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
  *
@@ -110,17 +135,25 @@ static inline uint16_t mavlink_msg_param_request_list_encode(uint8_t system_id,
 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[2];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, buf, 2, 159);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, (const char *)&packet, 2, 159);
+#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
 }
 
@@ -161,6 +194,6 @@ static inline void mavlink_msg_param_request_list_decode(const mavlink_message_t
 	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), 2);
+	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
index dba528df9..5d9113114 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h
@@ -13,6 +13,9 @@ typedef struct __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 { \
@@ -42,30 +45,34 @@ static inline uint16_t mavlink_msg_param_request_read_pack(uint8_t system_id, ui
 						       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[20];
+	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, 20);
+        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, 20);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ;
-	return mavlink_finalize_message(msg, system_id, component_id, 20, 214);
+#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 was sent over
+ * @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
@@ -78,27 +85,31 @@ static inline uint16_t mavlink_msg_param_request_read_pack_chan(uint8_t system_i
 						           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[20];
+	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, 20);
+        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, 20);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 214);
+#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 into a message
+ * @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)
@@ -111,6 +122,20 @@ static inline uint16_t mavlink_msg_param_request_read_encode(uint8_t system_id,
 }
 
 /**
+ * @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
  *
@@ -124,19 +149,27 @@ static inline uint16_t mavlink_msg_param_request_read_encode(uint8_t system_id,
 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[20];
+	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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, buf, 20, 214);
+#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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, (const char *)&packet, 20, 214);
+#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
 }
 
@@ -199,6 +232,6 @@ static inline void mavlink_msg_param_request_read_decode(const mavlink_message_t
 	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), 20);
+	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
index 8f00f22e9..1bd1f0059 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h
@@ -14,6 +14,9 @@ typedef struct __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 { \
@@ -45,13 +48,13 @@ static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t com
 						       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[23];
+	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, 23);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_SET_LEN);
 #else
 	mavlink_param_set_t packet;
 	packet.param_value = param_value;
@@ -59,18 +62,22 @@ static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t com
 	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, 23);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_SET_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_PARAM_SET;
-	return mavlink_finalize_message(msg, system_id, component_id, 23, 168);
+#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 was sent over
+ * @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
@@ -84,13 +91,13 @@ static inline uint16_t mavlink_msg_param_set_pack_chan(uint8_t system_id, uint8_
 						           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[23];
+	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, 23);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_SET_LEN);
 #else
 	mavlink_param_set_t packet;
 	packet.param_value = param_value;
@@ -98,15 +105,19 @@ static inline uint16_t mavlink_msg_param_set_pack_chan(uint8_t system_id, uint8_
 	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, 23);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_SET_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_PARAM_SET;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 23, 168);
+#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 into a message
+ * @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)
@@ -119,6 +130,20 @@ static inline uint16_t mavlink_msg_param_set_encode(uint8_t system_id, uint8_t c
 }
 
 /**
+ * @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
  *
@@ -133,13 +158,17 @@ static inline uint16_t mavlink_msg_param_set_encode(uint8_t system_id, uint8_t c
 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[23];
+	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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, buf, 23, 168);
+#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;
@@ -147,7 +176,11 @@ static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t ta
 	packet.target_component = target_component;
 	packet.param_type = param_type;
 	mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, (const char *)&packet, 23, 168);
+#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
 }
 
@@ -221,6 +254,6 @@ static inline void mavlink_msg_param_set_decode(const mavlink_message_t* msg, ma
 	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), 23);
+	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
index 03a631984..17c759811 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h
@@ -14,6 +14,9 @@ typedef struct __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 { \
@@ -45,13 +48,13 @@ static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t c
 						       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[25];
+	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, 25);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN);
 #else
 	mavlink_param_value_t packet;
 	packet.param_value = param_value;
@@ -59,18 +62,22 @@ static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t c
 	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, 25);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE;
-	return mavlink_finalize_message(msg, system_id, component_id, 25, 220);
+#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 was sent over
+ * @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
@@ -84,13 +91,13 @@ static inline uint16_t mavlink_msg_param_value_pack_chan(uint8_t system_id, uint
 						           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[25];
+	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, 25);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN);
 #else
 	mavlink_param_value_t packet;
 	packet.param_value = param_value;
@@ -98,15 +105,19 @@ static inline uint16_t mavlink_msg_param_value_pack_chan(uint8_t system_id, uint
 	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, 25);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 25, 220);
+#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 into a message
+ * @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)
@@ -119,6 +130,20 @@ static inline uint16_t mavlink_msg_param_value_encode(uint8_t system_id, uint8_t
 }
 
 /**
+ * @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
  *
@@ -133,13 +158,17 @@ static inline uint16_t mavlink_msg_param_value_encode(uint8_t system_id, uint8_t
 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[25];
+	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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, buf, 25, 220);
+#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;
@@ -147,7 +176,11 @@ static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const ch
 	packet.param_index = param_index;
 	packet.param_type = param_type;
 	mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, (const char *)&packet, 25, 220);
+#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
 }
 
@@ -221,6 +254,6 @@ static inline void mavlink_msg_param_value_decode(const mavlink_message_t* 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), 25);
+	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
index 3ed1b9d7c..0c68ca176 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h
@@ -13,6 +13,9 @@ typedef struct __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 { \
@@ -42,13 +45,13 @@ static inline uint16_t mavlink_msg_ping_pack(uint8_t system_id, uint8_t componen
 						       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[14];
+	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, 14);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PING_LEN);
 #else
 	mavlink_ping_t packet;
 	packet.time_usec = time_usec;
@@ -56,18 +59,22 @@ static inline uint16_t mavlink_msg_ping_pack(uint8_t system_id, uint8_t componen
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 14);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PING_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_PING;
-	return mavlink_finalize_message(msg, system_id, component_id, 14, 237);
+#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 was sent over
+ * @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
@@ -80,13 +87,13 @@ static inline uint16_t mavlink_msg_ping_pack_chan(uint8_t system_id, uint8_t com
 						           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[14];
+	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, 14);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PING_LEN);
 #else
 	mavlink_ping_t packet;
 	packet.time_usec = time_usec;
@@ -94,15 +101,19 @@ static inline uint16_t mavlink_msg_ping_pack_chan(uint8_t system_id, uint8_t com
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 14);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PING_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_PING;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14, 237);
+#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 into a message
+ * @brief Encode a ping struct
  *
  * @param system_id ID of this system
  * @param component_id ID of this component (e.g. 200 for IMU)
@@ -115,6 +126,20 @@ static inline uint16_t mavlink_msg_ping_encode(uint8_t system_id, uint8_t compon
 }
 
 /**
+ * @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
  *
@@ -128,13 +153,17 @@ static inline uint16_t mavlink_msg_ping_encode(uint8_t system_id, uint8_t compon
 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[14];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, buf, 14, 237);
+#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;
@@ -142,7 +171,11 @@ static inline void mavlink_msg_ping_send(mavlink_channel_t chan, uint64_t time_u
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, (const char *)&packet, 14, 237);
+#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
 }
 
@@ -205,6 +238,6 @@ static inline void mavlink_msg_ping_decode(const mavlink_message_t* msg, mavlink
 	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), 14);
+	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
new file mode 100644
index 000000000..fceb7d168
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h
@@ -0,0 +1,309 @@
+// 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
index 1c1d48388..62a9b6eea 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h
@@ -19,6 +19,9 @@ typedef struct __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 { \
@@ -60,7 +63,7 @@ static inline uint16_t mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t compo
 						       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[26];
+	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);
@@ -72,7 +75,7 @@ static inline uint16_t mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t compo
 	_mav_put_int16_t(buf, 22, ymag);
 	_mav_put_int16_t(buf, 24, zmag);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 26);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_IMU_LEN);
 #else
 	mavlink_raw_imu_t packet;
 	packet.time_usec = time_usec;
@@ -86,18 +89,22 @@ static inline uint16_t mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t compo
 	packet.ymag = ymag;
 	packet.zmag = zmag;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 26);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_IMU_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_RAW_IMU;
-	return mavlink_finalize_message(msg, system_id, component_id, 26, 144);
+#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 was sent over
+ * @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)
@@ -116,7 +123,7 @@ static inline uint16_t mavlink_msg_raw_imu_pack_chan(uint8_t system_id, uint8_t
 						           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[26];
+	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);
@@ -128,7 +135,7 @@ static inline uint16_t mavlink_msg_raw_imu_pack_chan(uint8_t system_id, uint8_t
 	_mav_put_int16_t(buf, 22, ymag);
 	_mav_put_int16_t(buf, 24, zmag);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 26);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_IMU_LEN);
 #else
 	mavlink_raw_imu_t packet;
 	packet.time_usec = time_usec;
@@ -142,15 +149,19 @@ static inline uint16_t mavlink_msg_raw_imu_pack_chan(uint8_t system_id, uint8_t
 	packet.ymag = ymag;
 	packet.zmag = zmag;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 26);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_IMU_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_RAW_IMU;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 26, 144);
+#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 into a message
+ * @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)
@@ -163,6 +174,20 @@ static inline uint16_t mavlink_msg_raw_imu_encode(uint8_t system_id, uint8_t com
 }
 
 /**
+ * @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
  *
@@ -182,7 +207,7 @@ static inline uint16_t mavlink_msg_raw_imu_encode(uint8_t system_id, uint8_t com
 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[26];
+	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);
@@ -194,7 +219,11 @@ static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t tim
 	_mav_put_int16_t(buf, 22, ymag);
 	_mav_put_int16_t(buf, 24, zmag);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, buf, 26, 144);
+#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;
@@ -208,7 +237,11 @@ static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t tim
 	packet.ymag = ymag;
 	packet.zmag = zmag;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, (const char *)&packet, 26, 144);
+#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
 }
 
@@ -337,6 +370,6 @@ static inline void mavlink_msg_raw_imu_decode(const mavlink_message_t* msg, mavl
 	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), 26);
+	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
index f3e4e8404..82c5fad4a 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h
@@ -14,6 +14,9 @@ typedef struct __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 { \
@@ -45,14 +48,14 @@ static inline uint16_t mavlink_msg_raw_pressure_pack(uint8_t system_id, uint8_t
 						       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[16];
+	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, 16);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_PRESSURE_LEN);
 #else
 	mavlink_raw_pressure_t packet;
 	packet.time_usec = time_usec;
@@ -61,18 +64,22 @@ static inline uint16_t mavlink_msg_raw_pressure_pack(uint8_t system_id, uint8_t
 	packet.press_diff2 = press_diff2;
 	packet.temperature = temperature;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 16);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_PRESSURE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_RAW_PRESSURE;
-	return mavlink_finalize_message(msg, system_id, component_id, 16, 67);
+#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 was sent over
+ * @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)
@@ -86,14 +93,14 @@ static inline uint16_t mavlink_msg_raw_pressure_pack_chan(uint8_t system_id, uin
 						           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[16];
+	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, 16);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_PRESSURE_LEN);
 #else
 	mavlink_raw_pressure_t packet;
 	packet.time_usec = time_usec;
@@ -102,15 +109,19 @@ static inline uint16_t mavlink_msg_raw_pressure_pack_chan(uint8_t system_id, uin
 	packet.press_diff2 = press_diff2;
 	packet.temperature = temperature;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 16);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_PRESSURE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_RAW_PRESSURE;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16, 67);
+#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 into a message
+ * @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)
@@ -123,6 +134,20 @@ static inline uint16_t mavlink_msg_raw_pressure_encode(uint8_t system_id, uint8_
 }
 
 /**
+ * @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
  *
@@ -137,14 +162,18 @@ static inline uint16_t mavlink_msg_raw_pressure_encode(uint8_t system_id, uint8_
 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[16];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, buf, 16, 67);
+#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;
@@ -153,7 +182,11 @@ static inline void mavlink_msg_raw_pressure_send(mavlink_channel_t chan, uint64_
 	packet.press_diff2 = press_diff2;
 	packet.temperature = temperature;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, (const char *)&packet, 16, 67);
+#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
 }
 
@@ -227,6 +260,6 @@ static inline void mavlink_msg_raw_pressure_decode(const mavlink_message_t* 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), 16);
+	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
index d719c7fca..0d8a1514b 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h
@@ -4,14 +4,14 @@
 
 typedef struct __mavlink_rc_channels_override_t
 {
- 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 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;
@@ -19,6 +19,9 @@ typedef struct __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 { \
@@ -46,21 +49,21 @@ typedef struct __mavlink_rc_channels_override_t
  *
  * @param target_system System ID
  * @param target_component Component ID
- * @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 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[18];
+	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);
@@ -72,7 +75,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id,
 	_mav_put_uint8_t(buf, 16, target_system);
 	_mav_put_uint8_t(buf, 17, target_component);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 18);
+        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;
@@ -86,29 +89,33 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id,
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 18);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE;
-	return mavlink_finalize_message(msg, system_id, component_id, 18, 124);
+#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 was sent over
+ * @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
- * @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 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,
@@ -116,7 +123,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack_chan(uint8_t system
 						           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[18];
+	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);
@@ -128,7 +135,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack_chan(uint8_t system
 	_mav_put_uint8_t(buf, 16, target_system);
 	_mav_put_uint8_t(buf, 17, target_component);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 18);
+        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;
@@ -142,15 +149,19 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack_chan(uint8_t system
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 18);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 124);
+#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 into a message
+ * @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)
@@ -163,26 +174,40 @@ static inline uint16_t mavlink_msg_rc_channels_override_encode(uint8_t system_id
 }
 
 /**
+ * @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
- * @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 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[18];
+	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);
@@ -194,7 +219,11 @@ static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan,
 	_mav_put_uint8_t(buf, 16, target_system);
 	_mav_put_uint8_t(buf, 17, target_component);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, buf, 18, 124);
+#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;
@@ -208,7 +237,11 @@ static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan,
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, (const char *)&packet, 18, 124);
+#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
 }
 
@@ -240,7 +273,7 @@ static inline uint8_t mavlink_msg_rc_channels_override_get_target_component(cons
 /**
  * @brief Get field chan1_raw from rc_channels_override message
  *
- * @return RC channel 1 value, in microseconds
+ * @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)
 {
@@ -250,7 +283,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan1_raw(const mavl
 /**
  * @brief Get field chan2_raw from rc_channels_override message
  *
- * @return RC channel 2 value, in microseconds
+ * @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)
 {
@@ -260,7 +293,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan2_raw(const mavl
 /**
  * @brief Get field chan3_raw from rc_channels_override message
  *
- * @return RC channel 3 value, in microseconds
+ * @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)
 {
@@ -270,7 +303,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan3_raw(const mavl
 /**
  * @brief Get field chan4_raw from rc_channels_override message
  *
- * @return RC channel 4 value, in microseconds
+ * @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)
 {
@@ -280,7 +313,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan4_raw(const mavl
 /**
  * @brief Get field chan5_raw from rc_channels_override message
  *
- * @return RC channel 5 value, in microseconds
+ * @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)
 {
@@ -290,7 +323,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan5_raw(const mavl
 /**
  * @brief Get field chan6_raw from rc_channels_override message
  *
- * @return RC channel 6 value, in microseconds
+ * @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)
 {
@@ -300,7 +333,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan6_raw(const mavl
 /**
  * @brief Get field chan7_raw from rc_channels_override message
  *
- * @return RC channel 7 value, in microseconds
+ * @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)
 {
@@ -310,7 +343,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan7_raw(const mavl
 /**
  * @brief Get field chan8_raw from rc_channels_override message
  *
- * @return RC channel 8 value, in microseconds
+ * @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)
 {
@@ -337,6 +370,6 @@ static inline void mavlink_msg_rc_channels_override_decode(const mavlink_message
 	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), 18);
+	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
index a4d40da38..3c0aed020 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h
@@ -5,14 +5,14 @@
 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 65535 implies the channel is unused.
- uint16_t chan2_raw; ///< RC channel 2 value, in microseconds. A value of 65535 implies the channel is unused.
- uint16_t chan3_raw; ///< RC channel 3 value, in microseconds. A value of 65535 implies the channel is unused.
- uint16_t chan4_raw; ///< RC channel 4 value, in microseconds. A value of 65535 implies the channel is unused.
- uint16_t chan5_raw; ///< RC channel 5 value, in microseconds. A value of 65535 implies the channel is unused.
- uint16_t chan6_raw; ///< RC channel 6 value, in microseconds. A value of 65535 implies the channel is unused.
- uint16_t chan7_raw; ///< RC channel 7 value, in microseconds. A value of 65535 implies the channel is unused.
- uint16_t chan8_raw; ///< RC channel 8 value, in microseconds. A value of 65535 implies the channel is unused.
+ 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;
@@ -20,6 +20,9 @@ typedef struct __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 { \
@@ -48,14 +51,14 @@ typedef struct __mavlink_rc_channels_raw_t
  *
  * @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 65535 implies the channel is unused.
- * @param chan2_raw RC channel 2 value, in microseconds. A value of 65535 implies the channel is unused.
- * @param chan3_raw RC channel 3 value, in microseconds. A value of 65535 implies the channel is unused.
- * @param chan4_raw RC channel 4 value, in microseconds. A value of 65535 implies the channel is unused.
- * @param chan5_raw RC channel 5 value, in microseconds. A value of 65535 implies the channel is unused.
- * @param chan6_raw RC channel 6 value, in microseconds. A value of 65535 implies the channel is unused.
- * @param chan7_raw RC channel 7 value, in microseconds. A value of 65535 implies the channel is unused.
- * @param chan8_raw RC channel 8 value, in microseconds. A value of 65535 implies the channel is unused.
+ * @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)
  */
@@ -63,7 +66,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_pack(uint8_t system_id, uint8
 						       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[22];
+	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);
@@ -76,7 +79,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_pack(uint8_t system_id, uint8
 	_mav_put_uint8_t(buf, 20, port);
 	_mav_put_uint8_t(buf, 21, rssi);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 22);
+        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;
@@ -91,29 +94,33 @@ static inline uint16_t mavlink_msg_rc_channels_raw_pack(uint8_t system_id, uint8
 	packet.port = port;
 	packet.rssi = rssi;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 22);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW;
-	return mavlink_finalize_message(msg, system_id, component_id, 22, 244);
+#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 was sent over
+ * @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 65535 implies the channel is unused.
- * @param chan2_raw RC channel 2 value, in microseconds. A value of 65535 implies the channel is unused.
- * @param chan3_raw RC channel 3 value, in microseconds. A value of 65535 implies the channel is unused.
- * @param chan4_raw RC channel 4 value, in microseconds. A value of 65535 implies the channel is unused.
- * @param chan5_raw RC channel 5 value, in microseconds. A value of 65535 implies the channel is unused.
- * @param chan6_raw RC channel 6 value, in microseconds. A value of 65535 implies the channel is unused.
- * @param chan7_raw RC channel 7 value, in microseconds. A value of 65535 implies the channel is unused.
- * @param chan8_raw RC channel 8 value, in microseconds. A value of 65535 implies the channel is unused.
+ * @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)
  */
@@ -122,7 +129,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_pack_chan(uint8_t system_id,
 						           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[22];
+	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);
@@ -135,7 +142,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_pack_chan(uint8_t system_id,
 	_mav_put_uint8_t(buf, 20, port);
 	_mav_put_uint8_t(buf, 21, rssi);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 22);
+        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;
@@ -150,15 +157,19 @@ static inline uint16_t mavlink_msg_rc_channels_raw_pack_chan(uint8_t system_id,
 	packet.port = port;
 	packet.rssi = rssi;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 22);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 22, 244);
+#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 into a message
+ * @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)
@@ -171,19 +182,33 @@ static inline uint16_t mavlink_msg_rc_channels_raw_encode(uint8_t system_id, uin
 }
 
 /**
+ * @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 65535 implies the channel is unused.
- * @param chan2_raw RC channel 2 value, in microseconds. A value of 65535 implies the channel is unused.
- * @param chan3_raw RC channel 3 value, in microseconds. A value of 65535 implies the channel is unused.
- * @param chan4_raw RC channel 4 value, in microseconds. A value of 65535 implies the channel is unused.
- * @param chan5_raw RC channel 5 value, in microseconds. A value of 65535 implies the channel is unused.
- * @param chan6_raw RC channel 6 value, in microseconds. A value of 65535 implies the channel is unused.
- * @param chan7_raw RC channel 7 value, in microseconds. A value of 65535 implies the channel is unused.
- * @param chan8_raw RC channel 8 value, in microseconds. A value of 65535 implies the channel is unused.
+ * @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
@@ -191,7 +216,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_encode(uint8_t system_id, uin
 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[22];
+	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);
@@ -204,7 +229,11 @@ static inline void mavlink_msg_rc_channels_raw_send(mavlink_channel_t chan, uint
 	_mav_put_uint8_t(buf, 20, port);
 	_mav_put_uint8_t(buf, 21, rssi);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, buf, 22, 244);
+#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;
@@ -219,7 +248,11 @@ static inline void mavlink_msg_rc_channels_raw_send(mavlink_channel_t chan, uint
 	packet.port = port;
 	packet.rssi = rssi;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, (const char *)&packet, 22, 244);
+#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
 }
 
@@ -251,7 +284,7 @@ static inline uint8_t mavlink_msg_rc_channels_raw_get_port(const mavlink_message
 /**
  * @brief Get field chan1_raw from rc_channels_raw message
  *
- * @return RC channel 1 value, in microseconds. A value of 65535 implies the channel is unused.
+ * @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)
 {
@@ -261,7 +294,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan1_raw(const mavlink_m
 /**
  * @brief Get field chan2_raw from rc_channels_raw message
  *
- * @return RC channel 2 value, in microseconds. A value of 65535 implies the channel is unused.
+ * @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)
 {
@@ -271,7 +304,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan2_raw(const mavlink_m
 /**
  * @brief Get field chan3_raw from rc_channels_raw message
  *
- * @return RC channel 3 value, in microseconds. A value of 65535 implies the channel is unused.
+ * @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)
 {
@@ -281,7 +314,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan3_raw(const mavlink_m
 /**
  * @brief Get field chan4_raw from rc_channels_raw message
  *
- * @return RC channel 4 value, in microseconds. A value of 65535 implies the channel is unused.
+ * @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)
 {
@@ -291,7 +324,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan4_raw(const mavlink_m
 /**
  * @brief Get field chan5_raw from rc_channels_raw message
  *
- * @return RC channel 5 value, in microseconds. A value of 65535 implies the channel is unused.
+ * @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)
 {
@@ -301,7 +334,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan5_raw(const mavlink_m
 /**
  * @brief Get field chan6_raw from rc_channels_raw message
  *
- * @return RC channel 6 value, in microseconds. A value of 65535 implies the channel is unused.
+ * @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)
 {
@@ -311,7 +344,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan6_raw(const mavlink_m
 /**
  * @brief Get field chan7_raw from rc_channels_raw message
  *
- * @return RC channel 7 value, in microseconds. A value of 65535 implies the channel is unused.
+ * @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)
 {
@@ -321,7 +354,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan7_raw(const mavlink_m
 /**
  * @brief Get field chan8_raw from rc_channels_raw message
  *
- * @return RC channel 8 value, in microseconds. A value of 65535 implies the channel is unused.
+ * @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)
 {
@@ -359,6 +392,6 @@ static inline void mavlink_msg_rc_channels_raw_decode(const mavlink_message_t* m
 	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), 22);
+	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
index dd21d4162..2153ab392 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h
@@ -5,14 +5,14 @@
 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) 32767.
- int16_t chan2_scaled; ///< RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
- int16_t chan3_scaled; ///< RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
- int16_t chan4_scaled; ///< RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
- int16_t chan5_scaled; ///< RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
- int16_t chan6_scaled; ///< RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
- int16_t chan7_scaled; ///< RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
- int16_t chan8_scaled; ///< RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
+ 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;
@@ -20,6 +20,9 @@ typedef struct __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 { \
@@ -48,14 +51,14 @@ typedef struct __mavlink_rc_channels_scaled_t
  *
  * @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) 32767.
- * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
- * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
- * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
- * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
- * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
- * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
- * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
+ * @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)
  */
@@ -63,7 +66,7 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_pack(uint8_t system_id, ui
 						       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[22];
+	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);
@@ -76,7 +79,7 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_pack(uint8_t system_id, ui
 	_mav_put_uint8_t(buf, 20, port);
 	_mav_put_uint8_t(buf, 21, rssi);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 22);
+        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;
@@ -91,29 +94,33 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_pack(uint8_t system_id, ui
 	packet.port = port;
 	packet.rssi = rssi;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 22);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED;
-	return mavlink_finalize_message(msg, system_id, component_id, 22, 237);
+#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 was sent over
+ * @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) 32767.
- * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
- * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
- * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
- * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
- * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
- * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
- * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
+ * @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)
  */
@@ -122,7 +129,7 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_pack_chan(uint8_t system_i
 						           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[22];
+	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);
@@ -135,7 +142,7 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_pack_chan(uint8_t system_i
 	_mav_put_uint8_t(buf, 20, port);
 	_mav_put_uint8_t(buf, 21, rssi);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 22);
+        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;
@@ -150,15 +157,19 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_pack_chan(uint8_t system_i
 	packet.port = port;
 	packet.rssi = rssi;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 22);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 22, 237);
+#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 into a message
+ * @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)
@@ -171,19 +182,33 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_encode(uint8_t system_id,
 }
 
 /**
+ * @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) 32767.
- * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
- * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
- * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
- * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
- * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
- * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
- * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
+ * @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
@@ -191,7 +216,7 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_encode(uint8_t system_id,
 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[22];
+	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);
@@ -204,7 +229,11 @@ static inline void mavlink_msg_rc_channels_scaled_send(mavlink_channel_t chan, u
 	_mav_put_uint8_t(buf, 20, port);
 	_mav_put_uint8_t(buf, 21, rssi);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, buf, 22, 237);
+#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;
@@ -219,7 +248,11 @@ static inline void mavlink_msg_rc_channels_scaled_send(mavlink_channel_t chan, u
 	packet.port = port;
 	packet.rssi = rssi;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, (const char *)&packet, 22, 237);
+#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
 }
 
@@ -251,7 +284,7 @@ static inline uint8_t mavlink_msg_rc_channels_scaled_get_port(const mavlink_mess
 /**
  * @brief Get field chan1_scaled from rc_channels_scaled message
  *
- * @return RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
+ * @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)
 {
@@ -261,7 +294,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan1_scaled(const mavl
 /**
  * @brief Get field chan2_scaled from rc_channels_scaled message
  *
- * @return RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
+ * @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)
 {
@@ -271,7 +304,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan2_scaled(const mavl
 /**
  * @brief Get field chan3_scaled from rc_channels_scaled message
  *
- * @return RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
+ * @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)
 {
@@ -281,7 +314,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan3_scaled(const mavl
 /**
  * @brief Get field chan4_scaled from rc_channels_scaled message
  *
- * @return RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
+ * @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)
 {
@@ -291,7 +324,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan4_scaled(const mavl
 /**
  * @brief Get field chan5_scaled from rc_channels_scaled message
  *
- * @return RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
+ * @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)
 {
@@ -301,7 +334,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan5_scaled(const mavl
 /**
  * @brief Get field chan6_scaled from rc_channels_scaled message
  *
- * @return RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
+ * @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)
 {
@@ -311,7 +344,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan6_scaled(const mavl
 /**
  * @brief Get field chan7_scaled from rc_channels_scaled message
  *
- * @return RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
+ * @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)
 {
@@ -321,7 +354,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan7_scaled(const mavl
 /**
  * @brief Get field chan8_scaled from rc_channels_scaled message
  *
- * @return RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767.
+ * @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)
 {
@@ -359,6 +392,6 @@ static inline void mavlink_msg_rc_channels_scaled_decode(const mavlink_message_t
 	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), 22);
+	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
index d8653ad10..c754ad876 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h
@@ -14,6 +14,9 @@ typedef struct __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 { \
@@ -45,14 +48,14 @@ static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, u
 						       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[6];
+	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, 6);
+        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;
@@ -61,18 +64,22 @@ static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, u
 	packet.req_stream_id = req_stream_id;
 	packet.start_stop = start_stop;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 6);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM;
-	return mavlink_finalize_message(msg, system_id, component_id, 6, 148);
+#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 was sent over
+ * @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.
@@ -86,14 +93,14 @@ static inline uint16_t mavlink_msg_request_data_stream_pack_chan(uint8_t system_
 						           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[6];
+	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, 6);
+        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;
@@ -102,15 +109,19 @@ static inline uint16_t mavlink_msg_request_data_stream_pack_chan(uint8_t system_
 	packet.req_stream_id = req_stream_id;
 	packet.start_stop = start_stop;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 6);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 6, 148);
+#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 into a message
+ * @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)
@@ -123,6 +134,20 @@ static inline uint16_t mavlink_msg_request_data_stream_encode(uint8_t system_id,
 }
 
 /**
+ * @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
  *
@@ -137,14 +162,18 @@ static inline uint16_t mavlink_msg_request_data_stream_encode(uint8_t system_id,
 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[6];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, buf, 6, 148);
+#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;
@@ -153,7 +182,11 @@ static inline void mavlink_msg_request_data_stream_send(mavlink_channel_t chan,
 	packet.req_stream_id = req_stream_id;
 	packet.start_stop = start_stop;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, (const char *)&packet, 6, 148);
+#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
 }
 
@@ -227,6 +260,6 @@ static inline void mavlink_msg_request_data_stream_decode(const mavlink_message_
 	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), 6);
+	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
index 47772e004..ac3ef4fa9 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h
@@ -14,6 +14,9 @@ typedef struct __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 { \
@@ -45,14 +48,14 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack(uin
 						       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[20];
+	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, 20);
+        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;
@@ -61,18 +64,22 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack(uin
 	packet.yaw_rate = yaw_rate;
 	packet.thrust = thrust;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 20);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 20, 127);
+#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 was sent over
+ * @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
@@ -86,14 +93,14 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack_cha
 						           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[20];
+	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, 20);
+        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;
@@ -102,15 +109,19 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack_cha
 	packet.yaw_rate = yaw_rate;
 	packet.thrust = thrust;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 20);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 127);
+#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 into a message
+ * @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)
@@ -123,6 +134,20 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_encode(u
 }
 
 /**
+ * @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
  *
@@ -137,14 +162,18 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_encode(u
 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[20];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, buf, 20, 127);
+#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;
@@ -153,7 +182,11 @@ static inline void mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(mavlink
 	packet.yaw_rate = yaw_rate;
 	packet.thrust = thrust;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, (const char *)&packet, 20, 127);
+#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
 }
 
@@ -227,6 +260,6 @@ static inline void mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_decode(const
 	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), 20);
+	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
index 5751badc3..626477322 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h
@@ -14,6 +14,9 @@ typedef struct __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 { \
@@ -45,14 +48,14 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack(uin
 						       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[20];
+	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, 20);
+        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;
@@ -61,18 +64,22 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack(uin
 	packet.yaw_speed = yaw_speed;
 	packet.thrust = thrust;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 20);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 20, 238);
+#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 was sent over
+ * @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
@@ -86,14 +93,14 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_cha
 						           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[20];
+	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, 20);
+        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;
@@ -102,15 +109,19 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_cha
 	packet.yaw_speed = yaw_speed;
 	packet.thrust = thrust;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 20);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 238);
+#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 into a message
+ * @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)
@@ -123,6 +134,20 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_encode(u
 }
 
 /**
+ * @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
  *
@@ -137,14 +162,18 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_encode(u
 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[20];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, buf, 20, 238);
+#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;
@@ -153,7 +182,11 @@ static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_send(mavlink
 	packet.yaw_speed = yaw_speed;
 	packet.thrust = thrust;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, (const char *)&packet, 20, 238);
+#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
 }
 
@@ -227,6 +260,6 @@ static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_decode(const
 	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), 20);
+	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
index a9f6ad0ca..ffcdc547b 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h
@@ -14,6 +14,9 @@ typedef struct __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 { \
@@ -45,14 +48,14 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack(uint8_t s
 						       uint32_t time_boot_ms, float roll, float pitch, float yaw, float thrust)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[20];
+	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, 20);
+        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;
@@ -61,18 +64,22 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack(uint8_t s
 	packet.yaw = yaw;
 	packet.thrust = thrust;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 20);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 20, 239);
+#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 was sent over
+ * @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
@@ -86,14 +93,14 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan(uint
 						           uint32_t time_boot_ms,float roll,float pitch,float yaw,float thrust)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[20];
+	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, 20);
+        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;
@@ -102,15 +109,19 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan(uint
 	packet.yaw = yaw;
 	packet.thrust = thrust;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 20);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 239);
+#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 into a message
+ * @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)
@@ -123,6 +134,20 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_encode(uint8_t
 }
 
 /**
+ * @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
  *
@@ -137,14 +162,18 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_encode(uint8_t
 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[20];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, buf, 20, 239);
+#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;
@@ -153,7 +182,11 @@ static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(mavlink_chann
 	packet.yaw = yaw;
 	packet.thrust = thrust;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, (const char *)&packet, 20, 239);
+#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
 }
 
@@ -227,6 +260,6 @@ static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(const mavli
 	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), 20);
+	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
index aae6fd206..fcd54cbb7 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h
@@ -16,6 +16,9 @@ typedef struct __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 { \
@@ -51,7 +54,7 @@ static inline uint16_t mavlink_msg_safety_allowed_area_pack(uint8_t system_id, u
 						       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[25];
+	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);
@@ -60,7 +63,7 @@ static inline uint16_t mavlink_msg_safety_allowed_area_pack(uint8_t system_id, u
 	_mav_put_float(buf, 20, p2z);
 	_mav_put_uint8_t(buf, 24, frame);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 25);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN);
 #else
 	mavlink_safety_allowed_area_t packet;
 	packet.p1x = p1x;
@@ -71,18 +74,22 @@ static inline uint16_t mavlink_msg_safety_allowed_area_pack(uint8_t system_id, u
 	packet.p2z = p2z;
 	packet.frame = frame;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 25);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA;
-	return mavlink_finalize_message(msg, system_id, component_id, 25, 3);
+#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 was sent over
+ * @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
@@ -98,7 +105,7 @@ static inline uint16_t mavlink_msg_safety_allowed_area_pack_chan(uint8_t system_
 						           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[25];
+	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);
@@ -107,7 +114,7 @@ static inline uint16_t mavlink_msg_safety_allowed_area_pack_chan(uint8_t system_
 	_mav_put_float(buf, 20, p2z);
 	_mav_put_uint8_t(buf, 24, frame);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 25);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN);
 #else
 	mavlink_safety_allowed_area_t packet;
 	packet.p1x = p1x;
@@ -118,15 +125,19 @@ static inline uint16_t mavlink_msg_safety_allowed_area_pack_chan(uint8_t system_
 	packet.p2z = p2z;
 	packet.frame = frame;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 25);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 25, 3);
+#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 into a message
+ * @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)
@@ -139,6 +150,20 @@ static inline uint16_t mavlink_msg_safety_allowed_area_encode(uint8_t system_id,
 }
 
 /**
+ * @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
  *
@@ -155,7 +180,7 @@ static inline uint16_t mavlink_msg_safety_allowed_area_encode(uint8_t system_id,
 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[25];
+	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);
@@ -164,7 +189,11 @@ static inline void mavlink_msg_safety_allowed_area_send(mavlink_channel_t chan,
 	_mav_put_float(buf, 20, p2z);
 	_mav_put_uint8_t(buf, 24, frame);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, buf, 25, 3);
+#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;
@@ -175,7 +204,11 @@ static inline void mavlink_msg_safety_allowed_area_send(mavlink_channel_t chan,
 	packet.p2z = p2z;
 	packet.frame = frame;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, (const char *)&packet, 25, 3);
+#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
 }
 
@@ -271,6 +304,6 @@ static inline void mavlink_msg_safety_allowed_area_decode(const mavlink_message_
 	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), 25);
+	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
index 8fb410c2d..61f6af155 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h
@@ -18,6 +18,9 @@ typedef struct __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 { \
@@ -57,7 +60,7 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_pack(uint8_t system_i
 						       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[27];
+	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);
@@ -68,7 +71,7 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_pack(uint8_t system_i
 	_mav_put_uint8_t(buf, 25, target_component);
 	_mav_put_uint8_t(buf, 26, frame);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 27);
+        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;
@@ -81,18 +84,22 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_pack(uint8_t system_i
 	packet.target_component = target_component;
 	packet.frame = frame;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 27);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 27, 15);
+#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 was sent over
+ * @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
@@ -110,7 +117,7 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_pack_chan(uint8_t sys
 						           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[27];
+	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);
@@ -121,7 +128,7 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_pack_chan(uint8_t sys
 	_mav_put_uint8_t(buf, 25, target_component);
 	_mav_put_uint8_t(buf, 26, frame);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 27);
+        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;
@@ -134,15 +141,19 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_pack_chan(uint8_t sys
 	packet.target_component = target_component;
 	packet.frame = frame;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 27);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 27, 15);
+#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 into a message
+ * @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)
@@ -155,6 +166,20 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_encode(uint8_t system
 }
 
 /**
+ * @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
  *
@@ -173,7 +198,7 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_encode(uint8_t system
 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[27];
+	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);
@@ -184,7 +209,11 @@ static inline void mavlink_msg_safety_set_allowed_area_send(mavlink_channel_t ch
 	_mav_put_uint8_t(buf, 25, target_component);
 	_mav_put_uint8_t(buf, 26, frame);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, buf, 27, 15);
+#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;
@@ -197,7 +226,11 @@ static inline void mavlink_msg_safety_set_allowed_area_send(mavlink_channel_t ch
 	packet.target_component = target_component;
 	packet.frame = frame;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, (const char *)&packet, 27, 15);
+#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
 }
 
@@ -315,6 +348,6 @@ static inline void mavlink_msg_safety_set_allowed_area_decode(const mavlink_mess
 	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), 27);
+	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
index 8ff098f39..3010d051a 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h
@@ -19,6 +19,9 @@ typedef struct __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 { \
@@ -60,7 +63,7 @@ static inline uint16_t mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t co
 						       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[22];
+	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);
@@ -72,7 +75,7 @@ static inline uint16_t mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t co
 	_mav_put_int16_t(buf, 18, ymag);
 	_mav_put_int16_t(buf, 20, zmag);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 22);
+        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;
@@ -86,18 +89,22 @@ static inline uint16_t mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t co
 	packet.ymag = ymag;
 	packet.zmag = zmag;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 22);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_SCALED_IMU;
-	return mavlink_finalize_message(msg, system_id, component_id, 22, 170);
+#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 was sent over
+ * @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)
@@ -116,7 +123,7 @@ static inline uint16_t mavlink_msg_scaled_imu_pack_chan(uint8_t system_id, uint8
 						           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[22];
+	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);
@@ -128,7 +135,7 @@ static inline uint16_t mavlink_msg_scaled_imu_pack_chan(uint8_t system_id, uint8
 	_mav_put_int16_t(buf, 18, ymag);
 	_mav_put_int16_t(buf, 20, zmag);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 22);
+        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;
@@ -142,15 +149,19 @@ static inline uint16_t mavlink_msg_scaled_imu_pack_chan(uint8_t system_id, uint8
 	packet.ymag = ymag;
 	packet.zmag = zmag;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 22);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_SCALED_IMU;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 22, 170);
+#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 into a message
+ * @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)
@@ -163,6 +174,20 @@ static inline uint16_t mavlink_msg_scaled_imu_encode(uint8_t system_id, uint8_t
 }
 
 /**
+ * @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
  *
@@ -182,7 +207,7 @@ static inline uint16_t mavlink_msg_scaled_imu_encode(uint8_t system_id, uint8_t
 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[22];
+	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);
@@ -194,7 +219,11 @@ static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint32_t
 	_mav_put_int16_t(buf, 18, ymag);
 	_mav_put_int16_t(buf, 20, zmag);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, buf, 22, 170);
+#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;
@@ -208,7 +237,11 @@ static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint32_t
 	packet.ymag = ymag;
 	packet.zmag = zmag;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, (const char *)&packet, 22, 170);
+#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
 }
 
@@ -337,6 +370,6 @@ static inline void mavlink_msg_scaled_imu_decode(const mavlink_message_t* msg, m
 	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), 22);
+	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
new file mode 100644
index 000000000..ea4dbbf81
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu2.h
@@ -0,0 +1,375 @@
+// 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
index d9ddcd47f..10324bc94 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h
@@ -13,6 +13,9 @@ typedef struct __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 { \
@@ -42,13 +45,13 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack(uint8_t system_id, uint8
 						       uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[14];
+	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, 14);
+        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;
@@ -56,18 +59,22 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack(uint8_t system_id, uint8
 	packet.press_diff = press_diff;
 	packet.temperature = temperature;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 14);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE;
-	return mavlink_finalize_message(msg, system_id, component_id, 14, 115);
+#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 was sent over
+ * @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)
@@ -80,13 +87,13 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack_chan(uint8_t system_id,
 						           uint32_t time_boot_ms,float press_abs,float press_diff,int16_t temperature)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[14];
+	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, 14);
+        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;
@@ -94,15 +101,19 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack_chan(uint8_t system_id,
 	packet.press_diff = press_diff;
 	packet.temperature = temperature;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 14);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14, 115);
+#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 into a message
+ * @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)
@@ -115,6 +126,20 @@ static inline uint16_t mavlink_msg_scaled_pressure_encode(uint8_t system_id, uin
 }
 
 /**
+ * @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
  *
@@ -128,13 +153,17 @@ static inline uint16_t mavlink_msg_scaled_pressure_encode(uint8_t system_id, uin
 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[14];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, buf, 14, 115);
+#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;
@@ -142,7 +171,11 @@ static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint
 	packet.press_diff = press_diff;
 	packet.temperature = temperature;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, (const char *)&packet, 14, 115);
+#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
 }
 
@@ -205,6 +238,6 @@ static inline void mavlink_msg_scaled_pressure_decode(const mavlink_message_t* m
 	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), 14);
+	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
index 45c94d8b9..6a14e93ed 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h
@@ -19,6 +19,9 @@ typedef struct __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 { \
@@ -60,7 +63,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint
 						       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[21];
+	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);
@@ -72,7 +75,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint
 	_mav_put_uint16_t(buf, 18, servo8_raw);
 	_mav_put_uint8_t(buf, 20, port);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 21);
+        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;
@@ -86,18 +89,22 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint
 	packet.servo8_raw = servo8_raw;
 	packet.port = port;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 21);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW;
-	return mavlink_finalize_message(msg, system_id, component_id, 21, 222);
+#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 was sent over
+ * @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.
@@ -116,7 +123,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack_chan(uint8_t system_id,
 						           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[21];
+	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);
@@ -128,7 +135,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack_chan(uint8_t system_id,
 	_mav_put_uint16_t(buf, 18, servo8_raw);
 	_mav_put_uint8_t(buf, 20, port);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 21);
+        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;
@@ -142,15 +149,19 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack_chan(uint8_t system_id,
 	packet.servo8_raw = servo8_raw;
 	packet.port = port;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 21);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 21, 222);
+#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 into a message
+ * @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)
@@ -163,6 +174,20 @@ static inline uint16_t mavlink_msg_servo_output_raw_encode(uint8_t system_id, ui
 }
 
 /**
+ * @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
  *
@@ -182,7 +207,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_encode(uint8_t system_id, ui
 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[21];
+	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);
@@ -194,7 +219,11 @@ static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uin
 	_mav_put_uint16_t(buf, 18, servo8_raw);
 	_mav_put_uint8_t(buf, 20, port);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, buf, 21, 222);
+#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;
@@ -208,7 +237,11 @@ static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uin
 	packet.servo8_raw = servo8_raw;
 	packet.port = port;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, (const char *)&packet, 21, 222);
+#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
 }
 
@@ -337,6 +370,6 @@ static inline void mavlink_msg_servo_output_raw_decode(const mavlink_message_t*
 	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), 21);
+	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
index 5b706fb50..6f0d7a69d 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h
@@ -4,9 +4,9 @@
 
 typedef struct __mavlink_set_global_position_setpoint_int_t
 {
- int32_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 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;
@@ -14,6 +14,9 @@ typedef struct __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 { \
@@ -35,9 +38,9 @@ typedef struct __mavlink_set_global_position_setpoint_int_t
  * @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 WGS84 Latitude position in degrees * 1E7
- * @param longitude WGS84 Longitude position in degrees * 1E7
- * @param altitude WGS84 Altitude in meters * 1000 (positive for up)
+ * @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)
  */
@@ -45,14 +48,14 @@ static inline uint16_t mavlink_msg_set_global_position_setpoint_int_pack(uint8_t
 						       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[15];
+	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, 15);
+        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;
@@ -61,23 +64,27 @@ static inline uint16_t mavlink_msg_set_global_position_setpoint_int_pack(uint8_t
 	packet.yaw = yaw;
 	packet.coordinate_frame = coordinate_frame;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 15);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 15, 33);
+#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 was sent over
+ * @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 WGS84 Latitude position in degrees * 1E7
- * @param longitude WGS84 Longitude position in degrees * 1E7
- * @param altitude WGS84 Altitude in meters * 1000 (positive for up)
+ * @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)
  */
@@ -86,14 +93,14 @@ static inline uint16_t mavlink_msg_set_global_position_setpoint_int_pack_chan(ui
 						           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[15];
+	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, 15);
+        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;
@@ -102,15 +109,19 @@ static inline uint16_t mavlink_msg_set_global_position_setpoint_int_pack_chan(ui
 	packet.yaw = yaw;
 	packet.coordinate_frame = coordinate_frame;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 15);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 15, 33);
+#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 into a message
+ * @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)
@@ -123,13 +134,27 @@ static inline uint16_t mavlink_msg_set_global_position_setpoint_int_encode(uint8
 }
 
 /**
+ * @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 WGS84 Latitude position in degrees * 1E7
- * @param longitude WGS84 Longitude position in degrees * 1E7
- * @param altitude WGS84 Altitude in meters * 1000 (positive for up)
+ * @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
@@ -137,14 +162,18 @@ static inline uint16_t mavlink_msg_set_global_position_setpoint_int_encode(uint8
 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[15];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, buf, 15, 33);
+#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;
@@ -153,7 +182,11 @@ static inline void mavlink_msg_set_global_position_setpoint_int_send(mavlink_cha
 	packet.yaw = yaw;
 	packet.coordinate_frame = coordinate_frame;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, (const char *)&packet, 15, 33);
+#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
 }
 
@@ -175,7 +208,7 @@ static inline uint8_t mavlink_msg_set_global_position_setpoint_int_get_coordinat
 /**
  * @brief Get field latitude from set_global_position_setpoint_int message
  *
- * @return WGS84 Latitude position in degrees * 1E7
+ * @return Latitude (WGS84), in degrees * 1E7
  */
 static inline int32_t mavlink_msg_set_global_position_setpoint_int_get_latitude(const mavlink_message_t* msg)
 {
@@ -185,7 +218,7 @@ static inline int32_t mavlink_msg_set_global_position_setpoint_int_get_latitude(
 /**
  * @brief Get field longitude from set_global_position_setpoint_int message
  *
- * @return WGS84 Longitude position in degrees * 1E7
+ * @return Longitude (WGS84), in degrees * 1E7
  */
 static inline int32_t mavlink_msg_set_global_position_setpoint_int_get_longitude(const mavlink_message_t* msg)
 {
@@ -195,7 +228,7 @@ static inline int32_t mavlink_msg_set_global_position_setpoint_int_get_longitude
 /**
  * @brief Get field altitude from set_global_position_setpoint_int message
  *
- * @return WGS84 Altitude in meters * 1000 (positive for up)
+ * @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)
 {
@@ -227,6 +260,6 @@ static inline void mavlink_msg_set_global_position_setpoint_int_decode(const mav
 	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), 15);
+	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
index af404b110..c444d8d52 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h
@@ -4,15 +4,18 @@
 
 typedef struct __mavlink_set_gps_global_origin_t
 {
- int32_t latitude; ///< global position * 1E7
- int32_t longitude; ///< global position * 1E7
- int32_t altitude; ///< global position * 1000
+ 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 { \
@@ -33,22 +36,22 @@ typedef struct __mavlink_set_gps_global_origin_t
  * @param msg The MAVLink message to compress the data into
  *
  * @param target_system System ID
- * @param latitude global position * 1E7
- * @param longitude global position * 1E7
- * @param altitude global position * 1000
+ * @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[13];
+	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, 13);
+        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;
@@ -56,23 +59,27 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_pack(uint8_t system_id,
 	packet.altitude = altitude;
 	packet.target_system = target_system;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 13);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 13, 41);
+#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 was sent over
+ * @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 global position * 1E7
- * @param longitude global position * 1E7
- * @param altitude global position * 1000
+ * @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,
@@ -80,13 +87,13 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_pack_chan(uint8_t syste
 						           uint8_t target_system,int32_t latitude,int32_t longitude,int32_t altitude)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[13];
+	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, 13);
+        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;
@@ -94,15 +101,19 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_pack_chan(uint8_t syste
 	packet.altitude = altitude;
 	packet.target_system = target_system;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 13);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 13, 41);
+#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 into a message
+ * @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)
@@ -115,26 +126,44 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_encode(uint8_t system_i
 }
 
 /**
+ * @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 global position * 1E7
- * @param longitude global position * 1E7
- * @param altitude global position * 1000
+ * @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[13];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, buf, 13, 41);
+#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;
@@ -142,7 +171,11 @@ static inline void mavlink_msg_set_gps_global_origin_send(mavlink_channel_t chan
 	packet.altitude = altitude;
 	packet.target_system = target_system;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, (const char *)&packet, 13, 41);
+#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
 }
 
@@ -164,7 +197,7 @@ static inline uint8_t mavlink_msg_set_gps_global_origin_get_target_system(const
 /**
  * @brief Get field latitude from set_gps_global_origin message
  *
- * @return global position * 1E7
+ * @return Latitude (WGS84), in degrees * 1E7
  */
 static inline int32_t mavlink_msg_set_gps_global_origin_get_latitude(const mavlink_message_t* msg)
 {
@@ -174,7 +207,7 @@ static inline int32_t mavlink_msg_set_gps_global_origin_get_latitude(const mavli
 /**
  * @brief Get field longitude from set_gps_global_origin message
  *
- * @return global position * 1E7
+ * @return Longitude (WGS84, in degrees * 1E7
  */
 static inline int32_t mavlink_msg_set_gps_global_origin_get_longitude(const mavlink_message_t* msg)
 {
@@ -184,7 +217,7 @@ static inline int32_t mavlink_msg_set_gps_global_origin_get_longitude(const mavl
 /**
  * @brief Get field altitude from set_gps_global_origin message
  *
- * @return global position * 1000
+ * @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)
 {
@@ -205,6 +238,6 @@ static inline void mavlink_msg_set_gps_global_origin_decode(const mavlink_messag
 	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), 13);
+	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
index 233e07d65..6f2835e03 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h
@@ -16,6 +16,9 @@ typedef struct __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 { \
@@ -51,7 +54,7 @@ static inline uint16_t mavlink_msg_set_local_position_setpoint_pack(uint8_t syst
 						       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[19];
+	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);
@@ -60,7 +63,7 @@ static inline uint16_t mavlink_msg_set_local_position_setpoint_pack(uint8_t syst
 	_mav_put_uint8_t(buf, 17, target_component);
 	_mav_put_uint8_t(buf, 18, coordinate_frame);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 19);
+        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;
@@ -71,18 +74,22 @@ static inline uint16_t mavlink_msg_set_local_position_setpoint_pack(uint8_t syst
 	packet.target_component = target_component;
 	packet.coordinate_frame = coordinate_frame;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 19);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 19, 214);
+#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 was sent over
+ * @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
@@ -98,7 +105,7 @@ static inline uint16_t mavlink_msg_set_local_position_setpoint_pack_chan(uint8_t
 						           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[19];
+	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);
@@ -107,7 +114,7 @@ static inline uint16_t mavlink_msg_set_local_position_setpoint_pack_chan(uint8_t
 	_mav_put_uint8_t(buf, 17, target_component);
 	_mav_put_uint8_t(buf, 18, coordinate_frame);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 19);
+        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;
@@ -118,15 +125,19 @@ static inline uint16_t mavlink_msg_set_local_position_setpoint_pack_chan(uint8_t
 	packet.target_component = target_component;
 	packet.coordinate_frame = coordinate_frame;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 19);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 19, 214);
+#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 into a message
+ * @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)
@@ -139,6 +150,20 @@ static inline uint16_t mavlink_msg_set_local_position_setpoint_encode(uint8_t sy
 }
 
 /**
+ * @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
  *
@@ -155,7 +180,7 @@ static inline uint16_t mavlink_msg_set_local_position_setpoint_encode(uint8_t sy
 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[19];
+	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);
@@ -164,7 +189,11 @@ static inline void mavlink_msg_set_local_position_setpoint_send(mavlink_channel_
 	_mav_put_uint8_t(buf, 17, target_component);
 	_mav_put_uint8_t(buf, 18, coordinate_frame);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, buf, 19, 214);
+#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;
@@ -175,7 +204,11 @@ static inline void mavlink_msg_set_local_position_setpoint_send(mavlink_channel_
 	packet.target_component = target_component;
 	packet.coordinate_frame = coordinate_frame;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, (const char *)&packet, 19, 214);
+#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
 }
 
@@ -271,6 +304,6 @@ static inline void mavlink_msg_set_local_position_setpoint_decode(const mavlink_
 	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), 19);
+	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
index fec21ab13..1aff42cce 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h
@@ -12,6 +12,9 @@ typedef struct __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 { \
@@ -39,30 +42,34 @@ static inline uint16_t mavlink_msg_set_mode_pack(uint8_t system_id, uint8_t comp
 						       uint8_t target_system, uint8_t base_mode, uint32_t custom_mode)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[6];
+	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, 6);
+        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, 6);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_MODE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_SET_MODE;
-	return mavlink_finalize_message(msg, system_id, component_id, 6, 89);
+#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 was sent over
+ * @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
@@ -74,27 +81,31 @@ static inline uint16_t mavlink_msg_set_mode_pack_chan(uint8_t system_id, uint8_t
 						           uint8_t target_system,uint8_t base_mode,uint32_t custom_mode)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[6];
+	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, 6);
+        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, 6);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_MODE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_SET_MODE;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 6, 89);
+#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 into a message
+ * @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)
@@ -107,6 +118,20 @@ static inline uint16_t mavlink_msg_set_mode_encode(uint8_t system_id, uint8_t co
 }
 
 /**
+ * @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
  *
@@ -119,19 +144,27 @@ static inline uint16_t mavlink_msg_set_mode_encode(uint8_t system_id, uint8_t co
 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[6];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, buf, 6, 89);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, (const char *)&packet, 6, 89);
+#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
 }
 
@@ -183,6 +216,6 @@ static inline void mavlink_msg_set_mode_decode(const mavlink_message_t* msg, mav
 	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), 6);
+	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
index 40ff8998e..8ceb8888f 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h
@@ -14,6 +14,9 @@ typedef struct __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 { \
@@ -45,14 +48,14 @@ static inline uint16_t mavlink_msg_set_quad_motors_setpoint_pack(uint8_t system_
 						       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[9];
+	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, 9);
+        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;
@@ -61,18 +64,22 @@ static inline uint16_t mavlink_msg_set_quad_motors_setpoint_pack(uint8_t system_
 	packet.motor_left_sw = motor_left_sw;
 	packet.target_system = target_system;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 9);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 9, 30);
+#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 was sent over
+ * @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
@@ -86,14 +93,14 @@ static inline uint16_t mavlink_msg_set_quad_motors_setpoint_pack_chan(uint8_t sy
 						           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[9];
+	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, 9);
+        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;
@@ -102,15 +109,19 @@ static inline uint16_t mavlink_msg_set_quad_motors_setpoint_pack_chan(uint8_t sy
 	packet.motor_left_sw = motor_left_sw;
 	packet.target_system = target_system;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 9);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 9, 30);
+#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 into a message
+ * @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)
@@ -123,6 +134,20 @@ static inline uint16_t mavlink_msg_set_quad_motors_setpoint_encode(uint8_t syste
 }
 
 /**
+ * @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
  *
@@ -137,14 +162,18 @@ static inline uint16_t mavlink_msg_set_quad_motors_setpoint_encode(uint8_t syste
 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[9];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT, buf, 9, 30);
+#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;
@@ -153,7 +182,11 @@ static inline void mavlink_msg_set_quad_motors_setpoint_send(mavlink_channel_t c
 	packet.motor_left_sw = motor_left_sw;
 	packet.target_system = target_system;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT, (const char *)&packet, 9, 30);
+#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
 }
 
@@ -227,6 +260,6 @@ static inline void mavlink_msg_set_quad_motors_setpoint_decode(const mavlink_mes
 	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), 9);
+	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
index 75a1420a1..9ef294cc9 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h
@@ -4,10 +4,10 @@
 
 typedef struct __mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t
 {
- int16_t roll[4]; ///< Desired roll angle in radians +-PI (+-32767)
- int16_t pitch[4]; ///< Desired pitch angle in radians +-PI (+-32767)
- int16_t yaw[4]; ///< Desired yaw angle in radians, scaled to int16 +-PI (+-32767)
- uint16_t thrust[4]; ///< Collective thrust, scaled to uint16 (0..65535)
+ 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)
@@ -18,6 +18,9 @@ typedef struct __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
@@ -53,17 +56,17 @@ typedef struct __mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t
  * @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 (+-32767)
- * @param pitch Desired pitch angle in radians +-PI (+-32767)
- * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-32767)
- * @param thrust Collective thrust, scaled to uint16 (0..65535)
+ * @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[46];
+	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);
@@ -73,7 +76,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack
 	_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, 46);
+        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;
@@ -85,28 +88,32 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack
 	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, 46);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 46, 130);
+#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 was sent over
+ * @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 (+-32767)
- * @param pitch Desired pitch angle in radians +-PI (+-32767)
- * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-32767)
- * @param thrust Collective thrust, scaled to uint16 (0..65535)
+ * @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,
@@ -114,7 +121,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack
 						           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[46];
+	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);
@@ -124,7 +131,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack
 	_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, 46);
+        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;
@@ -136,15 +143,19 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack
 	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, 46);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 46, 130);
+#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 into a message
+ * @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)
@@ -157,6 +168,20 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_enco
 }
 
 /**
+ * @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
  *
@@ -165,17 +190,17 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_enco
  * @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 (+-32767)
- * @param pitch Desired pitch angle in radians +-PI (+-32767)
- * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-32767)
- * @param thrust Collective thrust, scaled to uint16 (0..65535)
+ * @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[46];
+	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);
@@ -185,7 +210,11 @@ static inline void mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_send(mav
 	_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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, buf, 46, 130);
+#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;
@@ -197,7 +226,11 @@ static inline void mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_send(mav
 	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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, (const char *)&packet, 46, 130);
+#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
 }
 
@@ -259,7 +292,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_
 /**
  * @brief Get field roll from set_quad_swarm_led_roll_pitch_yaw_thrust message
  *
- * @return Desired roll angle in radians +-PI (+-32767)
+ * @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)
 {
@@ -269,7 +302,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_
 /**
  * @brief Get field pitch from set_quad_swarm_led_roll_pitch_yaw_thrust message
  *
- * @return Desired pitch angle in radians +-PI (+-32767)
+ * @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)
 {
@@ -279,7 +312,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_
 /**
  * @brief Get field yaw from set_quad_swarm_led_roll_pitch_yaw_thrust message
  *
- * @return Desired yaw angle in radians, scaled to int16 +-PI (+-32767)
+ * @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)
 {
@@ -289,7 +322,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_
 /**
  * @brief Get field thrust from set_quad_swarm_led_roll_pitch_yaw_thrust message
  *
- * @return Collective thrust, scaled to uint16 (0..65535)
+ * @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)
 {
@@ -315,6 +348,6 @@ static inline void mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_decode(c
 	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), 46);
+	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
index 35c5db18c..7d8d526f8 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h
@@ -4,10 +4,10 @@
 
 typedef struct __mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t
 {
- int16_t roll[4]; ///< Desired roll angle in radians +-PI (+-32767)
- int16_t pitch[4]; ///< Desired pitch angle in radians +-PI (+-32767)
- int16_t yaw[4]; ///< Desired yaw angle in radians, scaled to int16 +-PI (+-32767)
- uint16_t thrust[4]; ///< Collective thrust, scaled to uint16 (0..65535)
+ 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;
@@ -15,6 +15,9 @@ typedef struct __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
@@ -41,24 +44,24 @@ typedef struct __mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t
  *
  * @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 (+-32767)
- * @param pitch Desired pitch angle in radians +-PI (+-32767)
- * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-32767)
- * @param thrust Collective thrust, scaled to uint16 (0..65535)
+ * @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[34];
+	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, 34);
+        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;
@@ -67,25 +70,29 @@ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack(uin
 	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, 34);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 34, 240);
+#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 was sent over
+ * @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 (+-32767)
- * @param pitch Desired pitch angle in radians +-PI (+-32767)
- * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-32767)
- * @param thrust Collective thrust, scaled to uint16 (0..65535)
+ * @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,
@@ -93,14 +100,14 @@ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack_cha
 						           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[34];
+	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, 34);
+        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;
@@ -109,15 +116,19 @@ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack_cha
 	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, 34);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 34, 240);
+#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 into a message
+ * @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)
@@ -130,29 +141,47 @@ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_encode(u
 }
 
 /**
+ * @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 (+-32767)
- * @param pitch Desired pitch angle in radians +-PI (+-32767)
- * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-32767)
- * @param thrust Collective thrust, scaled to uint16 (0..65535)
+ * @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[34];
+	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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, buf, 34, 240);
+#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;
@@ -161,7 +190,11 @@ static inline void mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_send(mavlink
 	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_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, (const char *)&packet, 34, 240);
+#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
 }
 
@@ -193,7 +226,7 @@ static inline uint8_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_mode(
 /**
  * @brief Get field roll from set_quad_swarm_roll_pitch_yaw_thrust message
  *
- * @return Desired roll angle in radians +-PI (+-32767)
+ * @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)
 {
@@ -203,7 +236,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_roll
 /**
  * @brief Get field pitch from set_quad_swarm_roll_pitch_yaw_thrust message
  *
- * @return Desired pitch angle in radians +-PI (+-32767)
+ * @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)
 {
@@ -213,7 +246,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_pitc
 /**
  * @brief Get field yaw from set_quad_swarm_roll_pitch_yaw_thrust message
  *
- * @return Desired yaw angle in radians, scaled to int16 +-PI (+-32767)
+ * @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)
 {
@@ -223,7 +256,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_yaw(
 /**
  * @brief Get field thrust from set_quad_swarm_roll_pitch_yaw_thrust message
  *
- * @return Collective thrust, scaled to uint16 (0..65535)
+ * @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)
 {
@@ -246,6 +279,6 @@ static inline void mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(const
 	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), 34);
+	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
index b79a7e9f2..5846ba41f 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h
@@ -15,6 +15,9 @@ typedef struct __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 { \
@@ -48,7 +51,7 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(uint8_t
 						       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[18];
+	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);
@@ -56,7 +59,7 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(uint8_t
 	_mav_put_uint8_t(buf, 16, target_system);
 	_mav_put_uint8_t(buf, 17, target_component);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 18);
+        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;
@@ -66,18 +69,22 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(uint8_t
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 18);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 18, 24);
+#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 was sent over
+ * @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
@@ -92,7 +99,7 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan(uin
 						           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[18];
+	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);
@@ -100,7 +107,7 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan(uin
 	_mav_put_uint8_t(buf, 16, target_system);
 	_mav_put_uint8_t(buf, 17, target_component);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 18);
+        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;
@@ -110,15 +117,19 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan(uin
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 18);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 24);
+#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 into a message
+ * @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)
@@ -131,6 +142,20 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_encode(uint8_
 }
 
 /**
+ * @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
  *
@@ -146,7 +171,7 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_encode(uint8_
 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[18];
+	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);
@@ -154,7 +179,11 @@ static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_send(mavlink_chan
 	_mav_put_uint8_t(buf, 16, target_system);
 	_mav_put_uint8_t(buf, 17, target_component);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, buf, 18, 24);
+#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;
@@ -164,7 +193,11 @@ static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_send(mavlink_chan
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, (const char *)&packet, 18, 24);
+#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
 }
 
@@ -249,6 +282,6 @@ static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_decode(const mavl
 	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), 18);
+	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
index 8cd573a26..334fd39e3 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h
@@ -15,6 +15,9 @@ typedef struct __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 { \
@@ -48,7 +51,7 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack(uint8_t system
 						       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[18];
+	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);
@@ -56,7 +59,7 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack(uint8_t system
 	_mav_put_uint8_t(buf, 16, target_system);
 	_mav_put_uint8_t(buf, 17, target_component);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 18);
+        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;
@@ -66,18 +69,22 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack(uint8_t system
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 18);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 18, 100);
+#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 was sent over
+ * @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
@@ -92,7 +99,7 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan(uint8_t s
 						           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[18];
+	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);
@@ -100,7 +107,7 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan(uint8_t s
 	_mav_put_uint8_t(buf, 16, target_system);
 	_mav_put_uint8_t(buf, 17, target_component);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 18);
+        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;
@@ -110,15 +117,19 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan(uint8_t s
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 18);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 100);
+#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 into a message
+ * @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)
@@ -131,6 +142,20 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_encode(uint8_t syst
 }
 
 /**
+ * @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
  *
@@ -146,7 +171,7 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_encode(uint8_t syst
 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[18];
+	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);
@@ -154,7 +179,11 @@ static inline void mavlink_msg_set_roll_pitch_yaw_thrust_send(mavlink_channel_t
 	_mav_put_uint8_t(buf, 16, target_system);
 	_mav_put_uint8_t(buf, 17, target_component);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, buf, 18, 100);
+#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;
@@ -164,7 +193,11 @@ static inline void mavlink_msg_set_roll_pitch_yaw_thrust_send(mavlink_channel_t
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, (const char *)&packet, 18, 100);
+#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
 }
 
@@ -249,6 +282,6 @@ static inline void mavlink_msg_set_roll_pitch_yaw_thrust_decode(const mavlink_me
 	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), 18);
+	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
index fec38a6a4..93ce345b6 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h
@@ -16,6 +16,9 @@ typedef struct __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 { \
@@ -51,7 +54,7 @@ static inline uint16_t mavlink_msg_setpoint_6dof_pack(uint8_t system_id, uint8_t
 						       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[25];
+	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);
@@ -60,7 +63,7 @@ static inline uint16_t mavlink_msg_setpoint_6dof_pack(uint8_t system_id, uint8_t
 	_mav_put_float(buf, 20, rot_z);
 	_mav_put_uint8_t(buf, 24, target_system);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 25);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN);
 #else
 	mavlink_setpoint_6dof_t packet;
 	packet.trans_x = trans_x;
@@ -71,18 +74,22 @@ static inline uint16_t mavlink_msg_setpoint_6dof_pack(uint8_t system_id, uint8_t
 	packet.rot_z = rot_z;
 	packet.target_system = target_system;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 25);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_SETPOINT_6DOF;
-	return mavlink_finalize_message(msg, system_id, component_id, 25, 15);
+#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 was sent over
+ * @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
@@ -98,7 +105,7 @@ static inline uint16_t mavlink_msg_setpoint_6dof_pack_chan(uint8_t system_id, ui
 						           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[25];
+	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);
@@ -107,7 +114,7 @@ static inline uint16_t mavlink_msg_setpoint_6dof_pack_chan(uint8_t system_id, ui
 	_mav_put_float(buf, 20, rot_z);
 	_mav_put_uint8_t(buf, 24, target_system);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 25);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN);
 #else
 	mavlink_setpoint_6dof_t packet;
 	packet.trans_x = trans_x;
@@ -118,15 +125,19 @@ static inline uint16_t mavlink_msg_setpoint_6dof_pack_chan(uint8_t system_id, ui
 	packet.rot_z = rot_z;
 	packet.target_system = target_system;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 25);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_SETPOINT_6DOF;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 25, 15);
+#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 into a message
+ * @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)
@@ -139,6 +150,20 @@ static inline uint16_t mavlink_msg_setpoint_6dof_encode(uint8_t system_id, uint8
 }
 
 /**
+ * @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
  *
@@ -155,7 +180,7 @@ static inline uint16_t mavlink_msg_setpoint_6dof_encode(uint8_t system_id, uint8
 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[25];
+	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);
@@ -164,7 +189,11 @@ static inline void mavlink_msg_setpoint_6dof_send(mavlink_channel_t chan, uint8_
 	_mav_put_float(buf, 20, rot_z);
 	_mav_put_uint8_t(buf, 24, target_system);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_6DOF, buf, 25, 15);
+#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;
@@ -175,7 +204,11 @@ static inline void mavlink_msg_setpoint_6dof_send(mavlink_channel_t chan, uint8_
 	packet.rot_z = rot_z;
 	packet.target_system = target_system;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_6DOF, (const char *)&packet, 25, 15);
+#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
 }
 
@@ -271,6 +304,6 @@ static inline void mavlink_msg_setpoint_6dof_decode(const mavlink_message_t* 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), 25);
+	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
index bc761be08..de80e4645 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h
@@ -18,6 +18,9 @@ typedef struct __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 { \
@@ -57,7 +60,7 @@ static inline uint16_t mavlink_msg_setpoint_8dof_pack(uint8_t system_id, uint8_t
 						       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[33];
+	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);
@@ -68,7 +71,7 @@ static inline uint16_t mavlink_msg_setpoint_8dof_pack(uint8_t system_id, uint8_t
 	_mav_put_float(buf, 28, val8);
 	_mav_put_uint8_t(buf, 32, target_system);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 33);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN);
 #else
 	mavlink_setpoint_8dof_t packet;
 	packet.val1 = val1;
@@ -81,18 +84,22 @@ static inline uint16_t mavlink_msg_setpoint_8dof_pack(uint8_t system_id, uint8_t
 	packet.val8 = val8;
 	packet.target_system = target_system;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 33);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_SETPOINT_8DOF;
-	return mavlink_finalize_message(msg, system_id, component_id, 33, 241);
+#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 was sent over
+ * @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
@@ -110,7 +117,7 @@ static inline uint16_t mavlink_msg_setpoint_8dof_pack_chan(uint8_t system_id, ui
 						           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[33];
+	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);
@@ -121,7 +128,7 @@ static inline uint16_t mavlink_msg_setpoint_8dof_pack_chan(uint8_t system_id, ui
 	_mav_put_float(buf, 28, val8);
 	_mav_put_uint8_t(buf, 32, target_system);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 33);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN);
 #else
 	mavlink_setpoint_8dof_t packet;
 	packet.val1 = val1;
@@ -134,15 +141,19 @@ static inline uint16_t mavlink_msg_setpoint_8dof_pack_chan(uint8_t system_id, ui
 	packet.val8 = val8;
 	packet.target_system = target_system;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 33);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_SETPOINT_8DOF;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 33, 241);
+#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 into a message
+ * @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)
@@ -155,6 +166,20 @@ static inline uint16_t mavlink_msg_setpoint_8dof_encode(uint8_t system_id, uint8
 }
 
 /**
+ * @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
  *
@@ -173,7 +198,7 @@ static inline uint16_t mavlink_msg_setpoint_8dof_encode(uint8_t system_id, uint8
 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[33];
+	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);
@@ -184,7 +209,11 @@ static inline void mavlink_msg_setpoint_8dof_send(mavlink_channel_t chan, uint8_
 	_mav_put_float(buf, 28, val8);
 	_mav_put_uint8_t(buf, 32, target_system);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_8DOF, buf, 33, 241);
+#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;
@@ -197,7 +226,11 @@ static inline void mavlink_msg_setpoint_8dof_send(mavlink_channel_t chan, uint8_
 	packet.val8 = val8;
 	packet.target_system = target_system;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_8DOF, (const char *)&packet, 33, 241);
+#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
 }
 
@@ -315,6 +348,6 @@ static inline void mavlink_msg_setpoint_8dof_decode(const mavlink_message_t* 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), 33);
+	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
new file mode 100644
index 000000000..ebd657cf3
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h
@@ -0,0 +1,617 @@
+// 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
index 0f4f1f5e2..2db627b96 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h
@@ -18,6 +18,9 @@ typedef struct __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 { \
@@ -57,7 +60,7 @@ static inline uint16_t mavlink_msg_state_correction_pack(uint8_t system_id, uint
 						       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[36];
+	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);
@@ -68,7 +71,7 @@ static inline uint16_t mavlink_msg_state_correction_pack(uint8_t system_id, uint
 	_mav_put_float(buf, 28, vyErr);
 	_mav_put_float(buf, 32, vzErr);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 36);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_STATE_CORRECTION_LEN);
 #else
 	mavlink_state_correction_t packet;
 	packet.xErr = xErr;
@@ -81,18 +84,22 @@ static inline uint16_t mavlink_msg_state_correction_pack(uint8_t system_id, uint
 	packet.vyErr = vyErr;
 	packet.vzErr = vzErr;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 36);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STATE_CORRECTION_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_STATE_CORRECTION;
-	return mavlink_finalize_message(msg, system_id, component_id, 36, 130);
+#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 was sent over
+ * @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
@@ -110,7 +117,7 @@ static inline uint16_t mavlink_msg_state_correction_pack_chan(uint8_t system_id,
 						           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[36];
+	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);
@@ -121,7 +128,7 @@ static inline uint16_t mavlink_msg_state_correction_pack_chan(uint8_t system_id,
 	_mav_put_float(buf, 28, vyErr);
 	_mav_put_float(buf, 32, vzErr);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 36);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_STATE_CORRECTION_LEN);
 #else
 	mavlink_state_correction_t packet;
 	packet.xErr = xErr;
@@ -134,15 +141,19 @@ static inline uint16_t mavlink_msg_state_correction_pack_chan(uint8_t system_id,
 	packet.vyErr = vyErr;
 	packet.vzErr = vzErr;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 36);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STATE_CORRECTION_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_STATE_CORRECTION;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 36, 130);
+#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 into a message
+ * @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)
@@ -155,6 +166,20 @@ static inline uint16_t mavlink_msg_state_correction_encode(uint8_t system_id, ui
 }
 
 /**
+ * @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
  *
@@ -173,7 +198,7 @@ static inline uint16_t mavlink_msg_state_correction_encode(uint8_t system_id, ui
 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[36];
+	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);
@@ -184,7 +209,11 @@ static inline void mavlink_msg_state_correction_send(mavlink_channel_t chan, flo
 	_mav_put_float(buf, 28, vyErr);
 	_mav_put_float(buf, 32, vzErr);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, buf, 36, 130);
+#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;
@@ -197,7 +226,11 @@ static inline void mavlink_msg_state_correction_send(mavlink_channel_t chan, flo
 	packet.vyErr = vyErr;
 	packet.vzErr = vzErr;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, (const char *)&packet, 36, 130);
+#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
 }
 
@@ -315,6 +348,6 @@ static inline void mavlink_msg_state_correction_decode(const mavlink_message_t*
 	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), 36);
+	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
index f915b6858..b10ae2b1d 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h
@@ -2,6 +2,16 @@
 
 #define MAVLINK_MSG_ID_STATUSTEXT 253
 
+#define MAVLINK_MSG_ID_STATUSTEXT_LEN 51
+#define MAVLINK_MSG_ID_253_LEN MAVLINK_MSG_ID_STATUSTEXT_LEN
+
+#define MAVLINK_MSG_ID_STATUSTEXT_CRC 83
+#define MAVLINK_MSG_ID_253_CRC MAVLINK_MSG_ID_STATUSTEXT_CRC
+
+#define MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN 49		///< Length in bytes
+
+#define MAVLINK_MSG_STATUSTEXT_VARLEN_BYTES 1
+
 typedef struct __mavlink_statustext_t
 {
  uint8_t severity; ///< Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY.
@@ -9,17 +19,12 @@ typedef struct __mavlink_statustext_t
  uint8_t _field_len_text; ///< Length of status text message
 } mavlink_statustext_t;
 
-#define MAVLINK_MSG_ID_STATUSTEXT_LEN 51
-#define MAVLINK_MSG_ID_253_LEN 51
-
-#define MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN 50
-
 #define MAVLINK_MESSAGE_INFO_STATUSTEXT { \
 	"STATUSTEXT", \
 	3, \
 	{  { "severity", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_statustext_t, severity) }, \
-         { "text", NULL, MAVLINK_TYPE_CHAR, 49, 1, offsetof(mavlink_statustext_t, text) }, \
-         { "_field_len_text", NULL, MAVLINK_TYPE_UINT8_T, 0, 50, offsetof(mavlink_statustext_t, _field_len_text) } \
+         { "text", NULL, MAVLINK_TYPE_CHAR, -1, -1, -1 }, \
+         { "_field_len_text", NULL, MAVLINK_TYPE_UINT8_T, 0, -1, 1 } \
          } \
 }
 
@@ -38,27 +43,22 @@ static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t co
 						       uint8_t severity, const char *text)
 {
 	uint8_t _field_len_text;
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[51];
+	char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN];
 	_mav_put_uint8_t(buf, 0, severity);
-	buf[50] = _field_len_text = mav_string_copy(&buf[1], text, sizeof(char)*49);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 51);
-#else
-	mavlink_statustext_t packet;
-	packet.severity = severity;
-	packet._field_len_text = _field_len_text = mav_string_copy(packet.text, text, sizeof(char)*49);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 51);
-#endif
+	_field_len_text = mav_string_copy(&buf[1], text, sizeof(char)*49);
+	_mav_put_uint8_t(buf, 1 + _field_len_text, _field_len_text);
+	uint16_t msglen = 1 + _field_len_text + MAVLINK_MSG_STATUSTEXT_VARLEN_BYTES + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, msglen);
 
 	msg->msgid = MAVLINK_MSG_ID_STATUSTEXT;
-	return mavlink_finalize_message(msg, system_id, component_id, 51 - (_field_len_text - 49), 83);
+	return mavlink_finalize_message(msg, system_id, component_id, msglen, MAVLINK_MSG_ID_STATUSTEXT_CRC);
 }
 
 /**
  * @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 was sent over
+ * @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
@@ -69,24 +69,19 @@ static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8
 						           uint8_t severity,const char *text)
 {
 	uint8_t _field_len_text;
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[51];
+	char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN];
 	_mav_put_uint8_t(buf, 0, severity);
-	buf[50] = _field_len_text = mav_string_copy(&buf[1], text, sizeof(char)*49);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 51);
-#else
-	mavlink_statustext_t packet;
-	packet.severity = severity;
-	packet._field_len_text = _field_len_text = mav_string_copy(packet.text, text, sizeof(char)*49);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 51);
-#endif
+	_field_len_text = mav_string_copy(&buf[1], text, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN);
+	_mav_put_uint8_t(buf, 1 + _field_len_text, _field_len_text);
+	uint16_t msglen = 1 + _field_len_text + MAVLINK_MSG_STATUSTEXT_VARLEN_BYTES + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, msglen);
 
 	msg->msgid = MAVLINK_MSG_ID_STATUSTEXT;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 51 - (_field_len_text - 49), 83);
+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, msglen, MAVLINK_MSG_ID_STATUSTEXT_CRC);
 }
 
 /**
- * @brief Encode a statustext struct into a message
+ * @brief Encode a statustext struct
  *
  * @param system_id ID of this system
  * @param component_id ID of this component (e.g. 200 for IMU)
@@ -99,6 +94,20 @@ static inline uint16_t mavlink_msg_statustext_encode(uint8_t system_id, uint8_t
 }
 
 /**
+ * @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
  *
@@ -109,19 +118,14 @@ static inline uint16_t mavlink_msg_statustext_encode(uint8_t system_id, uint8_t
 
 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[51];
+	uint8_t _field_len_text;
+	char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN];
 	_mav_put_uint8_t(buf, 0, severity);
-	buf[50] = mav_string_copy(&buf[1], text, sizeof(char)*49);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, 50, 83);
-#else
-	mavlink_statustext_t packet;
-	packet.severity = severity;
-	packet._field_len_text = mav_string_copy(packet.text, text, sizeof(char)*49);
-	/* Adjust message length based on actually used size in field */
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)&packet,
-		 51 - (packet._field_len_text - 49), 83);
-#endif
+	_field_len_text = mav_string_copy(&buf[1], text, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN);
+	_mav_put_uint8_t(buf, 1 + _field_len_text, _field_len_text);
+	uint16_t msglen = 1 + _field_len_text + MAVLINK_MSG_STATUSTEXT_VARLEN_BYTES + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, msglen);
+	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, msglen, MAVLINK_MSG_ID_STATUSTEXT_CRC);
 }
 
 #endif
@@ -146,7 +150,8 @@ static inline uint8_t mavlink_msg_statustext_get_severity(const mavlink_message_
  */
 static inline uint16_t mavlink_msg_statustext_get_text(const mavlink_message_t* msg, char *text)
 {
-	return _MAV_RETURN_char_array(msg, text, 49,  1);
+	uint8_t len = _MAV_RETURN_uint8_t(msg, msg->len - MAVLINK_MSG_STATUSTEXT_VARLEN_BYTES + 0);
+	return _MAV_RETURN_char_array(msg, text, len,  1);
 }
 
 /**
@@ -157,13 +162,8 @@ static inline uint16_t mavlink_msg_statustext_get_text(const mavlink_message_t*
  */
 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
-	statustext->_field_len_text = _MAV_RETURN_uint8_t(msg,  1);
+	statustext->_field_len_text = _MAV_RETURN_uint8_t(msg, msg->len - MAVLINK_MSG_STATUSTEXT_VARLEN_BYTES + 0);
 	statustext->severity = mavlink_msg_statustext_get_severity(msg);
 	/* copy the string, but at maximum to the full field length or the announced string length, depending on which is shorter */
-	strncpy(statustext->text, _MAV_PAYLOAD(msg+1), (statustext->_field_len_text < 49) ? statustext->_field_len_text : 49);
-#endif
+	strncpy(statustext->text, _MAV_PAYLOAD(msg+1), (statustext->_field_len_text < MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN) ? statustext->_field_len_text : MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN);
 }
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h
index ef09a652f..101b36678 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h
@@ -4,9 +4,9 @@
 
 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: 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 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
@@ -22,6 +22,9 @@ typedef struct __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 { \
@@ -50,9 +53,9 @@ typedef struct __mavlink_sys_status_t
  * @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: 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
- * @param 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
- * @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: 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
+ * @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
@@ -69,7 +72,7 @@ static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t co
 						       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[31];
+	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);
@@ -84,7 +87,7 @@ static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t co
 	_mav_put_uint16_t(buf, 28, errors_count4);
 	_mav_put_int8_t(buf, 30, battery_remaining);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 31);
+        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;
@@ -101,22 +104,26 @@ static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t co
 	packet.errors_count4 = errors_count4;
 	packet.battery_remaining = battery_remaining;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 31);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYS_STATUS_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_SYS_STATUS;
-	return mavlink_finalize_message(msg, system_id, component_id, 31, 124);
+#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 was sent over
+ * @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: 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
- * @param 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
- * @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: 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
+ * @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
@@ -134,7 +141,7 @@ static inline uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8
 						           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[31];
+	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);
@@ -149,7 +156,7 @@ static inline uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8
 	_mav_put_uint16_t(buf, 28, errors_count4);
 	_mav_put_int8_t(buf, 30, battery_remaining);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 31);
+        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;
@@ -166,15 +173,19 @@ static inline uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8
 	packet.errors_count4 = errors_count4;
 	packet.battery_remaining = battery_remaining;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 31);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYS_STATUS_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_SYS_STATUS;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 31, 124);
+#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 into a message
+ * @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)
@@ -187,12 +198,26 @@ static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t
 }
 
 /**
+ * @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: 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
- * @param 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
- * @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: 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
+ * @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
@@ -209,7 +234,7 @@ static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t
 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[31];
+	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);
@@ -224,7 +249,11 @@ static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint32_t
 	_mav_put_uint16_t(buf, 28, errors_count4);
 	_mav_put_int8_t(buf, 30, battery_remaining);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, buf, 31, 124);
+#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;
@@ -241,7 +270,11 @@ static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint32_t
 	packet.errors_count4 = errors_count4;
 	packet.battery_remaining = battery_remaining;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, (const char *)&packet, 31, 124);
+#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
 }
 
@@ -253,7 +286,7 @@ static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint32_t
 /**
  * @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: 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
+ * @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)
 {
@@ -263,7 +296,7 @@ static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_presen
 /**
  * @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: 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
+ * @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)
 {
@@ -273,7 +306,7 @@ static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_enable
 /**
  * @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: 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
+ * @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)
 {
@@ -403,6 +436,6 @@ static inline void mavlink_msg_sys_status_decode(const mavlink_message_t* msg, m
 	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), 31);
+	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
index c24808d1a..1807567ae 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h
@@ -11,6 +11,9 @@ typedef struct __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 { \
@@ -36,28 +39,32 @@ static inline uint16_t mavlink_msg_system_time_pack(uint8_t system_id, uint8_t c
 						       uint64_t time_unix_usec, uint32_t time_boot_ms)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[12];
+	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, 12);
+        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, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYSTEM_TIME_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME;
-	return mavlink_finalize_message(msg, system_id, component_id, 12, 137);
+#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 was sent over
+ * @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.
@@ -68,25 +75,29 @@ static inline uint16_t mavlink_msg_system_time_pack_chan(uint8_t system_id, uint
 						           uint64_t time_unix_usec,uint32_t time_boot_ms)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[12];
+	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, 12);
+        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, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYSTEM_TIME_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 137);
+#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 into a message
+ * @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)
@@ -99,6 +110,20 @@ static inline uint16_t mavlink_msg_system_time_encode(uint8_t system_id, uint8_t
 }
 
 /**
+ * @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
  *
@@ -110,17 +135,25 @@ static inline uint16_t mavlink_msg_system_time_encode(uint8_t system_id, uint8_t
 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[12];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, buf, 12, 137);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, (const char *)&packet, 12, 137);
+#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
 }
 
@@ -161,6 +194,6 @@ static inline void mavlink_msg_system_time_decode(const mavlink_message_t* msg,
 	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), 12);
+	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
index d7c1afe41..5b1093a3d 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h
@@ -15,6 +15,9 @@ typedef struct __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 { \
@@ -48,7 +51,7 @@ static inline uint16_t mavlink_msg_vfr_hud_pack(uint8_t system_id, uint8_t compo
 						       float airspeed, float groundspeed, int16_t heading, uint16_t throttle, float alt, float climb)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[20];
+	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);
@@ -56,7 +59,7 @@ static inline uint16_t mavlink_msg_vfr_hud_pack(uint8_t system_id, uint8_t compo
 	_mav_put_int16_t(buf, 16, heading);
 	_mav_put_uint16_t(buf, 18, throttle);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 20);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VFR_HUD_LEN);
 #else
 	mavlink_vfr_hud_t packet;
 	packet.airspeed = airspeed;
@@ -66,18 +69,22 @@ static inline uint16_t mavlink_msg_vfr_hud_pack(uint8_t system_id, uint8_t compo
 	packet.heading = heading;
 	packet.throttle = throttle;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 20);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VFR_HUD_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_VFR_HUD;
-	return mavlink_finalize_message(msg, system_id, component_id, 20, 20);
+#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 was sent over
+ * @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
@@ -92,7 +99,7 @@ static inline uint16_t mavlink_msg_vfr_hud_pack_chan(uint8_t system_id, uint8_t
 						           float airspeed,float groundspeed,int16_t heading,uint16_t throttle,float alt,float climb)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[20];
+	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);
@@ -100,7 +107,7 @@ static inline uint16_t mavlink_msg_vfr_hud_pack_chan(uint8_t system_id, uint8_t
 	_mav_put_int16_t(buf, 16, heading);
 	_mav_put_uint16_t(buf, 18, throttle);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 20);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VFR_HUD_LEN);
 #else
 	mavlink_vfr_hud_t packet;
 	packet.airspeed = airspeed;
@@ -110,15 +117,19 @@ static inline uint16_t mavlink_msg_vfr_hud_pack_chan(uint8_t system_id, uint8_t
 	packet.heading = heading;
 	packet.throttle = throttle;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 20);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VFR_HUD_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_VFR_HUD;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 20);
+#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 into a message
+ * @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)
@@ -131,6 +142,20 @@ static inline uint16_t mavlink_msg_vfr_hud_encode(uint8_t system_id, uint8_t com
 }
 
 /**
+ * @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
  *
@@ -146,7 +171,7 @@ static inline uint16_t mavlink_msg_vfr_hud_encode(uint8_t system_id, uint8_t com
 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[20];
+	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);
@@ -154,7 +179,11 @@ static inline void mavlink_msg_vfr_hud_send(mavlink_channel_t chan, float airspe
 	_mav_put_int16_t(buf, 16, heading);
 	_mav_put_uint16_t(buf, 18, throttle);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, buf, 20, 20);
+#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;
@@ -164,7 +193,11 @@ static inline void mavlink_msg_vfr_hud_send(mavlink_channel_t chan, float airspe
 	packet.heading = heading;
 	packet.throttle = throttle;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, (const char *)&packet, 20, 20);
+#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
 }
 
@@ -249,6 +282,6 @@ static inline void mavlink_msg_vfr_hud_decode(const mavlink_message_t* msg, mavl
 	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), 20);
+	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
index 93ad097f9..a254202e4 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h
@@ -16,6 +16,9 @@ typedef struct __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 { \
@@ -51,7 +54,7 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_i
 						       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[32];
+	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);
@@ -60,7 +63,7 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_i
 	_mav_put_float(buf, 24, pitch);
 	_mav_put_float(buf, 28, yaw);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 32);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN);
 #else
 	mavlink_vicon_position_estimate_t packet;
 	packet.usec = usec;
@@ -71,18 +74,22 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_i
 	packet.pitch = pitch;
 	packet.yaw = yaw;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 32);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE;
-	return mavlink_finalize_message(msg, system_id, component_id, 32, 56);
+#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 was sent over
+ * @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
@@ -98,7 +105,7 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack_chan(uint8_t sys
 						           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[32];
+	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);
@@ -107,7 +114,7 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack_chan(uint8_t sys
 	_mav_put_float(buf, 24, pitch);
 	_mav_put_float(buf, 28, yaw);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 32);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN);
 #else
 	mavlink_vicon_position_estimate_t packet;
 	packet.usec = usec;
@@ -118,15 +125,19 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack_chan(uint8_t sys
 	packet.pitch = pitch;
 	packet.yaw = yaw;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 32);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 56);
+#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 into a message
+ * @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)
@@ -139,6 +150,20 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_encode(uint8_t system
 }
 
 /**
+ * @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
  *
@@ -155,7 +180,7 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_encode(uint8_t system
 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[32];
+	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);
@@ -164,7 +189,11 @@ static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t ch
 	_mav_put_float(buf, 24, pitch);
 	_mav_put_float(buf, 28, yaw);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, buf, 32, 56);
+#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;
@@ -175,7 +204,11 @@ static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t ch
 	packet.pitch = pitch;
 	packet.yaw = yaw;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, (const char *)&packet, 32, 56);
+#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
 }
 
@@ -271,6 +304,6 @@ static inline void mavlink_msg_vicon_position_estimate_decode(const mavlink_mess
 	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), 32);
+	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
index ca567c119..f7a741b09 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h
@@ -16,6 +16,9 @@ typedef struct __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 { \
@@ -51,7 +54,7 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_
 						       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[32];
+	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);
@@ -60,7 +63,7 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_
 	_mav_put_float(buf, 24, pitch);
 	_mav_put_float(buf, 28, yaw);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 32);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN);
 #else
 	mavlink_vision_position_estimate_t packet;
 	packet.usec = usec;
@@ -71,18 +74,22 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_
 	packet.pitch = pitch;
 	packet.yaw = yaw;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 32);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE;
-	return mavlink_finalize_message(msg, system_id, component_id, 32, 158);
+#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 was sent over
+ * @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
@@ -98,7 +105,7 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack_chan(uint8_t sy
 						           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[32];
+	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);
@@ -107,7 +114,7 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack_chan(uint8_t sy
 	_mav_put_float(buf, 24, pitch);
 	_mav_put_float(buf, 28, yaw);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 32);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN);
 #else
 	mavlink_vision_position_estimate_t packet;
 	packet.usec = usec;
@@ -118,15 +125,19 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack_chan(uint8_t sy
 	packet.pitch = pitch;
 	packet.yaw = yaw;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 32);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 158);
+#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 into a message
+ * @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)
@@ -139,6 +150,20 @@ static inline uint16_t mavlink_msg_vision_position_estimate_encode(uint8_t syste
 }
 
 /**
+ * @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
  *
@@ -155,7 +180,7 @@ static inline uint16_t mavlink_msg_vision_position_estimate_encode(uint8_t syste
 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[32];
+	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);
@@ -164,7 +189,11 @@ static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t c
 	_mav_put_float(buf, 24, pitch);
 	_mav_put_float(buf, 28, yaw);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, buf, 32, 158);
+#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;
@@ -175,7 +204,11 @@ static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t c
 	packet.pitch = pitch;
 	packet.yaw = yaw;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, (const char *)&packet, 32, 158);
+#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
 }
 
@@ -271,6 +304,6 @@ static inline void mavlink_msg_vision_position_estimate_decode(const mavlink_mes
 	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), 32);
+	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
index 10ec1026c..660225128 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h
@@ -13,6 +13,9 @@ typedef struct __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 { \
@@ -42,13 +45,13 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id,
 						       uint64_t usec, float x, float y, float z)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[20];
+	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, 20);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN);
 #else
 	mavlink_vision_speed_estimate_t packet;
 	packet.usec = usec;
@@ -56,18 +59,22 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id,
 	packet.y = y;
 	packet.z = z;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 20);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE;
-	return mavlink_finalize_message(msg, system_id, component_id, 20, 208);
+#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 was sent over
+ * @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
@@ -80,13 +87,13 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack_chan(uint8_t syste
 						           uint64_t usec,float x,float y,float z)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[20];
+	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, 20);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN);
 #else
 	mavlink_vision_speed_estimate_t packet;
 	packet.usec = usec;
@@ -94,15 +101,19 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack_chan(uint8_t syste
 	packet.y = y;
 	packet.z = z;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 20);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 208);
+#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 into a message
+ * @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)
@@ -115,6 +126,20 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_encode(uint8_t system_i
 }
 
 /**
+ * @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
  *
@@ -128,13 +153,17 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_encode(uint8_t system_i
 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[20];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, buf, 20, 208);
+#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;
@@ -142,7 +171,11 @@ static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan
 	packet.y = y;
 	packet.z = z;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, (const char *)&packet, 20, 208);
+#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
 }
 
@@ -205,6 +238,6 @@ static inline void mavlink_msg_vision_speed_estimate_decode(const mavlink_messag
 	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), 20);
+	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
index 0da36c7d8..c5aa9ddf3 100644
--- a/mavlink/include/mavlink/v1.0/common/testsuite.h
+++ b/mavlink/include/mavlink/v1.0/common/testsuite.h
@@ -31,11 +31,11 @@ static void mavlink_test_heartbeat(uint8_t system_id, uint8_t component_id, mavl
         uint16_t i;
 	mavlink_heartbeat_t packet_in = {
 		963497464,
-	17,
-	84,
-	151,
-	218,
-	3,
+	}17,
+	}84,
+	}151,
+	}218,
+	}3,
 	};
 	mavlink_heartbeat_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -84,18 +84,18 @@ static void mavlink_test_sys_status(uint8_t system_id, uint8_t component_id, mav
         uint16_t i;
 	mavlink_sys_status_t packet_in = {
 		963497464,
-	963497672,
-	963497880,
-	17859,
-	17963,
-	18067,
-	18171,
-	18275,
-	18379,
-	18483,
-	18587,
-	18691,
-	223,
+	}963497672,
+	}963497880,
+	}17859,
+	}17963,
+	}18067,
+	}18171,
+	}18275,
+	}18379,
+	}18483,
+	}18587,
+	}18691,
+	}223,
 	};
 	mavlink_sys_status_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -151,7 +151,7 @@ static void mavlink_test_system_time(uint8_t system_id, uint8_t component_id, ma
         uint16_t i;
 	mavlink_system_time_t packet_in = {
 		93372036854775807ULL,
-	963497880,
+	}963497880,
 	};
 	mavlink_system_time_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -196,9 +196,9 @@ static void mavlink_test_ping(uint8_t system_id, uint8_t component_id, mavlink_m
         uint16_t i;
 	mavlink_ping_t packet_in = {
 		93372036854775807ULL,
-	963497880,
-	41,
-	108,
+	}963497880,
+	}41,
+	}108,
 	};
 	mavlink_ping_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -245,9 +245,9 @@ static void mavlink_test_change_operator_control(uint8_t system_id, uint8_t comp
         uint16_t i;
 	mavlink_change_operator_control_t packet_in = {
 		5,
-	72,
-	139,
-	"DEFGHIJKLMNOPQRSTUVWXYZA",
+	}72,
+	}139,
+	}"DEFGHIJKLMNOPQRSTUVWXYZA",
 	};
 	mavlink_change_operator_control_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -294,8 +294,8 @@ static void mavlink_test_change_operator_control_ack(uint8_t system_id, uint8_t
         uint16_t i;
 	mavlink_change_operator_control_ack_t packet_in = {
 		5,
-	72,
-	139,
+	}72,
+	}139,
 	};
 	mavlink_change_operator_control_ack_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -384,8 +384,8 @@ static void mavlink_test_set_mode(uint8_t system_id, uint8_t component_id, mavli
         uint16_t i;
 	mavlink_set_mode_t packet_in = {
 		963497464,
-	17,
-	84,
+	}17,
+	}84,
 	};
 	mavlink_set_mode_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -431,9 +431,9 @@ static void mavlink_test_param_request_read(uint8_t system_id, uint8_t component
         uint16_t i;
 	mavlink_param_request_read_t packet_in = {
 		17235,
-	139,
-	206,
-	"EFGHIJKLMNOPQRS",
+	}139,
+	}206,
+	}"EFGHIJKLMNOPQRS",
 	};
 	mavlink_param_request_read_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -480,7 +480,7 @@ static void mavlink_test_param_request_list(uint8_t system_id, uint8_t component
         uint16_t i;
 	mavlink_param_request_list_t packet_in = {
 		5,
-	72,
+	}72,
 	};
 	mavlink_param_request_list_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -525,10 +525,10 @@ static void mavlink_test_param_value(uint8_t system_id, uint8_t component_id, ma
         uint16_t i;
 	mavlink_param_value_t packet_in = {
 		17.0,
-	17443,
-	17547,
-	"IJKLMNOPQRSTUVW",
-	77,
+	}17443,
+	}17547,
+	}"IJKLMNOPQRSTUVW",
+	}77,
 	};
 	mavlink_param_value_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -576,10 +576,10 @@ static void mavlink_test_param_set(uint8_t system_id, uint8_t component_id, mavl
         uint16_t i;
 	mavlink_param_set_t packet_in = {
 		17.0,
-	17,
-	84,
-	"GHIJKLMNOPQRSTU",
-	199,
+	}17,
+	}84,
+	}"GHIJKLMNOPQRSTU",
+	}199,
 	};
 	mavlink_param_set_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -627,15 +627,15 @@ static void mavlink_test_gps_raw_int(uint8_t system_id, uint8_t component_id, ma
         uint16_t i;
 	mavlink_gps_raw_int_t packet_in = {
 		93372036854775807ULL,
-	963497880,
-	963498088,
-	963498296,
-	18275,
-	18379,
-	18483,
-	18587,
-	89,
-	156,
+	}963497880,
+	}963498088,
+	}963498296,
+	}18275,
+	}18379,
+	}18483,
+	}18587,
+	}89,
+	}156,
 	};
 	mavlink_gps_raw_int_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -688,11 +688,11 @@ static void mavlink_test_gps_status(uint8_t system_id, uint8_t component_id, mav
         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 },
+	}{ 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));
@@ -741,15 +741,15 @@ static void mavlink_test_scaled_imu(uint8_t system_id, uint8_t component_id, mav
         uint16_t i;
 	mavlink_scaled_imu_t packet_in = {
 		963497464,
-	17443,
-	17547,
-	17651,
-	17755,
-	17859,
-	17963,
-	18067,
-	18171,
-	18275,
+	}17443,
+	}17547,
+	}17651,
+	}17755,
+	}17859,
+	}17963,
+	}18067,
+	}18171,
+	}18275,
 	};
 	mavlink_scaled_imu_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -802,15 +802,15 @@ static void mavlink_test_raw_imu(uint8_t system_id, uint8_t component_id, mavlin
         uint16_t i;
 	mavlink_raw_imu_t packet_in = {
 		93372036854775807ULL,
-	17651,
-	17755,
-	17859,
-	17963,
-	18067,
-	18171,
-	18275,
-	18379,
-	18483,
+	}17651,
+	}17755,
+	}17859,
+	}17963,
+	}18067,
+	}18171,
+	}18275,
+	}18379,
+	}18483,
 	};
 	mavlink_raw_imu_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -863,10 +863,10 @@ static void mavlink_test_raw_pressure(uint8_t system_id, uint8_t component_id, m
         uint16_t i;
 	mavlink_raw_pressure_t packet_in = {
 		93372036854775807ULL,
-	17651,
-	17755,
-	17859,
-	17963,
+	}17651,
+	}17755,
+	}17859,
+	}17963,
 	};
 	mavlink_raw_pressure_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -914,9 +914,9 @@ static void mavlink_test_scaled_pressure(uint8_t system_id, uint8_t component_id
         uint16_t i;
 	mavlink_scaled_pressure_t packet_in = {
 		963497464,
-	45.0,
-	73.0,
-	17859,
+	}45.0,
+	}73.0,
+	}17859,
 	};
 	mavlink_scaled_pressure_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -963,12 +963,12 @@ static void mavlink_test_attitude(uint8_t system_id, uint8_t component_id, mavli
         uint16_t i;
 	mavlink_attitude_t packet_in = {
 		963497464,
-	45.0,
-	73.0,
-	101.0,
-	129.0,
-	157.0,
-	185.0,
+	}45.0,
+	}73.0,
+	}101.0,
+	}129.0,
+	}157.0,
+	}185.0,
 	};
 	mavlink_attitude_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -1018,13 +1018,13 @@ static void mavlink_test_attitude_quaternion(uint8_t system_id, uint8_t componen
         uint16_t i;
 	mavlink_attitude_quaternion_t packet_in = {
 		963497464,
-	45.0,
-	73.0,
-	101.0,
-	129.0,
-	157.0,
-	185.0,
-	213.0,
+	}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));
@@ -1075,12 +1075,12 @@ static void mavlink_test_local_position_ned(uint8_t system_id, uint8_t component
         uint16_t i;
 	mavlink_local_position_ned_t packet_in = {
 		963497464,
-	45.0,
-	73.0,
-	101.0,
-	129.0,
-	157.0,
-	185.0,
+	}45.0,
+	}73.0,
+	}101.0,
+	}129.0,
+	}157.0,
+	}185.0,
 	};
 	mavlink_local_position_ned_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -1130,14 +1130,14 @@ static void mavlink_test_global_position_int(uint8_t system_id, uint8_t componen
         uint16_t i;
 	mavlink_global_position_int_t packet_in = {
 		963497464,
-	963497672,
-	963497880,
-	963498088,
-	963498296,
-	18275,
-	18379,
-	18483,
-	18587,
+	}963497672,
+	}963497880,
+	}963498088,
+	}963498296,
+	}18275,
+	}18379,
+	}18483,
+	}18587,
 	};
 	mavlink_global_position_int_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -1189,16 +1189,16 @@ static void mavlink_test_rc_channels_scaled(uint8_t system_id, uint8_t component
         uint16_t i;
 	mavlink_rc_channels_scaled_t packet_in = {
 		963497464,
-	17443,
-	17547,
-	17651,
-	17755,
-	17859,
-	17963,
-	18067,
-	18171,
-	65,
-	132,
+	}17443,
+	}17547,
+	}17651,
+	}17755,
+	}17859,
+	}17963,
+	}18067,
+	}18171,
+	}65,
+	}132,
 	};
 	mavlink_rc_channels_scaled_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -1252,16 +1252,16 @@ static void mavlink_test_rc_channels_raw(uint8_t system_id, uint8_t component_id
         uint16_t i;
 	mavlink_rc_channels_raw_t packet_in = {
 		963497464,
-	17443,
-	17547,
-	17651,
-	17755,
-	17859,
-	17963,
-	18067,
-	18171,
-	65,
-	132,
+	}17443,
+	}17547,
+	}17651,
+	}17755,
+	}17859,
+	}17963,
+	}18067,
+	}18171,
+	}65,
+	}132,
 	};
 	mavlink_rc_channels_raw_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -1315,15 +1315,15 @@ static void mavlink_test_servo_output_raw(uint8_t system_id, uint8_t component_i
         uint16_t i;
 	mavlink_servo_output_raw_t packet_in = {
 		963497464,
-	17443,
-	17547,
-	17651,
-	17755,
-	17859,
-	17963,
-	18067,
-	18171,
-	65,
+	}17443,
+	}17547,
+	}17651,
+	}17755,
+	}17859,
+	}17963,
+	}18067,
+	}18171,
+	}65,
 	};
 	mavlink_servo_output_raw_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -1376,9 +1376,9 @@ static void mavlink_test_mission_request_partial_list(uint8_t system_id, uint8_t
         uint16_t i;
 	mavlink_mission_request_partial_list_t packet_in = {
 		17235,
-	17339,
-	17,
-	84,
+	}17339,
+	}17,
+	}84,
 	};
 	mavlink_mission_request_partial_list_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -1425,9 +1425,9 @@ static void mavlink_test_mission_write_partial_list(uint8_t system_id, uint8_t c
         uint16_t i;
 	mavlink_mission_write_partial_list_t packet_in = {
 		17235,
-	17339,
-	17,
-	84,
+	}17339,
+	}17,
+	}84,
 	};
 	mavlink_mission_write_partial_list_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -1474,19 +1474,19 @@ static void mavlink_test_mission_item(uint8_t system_id, uint8_t component_id, m
         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,
+	}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));
@@ -1543,8 +1543,8 @@ static void mavlink_test_mission_request(uint8_t system_id, uint8_t component_id
         uint16_t i;
 	mavlink_mission_request_t packet_in = {
 		17235,
-	139,
-	206,
+	}139,
+	}206,
 	};
 	mavlink_mission_request_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -1590,8 +1590,8 @@ static void mavlink_test_mission_set_current(uint8_t system_id, uint8_t componen
         uint16_t i;
 	mavlink_mission_set_current_t packet_in = {
 		17235,
-	139,
-	206,
+	}139,
+	}206,
 	};
 	mavlink_mission_set_current_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -1680,7 +1680,7 @@ static void mavlink_test_mission_request_list(uint8_t system_id, uint8_t compone
         uint16_t i;
 	mavlink_mission_request_list_t packet_in = {
 		5,
-	72,
+	}72,
 	};
 	mavlink_mission_request_list_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -1725,8 +1725,8 @@ static void mavlink_test_mission_count(uint8_t system_id, uint8_t component_id,
         uint16_t i;
 	mavlink_mission_count_t packet_in = {
 		17235,
-	139,
-	206,
+	}139,
+	}206,
 	};
 	mavlink_mission_count_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -1772,7 +1772,7 @@ static void mavlink_test_mission_clear_all(uint8_t system_id, uint8_t component_
         uint16_t i;
 	mavlink_mission_clear_all_t packet_in = {
 		5,
-	72,
+	}72,
 	};
 	mavlink_mission_clear_all_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -1860,8 +1860,8 @@ static void mavlink_test_mission_ack(uint8_t system_id, uint8_t component_id, ma
         uint16_t i;
 	mavlink_mission_ack_t packet_in = {
 		5,
-	72,
-	139,
+	}72,
+	}139,
 	};
 	mavlink_mission_ack_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -1907,9 +1907,9 @@ static void mavlink_test_set_gps_global_origin(uint8_t system_id, uint8_t compon
         uint16_t i;
 	mavlink_set_gps_global_origin_t packet_in = {
 		963497464,
-	963497672,
-	963497880,
-	41,
+	}963497672,
+	}963497880,
+	}41,
 	};
 	mavlink_set_gps_global_origin_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -1956,8 +1956,8 @@ static void mavlink_test_gps_global_origin(uint8_t system_id, uint8_t component_
         uint16_t i;
 	mavlink_gps_global_origin_t packet_in = {
 		963497464,
-	963497672,
-	963497880,
+	}963497672,
+	}963497880,
 	};
 	mavlink_gps_global_origin_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -2003,12 +2003,12 @@ static void mavlink_test_set_local_position_setpoint(uint8_t system_id, uint8_t
         uint16_t i;
 	mavlink_set_local_position_setpoint_t packet_in = {
 		17.0,
-	45.0,
-	73.0,
-	101.0,
-	53,
-	120,
-	187,
+	}45.0,
+	}73.0,
+	}101.0,
+	}53,
+	}120,
+	}187,
 	};
 	mavlink_set_local_position_setpoint_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -2058,10 +2058,10 @@ static void mavlink_test_local_position_setpoint(uint8_t system_id, uint8_t comp
         uint16_t i;
 	mavlink_local_position_setpoint_t packet_in = {
 		17.0,
-	45.0,
-	73.0,
-	101.0,
-	53,
+	}45.0,
+	}73.0,
+	}101.0,
+	}53,
 	};
 	mavlink_local_position_setpoint_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -2109,10 +2109,10 @@ static void mavlink_test_global_position_setpoint_int(uint8_t system_id, uint8_t
         uint16_t i;
 	mavlink_global_position_setpoint_int_t packet_in = {
 		963497464,
-	963497672,
-	963497880,
-	17859,
-	175,
+	}963497672,
+	}963497880,
+	}17859,
+	}175,
 	};
 	mavlink_global_position_setpoint_int_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -2160,10 +2160,10 @@ static void mavlink_test_set_global_position_setpoint_int(uint8_t system_id, uin
         uint16_t i;
 	mavlink_set_global_position_setpoint_int_t packet_in = {
 		963497464,
-	963497672,
-	963497880,
-	17859,
-	175,
+	}963497672,
+	}963497880,
+	}17859,
+	}175,
 	};
 	mavlink_set_global_position_setpoint_int_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -2211,14 +2211,14 @@ static void mavlink_test_safety_set_allowed_area(uint8_t system_id, uint8_t comp
         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,
+	}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));
@@ -2270,12 +2270,12 @@ static void mavlink_test_safety_allowed_area(uint8_t system_id, uint8_t componen
         uint16_t i;
 	mavlink_safety_allowed_area_t packet_in = {
 		17.0,
-	45.0,
-	73.0,
-	101.0,
-	129.0,
-	157.0,
-	77,
+	}45.0,
+	}73.0,
+	}101.0,
+	}129.0,
+	}157.0,
+	}77,
 	};
 	mavlink_safety_allowed_area_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -2325,11 +2325,11 @@ static void mavlink_test_set_roll_pitch_yaw_thrust(uint8_t system_id, uint8_t co
         uint16_t i;
 	mavlink_set_roll_pitch_yaw_thrust_t packet_in = {
 		17.0,
-	45.0,
-	73.0,
-	101.0,
-	53,
-	120,
+	}45.0,
+	}73.0,
+	}101.0,
+	}53,
+	}120,
 	};
 	mavlink_set_roll_pitch_yaw_thrust_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -2378,11 +2378,11 @@ static void mavlink_test_set_roll_pitch_yaw_speed_thrust(uint8_t system_id, uint
         uint16_t i;
 	mavlink_set_roll_pitch_yaw_speed_thrust_t packet_in = {
 		17.0,
-	45.0,
-	73.0,
-	101.0,
-	53,
-	120,
+	}45.0,
+	}73.0,
+	}101.0,
+	}53,
+	}120,
 	};
 	mavlink_set_roll_pitch_yaw_speed_thrust_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -2431,10 +2431,10 @@ static void mavlink_test_roll_pitch_yaw_thrust_setpoint(uint8_t system_id, uint8
         uint16_t i;
 	mavlink_roll_pitch_yaw_thrust_setpoint_t packet_in = {
 		963497464,
-	45.0,
-	73.0,
-	101.0,
-	129.0,
+	}45.0,
+	}73.0,
+	}101.0,
+	}129.0,
 	};
 	mavlink_roll_pitch_yaw_thrust_setpoint_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -2482,10 +2482,10 @@ static void mavlink_test_roll_pitch_yaw_speed_thrust_setpoint(uint8_t system_id,
         uint16_t i;
 	mavlink_roll_pitch_yaw_speed_thrust_setpoint_t packet_in = {
 		963497464,
-	45.0,
-	73.0,
-	101.0,
-	129.0,
+	}45.0,
+	}73.0,
+	}101.0,
+	}129.0,
 	};
 	mavlink_roll_pitch_yaw_speed_thrust_setpoint_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -2533,10 +2533,10 @@ static void mavlink_test_set_quad_motors_setpoint(uint8_t system_id, uint8_t com
         uint16_t i;
 	mavlink_set_quad_motors_setpoint_t packet_in = {
 		17235,
-	17339,
-	17443,
-	17547,
-	29,
+	}17339,
+	}17443,
+	}17547,
+	}29,
 	};
 	mavlink_set_quad_motors_setpoint_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -2584,11 +2584,11 @@ static void mavlink_test_set_quad_swarm_roll_pitch_yaw_thrust(uint8_t system_id,
         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,
+	}{ 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));
@@ -2637,13 +2637,13 @@ static void mavlink_test_nav_controller_output(uint8_t system_id, uint8_t compon
         uint16_t i;
 	mavlink_nav_controller_output_t packet_in = {
 		17.0,
-	45.0,
-	73.0,
-	101.0,
-	129.0,
-	18275,
-	18379,
-	18483,
+	}45.0,
+	}73.0,
+	}101.0,
+	}129.0,
+	}18275,
+	}18379,
+	}18483,
 	};
 	mavlink_nav_controller_output_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -2694,14 +2694,14 @@ static void mavlink_test_set_quad_swarm_led_roll_pitch_yaw_thrust(uint8_t system
         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 },
+	}{ 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));
@@ -2753,14 +2753,14 @@ static void mavlink_test_state_correction(uint8_t system_id, uint8_t component_i
         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,
+	}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));
@@ -2812,10 +2812,10 @@ static void mavlink_test_request_data_stream(uint8_t system_id, uint8_t componen
         uint16_t i;
 	mavlink_request_data_stream_t packet_in = {
 		17235,
-	139,
-	206,
-	17,
-	84,
+	}139,
+	}206,
+	}17,
+	}84,
 	};
 	mavlink_request_data_stream_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -2863,8 +2863,8 @@ static void mavlink_test_data_stream(uint8_t system_id, uint8_t component_id, ma
         uint16_t i;
 	mavlink_data_stream_t packet_in = {
 		17235,
-	139,
-	206,
+	}139,
+	}206,
 	};
 	mavlink_data_stream_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -2910,11 +2910,11 @@ static void mavlink_test_manual_control(uint8_t system_id, uint8_t component_id,
         uint16_t i;
 	mavlink_manual_control_t packet_in = {
 		17235,
-	17339,
-	17443,
-	17547,
-	17651,
-	163,
+	}17339,
+	}17443,
+	}17547,
+	}17651,
+	}163,
 	};
 	mavlink_manual_control_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -2963,15 +2963,15 @@ static void mavlink_test_rc_channels_override(uint8_t system_id, uint8_t compone
         uint16_t i;
 	mavlink_rc_channels_override_t packet_in = {
 		17235,
-	17339,
-	17443,
-	17547,
-	17651,
-	17755,
-	17859,
-	17963,
-	53,
-	120,
+	}17339,
+	}17443,
+	}17547,
+	}17651,
+	}17755,
+	}17859,
+	}17963,
+	}53,
+	}120,
 	};
 	mavlink_rc_channels_override_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -3024,11 +3024,11 @@ static void mavlink_test_vfr_hud(uint8_t system_id, uint8_t component_id, mavlin
         uint16_t i;
 	mavlink_vfr_hud_t packet_in = {
 		17.0,
-	45.0,
-	73.0,
-	101.0,
-	18067,
-	18171,
+	}45.0,
+	}73.0,
+	}101.0,
+	}18067,
+	}18171,
 	};
 	mavlink_vfr_hud_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -3077,16 +3077,16 @@ static void mavlink_test_command_long(uint8_t system_id, uint8_t component_id, m
         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,
+	}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));
@@ -3140,7 +3140,7 @@ static void mavlink_test_command_ack(uint8_t system_id, uint8_t component_id, ma
         uint16_t i;
 	mavlink_command_ack_t packet_in = {
 		17235,
-	139,
+	}139,
 	};
 	mavlink_command_ack_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -3185,10 +3185,10 @@ static void mavlink_test_roll_pitch_yaw_rates_thrust_setpoint(uint8_t system_id,
         uint16_t i;
 	mavlink_roll_pitch_yaw_rates_thrust_setpoint_t packet_in = {
 		963497464,
-	45.0,
-	73.0,
-	101.0,
-	129.0,
+	}45.0,
+	}73.0,
+	}101.0,
+	}129.0,
 	};
 	mavlink_roll_pitch_yaw_rates_thrust_setpoint_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -3236,12 +3236,12 @@ static void mavlink_test_manual_setpoint(uint8_t system_id, uint8_t component_id
         uint16_t i;
 	mavlink_manual_setpoint_t packet_in = {
 		963497464,
-	45.0,
-	73.0,
-	101.0,
-	129.0,
-	65,
-	132,
+	}45.0,
+	}73.0,
+	}101.0,
+	}129.0,
+	}65,
+	}132,
 	};
 	mavlink_manual_setpoint_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -3291,12 +3291,12 @@ static void mavlink_test_local_position_ned_system_global_offset(uint8_t system_
         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,
+	}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));
@@ -3346,21 +3346,21 @@ static void mavlink_test_hil_state(uint8_t system_id, uint8_t component_id, mavl
         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,
+	}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));
@@ -3419,16 +3419,16 @@ static void mavlink_test_hil_controls(uint8_t system_id, uint8_t component_id, m
         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,
+	}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));
@@ -3482,19 +3482,19 @@ static void mavlink_test_hil_rc_inputs_raw(uint8_t system_id, uint8_t component_
         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,
+	}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));
@@ -3551,13 +3551,13 @@ static void mavlink_test_optical_flow(uint8_t system_id, uint8_t component_id, m
         uint16_t i;
 	mavlink_optical_flow_t packet_in = {
 		93372036854775807ULL,
-	73.0,
-	101.0,
-	129.0,
-	18275,
-	18379,
-	77,
-	144,
+	}73.0,
+	}101.0,
+	}129.0,
+	}18275,
+	}18379,
+	}77,
+	}144,
 	};
 	mavlink_optical_flow_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -3608,12 +3608,12 @@ static void mavlink_test_global_vision_position_estimate(uint8_t system_id, uint
         uint16_t i;
 	mavlink_global_vision_position_estimate_t packet_in = {
 		93372036854775807ULL,
-	73.0,
-	101.0,
-	129.0,
-	157.0,
-	185.0,
-	213.0,
+	}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));
@@ -3663,12 +3663,12 @@ static void mavlink_test_vision_position_estimate(uint8_t system_id, uint8_t com
         uint16_t i;
 	mavlink_vision_position_estimate_t packet_in = {
 		93372036854775807ULL,
-	73.0,
-	101.0,
-	129.0,
-	157.0,
-	185.0,
-	213.0,
+	}73.0,
+	}101.0,
+	}129.0,
+	}157.0,
+	}185.0,
+	}213.0,
 	};
 	mavlink_vision_position_estimate_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -3718,9 +3718,9 @@ static void mavlink_test_vision_speed_estimate(uint8_t system_id, uint8_t compon
         uint16_t i;
 	mavlink_vision_speed_estimate_t packet_in = {
 		93372036854775807ULL,
-	73.0,
-	101.0,
-	129.0,
+	}73.0,
+	}101.0,
+	}129.0,
 	};
 	mavlink_vision_speed_estimate_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -3767,12 +3767,12 @@ static void mavlink_test_vicon_position_estimate(uint8_t system_id, uint8_t comp
         uint16_t i;
 	mavlink_vicon_position_estimate_t packet_in = {
 		93372036854775807ULL,
-	73.0,
-	101.0,
-	129.0,
-	157.0,
-	185.0,
-	213.0,
+	}73.0,
+	}101.0,
+	}129.0,
+	}157.0,
+	}185.0,
+	}213.0,
 	};
 	mavlink_vicon_position_estimate_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -3822,20 +3822,20 @@ static void mavlink_test_highres_imu(uint8_t system_id, uint8_t component_id, ma
         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,
+	}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));
@@ -3893,11 +3893,11 @@ static void mavlink_test_omnidirectional_flow(uint8_t system_id, uint8_t compone
         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,
+	}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));
@@ -3939,6 +3939,215 @@ static void mavlink_test_omnidirectional_flow(uint8_t system_id, uint8_t compone
         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;
@@ -3946,10 +4155,10 @@ static void mavlink_test_file_transfer_start(uint8_t system_id, uint8_t componen
         uint16_t i;
 	mavlink_file_transfer_start_t packet_in = {
 		93372036854775807ULL,
-	963497880,
-	"MNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQ",
-	249,
-	60,
+	}963497880,
+	}"MNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQ",
+	}249,
+	}60,
 	};
 	mavlink_file_transfer_start_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -3997,8 +4206,8 @@ static void mavlink_test_file_transfer_dir_list(uint8_t system_id, uint8_t compo
         uint16_t i;
 	mavlink_file_transfer_dir_list_t packet_in = {
 		93372036854775807ULL,
-	"IJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLM",
-	237,
+	}"IJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLM",
+	}237,
 	};
 	mavlink_file_transfer_dir_list_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -4044,7 +4253,7 @@ static void mavlink_test_file_transfer_res(uint8_t system_id, uint8_t component_
         uint16_t i;
 	mavlink_file_transfer_res_t packet_in = {
 		93372036854775807ULL,
-	29,
+	}29,
 	};
 	mavlink_file_transfer_res_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -4082,24 +4291,790 @@ static void mavlink_test_file_transfer_res(uint8_t system_id, uint8_t component_
         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 = {
-		17235,
-	17339,
-	17443,
-	17547,
-	17651,
-	17755,
-	17859,
-	175,
-	242,
+		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;
@@ -4118,12 +5093,12 @@ static void mavlink_test_battery_status(uint8_t system_id, uint8_t component_id,
         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.battery_remaining );
+	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.battery_remaining );
+	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);
 
@@ -4136,7 +5111,7 @@ static void mavlink_test_battery_status(uint8_t system_id, uint8_t component_id,
         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.battery_remaining );
+	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);
 }
@@ -4148,14 +5123,14 @@ static void mavlink_test_setpoint_8dof(uint8_t system_id, uint8_t component_id,
         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,
+	}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));
@@ -4207,12 +5182,12 @@ static void mavlink_test_setpoint_6dof(uint8_t system_id, uint8_t component_id,
         uint16_t i;
 	mavlink_setpoint_6dof_t packet_in = {
 		17.0,
-	45.0,
-	73.0,
-	101.0,
-	129.0,
-	157.0,
-	77,
+	}45.0,
+	}73.0,
+	}101.0,
+	}129.0,
+	}157.0,
+	}77,
 	};
 	mavlink_setpoint_6dof_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -4262,9 +5237,9 @@ static void mavlink_test_memory_vect(uint8_t system_id, uint8_t component_id, ma
         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 },
+	}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));
@@ -4311,10 +5286,10 @@ static void mavlink_test_debug_vect(uint8_t system_id, uint8_t component_id, mav
         uint16_t i;
 	mavlink_debug_vect_t packet_in = {
 		93372036854775807ULL,
-	73.0,
-	101.0,
-	129.0,
-	"UVWXYZABC",
+	}73.0,
+	}101.0,
+	}129.0,
+	}"UVWXYZABC",
 	};
 	mavlink_debug_vect_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -4362,8 +5337,8 @@ static void mavlink_test_named_value_float(uint8_t system_id, uint8_t component_
         uint16_t i;
 	mavlink_named_value_float_t packet_in = {
 		963497464,
-	45.0,
-	"IJKLMNOPQ",
+	}45.0,
+	}"IJKLMNOPQ",
 	};
 	mavlink_named_value_float_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -4409,8 +5384,8 @@ static void mavlink_test_named_value_int(uint8_t system_id, uint8_t component_id
         uint16_t i;
 	mavlink_named_value_int_t packet_in = {
 		963497464,
-	963497672,
-	"IJKLMNOPQ",
+	}963497672,
+	}"IJKLMNOPQ",
 	};
 	mavlink_named_value_int_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -4456,7 +5431,7 @@ static void mavlink_test_statustext(uint8_t system_id, uint8_t component_id, mav
         uint16_t i;
 	mavlink_statustext_t packet_in = {
 		5,
-	"BCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWX",
+	}"BCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWX",
 	};
 	mavlink_statustext_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -4501,8 +5476,8 @@ static void mavlink_test_debug(uint8_t system_id, uint8_t component_id, mavlink_
         uint16_t i;
 	mavlink_debug_t packet_in = {
 		963497464,
-	45.0,
-	29,
+	}45.0,
+	}29,
 	};
 	mavlink_debug_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -4616,9 +5591,26 @@ static void mavlink_test_common(uint8_t system_id, uint8_t component_id, mavlink
 	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);
diff --git a/mavlink/include/mavlink/v1.0/common/version.h b/mavlink/include/mavlink/v1.0/common/version.h
index 3080a214d..2f3736f45 100644
--- a/mavlink/include/mavlink/v1.0/common/version.h
+++ b/mavlink/include/mavlink/v1.0/common/version.h
@@ -5,8 +5,8 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Wed Jan  9 16:19:41 2013"
+#define MAVLINK_BUILD_DATE "Tue Feb  4 15:28:29 2014"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
-#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254
+#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
index 6e342f237..2f15d31c4 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/matrixpilot.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/matrixpilot.h
@@ -12,15 +12,15 @@ extern "C" {
 // MESSAGE LENGTHS AND CRCS
 
 #ifndef MAVLINK_MESSAGE_LENGTHS
-#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 0, 0, 0, 254, 249, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 33, 25, 2, 6, 58, 6, 0, 53, 7, 3, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 70, 10, 24, 20, 24, 28, 14, 17, 60, 110, 28, 16, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0}
+#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 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, 0, 0, 0, 235, 93, 124, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 241, 15, 181, 26, 101, 109, 0, 12, 218, 133, 208, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 150, 169, 191, 121, 54, 171, 142, 249, 123, 7, 222, 55, 154, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}
+#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 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, {"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_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_SET, MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_READ_REQ, MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_BUFFER_FUNCTION, MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_DIRECTORY, MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_DIRECTORY_ACK, MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_COMMAND, MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F2_A, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F2_B, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F4, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F5, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F6, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F7, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F8, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F13, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F14, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F15, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F16, MAVLINK_MESSAGE_INFO_ALTITUDES, MAVLINK_MESSAGE_INFO_AIRSPEEDS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}}
+#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"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"
@@ -77,6 +77,7 @@ enum MAV_CMD
 	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|  */
@@ -85,7 +86,8 @@ enum MAV_CMD
 	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_ENUM_END=401, /*  | */
+	MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX|  */
+	MAV_CMD_ENUM_END=501, /*  | */
 };
 #endif
 
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h
index eceb61d51..bc47a547a 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h
@@ -16,6 +16,9 @@ typedef struct __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 { \
@@ -51,7 +54,7 @@ static inline uint16_t mavlink_msg_airspeeds_pack(uint8_t system_id, uint8_t com
 						       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[16];
+	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);
@@ -60,7 +63,7 @@ static inline uint16_t mavlink_msg_airspeeds_pack(uint8_t system_id, uint8_t com
 	_mav_put_int16_t(buf, 12, aoa);
 	_mav_put_int16_t(buf, 14, aoy);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 16);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AIRSPEEDS_LEN);
 #else
 	mavlink_airspeeds_t packet;
 	packet.time_boot_ms = time_boot_ms;
@@ -71,18 +74,22 @@ static inline uint16_t mavlink_msg_airspeeds_pack(uint8_t system_id, uint8_t com
 	packet.aoa = aoa;
 	packet.aoy = aoy;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 16);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AIRSPEEDS_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_AIRSPEEDS;
-	return mavlink_finalize_message(msg, system_id, component_id, 16, 154);
+#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 was sent over
+ * @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
@@ -98,7 +105,7 @@ static inline uint16_t mavlink_msg_airspeeds_pack_chan(uint8_t system_id, uint8_
 						           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[16];
+	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);
@@ -107,7 +114,7 @@ static inline uint16_t mavlink_msg_airspeeds_pack_chan(uint8_t system_id, uint8_
 	_mav_put_int16_t(buf, 12, aoa);
 	_mav_put_int16_t(buf, 14, aoy);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 16);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AIRSPEEDS_LEN);
 #else
 	mavlink_airspeeds_t packet;
 	packet.time_boot_ms = time_boot_ms;
@@ -118,15 +125,19 @@ static inline uint16_t mavlink_msg_airspeeds_pack_chan(uint8_t system_id, uint8_
 	packet.aoa = aoa;
 	packet.aoy = aoy;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 16);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AIRSPEEDS_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_AIRSPEEDS;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16, 154);
+#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 into a message
+ * @brief Encode a airspeeds struct
  *
  * @param system_id ID of this system
  * @param component_id ID of this component (e.g. 200 for IMU)
@@ -139,6 +150,20 @@ static inline uint16_t mavlink_msg_airspeeds_encode(uint8_t system_id, uint8_t c
 }
 
 /**
+ * @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
  *
@@ -155,7 +180,7 @@ static inline uint16_t mavlink_msg_airspeeds_encode(uint8_t system_id, uint8_t c
 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[16];
+	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);
@@ -164,7 +189,11 @@ static inline void mavlink_msg_airspeeds_send(mavlink_channel_t chan, uint32_t t
 	_mav_put_int16_t(buf, 12, aoa);
 	_mav_put_int16_t(buf, 14, aoy);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEEDS, buf, 16, 154);
+#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;
@@ -175,7 +204,11 @@ static inline void mavlink_msg_airspeeds_send(mavlink_channel_t chan, uint32_t t
 	packet.aoa = aoa;
 	packet.aoy = aoy;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEEDS, (const char *)&packet, 16, 154);
+#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
 }
 
@@ -271,6 +304,6 @@ static inline void mavlink_msg_airspeeds_decode(const mavlink_message_t* msg, ma
 	airspeeds->aoa = mavlink_msg_airspeeds_get_aoa(msg);
 	airspeeds->aoy = mavlink_msg_airspeeds_get_aoy(msg);
 #else
-	memcpy(airspeeds, _MAV_PAYLOAD(msg), 16);
+	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
index e77dcde49..e64787cc7 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h
@@ -16,6 +16,9 @@ typedef struct __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 { \
@@ -51,7 +54,7 @@ static inline uint16_t mavlink_msg_altitudes_pack(uint8_t system_id, uint8_t com
 						       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[28];
+	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);
@@ -60,7 +63,7 @@ static inline uint16_t mavlink_msg_altitudes_pack(uint8_t system_id, uint8_t com
 	_mav_put_int32_t(buf, 20, alt_range_finder);
 	_mav_put_int32_t(buf, 24, alt_extra);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 28);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ALTITUDES_LEN);
 #else
 	mavlink_altitudes_t packet;
 	packet.time_boot_ms = time_boot_ms;
@@ -71,18 +74,22 @@ static inline uint16_t mavlink_msg_altitudes_pack(uint8_t system_id, uint8_t com
 	packet.alt_range_finder = alt_range_finder;
 	packet.alt_extra = alt_extra;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 28);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ALTITUDES_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_ALTITUDES;
-	return mavlink_finalize_message(msg, system_id, component_id, 28, 55);
+#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 was sent over
+ * @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
@@ -98,7 +105,7 @@ static inline uint16_t mavlink_msg_altitudes_pack_chan(uint8_t system_id, uint8_
 						           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[28];
+	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);
@@ -107,7 +114,7 @@ static inline uint16_t mavlink_msg_altitudes_pack_chan(uint8_t system_id, uint8_
 	_mav_put_int32_t(buf, 20, alt_range_finder);
 	_mav_put_int32_t(buf, 24, alt_extra);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 28);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ALTITUDES_LEN);
 #else
 	mavlink_altitudes_t packet;
 	packet.time_boot_ms = time_boot_ms;
@@ -118,15 +125,19 @@ static inline uint16_t mavlink_msg_altitudes_pack_chan(uint8_t system_id, uint8_
 	packet.alt_range_finder = alt_range_finder;
 	packet.alt_extra = alt_extra;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 28);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ALTITUDES_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_ALTITUDES;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 28, 55);
+#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 into a message
+ * @brief Encode a altitudes struct
  *
  * @param system_id ID of this system
  * @param component_id ID of this component (e.g. 200 for IMU)
@@ -139,6 +150,20 @@ static inline uint16_t mavlink_msg_altitudes_encode(uint8_t system_id, uint8_t c
 }
 
 /**
+ * @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
  *
@@ -155,7 +180,7 @@ static inline uint16_t mavlink_msg_altitudes_encode(uint8_t system_id, uint8_t c
 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[28];
+	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);
@@ -164,7 +189,11 @@ static inline void mavlink_msg_altitudes_send(mavlink_channel_t chan, uint32_t t
 	_mav_put_int32_t(buf, 20, alt_range_finder);
 	_mav_put_int32_t(buf, 24, alt_extra);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ALTITUDES, buf, 28, 55);
+#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;
@@ -175,7 +204,11 @@ static inline void mavlink_msg_altitudes_send(mavlink_channel_t chan, uint32_t t
 	packet.alt_range_finder = alt_range_finder;
 	packet.alt_extra = alt_extra;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ALTITUDES, (const char *)&packet, 28, 55);
+#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
 }
 
@@ -271,6 +304,6 @@ static inline void mavlink_msg_altitudes_decode(const mavlink_message_t* msg, ma
 	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), 28);
+	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
index 1f1d88fa7..581bd35dc 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h
@@ -16,6 +16,9 @@ typedef struct __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 { \
@@ -51,7 +54,7 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_pack(uint8_t sy
 						       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[58];
+	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);
@@ -59,7 +62,7 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_pack(uint8_t sy
 	_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, 58);
+        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;
@@ -69,18 +72,22 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_pack(uint8_t sy
 	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, 58);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION;
-	return mavlink_finalize_message(msg, system_id, component_id, 58, 101);
+#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 was sent over
+ * @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
@@ -96,7 +103,7 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_pack_chan(uint8
 						           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[58];
+	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);
@@ -104,7 +111,7 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_pack_chan(uint8
 	_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, 58);
+        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;
@@ -114,15 +121,19 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_pack_chan(uint8
 	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, 58);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 58, 101);
+#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 into a message
+ * @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)
@@ -135,6 +146,20 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_encode(uint8_t
 }
 
 /**
+ * @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
  *
@@ -151,7 +176,7 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_encode(uint8_t
 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[58];
+	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);
@@ -159,7 +184,11 @@ static inline void mavlink_msg_flexifunction_buffer_function_send(mavlink_channe
 	_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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION, buf, 58, 101);
+#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;
@@ -169,7 +198,11 @@ static inline void mavlink_msg_flexifunction_buffer_function_send(mavlink_channe
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 	mav_array_memcpy(packet.data, data, sizeof(int8_t)*48);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION, (const char *)&packet, 58, 101);
+#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
 }
 
@@ -265,6 +298,6 @@ static inline void mavlink_msg_flexifunction_buffer_function_decode(const mavlin
 	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), 58);
+	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
index dfbcc1313..790afd52b 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h
@@ -13,6 +13,9 @@ typedef struct __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 { \
@@ -42,13 +45,13 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_pack(uint8_
 						       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[6];
+	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, 6);
+        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;
@@ -56,18 +59,22 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_pack(uint8_
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 6);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 6, 109);
+#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 was sent over
+ * @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
@@ -80,13 +87,13 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_pack_chan(u
 						           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[6];
+	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, 6);
+        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;
@@ -94,15 +101,19 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_pack_chan(u
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 6);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 6, 109);
+#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 into a message
+ * @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)
@@ -115,6 +126,20 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_encode(uint
 }
 
 /**
+ * @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
  *
@@ -128,13 +153,17 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_encode(uint
 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[6];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, buf, 6, 109);
+#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;
@@ -142,7 +171,11 @@ static inline void mavlink_msg_flexifunction_buffer_function_ack_send(mavlink_ch
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, (const char *)&packet, 6, 109);
+#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
 }
 
@@ -205,6 +238,6 @@ static inline void mavlink_msg_flexifunction_buffer_function_ack_decode(const ma
 	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), 6);
+	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
index f60f9f0c5..ce722c8a4 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h
@@ -12,6 +12,9 @@ typedef struct __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 { \
@@ -39,30 +42,34 @@ static inline uint16_t mavlink_msg_flexifunction_command_pack(uint8_t system_id,
 						       uint8_t target_system, uint8_t target_component, uint8_t command_type)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[3];
+	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, 3);
+        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, 3);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND;
-	return mavlink_finalize_message(msg, system_id, component_id, 3, 133);
+#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 was sent over
+ * @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
@@ -74,27 +81,31 @@ static inline uint16_t mavlink_msg_flexifunction_command_pack_chan(uint8_t syste
 						           uint8_t target_system,uint8_t target_component,uint8_t command_type)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[3];
+	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, 3);
+        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, 3);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3, 133);
+#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 into a message
+ * @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)
@@ -107,6 +118,20 @@ static inline uint16_t mavlink_msg_flexifunction_command_encode(uint8_t system_i
 }
 
 /**
+ * @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
  *
@@ -119,19 +144,27 @@ static inline uint16_t mavlink_msg_flexifunction_command_encode(uint8_t system_i
 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[3];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND, buf, 3, 133);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND, (const char *)&packet, 3, 133);
+#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
 }
 
@@ -183,6 +216,6 @@ static inline void mavlink_msg_flexifunction_command_decode(const mavlink_messag
 	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), 3);
+	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
index 97035de00..070dc4bf8 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h
@@ -11,6 +11,9 @@ typedef struct __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 { \
@@ -36,28 +39,32 @@ static inline uint16_t mavlink_msg_flexifunction_command_ack_pack(uint8_t system
 						       uint16_t command_type, uint16_t result)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[4];
+	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, 4);
+        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, 4);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK;
-	return mavlink_finalize_message(msg, system_id, component_id, 4, 208);
+#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 was sent over
+ * @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
@@ -68,25 +75,29 @@ static inline uint16_t mavlink_msg_flexifunction_command_ack_pack_chan(uint8_t s
 						           uint16_t command_type,uint16_t result)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[4];
+	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, 4);
+        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, 4);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 208);
+#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 into a message
+ * @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)
@@ -99,6 +110,20 @@ static inline uint16_t mavlink_msg_flexifunction_command_ack_encode(uint8_t syst
 }
 
 /**
+ * @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
  *
@@ -110,17 +135,25 @@ static inline uint16_t mavlink_msg_flexifunction_command_ack_encode(uint8_t syst
 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[4];
+	char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN];
 	_mav_put_uint16_t(buf, 0, command_type);
 	_mav_put_uint16_t(buf, 2, result);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK, buf, 4, 208);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK, (const char *)&packet, 4, 208);
+#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
 }
 
@@ -161,6 +194,6 @@ static inline void mavlink_msg_flexifunction_command_ack_decode(const mavlink_me
 	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), 4);
+	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
index 0085b31ed..ef262a6b1 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h
@@ -15,6 +15,9 @@ typedef struct __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 { \
@@ -48,14 +51,14 @@ static inline uint16_t mavlink_msg_flexifunction_directory_pack(uint8_t system_i
 						       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[53];
+	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, 53);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN);
 #else
 	mavlink_flexifunction_directory_t packet;
 	packet.target_system = target_system;
@@ -64,18 +67,22 @@ static inline uint16_t mavlink_msg_flexifunction_directory_pack(uint8_t system_i
 	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, 53);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY;
-	return mavlink_finalize_message(msg, system_id, component_id, 53, 12);
+#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 was sent over
+ * @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
@@ -90,14 +97,14 @@ static inline uint16_t mavlink_msg_flexifunction_directory_pack_chan(uint8_t sys
 						           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[53];
+	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, 53);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN);
 #else
 	mavlink_flexifunction_directory_t packet;
 	packet.target_system = target_system;
@@ -106,15 +113,19 @@ static inline uint16_t mavlink_msg_flexifunction_directory_pack_chan(uint8_t sys
 	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, 53);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 53, 12);
+#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 into a message
+ * @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)
@@ -127,6 +138,20 @@ static inline uint16_t mavlink_msg_flexifunction_directory_encode(uint8_t system
 }
 
 /**
+ * @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
  *
@@ -142,14 +167,18 @@ static inline uint16_t mavlink_msg_flexifunction_directory_encode(uint8_t system
 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[53];
+	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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY, buf, 53, 12);
+#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;
@@ -158,7 +187,11 @@ static inline void mavlink_msg_flexifunction_directory_send(mavlink_channel_t ch
 	packet.start_index = start_index;
 	packet.count = count;
 	mav_array_memcpy(packet.directory_data, directory_data, sizeof(int8_t)*48);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY, (const char *)&packet, 53, 12);
+#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
 }
 
@@ -243,6 +276,6 @@ static inline void mavlink_msg_flexifunction_directory_decode(const mavlink_mess
 	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), 53);
+	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
index 5112aa067..d3a386ce5 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h
@@ -15,6 +15,9 @@ typedef struct __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 { \
@@ -48,7 +51,7 @@ static inline uint16_t mavlink_msg_flexifunction_directory_ack_pack(uint8_t syst
 						       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[7];
+	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);
@@ -56,7 +59,7 @@ static inline uint16_t mavlink_msg_flexifunction_directory_ack_pack(uint8_t syst
 	_mav_put_uint8_t(buf, 5, start_index);
 	_mav_put_uint8_t(buf, 6, count);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 7);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN);
 #else
 	mavlink_flexifunction_directory_ack_t packet;
 	packet.result = result;
@@ -66,18 +69,22 @@ static inline uint16_t mavlink_msg_flexifunction_directory_ack_pack(uint8_t syst
 	packet.start_index = start_index;
 	packet.count = count;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 7);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK;
-	return mavlink_finalize_message(msg, system_id, component_id, 7, 218);
+#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 was sent over
+ * @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
@@ -92,7 +99,7 @@ static inline uint16_t mavlink_msg_flexifunction_directory_ack_pack_chan(uint8_t
 						           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[7];
+	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);
@@ -100,7 +107,7 @@ static inline uint16_t mavlink_msg_flexifunction_directory_ack_pack_chan(uint8_t
 	_mav_put_uint8_t(buf, 5, start_index);
 	_mav_put_uint8_t(buf, 6, count);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 7);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN);
 #else
 	mavlink_flexifunction_directory_ack_t packet;
 	packet.result = result;
@@ -110,15 +117,19 @@ static inline uint16_t mavlink_msg_flexifunction_directory_ack_pack_chan(uint8_t
 	packet.start_index = start_index;
 	packet.count = count;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 7);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 7, 218);
+#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 into a message
+ * @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)
@@ -131,6 +142,20 @@ static inline uint16_t mavlink_msg_flexifunction_directory_ack_encode(uint8_t sy
 }
 
 /**
+ * @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
  *
@@ -146,7 +171,7 @@ static inline uint16_t mavlink_msg_flexifunction_directory_ack_encode(uint8_t sy
 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[7];
+	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);
@@ -154,7 +179,11 @@ static inline void mavlink_msg_flexifunction_directory_ack_send(mavlink_channel_
 	_mav_put_uint8_t(buf, 5, start_index);
 	_mav_put_uint8_t(buf, 6, count);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK, buf, 7, 218);
+#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;
@@ -164,7 +193,11 @@ static inline void mavlink_msg_flexifunction_directory_ack_send(mavlink_channel_
 	packet.start_index = start_index;
 	packet.count = count;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK, (const char *)&packet, 7, 218);
+#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
 }
 
@@ -249,6 +282,6 @@ static inline void mavlink_msg_flexifunction_directory_ack_decode(const mavlink_
 	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), 7);
+	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
index 431282420..e50f77b08 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h
@@ -13,6 +13,9 @@ typedef struct __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 { \
@@ -42,13 +45,13 @@ static inline uint16_t mavlink_msg_flexifunction_read_req_pack(uint8_t system_id
 						       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[6];
+	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, 6);
+        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;
@@ -56,18 +59,22 @@ static inline uint16_t mavlink_msg_flexifunction_read_req_pack(uint8_t system_id
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 6);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ;
-	return mavlink_finalize_message(msg, system_id, component_id, 6, 26);
+#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 was sent over
+ * @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
@@ -80,13 +87,13 @@ static inline uint16_t mavlink_msg_flexifunction_read_req_pack_chan(uint8_t syst
 						           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[6];
+	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, 6);
+        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;
@@ -94,15 +101,19 @@ static inline uint16_t mavlink_msg_flexifunction_read_req_pack_chan(uint8_t syst
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 6);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 6, 26);
+#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 into a message
+ * @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)
@@ -115,6 +126,20 @@ static inline uint16_t mavlink_msg_flexifunction_read_req_encode(uint8_t system_
 }
 
 /**
+ * @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
  *
@@ -128,13 +153,17 @@ static inline uint16_t mavlink_msg_flexifunction_read_req_encode(uint8_t system_
 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[6];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ, buf, 6, 26);
+#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;
@@ -142,7 +171,11 @@ static inline void mavlink_msg_flexifunction_read_req_send(mavlink_channel_t cha
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ, (const char *)&packet, 6, 26);
+#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
 }
 
@@ -205,6 +238,6 @@ static inline void mavlink_msg_flexifunction_read_req_decode(const mavlink_messa
 	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), 6);
+	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
index 021fcd10f..41afb3811 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h
@@ -11,6 +11,9 @@ typedef struct __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 { \
@@ -36,28 +39,32 @@ static inline uint16_t mavlink_msg_flexifunction_set_pack(uint8_t system_id, uin
 						       uint8_t target_system, uint8_t target_component)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[2];
+	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, 2);
+        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, 2);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_SET;
-	return mavlink_finalize_message(msg, system_id, component_id, 2, 181);
+#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 was sent over
+ * @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
@@ -68,25 +75,29 @@ static inline uint16_t mavlink_msg_flexifunction_set_pack_chan(uint8_t system_id
 						           uint8_t target_system,uint8_t target_component)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[2];
+	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, 2);
+        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, 2);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_SET;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 181);
+#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 into a message
+ * @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)
@@ -99,6 +110,20 @@ static inline uint16_t mavlink_msg_flexifunction_set_encode(uint8_t system_id, u
 }
 
 /**
+ * @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
  *
@@ -110,17 +135,25 @@ static inline uint16_t mavlink_msg_flexifunction_set_encode(uint8_t system_id, u
 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[2];
+	char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN];
 	_mav_put_uint8_t(buf, 0, target_system);
 	_mav_put_uint8_t(buf, 1, target_component);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET, buf, 2, 181);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET, (const char *)&packet, 2, 181);
+#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
 }
 
@@ -161,6 +194,6 @@ static inline void mavlink_msg_flexifunction_set_decode(const mavlink_message_t*
 	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), 2);
+	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
index c06d23ef8..d21ab4816 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h
@@ -13,6 +13,9 @@ typedef struct __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 { \
@@ -42,13 +45,13 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f13_pack(uint8_t system_id,
 						       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[14];
+	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, 14);
+        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;
@@ -56,18 +59,22 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f13_pack(uint8_t system_id,
 	packet.sue_alt_origin = sue_alt_origin;
 	packet.sue_week_no = sue_week_no;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 14);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 14, 249);
+#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 was sent over
+ * @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
@@ -80,13 +87,13 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f13_pack_chan(uint8_t system
 						           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[14];
+	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, 14);
+        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;
@@ -94,15 +101,19 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f13_pack_chan(uint8_t system
 	packet.sue_alt_origin = sue_alt_origin;
 	packet.sue_week_no = sue_week_no;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 14);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14, 249);
+#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 into a message
+ * @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)
@@ -115,6 +126,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f13_encode(uint8_t system_id
 }
 
 /**
+ * @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
  *
@@ -128,13 +153,17 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f13_encode(uint8_t system_id
 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[14];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13, buf, 14, 249);
+#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;
@@ -142,7 +171,11 @@ static inline void mavlink_msg_serial_udb_extra_f13_send(mavlink_channel_t chan,
 	packet.sue_alt_origin = sue_alt_origin;
 	packet.sue_week_no = sue_week_no;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13, (const char *)&packet, 14, 249);
+#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
 }
 
@@ -205,6 +238,6 @@ static inline void mavlink_msg_serial_udb_extra_f13_decode(const mavlink_message
 	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), 14);
+	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
index 4275b03d6..6ac12f28a 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h
@@ -20,6 +20,9 @@ typedef struct __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 { \
@@ -63,7 +66,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f14_pack(uint8_t system_id,
 						       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[17];
+	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);
@@ -76,7 +79,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f14_pack(uint8_t system_id,
 	_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, 17);
+        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;
@@ -91,18 +94,22 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f14_pack(uint8_t system_id,
 	packet.sue_CLOCK_CONFIG = sue_CLOCK_CONFIG;
 	packet.sue_FLIGHT_PLAN_TYPE = sue_FLIGHT_PLAN_TYPE;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 17);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 17, 123);
+#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 was sent over
+ * @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
@@ -122,7 +129,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f14_pack_chan(uint8_t system
 						           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[17];
+	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);
@@ -135,7 +142,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f14_pack_chan(uint8_t system
 	_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, 17);
+        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;
@@ -150,15 +157,19 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f14_pack_chan(uint8_t system
 	packet.sue_CLOCK_CONFIG = sue_CLOCK_CONFIG;
 	packet.sue_FLIGHT_PLAN_TYPE = sue_FLIGHT_PLAN_TYPE;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 17);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 17, 123);
+#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 into a message
+ * @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)
@@ -171,6 +182,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f14_encode(uint8_t system_id
 }
 
 /**
+ * @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
  *
@@ -191,7 +216,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f14_encode(uint8_t system_id
 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[17];
+	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);
@@ -204,7 +229,11 @@ static inline void mavlink_msg_serial_udb_extra_f14_send(mavlink_channel_t chan,
 	_mav_put_uint8_t(buf, 15, sue_CLOCK_CONFIG);
 	_mav_put_uint8_t(buf, 16, sue_FLIGHT_PLAN_TYPE);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14, buf, 17, 123);
+#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;
@@ -219,7 +248,11 @@ static inline void mavlink_msg_serial_udb_extra_f14_send(mavlink_channel_t chan,
 	packet.sue_CLOCK_CONFIG = sue_CLOCK_CONFIG;
 	packet.sue_FLIGHT_PLAN_TYPE = sue_FLIGHT_PLAN_TYPE;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14, (const char *)&packet, 17, 123);
+#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
 }
 
@@ -359,6 +392,6 @@ static inline void mavlink_msg_serial_udb_extra_f14_decode(const mavlink_message
 	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), 17);
+	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
index f4ada7838..10c3f4ca4 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h
@@ -11,6 +11,9 @@ typedef struct __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
 
@@ -37,28 +40,32 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f15_pack(uint8_t system_id,
 						       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[60];
+	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, 60);
+        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, 60);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 60, 7);
+#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 was sent over
+ * @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
@@ -69,25 +76,29 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f15_pack_chan(uint8_t system
 						           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[60];
+	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, 60);
+        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, 60);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 60, 7);
+#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 into a message
+ * @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)
@@ -100,6 +111,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f15_encode(uint8_t system_id
 }
 
 /**
+ * @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
  *
@@ -111,17 +136,25 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f15_encode(uint8_t system_id
 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[60];
+	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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15, buf, 60, 7);
+#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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15, (const char *)&packet, 60, 7);
+#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
 }
 
@@ -162,6 +195,6 @@ static inline void mavlink_msg_serial_udb_extra_f15_decode(const mavlink_message
 	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), 60);
+	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
index 20cadbf27..659e6b7c5 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h
@@ -11,6 +11,9 @@ typedef struct __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
 
@@ -37,28 +40,32 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f16_pack(uint8_t system_id,
 						       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[110];
+	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, 110);
+        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, 110);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 110, 222);
+#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 was sent over
+ * @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
@@ -69,25 +76,29 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f16_pack_chan(uint8_t system
 						           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[110];
+	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, 110);
+        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, 110);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 110, 222);
+#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 into a message
+ * @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)
@@ -100,6 +111,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f16_encode(uint8_t system_id
 }
 
 /**
+ * @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
  *
@@ -111,17 +136,25 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f16_encode(uint8_t system_id
 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[110];
+	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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16, buf, 110, 222);
+#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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16, (const char *)&packet, 110, 222);
+#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
 }
 
@@ -162,6 +195,6 @@ static inline void mavlink_msg_serial_udb_extra_f16_decode(const mavlink_message
 	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), 110);
+	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
index 30d0cb845..15ba68a34 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h
@@ -37,6 +37,9 @@ typedef struct __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 { \
@@ -114,7 +117,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_pack(uint8_t system_id,
 						       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[63];
+	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);
@@ -144,7 +147,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_pack(uint8_t system_id,
 	_mav_put_int16_t(buf, 60, sue_hdop);
 	_mav_put_uint8_t(buf, 62, sue_status);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 63);
+        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;
@@ -176,18 +179,22 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_pack(uint8_t system_id,
 	packet.sue_hdop = sue_hdop;
 	packet.sue_status = sue_status;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 63);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 63, 150);
+#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 was sent over
+ * @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
@@ -224,7 +231,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_pack_chan(uint8_t syste
 						           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[63];
+	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);
@@ -254,7 +261,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_pack_chan(uint8_t syste
 	_mav_put_int16_t(buf, 60, sue_hdop);
 	_mav_put_uint8_t(buf, 62, sue_status);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 63);
+        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;
@@ -286,15 +293,19 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_pack_chan(uint8_t syste
 	packet.sue_hdop = sue_hdop;
 	packet.sue_status = sue_status;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 63);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 63, 150);
+#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 into a message
+ * @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)
@@ -307,6 +318,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_encode(uint8_t system_i
 }
 
 /**
+ * @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
  *
@@ -344,7 +369,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_encode(uint8_t system_i
 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[63];
+	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);
@@ -374,7 +399,11 @@ static inline void mavlink_msg_serial_udb_extra_f2_a_send(mavlink_channel_t chan
 	_mav_put_int16_t(buf, 60, sue_hdop);
 	_mav_put_uint8_t(buf, 62, sue_status);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A, buf, 63, 150);
+#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;
@@ -406,7 +435,11 @@ static inline void mavlink_msg_serial_udb_extra_f2_a_send(mavlink_channel_t chan
 	packet.sue_hdop = sue_hdop;
 	packet.sue_status = sue_status;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A, (const char *)&packet, 63, 150);
+#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
 }
 
@@ -733,6 +766,6 @@ static inline void mavlink_msg_serial_udb_extra_f2_a_decode(const mavlink_messag
 	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), 63);
+	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
index 7f0c8fe0a..7cb8c87da 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h
@@ -42,6 +42,9 @@ typedef struct __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 { \
@@ -129,7 +132,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_pack(uint8_t system_id,
 						       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[70];
+	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);
@@ -164,7 +167,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_pack(uint8_t system_id,
 	_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, 70);
+        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;
@@ -201,18 +204,22 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_pack(uint8_t system_id,
 	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, 70);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 70, 169);
+#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 was sent over
+ * @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
@@ -254,7 +261,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_pack_chan(uint8_t syste
 						           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[70];
+	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);
@@ -289,7 +296,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_pack_chan(uint8_t syste
 	_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, 70);
+        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;
@@ -326,15 +333,19 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_pack_chan(uint8_t syste
 	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, 70);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 70, 169);
+#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 into a message
+ * @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)
@@ -347,6 +358,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_encode(uint8_t system_i
 }
 
 /**
+ * @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
  *
@@ -389,7 +414,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_encode(uint8_t system_i
 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[70];
+	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);
@@ -424,7 +449,11 @@ static inline void mavlink_msg_serial_udb_extra_f2_b_send(mavlink_channel_t chan
 	_mav_put_int16_t(buf, 66, sue_waypoint_goal_z);
 	_mav_put_int16_t(buf, 68, sue_memory_stack_free);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B, buf, 70, 169);
+#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;
@@ -461,7 +490,11 @@ static inline void mavlink_msg_serial_udb_extra_f2_b_send(mavlink_channel_t chan
 	packet.sue_waypoint_goal_z = sue_waypoint_goal_z;
 	packet.sue_memory_stack_free = sue_memory_stack_free;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B, (const char *)&packet, 70, 169);
+#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
 }
 
@@ -843,6 +876,6 @@ static inline void mavlink_msg_serial_udb_extra_f2_b_decode(const mavlink_messag
 	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), 70);
+	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
index 90c8e0428..77c616cc2 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h
@@ -19,6 +19,9 @@ typedef struct __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 { \
@@ -60,7 +63,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f4_pack(uint8_t system_id, u
 						       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[10];
+	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);
@@ -72,7 +75,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f4_pack(uint8_t system_id, u
 	_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, 10);
+        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;
@@ -86,18 +89,22 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f4_pack(uint8_t system_id, u
 	packet.sue_ALTITUDEHOLD_WAYPOINT = sue_ALTITUDEHOLD_WAYPOINT;
 	packet.sue_RACING_MODE = sue_RACING_MODE;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 10);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 10, 191);
+#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 was sent over
+ * @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
@@ -116,7 +123,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f4_pack_chan(uint8_t system_
 						           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[10];
+	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);
@@ -128,7 +135,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f4_pack_chan(uint8_t system_
 	_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, 10);
+        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;
@@ -142,15 +149,19 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f4_pack_chan(uint8_t system_
 	packet.sue_ALTITUDEHOLD_WAYPOINT = sue_ALTITUDEHOLD_WAYPOINT;
 	packet.sue_RACING_MODE = sue_RACING_MODE;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 10);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 10, 191);
+#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 into a message
+ * @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)
@@ -163,6 +174,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f4_encode(uint8_t system_id,
 }
 
 /**
+ * @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
  *
@@ -182,7 +207,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f4_encode(uint8_t system_id,
 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[10];
+	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);
@@ -194,7 +219,11 @@ static inline void mavlink_msg_serial_udb_extra_f4_send(mavlink_channel_t chan,
 	_mav_put_uint8_t(buf, 8, sue_ALTITUDEHOLD_WAYPOINT);
 	_mav_put_uint8_t(buf, 9, sue_RACING_MODE);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4, buf, 10, 191);
+#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;
@@ -208,7 +237,11 @@ static inline void mavlink_msg_serial_udb_extra_f4_send(mavlink_channel_t chan,
 	packet.sue_ALTITUDEHOLD_WAYPOINT = sue_ALTITUDEHOLD_WAYPOINT;
 	packet.sue_RACING_MODE = sue_RACING_MODE;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4, (const char *)&packet, 10, 191);
+#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
 }
 
@@ -337,6 +370,6 @@ static inline void mavlink_msg_serial_udb_extra_f4_decode(const mavlink_message_
 	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), 10);
+	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
index 79db8f1d8..e115a9b44 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h
@@ -15,6 +15,9 @@ typedef struct __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 { \
@@ -48,7 +51,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f5_pack(uint8_t system_id, u
 						       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[24];
+	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);
@@ -56,7 +59,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f5_pack(uint8_t system_id, u
 	_mav_put_float(buf, 16, sue_YAW_STABILIZATION_AILERON);
 	_mav_put_float(buf, 20, sue_AILERON_BOOST);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 24);
+        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;
@@ -66,18 +69,22 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f5_pack(uint8_t system_id, u
 	packet.sue_YAW_STABILIZATION_AILERON = sue_YAW_STABILIZATION_AILERON;
 	packet.sue_AILERON_BOOST = sue_AILERON_BOOST;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 24);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 24, 121);
+#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 was sent over
+ * @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
@@ -92,7 +99,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f5_pack_chan(uint8_t system_
 						           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[24];
+	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);
@@ -100,7 +107,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f5_pack_chan(uint8_t system_
 	_mav_put_float(buf, 16, sue_YAW_STABILIZATION_AILERON);
 	_mav_put_float(buf, 20, sue_AILERON_BOOST);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 24);
+        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;
@@ -110,15 +117,19 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f5_pack_chan(uint8_t system_
 	packet.sue_YAW_STABILIZATION_AILERON = sue_YAW_STABILIZATION_AILERON;
 	packet.sue_AILERON_BOOST = sue_AILERON_BOOST;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 24);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 24, 121);
+#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 into a message
+ * @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)
@@ -131,6 +142,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f5_encode(uint8_t system_id,
 }
 
 /**
+ * @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
  *
@@ -146,7 +171,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f5_encode(uint8_t system_id,
 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[24];
+	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);
@@ -154,7 +179,11 @@ static inline void mavlink_msg_serial_udb_extra_f5_send(mavlink_channel_t chan,
 	_mav_put_float(buf, 16, sue_YAW_STABILIZATION_AILERON);
 	_mav_put_float(buf, 20, sue_AILERON_BOOST);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5, buf, 24, 121);
+#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;
@@ -164,7 +193,11 @@ static inline void mavlink_msg_serial_udb_extra_f5_send(mavlink_channel_t chan,
 	packet.sue_YAW_STABILIZATION_AILERON = sue_YAW_STABILIZATION_AILERON;
 	packet.sue_AILERON_BOOST = sue_AILERON_BOOST;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5, (const char *)&packet, 24, 121);
+#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
 }
 
@@ -249,6 +282,6 @@ static inline void mavlink_msg_serial_udb_extra_f5_decode(const mavlink_message_
 	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), 24);
+	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
index 744866aff..656103d09 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h
@@ -14,6 +14,9 @@ typedef struct __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 { \
@@ -45,14 +48,14 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f6_pack(uint8_t system_id, u
 						       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[20];
+	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, 20);
+        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;
@@ -61,18 +64,22 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f6_pack(uint8_t system_id, u
 	packet.sue_ROLL_ELEV_MIX = sue_ROLL_ELEV_MIX;
 	packet.sue_ELEVATOR_BOOST = sue_ELEVATOR_BOOST;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 20);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 20, 54);
+#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 was sent over
+ * @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
@@ -86,14 +93,14 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f6_pack_chan(uint8_t system_
 						           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[20];
+	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, 20);
+        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;
@@ -102,15 +109,19 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f6_pack_chan(uint8_t system_
 	packet.sue_ROLL_ELEV_MIX = sue_ROLL_ELEV_MIX;
 	packet.sue_ELEVATOR_BOOST = sue_ELEVATOR_BOOST;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 20);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 54);
+#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 into a message
+ * @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)
@@ -123,6 +134,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f6_encode(uint8_t system_id,
 }
 
 /**
+ * @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
  *
@@ -137,14 +162,18 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f6_encode(uint8_t system_id,
 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[20];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6, buf, 20, 54);
+#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;
@@ -153,7 +182,11 @@ static inline void mavlink_msg_serial_udb_extra_f6_send(mavlink_channel_t chan,
 	packet.sue_ROLL_ELEV_MIX = sue_ROLL_ELEV_MIX;
 	packet.sue_ELEVATOR_BOOST = sue_ELEVATOR_BOOST;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6, (const char *)&packet, 20, 54);
+#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
 }
 
@@ -227,6 +260,6 @@ static inline void mavlink_msg_serial_udb_extra_f6_decode(const mavlink_message_
 	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), 20);
+	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
index 744ce0db0..51c98e6b7 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h
@@ -15,6 +15,9 @@ typedef struct __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 { \
@@ -48,7 +51,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f7_pack(uint8_t system_id, u
 						       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[24];
+	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);
@@ -56,7 +59,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f7_pack(uint8_t system_id, u
 	_mav_put_float(buf, 16, sue_RUDDER_BOOST);
 	_mav_put_float(buf, 20, sue_RTL_PITCH_DOWN);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 24);
+        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;
@@ -66,18 +69,22 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f7_pack(uint8_t system_id, u
 	packet.sue_RUDDER_BOOST = sue_RUDDER_BOOST;
 	packet.sue_RTL_PITCH_DOWN = sue_RTL_PITCH_DOWN;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 24);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 24, 171);
+#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 was sent over
+ * @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
@@ -92,7 +99,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f7_pack_chan(uint8_t system_
 						           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[24];
+	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);
@@ -100,7 +107,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f7_pack_chan(uint8_t system_
 	_mav_put_float(buf, 16, sue_RUDDER_BOOST);
 	_mav_put_float(buf, 20, sue_RTL_PITCH_DOWN);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 24);
+        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;
@@ -110,15 +117,19 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f7_pack_chan(uint8_t system_
 	packet.sue_RUDDER_BOOST = sue_RUDDER_BOOST;
 	packet.sue_RTL_PITCH_DOWN = sue_RTL_PITCH_DOWN;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 24);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 24, 171);
+#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 into a message
+ * @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)
@@ -131,6 +142,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f7_encode(uint8_t system_id,
 }
 
 /**
+ * @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
  *
@@ -146,7 +171,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f7_encode(uint8_t system_id,
 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[24];
+	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);
@@ -154,7 +179,11 @@ static inline void mavlink_msg_serial_udb_extra_f7_send(mavlink_channel_t chan,
 	_mav_put_float(buf, 16, sue_RUDDER_BOOST);
 	_mav_put_float(buf, 20, sue_RTL_PITCH_DOWN);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7, buf, 24, 171);
+#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;
@@ -164,7 +193,11 @@ static inline void mavlink_msg_serial_udb_extra_f7_send(mavlink_channel_t chan,
 	packet.sue_RUDDER_BOOST = sue_RUDDER_BOOST;
 	packet.sue_RTL_PITCH_DOWN = sue_RTL_PITCH_DOWN;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7, (const char *)&packet, 24, 171);
+#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
 }
 
@@ -249,6 +282,6 @@ static inline void mavlink_msg_serial_udb_extra_f7_decode(const mavlink_message_
 	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), 24);
+	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
index c92630e03..8557a95e2 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h
@@ -16,6 +16,9 @@ typedef struct __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 { \
@@ -51,7 +54,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f8_pack(uint8_t system_id, u
 						       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[28];
+	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);
@@ -60,7 +63,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f8_pack(uint8_t system_id, u
 	_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, 28);
+        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;
@@ -71,18 +74,22 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f8_pack(uint8_t system_id, u
 	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, 28);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 28, 142);
+#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 was sent over
+ * @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
@@ -98,7 +105,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f8_pack_chan(uint8_t system_
 						           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[28];
+	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);
@@ -107,7 +114,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f8_pack_chan(uint8_t system_
 	_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, 28);
+        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;
@@ -118,15 +125,19 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f8_pack_chan(uint8_t system_
 	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, 28);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 28, 142);
+#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 into a message
+ * @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)
@@ -139,6 +150,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f8_encode(uint8_t system_id,
 }
 
 /**
+ * @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
  *
@@ -155,7 +180,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f8_encode(uint8_t system_id,
 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[28];
+	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);
@@ -164,7 +189,11 @@ static inline void mavlink_msg_serial_udb_extra_f8_send(mavlink_channel_t chan,
 	_mav_put_float(buf, 20, sue_ALT_HOLD_PITCH_MAX);
 	_mav_put_float(buf, 24, sue_ALT_HOLD_PITCH_HIGH);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8, buf, 28, 142);
+#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;
@@ -175,7 +204,11 @@ static inline void mavlink_msg_serial_udb_extra_f8_send(mavlink_channel_t chan,
 	packet.sue_ALT_HOLD_PITCH_MAX = sue_ALT_HOLD_PITCH_MAX;
 	packet.sue_ALT_HOLD_PITCH_HIGH = sue_ALT_HOLD_PITCH_HIGH;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8, (const char *)&packet, 28, 142);
+#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
 }
 
@@ -271,6 +304,6 @@ static inline void mavlink_msg_serial_udb_extra_f8_decode(const mavlink_message_
 	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), 28);
+	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
index 5ccef5390..6856839e9 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/testsuite.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/testsuite.h
@@ -31,7 +31,7 @@ static void mavlink_test_flexifunction_set(uint8_t system_id, uint8_t component_
         uint16_t i;
 	mavlink_flexifunction_set_t packet_in = {
 		5,
-	72,
+	}72,
 	};
 	mavlink_flexifunction_set_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -76,9 +76,9 @@ static void mavlink_test_flexifunction_read_req(uint8_t system_id, uint8_t compo
         uint16_t i;
 	mavlink_flexifunction_read_req_t packet_in = {
 		17235,
-	17339,
-	17,
-	84,
+	}17339,
+	}17,
+	}84,
 	};
 	mavlink_flexifunction_read_req_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -125,12 +125,12 @@ static void mavlink_test_flexifunction_buffer_function(uint8_t system_id, uint8_
         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 },
+	}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));
@@ -180,9 +180,9 @@ static void mavlink_test_flexifunction_buffer_function_ack(uint8_t system_id, ui
         uint16_t i;
 	mavlink_flexifunction_buffer_function_ack_t packet_in = {
 		17235,
-	17339,
-	17,
-	84,
+	}17339,
+	}17,
+	}84,
 	};
 	mavlink_flexifunction_buffer_function_ack_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -229,11 +229,11 @@ static void mavlink_test_flexifunction_directory(uint8_t system_id, uint8_t comp
         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 },
+	}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));
@@ -282,11 +282,11 @@ static void mavlink_test_flexifunction_directory_ack(uint8_t system_id, uint8_t
         uint16_t i;
 	mavlink_flexifunction_directory_ack_t packet_in = {
 		17235,
-	139,
-	206,
-	17,
-	84,
-	151,
+	}139,
+	}206,
+	}17,
+	}84,
+	}151,
 	};
 	mavlink_flexifunction_directory_ack_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -335,8 +335,8 @@ static void mavlink_test_flexifunction_command(uint8_t system_id, uint8_t compon
         uint16_t i;
 	mavlink_flexifunction_command_t packet_in = {
 		5,
-	72,
-	139,
+	}72,
+	}139,
 	};
 	mavlink_flexifunction_command_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -382,7 +382,7 @@ static void mavlink_test_flexifunction_command_ack(uint8_t system_id, uint8_t co
         uint16_t i;
 	mavlink_flexifunction_command_ack_t packet_in = {
 		17235,
-	17339,
+	}17339,
 	};
 	mavlink_flexifunction_command_ack_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -427,33 +427,33 @@ static void mavlink_test_serial_udb_extra_f2_a(uint8_t system_id, uint8_t compon
         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,
+	}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));
@@ -524,38 +524,38 @@ static void mavlink_test_serial_udb_extra_f2_b(uint8_t system_id, uint8_t compon
         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,
+	}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));
@@ -631,15 +631,15 @@ static void mavlink_test_serial_udb_extra_f4(uint8_t system_id, uint8_t componen
         uint16_t i;
 	mavlink_serial_udb_extra_f4_t packet_in = {
 		5,
-	72,
-	139,
-	206,
-	17,
-	84,
-	151,
-	218,
-	29,
-	96,
+	}72,
+	}139,
+	}206,
+	}17,
+	}84,
+	}151,
+	}218,
+	}29,
+	}96,
 	};
 	mavlink_serial_udb_extra_f4_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -692,11 +692,11 @@ static void mavlink_test_serial_udb_extra_f5(uint8_t system_id, uint8_t componen
         uint16_t i;
 	mavlink_serial_udb_extra_f5_t packet_in = {
 		17.0,
-	45.0,
-	73.0,
-	101.0,
-	129.0,
-	157.0,
+	}45.0,
+	}73.0,
+	}101.0,
+	}129.0,
+	}157.0,
 	};
 	mavlink_serial_udb_extra_f5_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -745,10 +745,10 @@ static void mavlink_test_serial_udb_extra_f6(uint8_t system_id, uint8_t componen
         uint16_t i;
 	mavlink_serial_udb_extra_f6_t packet_in = {
 		17.0,
-	45.0,
-	73.0,
-	101.0,
-	129.0,
+	}45.0,
+	}73.0,
+	}101.0,
+	}129.0,
 	};
 	mavlink_serial_udb_extra_f6_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -796,11 +796,11 @@ static void mavlink_test_serial_udb_extra_f7(uint8_t system_id, uint8_t componen
         uint16_t i;
 	mavlink_serial_udb_extra_f7_t packet_in = {
 		17.0,
-	45.0,
-	73.0,
-	101.0,
-	129.0,
-	157.0,
+	}45.0,
+	}73.0,
+	}101.0,
+	}129.0,
+	}157.0,
 	};
 	mavlink_serial_udb_extra_f7_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -849,12 +849,12 @@ static void mavlink_test_serial_udb_extra_f8(uint8_t system_id, uint8_t componen
         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,
+	}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));
@@ -904,9 +904,9 @@ static void mavlink_test_serial_udb_extra_f13(uint8_t system_id, uint8_t compone
         uint16_t i;
 	mavlink_serial_udb_extra_f13_t packet_in = {
 		963497464,
-	963497672,
-	963497880,
-	17859,
+	}963497672,
+	}963497880,
+	}17859,
 	};
 	mavlink_serial_udb_extra_f13_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -953,16 +953,16 @@ static void mavlink_test_serial_udb_extra_f14(uint8_t system_id, uint8_t compone
         uint16_t i;
 	mavlink_serial_udb_extra_f14_t packet_in = {
 		963497464,
-	17443,
-	17547,
-	17651,
-	163,
-	230,
-	41,
-	108,
-	175,
-	242,
-	53,
+	}17443,
+	}17547,
+	}17651,
+	}163,
+	}230,
+	}41,
+	}108,
+	}175,
+	}242,
+	}53,
 	};
 	mavlink_serial_udb_extra_f14_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -1016,7 +1016,7 @@ static void mavlink_test_serial_udb_extra_f15(uint8_t system_id, uint8_t compone
         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 },
+	}{ 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));
@@ -1061,7 +1061,7 @@ static void mavlink_test_serial_udb_extra_f16(uint8_t system_id, uint8_t compone
         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 },
+	}{ 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));
@@ -1106,12 +1106,12 @@ static void mavlink_test_altitudes(uint8_t system_id, uint8_t component_id, mavl
         uint16_t i;
 	mavlink_altitudes_t packet_in = {
 		963497464,
-	963497672,
-	963497880,
-	963498088,
-	963498296,
-	963498504,
-	963498712,
+	}963497672,
+	}963497880,
+	}963498088,
+	}963498296,
+	}963498504,
+	}963498712,
 	};
 	mavlink_altitudes_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -1161,12 +1161,12 @@ static void mavlink_test_airspeeds(uint8_t system_id, uint8_t component_id, mavl
         uint16_t i;
 	mavlink_airspeeds_t packet_in = {
 		963497464,
-	17443,
-	17547,
-	17651,
-	17755,
-	17859,
-	17963,
+	}17443,
+	}17547,
+	}17651,
+	}17755,
+	}17859,
+	}17963,
 	};
 	mavlink_airspeeds_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/version.h b/mavlink/include/mavlink/v1.0/matrixpilot/version.h
index 7d9e1eafd..a8be16c42 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/version.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/version.h
@@ -5,8 +5,8 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Wed Jan  9 16:18:08 2013"
+#define MAVLINK_BUILD_DATE "Tue Feb  4 15:28:04 2014"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
-#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254
+#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
new file mode 100644
index 000000000..51afac87c
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/mavlink_conversions.h
@@ -0,0 +1,135 @@
+#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
index f3e49e88a..96672f847 100644
--- a/mavlink/include/mavlink/v1.0/mavlink_helpers.h
+++ b/mavlink/include/mavlink/v1.0/mavlink_helpers.h
@@ -4,6 +4,7 @@
 #include "string.h"
 #include "checksum.h"
 #include "mavlink_types.h"
+#include "mavlink_conversions.h"
 
 #ifndef MAVLINK_HELPER
 #define MAVLINK_HELPER
@@ -15,7 +16,12 @@
 #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
@@ -28,11 +34,8 @@ 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,
+	// No m_mavlink_buffer 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
diff --git a/mavlink/include/mavlink/v1.0/mavlink_types.h b/mavlink/include/mavlink/v1.0/mavlink_types.h
index 5fbde97f7..4019c619e 100644
--- a/mavlink/include/mavlink/v1.0/mavlink_types.h
+++ b/mavlink/include/mavlink/v1.0/mavlink_types.h
@@ -33,6 +33,9 @@ typedef struct param_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];
 	};
@@ -45,7 +48,7 @@ typedef struct __mavlink_system {
     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
+    uint32_t nav_mode;    ///< Unused, can be used by user to store the system's navigation mode
 } mavlink_system_t;
 
 typedef struct __mavlink_message {
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h
index 819c45bf2..ef5354d5e 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h
@@ -18,6 +18,9 @@ typedef struct __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 { \
@@ -57,7 +60,7 @@ static inline uint16_t mavlink_msg_attitude_control_pack(uint8_t system_id, uint
 						       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[21];
+	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);
@@ -68,7 +71,7 @@ static inline uint16_t mavlink_msg_attitude_control_pack(uint8_t system_id, uint
 	_mav_put_uint8_t(buf, 19, yaw_manual);
 	_mav_put_uint8_t(buf, 20, thrust_manual);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 21);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN);
 #else
 	mavlink_attitude_control_t packet;
 	packet.roll = roll;
@@ -81,18 +84,22 @@ static inline uint16_t mavlink_msg_attitude_control_pack(uint8_t system_id, uint
 	packet.yaw_manual = yaw_manual;
 	packet.thrust_manual = thrust_manual;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 21);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL;
-	return mavlink_finalize_message(msg, system_id, component_id, 21, 254);
+#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 was sent over
+ * @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
@@ -110,7 +117,7 @@ static inline uint16_t mavlink_msg_attitude_control_pack_chan(uint8_t system_id,
 						           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[21];
+	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);
@@ -121,7 +128,7 @@ static inline uint16_t mavlink_msg_attitude_control_pack_chan(uint8_t system_id,
 	_mav_put_uint8_t(buf, 19, yaw_manual);
 	_mav_put_uint8_t(buf, 20, thrust_manual);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 21);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN);
 #else
 	mavlink_attitude_control_t packet;
 	packet.roll = roll;
@@ -134,15 +141,19 @@ static inline uint16_t mavlink_msg_attitude_control_pack_chan(uint8_t system_id,
 	packet.yaw_manual = yaw_manual;
 	packet.thrust_manual = thrust_manual;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 21);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 21, 254);
+#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 into a message
+ * @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)
@@ -155,6 +166,20 @@ static inline uint16_t mavlink_msg_attitude_control_encode(uint8_t system_id, ui
 }
 
 /**
+ * @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
  *
@@ -173,7 +198,7 @@ static inline uint16_t mavlink_msg_attitude_control_encode(uint8_t system_id, ui
 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[21];
+	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);
@@ -184,7 +209,11 @@ static inline void mavlink_msg_attitude_control_send(mavlink_channel_t chan, uin
 	_mav_put_uint8_t(buf, 19, yaw_manual);
 	_mav_put_uint8_t(buf, 20, thrust_manual);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, buf, 21, 254);
+#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;
@@ -197,7 +226,11 @@ static inline void mavlink_msg_attitude_control_send(mavlink_channel_t chan, uin
 	packet.yaw_manual = yaw_manual;
 	packet.thrust_manual = thrust_manual;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, (const char *)&packet, 21, 254);
+#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
 }
 
@@ -315,6 +348,6 @@ static inline void mavlink_msg_attitude_control_decode(const mavlink_message_t*
 	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), 21);
+	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
index cde782b48..d41093792 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h
@@ -17,6 +17,9 @@ typedef struct __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 { \
@@ -54,7 +57,7 @@ static inline uint16_t mavlink_msg_brief_feature_pack(uint8_t system_id, uint8_t
 						       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[53];
+	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);
@@ -63,7 +66,7 @@ static inline uint16_t mavlink_msg_brief_feature_pack(uint8_t system_id, uint8_t
 	_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, 53);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN);
 #else
 	mavlink_brief_feature_t packet;
 	packet.x = x;
@@ -74,18 +77,22 @@ static inline uint16_t mavlink_msg_brief_feature_pack(uint8_t system_id, uint8_t
 	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, 53);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE;
-	return mavlink_finalize_message(msg, system_id, component_id, 53, 88);
+#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 was sent over
+ * @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
@@ -102,7 +109,7 @@ static inline uint16_t mavlink_msg_brief_feature_pack_chan(uint8_t system_id, ui
 						           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[53];
+	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);
@@ -111,7 +118,7 @@ static inline uint16_t mavlink_msg_brief_feature_pack_chan(uint8_t system_id, ui
 	_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, 53);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN);
 #else
 	mavlink_brief_feature_t packet;
 	packet.x = x;
@@ -122,15 +129,19 @@ static inline uint16_t mavlink_msg_brief_feature_pack_chan(uint8_t system_id, ui
 	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, 53);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 53, 88);
+#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 into a message
+ * @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)
@@ -143,6 +154,20 @@ static inline uint16_t mavlink_msg_brief_feature_encode(uint8_t system_id, uint8
 }
 
 /**
+ * @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
  *
@@ -160,7 +185,7 @@ static inline uint16_t mavlink_msg_brief_feature_encode(uint8_t system_id, uint8
 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[53];
+	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);
@@ -169,7 +194,11 @@ static inline void mavlink_msg_brief_feature_send(mavlink_channel_t chan, float
 	_mav_put_uint16_t(buf, 18, orientation);
 	_mav_put_uint8_t(buf, 20, orientation_assignment);
 	_mav_put_uint8_t_array(buf, 21, descriptor, 32);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, buf, 53, 88);
+#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;
@@ -180,7 +209,11 @@ static inline void mavlink_msg_brief_feature_send(mavlink_channel_t chan, float
 	packet.orientation = orientation;
 	packet.orientation_assignment = orientation_assignment;
 	mav_array_memcpy(packet.descriptor, descriptor, sizeof(uint8_t)*32);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, (const char *)&packet, 53, 88);
+#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
 }
 
@@ -287,6 +320,6 @@ static inline void mavlink_msg_brief_feature_decode(const mavlink_message_t* 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), 53);
+	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
index 913fcd94c..ae4db825d 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h
@@ -32,6 +32,9 @@ typedef struct __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 { \
@@ -99,7 +102,7 @@ static inline uint16_t mavlink_msg_image_available_pack(uint8_t system_id, uint8
 						       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[92];
+	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);
@@ -124,7 +127,7 @@ static inline uint16_t mavlink_msg_image_available_pack(uint8_t system_id, uint8
 	_mav_put_uint8_t(buf, 90, cam_no);
 	_mav_put_uint8_t(buf, 91, channels);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 92);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN);
 #else
 	mavlink_image_available_t packet;
 	packet.cam_id = cam_id;
@@ -151,18 +154,22 @@ static inline uint16_t mavlink_msg_image_available_pack(uint8_t system_id, uint8
 	packet.cam_no = cam_no;
 	packet.channels = channels;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 92);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE;
-	return mavlink_finalize_message(msg, system_id, component_id, 92, 224);
+#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 was sent over
+ * @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)
@@ -194,7 +201,7 @@ static inline uint16_t mavlink_msg_image_available_pack_chan(uint8_t system_id,
 						           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[92];
+	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);
@@ -219,7 +226,7 @@ static inline uint16_t mavlink_msg_image_available_pack_chan(uint8_t system_id,
 	_mav_put_uint8_t(buf, 90, cam_no);
 	_mav_put_uint8_t(buf, 91, channels);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 92);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN);
 #else
 	mavlink_image_available_t packet;
 	packet.cam_id = cam_id;
@@ -246,15 +253,19 @@ static inline uint16_t mavlink_msg_image_available_pack_chan(uint8_t system_id,
 	packet.cam_no = cam_no;
 	packet.channels = channels;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 92);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 92, 224);
+#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 into a message
+ * @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)
@@ -267,6 +278,20 @@ static inline uint16_t mavlink_msg_image_available_encode(uint8_t system_id, uin
 }
 
 /**
+ * @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
  *
@@ -299,7 +324,7 @@ static inline uint16_t mavlink_msg_image_available_encode(uint8_t system_id, uin
 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[92];
+	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);
@@ -324,7 +349,11 @@ static inline void mavlink_msg_image_available_send(mavlink_channel_t chan, uint
 	_mav_put_uint8_t(buf, 90, cam_no);
 	_mav_put_uint8_t(buf, 91, channels);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, buf, 92, 224);
+#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;
@@ -351,7 +380,11 @@ static inline void mavlink_msg_image_available_send(mavlink_channel_t chan, uint
 	packet.cam_no = cam_no;
 	packet.channels = channels;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, (const char *)&packet, 92, 224);
+#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
 }
 
@@ -623,6 +656,6 @@ static inline void mavlink_msg_image_available_decode(const mavlink_message_t* m
 	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), 92);
+	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
index deb05769a..664574be9 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h
@@ -10,6 +10,9 @@ typedef struct __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 { \
@@ -33,26 +36,30 @@ static inline uint16_t mavlink_msg_image_trigger_control_pack(uint8_t system_id,
 						       uint8_t enable)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[1];
+	char buf[MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN];
 	_mav_put_uint8_t(buf, 0, enable);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 1);
+        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, 1);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL;
-	return mavlink_finalize_message(msg, system_id, component_id, 1, 95);
+#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 was sent over
+ * @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)
@@ -62,23 +69,27 @@ static inline uint16_t mavlink_msg_image_trigger_control_pack_chan(uint8_t syste
 						           uint8_t enable)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[1];
+	char buf[MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN];
 	_mav_put_uint8_t(buf, 0, enable);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 1);
+        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, 1);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 1, 95);
+#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 into a message
+ * @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)
@@ -91,6 +102,20 @@ static inline uint16_t mavlink_msg_image_trigger_control_encode(uint8_t system_i
 }
 
 /**
+ * @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
  *
@@ -101,15 +126,23 @@ static inline uint16_t mavlink_msg_image_trigger_control_encode(uint8_t system_i
 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[1];
+	char buf[MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN];
 	_mav_put_uint8_t(buf, 0, enable);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, buf, 1, 95);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, (const char *)&packet, 1, 95);
+#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
 }
 
@@ -139,6 +172,6 @@ static inline void mavlink_msg_image_trigger_control_decode(const mavlink_messag
 #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), 1);
+	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
index 557d748e1..f3a2243af 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h
@@ -21,6 +21,9 @@ typedef struct __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 { \
@@ -66,7 +69,7 @@ static inline uint16_t mavlink_msg_image_triggered_pack(uint8_t system_id, uint8
 						       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[52];
+	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);
@@ -80,7 +83,7 @@ static inline uint16_t mavlink_msg_image_triggered_pack(uint8_t system_id, uint8
 	_mav_put_float(buf, 44, ground_y);
 	_mav_put_float(buf, 48, ground_z);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 52);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN);
 #else
 	mavlink_image_triggered_t packet;
 	packet.timestamp = timestamp;
@@ -96,18 +99,22 @@ static inline uint16_t mavlink_msg_image_triggered_pack(uint8_t system_id, uint8
 	packet.ground_y = ground_y;
 	packet.ground_z = ground_z;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 52);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED;
-	return mavlink_finalize_message(msg, system_id, component_id, 52, 86);
+#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 was sent over
+ * @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
@@ -128,7 +135,7 @@ static inline uint16_t mavlink_msg_image_triggered_pack_chan(uint8_t system_id,
 						           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[52];
+	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);
@@ -142,7 +149,7 @@ static inline uint16_t mavlink_msg_image_triggered_pack_chan(uint8_t system_id,
 	_mav_put_float(buf, 44, ground_y);
 	_mav_put_float(buf, 48, ground_z);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 52);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN);
 #else
 	mavlink_image_triggered_t packet;
 	packet.timestamp = timestamp;
@@ -158,15 +165,19 @@ static inline uint16_t mavlink_msg_image_triggered_pack_chan(uint8_t system_id,
 	packet.ground_y = ground_y;
 	packet.ground_z = ground_z;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 52);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 52, 86);
+#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 into a message
+ * @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)
@@ -179,6 +190,20 @@ static inline uint16_t mavlink_msg_image_triggered_encode(uint8_t system_id, uin
 }
 
 /**
+ * @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
  *
@@ -200,7 +225,7 @@ static inline uint16_t mavlink_msg_image_triggered_encode(uint8_t system_id, uin
 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[52];
+	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);
@@ -214,7 +239,11 @@ static inline void mavlink_msg_image_triggered_send(mavlink_channel_t chan, uint
 	_mav_put_float(buf, 44, ground_y);
 	_mav_put_float(buf, 48, ground_z);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, buf, 52, 86);
+#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;
@@ -230,7 +259,11 @@ static inline void mavlink_msg_image_triggered_send(mavlink_channel_t chan, uint
 	packet.ground_y = ground_y;
 	packet.ground_z = ground_z;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, (const char *)&packet, 52, 86);
+#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
 }
 
@@ -381,6 +414,6 @@ static inline void mavlink_msg_image_triggered_decode(const mavlink_message_t* m
 	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), 52);
+	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
index 0c8cc6f18..6bdcceaf9 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h
@@ -16,6 +16,9 @@ typedef struct __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 { \
@@ -51,7 +54,7 @@ static inline uint16_t mavlink_msg_marker_pack(uint8_t system_id, uint8_t compon
 						       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[26];
+	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);
@@ -60,7 +63,7 @@ static inline uint16_t mavlink_msg_marker_pack(uint8_t system_id, uint8_t compon
 	_mav_put_float(buf, 20, yaw);
 	_mav_put_uint16_t(buf, 24, id);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 26);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MARKER_LEN);
 #else
 	mavlink_marker_t packet;
 	packet.x = x;
@@ -71,18 +74,22 @@ static inline uint16_t mavlink_msg_marker_pack(uint8_t system_id, uint8_t compon
 	packet.yaw = yaw;
 	packet.id = id;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 26);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MARKER_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MARKER;
-	return mavlink_finalize_message(msg, system_id, component_id, 26, 249);
+#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 was sent over
+ * @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
@@ -98,7 +105,7 @@ static inline uint16_t mavlink_msg_marker_pack_chan(uint8_t system_id, uint8_t c
 						           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[26];
+	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);
@@ -107,7 +114,7 @@ static inline uint16_t mavlink_msg_marker_pack_chan(uint8_t system_id, uint8_t c
 	_mav_put_float(buf, 20, yaw);
 	_mav_put_uint16_t(buf, 24, id);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 26);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MARKER_LEN);
 #else
 	mavlink_marker_t packet;
 	packet.x = x;
@@ -118,15 +125,19 @@ static inline uint16_t mavlink_msg_marker_pack_chan(uint8_t system_id, uint8_t c
 	packet.yaw = yaw;
 	packet.id = id;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 26);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MARKER_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_MARKER;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 26, 249);
+#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 into a message
+ * @brief Encode a marker struct
  *
  * @param system_id ID of this system
  * @param component_id ID of this component (e.g. 200 for IMU)
@@ -139,6 +150,20 @@ static inline uint16_t mavlink_msg_marker_encode(uint8_t system_id, uint8_t comp
 }
 
 /**
+ * @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
  *
@@ -155,7 +180,7 @@ static inline uint16_t mavlink_msg_marker_encode(uint8_t system_id, uint8_t comp
 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[26];
+	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);
@@ -164,7 +189,11 @@ static inline void mavlink_msg_marker_send(mavlink_channel_t chan, uint16_t id,
 	_mav_put_float(buf, 20, yaw);
 	_mav_put_uint16_t(buf, 24, id);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, buf, 26, 249);
+#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;
@@ -175,7 +204,11 @@ static inline void mavlink_msg_marker_send(mavlink_channel_t chan, uint16_t id,
 	packet.yaw = yaw;
 	packet.id = id;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, (const char *)&packet, 26, 249);
+#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
 }
 
@@ -271,6 +304,6 @@ static inline void mavlink_msg_marker_decode(const mavlink_message_t* msg, mavli
 	marker->yaw = mavlink_msg_marker_get_yaw(msg);
 	marker->id = mavlink_msg_marker_get_id(msg);
 #else
-	memcpy(marker, _MAV_PAYLOAD(msg), 26);
+	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
index 907246b20..aa7b827a3 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h
@@ -13,6 +13,9 @@ typedef struct __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 { \
@@ -42,30 +45,34 @@ static inline uint16_t mavlink_msg_pattern_detected_pack(uint8_t system_id, uint
 						       uint8_t type, float confidence, const char *file, uint8_t detected)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[106];
+	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, 106);
+        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, 106);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED;
-	return mavlink_finalize_message(msg, system_id, component_id, 106, 90);
+#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 was sent over
+ * @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
@@ -78,27 +85,31 @@ static inline uint16_t mavlink_msg_pattern_detected_pack_chan(uint8_t system_id,
 						           uint8_t type,float confidence,const char *file,uint8_t detected)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[106];
+	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, 106);
+        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, 106);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 106, 90);
+#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 into a message
+ * @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)
@@ -111,6 +122,20 @@ static inline uint16_t mavlink_msg_pattern_detected_encode(uint8_t system_id, ui
 }
 
 /**
+ * @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
  *
@@ -124,19 +149,27 @@ static inline uint16_t mavlink_msg_pattern_detected_encode(uint8_t system_id, ui
 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[106];
+	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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, buf, 106, 90);
+#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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, (const char *)&packet, 106, 90);
+#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
 }
 
@@ -199,6 +232,6 @@ static inline void mavlink_msg_pattern_detected_decode(const mavlink_message_t*
 	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), 106);
+	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
index eec8d4069..913a52897 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h
@@ -17,6 +17,9 @@ typedef struct __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 { \
@@ -54,7 +57,7 @@ static inline uint16_t mavlink_msg_point_of_interest_pack(uint8_t system_id, uin
 						       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[43];
+	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);
@@ -63,7 +66,7 @@ static inline uint16_t mavlink_msg_point_of_interest_pack(uint8_t system_id, uin
 	_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, 43);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN);
 #else
 	mavlink_point_of_interest_t packet;
 	packet.x = x;
@@ -74,18 +77,22 @@ static inline uint16_t mavlink_msg_point_of_interest_pack(uint8_t system_id, uin
 	packet.color = color;
 	packet.coordinate_system = coordinate_system;
 	mav_array_memcpy(packet.name, name, sizeof(char)*26);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 43);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST;
-	return mavlink_finalize_message(msg, system_id, component_id, 43, 95);
+#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 was sent over
+ * @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
@@ -102,7 +109,7 @@ static inline uint16_t mavlink_msg_point_of_interest_pack_chan(uint8_t system_id
 						           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[43];
+	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);
@@ -111,7 +118,7 @@ static inline uint16_t mavlink_msg_point_of_interest_pack_chan(uint8_t system_id
 	_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, 43);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN);
 #else
 	mavlink_point_of_interest_t packet;
 	packet.x = x;
@@ -122,15 +129,19 @@ static inline uint16_t mavlink_msg_point_of_interest_pack_chan(uint8_t system_id
 	packet.color = color;
 	packet.coordinate_system = coordinate_system;
 	mav_array_memcpy(packet.name, name, sizeof(char)*26);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 43);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 43, 95);
+#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 into a message
+ * @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)
@@ -143,6 +154,20 @@ static inline uint16_t mavlink_msg_point_of_interest_encode(uint8_t system_id, u
 }
 
 /**
+ * @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
  *
@@ -160,7 +185,7 @@ static inline uint16_t mavlink_msg_point_of_interest_encode(uint8_t system_id, u
 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[43];
+	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);
@@ -169,7 +194,11 @@ static inline void mavlink_msg_point_of_interest_send(mavlink_channel_t chan, ui
 	_mav_put_uint8_t(buf, 15, color);
 	_mav_put_uint8_t(buf, 16, coordinate_system);
 	_mav_put_char_array(buf, 17, name, 26);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, buf, 43, 95);
+#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;
@@ -180,7 +209,11 @@ static inline void mavlink_msg_point_of_interest_send(mavlink_channel_t chan, ui
 	packet.color = color;
 	packet.coordinate_system = coordinate_system;
 	mav_array_memcpy(packet.name, name, sizeof(char)*26);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, (const char *)&packet, 43, 95);
+#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
 }
 
@@ -287,6 +320,6 @@ static inline void mavlink_msg_point_of_interest_decode(const mavlink_message_t*
 	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), 43);
+	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
index f83630093..e24436431 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h
@@ -20,6 +20,9 @@ typedef struct __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 { \
@@ -63,7 +66,7 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_pack(uint8_t sys
 						       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[55];
+	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);
@@ -75,7 +78,7 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_pack(uint8_t sys
 	_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, 55);
+        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;
@@ -89,18 +92,22 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_pack(uint8_t sys
 	packet.color = color;
 	packet.coordinate_system = coordinate_system;
 	mav_array_memcpy(packet.name, name, sizeof(char)*26);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 55);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 55, 36);
+#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 was sent over
+ * @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
@@ -120,7 +127,7 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_pack_chan(uint8_
 						           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[55];
+	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);
@@ -132,7 +139,7 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_pack_chan(uint8_
 	_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, 55);
+        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;
@@ -146,15 +153,19 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_pack_chan(uint8_
 	packet.color = color;
 	packet.coordinate_system = coordinate_system;
 	mav_array_memcpy(packet.name, name, sizeof(char)*26);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 55);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 55, 36);
+#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 into a message
+ * @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)
@@ -167,6 +178,20 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_encode(uint8_t s
 }
 
 /**
+ * @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
  *
@@ -187,7 +212,7 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_encode(uint8_t s
 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[55];
+	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);
@@ -199,7 +224,11 @@ static inline void mavlink_msg_point_of_interest_connection_send(mavlink_channel
 	_mav_put_uint8_t(buf, 27, color);
 	_mav_put_uint8_t(buf, 28, coordinate_system);
 	_mav_put_char_array(buf, 29, name, 26);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, buf, 55, 36);
+#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;
@@ -213,7 +242,11 @@ static inline void mavlink_msg_point_of_interest_connection_send(mavlink_channel
 	packet.color = color;
 	packet.coordinate_system = coordinate_system;
 	mav_array_memcpy(packet.name, name, sizeof(char)*26);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, (const char *)&packet, 55, 36);
+#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
 }
 
@@ -353,6 +386,6 @@ static inline void mavlink_msg_point_of_interest_connection_decode(const mavlink
 	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), 55);
+	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
index 495fb884c..6f4ca510a 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h
@@ -14,6 +14,9 @@ typedef struct __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 { \
@@ -45,14 +48,14 @@ static inline uint16_t mavlink_msg_position_control_setpoint_pack(uint8_t system
 						       uint16_t id, float x, float y, float z, float yaw)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[18];
+	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, 18);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN);
 #else
 	mavlink_position_control_setpoint_t packet;
 	packet.x = x;
@@ -61,18 +64,22 @@ static inline uint16_t mavlink_msg_position_control_setpoint_pack(uint8_t system
 	packet.yaw = yaw;
 	packet.id = id;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 18);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT;
-	return mavlink_finalize_message(msg, system_id, component_id, 18, 28);
+#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 was sent over
+ * @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
@@ -86,14 +93,14 @@ static inline uint16_t mavlink_msg_position_control_setpoint_pack_chan(uint8_t s
 						           uint16_t id,float x,float y,float z,float yaw)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[18];
+	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, 18);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN);
 #else
 	mavlink_position_control_setpoint_t packet;
 	packet.x = x;
@@ -102,15 +109,19 @@ static inline uint16_t mavlink_msg_position_control_setpoint_pack_chan(uint8_t s
 	packet.yaw = yaw;
 	packet.id = id;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 18);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 28);
+#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 into a message
+ * @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)
@@ -123,6 +134,20 @@ static inline uint16_t mavlink_msg_position_control_setpoint_encode(uint8_t syst
 }
 
 /**
+ * @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
  *
@@ -137,14 +162,18 @@ static inline uint16_t mavlink_msg_position_control_setpoint_encode(uint8_t syst
 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[18];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, buf, 18, 28);
+#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;
@@ -153,7 +182,11 @@ static inline void mavlink_msg_position_control_setpoint_send(mavlink_channel_t
 	packet.yaw = yaw;
 	packet.id = id;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, (const char *)&packet, 18, 28);
+#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
 }
 
@@ -227,6 +260,6 @@ static inline void mavlink_msg_position_control_setpoint_decode(const mavlink_me
 	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), 18);
+	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
index 507e0f2f9..08cedbb4b 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h
@@ -16,6 +16,9 @@ typedef struct __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 { \
@@ -51,7 +54,7 @@ static inline uint16_t mavlink_msg_raw_aux_pack(uint8_t system_id, uint8_t compo
 						       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[16];
+	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);
@@ -60,7 +63,7 @@ static inline uint16_t mavlink_msg_raw_aux_pack(uint8_t system_id, uint8_t compo
 	_mav_put_uint16_t(buf, 12, vbat);
 	_mav_put_int16_t(buf, 14, temp);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 16);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_AUX_LEN);
 #else
 	mavlink_raw_aux_t packet;
 	packet.baro = baro;
@@ -71,18 +74,22 @@ static inline uint16_t mavlink_msg_raw_aux_pack(uint8_t system_id, uint8_t compo
 	packet.vbat = vbat;
 	packet.temp = temp;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 16);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_AUX_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_RAW_AUX;
-	return mavlink_finalize_message(msg, system_id, component_id, 16, 182);
+#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 was sent over
+ * @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)
@@ -98,7 +105,7 @@ static inline uint16_t mavlink_msg_raw_aux_pack_chan(uint8_t system_id, uint8_t
 						           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[16];
+	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);
@@ -107,7 +114,7 @@ static inline uint16_t mavlink_msg_raw_aux_pack_chan(uint8_t system_id, uint8_t
 	_mav_put_uint16_t(buf, 12, vbat);
 	_mav_put_int16_t(buf, 14, temp);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 16);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_AUX_LEN);
 #else
 	mavlink_raw_aux_t packet;
 	packet.baro = baro;
@@ -118,15 +125,19 @@ static inline uint16_t mavlink_msg_raw_aux_pack_chan(uint8_t system_id, uint8_t
 	packet.vbat = vbat;
 	packet.temp = temp;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 16);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_AUX_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_RAW_AUX;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16, 182);
+#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 into a message
+ * @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)
@@ -139,6 +150,20 @@ static inline uint16_t mavlink_msg_raw_aux_encode(uint8_t system_id, uint8_t com
 }
 
 /**
+ * @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
  *
@@ -155,7 +180,7 @@ static inline uint16_t mavlink_msg_raw_aux_encode(uint8_t system_id, uint8_t com
 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[16];
+	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);
@@ -164,7 +189,11 @@ static inline void mavlink_msg_raw_aux_send(mavlink_channel_t chan, uint16_t adc
 	_mav_put_uint16_t(buf, 12, vbat);
 	_mav_put_int16_t(buf, 14, temp);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, buf, 16, 182);
+#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;
@@ -175,7 +204,11 @@ static inline void mavlink_msg_raw_aux_send(mavlink_channel_t chan, uint16_t adc
 	packet.vbat = vbat;
 	packet.temp = temp;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, (const char *)&packet, 16, 182);
+#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
 }
 
@@ -271,6 +304,6 @@ static inline void mavlink_msg_raw_aux_decode(const mavlink_message_t* msg, mavl
 	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), 16);
+	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
index 698625b7e..b26d748c2 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h
@@ -15,6 +15,9 @@ typedef struct __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 { \
@@ -48,7 +51,7 @@ static inline uint16_t mavlink_msg_set_cam_shutter_pack(uint8_t system_id, uint8
 						       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[11];
+	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);
@@ -56,7 +59,7 @@ static inline uint16_t mavlink_msg_set_cam_shutter_pack(uint8_t system_id, uint8
 	_mav_put_uint8_t(buf, 9, cam_mode);
 	_mav_put_uint8_t(buf, 10, trigger_pin);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 11);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN);
 #else
 	mavlink_set_cam_shutter_t packet;
 	packet.gain = gain;
@@ -66,18 +69,22 @@ static inline uint16_t mavlink_msg_set_cam_shutter_pack(uint8_t system_id, uint8
 	packet.cam_mode = cam_mode;
 	packet.trigger_pin = trigger_pin;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 11);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER;
-	return mavlink_finalize_message(msg, system_id, component_id, 11, 108);
+#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 was sent over
+ * @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
@@ -92,7 +99,7 @@ static inline uint16_t mavlink_msg_set_cam_shutter_pack_chan(uint8_t system_id,
 						           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[11];
+	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);
@@ -100,7 +107,7 @@ static inline uint16_t mavlink_msg_set_cam_shutter_pack_chan(uint8_t system_id,
 	_mav_put_uint8_t(buf, 9, cam_mode);
 	_mav_put_uint8_t(buf, 10, trigger_pin);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 11);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN);
 #else
 	mavlink_set_cam_shutter_t packet;
 	packet.gain = gain;
@@ -110,15 +117,19 @@ static inline uint16_t mavlink_msg_set_cam_shutter_pack_chan(uint8_t system_id,
 	packet.cam_mode = cam_mode;
 	packet.trigger_pin = trigger_pin;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 11);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 11, 108);
+#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 into a message
+ * @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)
@@ -131,6 +142,20 @@ static inline uint16_t mavlink_msg_set_cam_shutter_encode(uint8_t system_id, uin
 }
 
 /**
+ * @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
  *
@@ -146,7 +171,7 @@ static inline uint16_t mavlink_msg_set_cam_shutter_encode(uint8_t system_id, uin
 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[11];
+	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);
@@ -154,7 +179,11 @@ static inline void mavlink_msg_set_cam_shutter_send(mavlink_channel_t chan, uint
 	_mav_put_uint8_t(buf, 9, cam_mode);
 	_mav_put_uint8_t(buf, 10, trigger_pin);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, buf, 11, 108);
+#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;
@@ -164,7 +193,11 @@ static inline void mavlink_msg_set_cam_shutter_send(mavlink_channel_t chan, uint
 	packet.cam_mode = cam_mode;
 	packet.trigger_pin = trigger_pin;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, (const char *)&packet, 11, 108);
+#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
 }
 
@@ -249,6 +282,6 @@ static inline void mavlink_msg_set_cam_shutter_decode(const mavlink_message_t* m
 	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), 11);
+	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
index 27c08b7c9..f1f9e698f 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h
@@ -15,6 +15,9 @@ typedef struct __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 { \
@@ -48,7 +51,7 @@ static inline uint16_t mavlink_msg_set_position_control_offset_pack(uint8_t syst
 						       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[18];
+	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);
@@ -56,7 +59,7 @@ static inline uint16_t mavlink_msg_set_position_control_offset_pack(uint8_t syst
 	_mav_put_uint8_t(buf, 16, target_system);
 	_mav_put_uint8_t(buf, 17, target_component);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 18);
+        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;
@@ -66,18 +69,22 @@ static inline uint16_t mavlink_msg_set_position_control_offset_pack(uint8_t syst
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 18);
+        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;
-	return mavlink_finalize_message(msg, system_id, component_id, 18, 22);
+#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 was sent over
+ * @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
@@ -92,7 +99,7 @@ static inline uint16_t mavlink_msg_set_position_control_offset_pack_chan(uint8_t
 						           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[18];
+	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);
@@ -100,7 +107,7 @@ static inline uint16_t mavlink_msg_set_position_control_offset_pack_chan(uint8_t
 	_mav_put_uint8_t(buf, 16, target_system);
 	_mav_put_uint8_t(buf, 17, target_component);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 18);
+        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;
@@ -110,15 +117,19 @@ static inline uint16_t mavlink_msg_set_position_control_offset_pack_chan(uint8_t
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 18);
+        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;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 22);
+#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 into a message
+ * @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)
@@ -131,6 +142,20 @@ static inline uint16_t mavlink_msg_set_position_control_offset_encode(uint8_t sy
 }
 
 /**
+ * @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
  *
@@ -146,7 +171,7 @@ static inline uint16_t mavlink_msg_set_position_control_offset_encode(uint8_t sy
 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[18];
+	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);
@@ -154,7 +179,11 @@ static inline void mavlink_msg_set_position_control_offset_send(mavlink_channel_
 	_mav_put_uint8_t(buf, 16, target_system);
 	_mav_put_uint8_t(buf, 17, target_component);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET, buf, 18, 22);
+#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;
@@ -164,7 +193,11 @@ static inline void mavlink_msg_set_position_control_offset_send(mavlink_channel_
 	packet.target_system = target_system;
 	packet.target_component = target_component;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET, (const char *)&packet, 18, 22);
+#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
 }
 
@@ -249,6 +282,6 @@ static inline void mavlink_msg_set_position_control_offset_decode(const mavlink_
 	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), 18);
+	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
index 240f69e72..9458087da 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h
@@ -13,6 +13,9 @@ typedef struct __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 { \
@@ -42,13 +45,13 @@ static inline uint16_t mavlink_msg_watchdog_command_pack(uint8_t system_id, uint
 						       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[6];
+	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, 6);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN);
 #else
 	mavlink_watchdog_command_t packet;
 	packet.watchdog_id = watchdog_id;
@@ -56,18 +59,22 @@ static inline uint16_t mavlink_msg_watchdog_command_pack(uint8_t system_id, uint
 	packet.target_system_id = target_system_id;
 	packet.command_id = command_id;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 6);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND;
-	return mavlink_finalize_message(msg, system_id, component_id, 6, 162);
+#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 was sent over
+ * @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
@@ -80,13 +87,13 @@ static inline uint16_t mavlink_msg_watchdog_command_pack_chan(uint8_t system_id,
 						           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[6];
+	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, 6);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN);
 #else
 	mavlink_watchdog_command_t packet;
 	packet.watchdog_id = watchdog_id;
@@ -94,15 +101,19 @@ static inline uint16_t mavlink_msg_watchdog_command_pack_chan(uint8_t system_id,
 	packet.target_system_id = target_system_id;
 	packet.command_id = command_id;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 6);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 6, 162);
+#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 into a message
+ * @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)
@@ -115,6 +126,20 @@ static inline uint16_t mavlink_msg_watchdog_command_encode(uint8_t system_id, ui
 }
 
 /**
+ * @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
  *
@@ -128,13 +153,17 @@ static inline uint16_t mavlink_msg_watchdog_command_encode(uint8_t system_id, ui
 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[6];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, buf, 6, 162);
+#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;
@@ -142,7 +171,11 @@ static inline void mavlink_msg_watchdog_command_send(mavlink_channel_t chan, uin
 	packet.target_system_id = target_system_id;
 	packet.command_id = command_id;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, (const char *)&packet, 6, 162);
+#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
 }
 
@@ -205,6 +238,6 @@ static inline void mavlink_msg_watchdog_command_decode(const mavlink_message_t*
 	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), 6);
+	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
index f1fe5eb86..3f4295ee5 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h
@@ -11,6 +11,9 @@ typedef struct __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 { \
@@ -36,28 +39,32 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_pack(uint8_t system_id, ui
 						       uint16_t watchdog_id, uint16_t process_count)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[4];
+	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, 4);
+        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, 4);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT;
-	return mavlink_finalize_message(msg, system_id, component_id, 4, 153);
+#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 was sent over
+ * @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
@@ -68,25 +75,29 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_pack_chan(uint8_t system_i
 						           uint16_t watchdog_id,uint16_t process_count)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[4];
+	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, 4);
+        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, 4);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 153);
+#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 into a message
+ * @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)
@@ -99,6 +110,20 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_encode(uint8_t system_id,
 }
 
 /**
+ * @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
  *
@@ -110,17 +135,25 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_encode(uint8_t system_id,
 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[4];
+	char buf[MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN];
 	_mav_put_uint16_t(buf, 0, watchdog_id);
 	_mav_put_uint16_t(buf, 2, process_count);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, buf, 4, 153);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, (const char *)&packet, 4, 153);
+#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
 }
 
@@ -161,6 +194,6 @@ static inline void mavlink_msg_watchdog_heartbeat_decode(const mavlink_message_t
 	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), 4);
+	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
index 7ba3ddf03..55853cdde 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h
@@ -14,6 +14,9 @@ typedef struct __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
 
@@ -46,13 +49,13 @@ static inline uint16_t mavlink_msg_watchdog_process_info_pack(uint8_t system_id,
 						       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[255];
+	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, 255);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN);
 #else
 	mavlink_watchdog_process_info_t packet;
 	packet.timeout = timeout;
@@ -60,18 +63,22 @@ static inline uint16_t mavlink_msg_watchdog_process_info_pack(uint8_t system_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, 255);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO;
-	return mavlink_finalize_message(msg, system_id, component_id, 255, 16);
+#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 was sent over
+ * @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
@@ -85,13 +92,13 @@ static inline uint16_t mavlink_msg_watchdog_process_info_pack_chan(uint8_t syste
 						           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[255];
+	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, 255);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN);
 #else
 	mavlink_watchdog_process_info_t packet;
 	packet.timeout = timeout;
@@ -99,15 +106,19 @@ static inline uint16_t mavlink_msg_watchdog_process_info_pack_chan(uint8_t syste
 	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, 255);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 255, 16);
+#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 into a message
+ * @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)
@@ -120,6 +131,20 @@ static inline uint16_t mavlink_msg_watchdog_process_info_encode(uint8_t system_i
 }
 
 /**
+ * @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
  *
@@ -134,13 +159,17 @@ static inline uint16_t mavlink_msg_watchdog_process_info_encode(uint8_t system_i
 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[255];
+	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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, buf, 255, 16);
+#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;
@@ -148,7 +177,11 @@ static inline void mavlink_msg_watchdog_process_info_send(mavlink_channel_t chan
 	packet.process_id = process_id;
 	mav_array_memcpy(packet.name, name, sizeof(char)*100);
 	mav_array_memcpy(packet.arguments, arguments, sizeof(char)*147);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, (const char *)&packet, 255, 16);
+#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
 }
 
@@ -222,6 +255,6 @@ static inline void mavlink_msg_watchdog_process_info_decode(const mavlink_messag
 	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), 255);
+	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
index 5795c6328..a0410d803 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h
@@ -15,6 +15,9 @@ typedef struct __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 { \
@@ -48,7 +51,7 @@ static inline uint16_t mavlink_msg_watchdog_process_status_pack(uint8_t system_i
 						       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[12];
+	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);
@@ -56,7 +59,7 @@ static inline uint16_t mavlink_msg_watchdog_process_status_pack(uint8_t system_i
 	_mav_put_uint8_t(buf, 10, state);
 	_mav_put_uint8_t(buf, 11, muted);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN);
 #else
 	mavlink_watchdog_process_status_t packet;
 	packet.pid = pid;
@@ -66,18 +69,22 @@ static inline uint16_t mavlink_msg_watchdog_process_status_pack(uint8_t system_i
 	packet.state = state;
 	packet.muted = muted;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS;
-	return mavlink_finalize_message(msg, system_id, component_id, 12, 29);
+#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 was sent over
+ * @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
@@ -92,7 +99,7 @@ static inline uint16_t mavlink_msg_watchdog_process_status_pack_chan(uint8_t sys
 						           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[12];
+	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);
@@ -100,7 +107,7 @@ static inline uint16_t mavlink_msg_watchdog_process_status_pack_chan(uint8_t sys
 	_mav_put_uint8_t(buf, 10, state);
 	_mav_put_uint8_t(buf, 11, muted);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN);
 #else
 	mavlink_watchdog_process_status_t packet;
 	packet.pid = pid;
@@ -110,15 +117,19 @@ static inline uint16_t mavlink_msg_watchdog_process_status_pack_chan(uint8_t sys
 	packet.state = state;
 	packet.muted = muted;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 29);
+#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 into a message
+ * @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)
@@ -131,6 +142,20 @@ static inline uint16_t mavlink_msg_watchdog_process_status_encode(uint8_t system
 }
 
 /**
+ * @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
  *
@@ -146,7 +171,7 @@ static inline uint16_t mavlink_msg_watchdog_process_status_encode(uint8_t system
 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[12];
+	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);
@@ -154,7 +179,11 @@ static inline void mavlink_msg_watchdog_process_status_send(mavlink_channel_t ch
 	_mav_put_uint8_t(buf, 10, state);
 	_mav_put_uint8_t(buf, 11, muted);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, buf, 12, 29);
+#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;
@@ -164,7 +193,11 @@ static inline void mavlink_msg_watchdog_process_status_send(mavlink_channel_t ch
 	packet.state = state;
 	packet.muted = muted;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, (const char *)&packet, 12, 29);
+#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
 }
 
@@ -249,6 +282,6 @@ static inline void mavlink_msg_watchdog_process_status_decode(const mavlink_mess
 	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), 12);
+	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
index 574b92d7a..428619eed 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h
@@ -12,15 +12,15 @@ extern "C" {
 // MESSAGE LENGTHS AND CRCS
 
 #ifndef MAVLINK_MESSAGE_LENGTHS
-#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 0, 0, 0, 254, 249, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 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, 12, 255, 53, 0, 0, 0, 0, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0}
+#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 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, 0, 0, 0, 235, 93, 124, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 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, 23, 223, 88, 0, 0, 0, 0, 254, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}
+#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 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, {"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_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"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, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_BRIEF_FEATURE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ATTITUDE_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}}
+#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"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"
@@ -72,6 +72,7 @@ enum MAV_CMD
 	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|  */
@@ -80,7 +81,8 @@ enum MAV_CMD
 	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_ENUM_END=401, /*  | */
+	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|  */
@@ -116,8 +118,6 @@ enum MAV_CMD
 #include "./mavlink_msg_pattern_detected.h"
 #include "./mavlink_msg_point_of_interest.h"
 #include "./mavlink_msg_point_of_interest_connection.h"
-#include "./mavlink_msg_data_transmission_handshake.h"
-#include "./mavlink_msg_encapsulated_data.h"
 #include "./mavlink_msg_brief_feature.h"
 #include "./mavlink_msg_attitude_control.h"
 
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/testsuite.h b/mavlink/include/mavlink/v1.0/pixhawk/testsuite.h
index 54a7ae5ba..caa4d3dcb 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/testsuite.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/testsuite.h
@@ -31,11 +31,11 @@ static void mavlink_test_set_cam_shutter(uint8_t system_id, uint8_t component_id
         uint16_t i;
 	mavlink_set_cam_shutter_t packet_in = {
 		17.0,
-	17443,
-	17547,
-	29,
-	96,
-	163,
+	}17443,
+	}17547,
+	}29,
+	}96,
+	}163,
 	};
 	mavlink_set_cam_shutter_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -84,17 +84,17 @@ static void mavlink_test_image_triggered(uint8_t system_id, uint8_t component_id
         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,
+	}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));
@@ -192,28 +192,28 @@ static void mavlink_test_image_available(uint8_t system_id, uint8_t component_id
         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,
+	}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));
@@ -279,11 +279,11 @@ static void mavlink_test_set_position_control_offset(uint8_t system_id, uint8_t
         uint16_t i;
 	mavlink_set_position_control_offset_t packet_in = {
 		17.0,
-	45.0,
-	73.0,
-	101.0,
-	53,
-	120,
+	}45.0,
+	}73.0,
+	}101.0,
+	}53,
+	}120,
 	};
 	mavlink_set_position_control_offset_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -332,10 +332,10 @@ static void mavlink_test_position_control_setpoint(uint8_t system_id, uint8_t co
         uint16_t i;
 	mavlink_position_control_setpoint_t packet_in = {
 		17.0,
-	45.0,
-	73.0,
-	101.0,
-	18067,
+	}45.0,
+	}73.0,
+	}101.0,
+	}18067,
 	};
 	mavlink_position_control_setpoint_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -383,12 +383,12 @@ static void mavlink_test_marker(uint8_t system_id, uint8_t component_id, mavlink
         uint16_t i;
 	mavlink_marker_t packet_in = {
 		17.0,
-	45.0,
-	73.0,
-	101.0,
-	129.0,
-	157.0,
-	18483,
+	}45.0,
+	}73.0,
+	}101.0,
+	}129.0,
+	}157.0,
+	}18483,
 	};
 	mavlink_marker_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -438,12 +438,12 @@ static void mavlink_test_raw_aux(uint8_t system_id, uint8_t component_id, mavlin
         uint16_t i;
 	mavlink_raw_aux_t packet_in = {
 		963497464,
-	17443,
-	17547,
-	17651,
-	17755,
-	17859,
-	17963,
+	}17443,
+	}17547,
+	}17651,
+	}17755,
+	}17859,
+	}17963,
 	};
 	mavlink_raw_aux_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -493,7 +493,7 @@ static void mavlink_test_watchdog_heartbeat(uint8_t system_id, uint8_t component
         uint16_t i;
 	mavlink_watchdog_heartbeat_t packet_in = {
 		17235,
-	17339,
+	}17339,
 	};
 	mavlink_watchdog_heartbeat_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -538,10 +538,10 @@ static void mavlink_test_watchdog_process_info(uint8_t system_id, uint8_t compon
         uint16_t i;
 	mavlink_watchdog_process_info_t packet_in = {
 		963497464,
-	17443,
-	17547,
-	"IJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABC",
-	"EFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRST",
+	}17443,
+	}17547,
+	}"IJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABC",
+	}"EFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRST",
 	};
 	mavlink_watchdog_process_info_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -589,11 +589,11 @@ static void mavlink_test_watchdog_process_status(uint8_t system_id, uint8_t comp
         uint16_t i;
 	mavlink_watchdog_process_status_t packet_in = {
 		963497464,
-	17443,
-	17547,
-	17651,
-	163,
-	230,
+	}17443,
+	}17547,
+	}17651,
+	}163,
+	}230,
 	};
 	mavlink_watchdog_process_status_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -642,9 +642,9 @@ static void mavlink_test_watchdog_command(uint8_t system_id, uint8_t component_i
         uint16_t i;
 	mavlink_watchdog_command_t packet_in = {
 		17235,
-	17339,
-	17,
-	84,
+	}17339,
+	}17,
+	}84,
 	};
 	mavlink_watchdog_command_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -691,9 +691,9 @@ static void mavlink_test_pattern_detected(uint8_t system_id, uint8_t component_i
         uint16_t i;
 	mavlink_pattern_detected_t packet_in = {
 		17.0,
-	17,
-	"FGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZ",
-	128,
+	}17,
+	}"FGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZ",
+	}128,
 	};
 	mavlink_pattern_detected_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -740,13 +740,13 @@ static void mavlink_test_point_of_interest(uint8_t system_id, uint8_t component_
         uint16_t i;
 	mavlink_point_of_interest_t packet_in = {
 		17.0,
-	45.0,
-	73.0,
-	17859,
-	175,
-	242,
-	53,
-	"RSTUVWXYZABCDEFGHIJKLMNOP",
+	}45.0,
+	}73.0,
+	}17859,
+	}175,
+	}242,
+	}53,
+	}"RSTUVWXYZABCDEFGHIJKLMNOP",
 	};
 	mavlink_point_of_interest_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -797,16 +797,16 @@ static void mavlink_test_point_of_interest_connection(uint8_t system_id, uint8_t
         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",
+	}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));
@@ -853,106 +853,6 @@ static void mavlink_test_point_of_interest_connection(uint8_t system_id, uint8_t
         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,
-	29,
-	96,
-	163,
-	230,
-	};
-	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.type = packet_in.type;
-        	packet1.packets = packet_in.packets;
-        	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_brief_feature(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
 {
 	mavlink_message_t msg;
@@ -960,13 +860,13 @@ static void mavlink_test_brief_feature(uint8_t system_id, uint8_t component_id,
         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 },
+	}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));
@@ -1017,14 +917,14 @@ static void mavlink_test_attitude_control(uint8_t system_id, uint8_t component_i
         uint16_t i;
 	mavlink_attitude_control_t packet_in = {
 		17.0,
-	45.0,
-	73.0,
-	101.0,
-	53,
-	120,
-	187,
-	254,
-	65,
+	}45.0,
+	}73.0,
+	}101.0,
+	}53,
+	}120,
+	}187,
+	}254,
+	}65,
 	};
 	mavlink_attitude_control_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -1086,8 +986,6 @@ static void mavlink_test_pixhawk(uint8_t system_id, uint8_t component_id, mavlin
 	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_data_transmission_handshake(system_id, component_id, last_msg);
-	mavlink_test_encapsulated_data(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);
 }
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/version.h b/mavlink/include/mavlink/v1.0/pixhawk/version.h
index 2ece89409..aa79f6ffb 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/version.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/version.h
@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Wed Jan  9 16:18:21 2013"
+#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
  
diff --git a/mavlink/include/mavlink/v1.0/protocol.h b/mavlink/include/mavlink/v1.0/protocol.h
index 7eb967e2c..fd8a07e07 100644
--- a/mavlink/include/mavlink/v1.0/protocol.h
+++ b/mavlink/include/mavlink/v1.0/protocol.h
@@ -60,7 +60,9 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, ui
 						      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);
@@ -160,7 +162,7 @@ static inline void byte_copy_8(char *dst, const char *src)
 /*
   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)
+static inline void mav_array_memcpy(void *dest, const void *src, size_t n)
 {
 	if (src == NULL) {
 		memset(dest, 0, n);
diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h
index 74b55af30..73e9d75db 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h
@@ -15,6 +15,9 @@ typedef struct __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 { \
@@ -44,28 +47,32 @@ static inline uint16_t mavlink_msg_cmd_airspeed_ack_pack(uint8_t system_id, uint
 						       float spCmd, uint8_t ack)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[5];
+	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, 5);
+        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, 5);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_CMD_AIRSPEED_ACK;
-	return mavlink_finalize_message(msg, system_id, component_id, 5, 243);
+#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 was sent over
+ * @param chan The MAVLink channel this message will be sent over
  * @param msg The MAVLink message to compress the data into
  * @param spCmd 
                 
@@ -80,25 +87,29 @@ static inline uint16_t mavlink_msg_cmd_airspeed_ack_pack_chan(uint8_t system_id,
 						           float spCmd,uint8_t ack)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[5];
+	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, 5);
+        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, 5);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_CMD_AIRSPEED_ACK;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 5, 243);
+#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 into a message
+ * @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)
@@ -111,6 +122,20 @@ static inline uint16_t mavlink_msg_cmd_airspeed_ack_encode(uint8_t system_id, ui
 }
 
 /**
+ * @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
  *
@@ -126,17 +151,25 @@ static inline uint16_t mavlink_msg_cmd_airspeed_ack_encode(uint8_t system_id, ui
 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[5];
+	char buf[MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN];
 	_mav_put_float(buf, 0, spCmd);
 	_mav_put_uint8_t(buf, 4, ack);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, buf, 5, 243);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, (const char *)&packet, 5, 243);
+#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
 }
 
@@ -181,6 +214,6 @@ static inline void mavlink_msg_cmd_airspeed_ack_decode(const mavlink_message_t*
 	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), 5);
+	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
index 2ac1b4eab..b6b567f99 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h
@@ -15,6 +15,9 @@ typedef struct __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 { \
@@ -44,28 +47,32 @@ static inline uint16_t mavlink_msg_cmd_airspeed_chng_pack(uint8_t system_id, uin
 						       uint8_t target, float spCmd)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[5];
+	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, 5);
+        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, 5);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG;
-	return mavlink_finalize_message(msg, system_id, component_id, 5, 209);
+#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 was sent over
+ * @param chan The MAVLink channel this message will be sent over
  * @param msg The MAVLink message to compress the data into
  * @param target 
                 
@@ -80,25 +87,29 @@ static inline uint16_t mavlink_msg_cmd_airspeed_chng_pack_chan(uint8_t system_id
 						           uint8_t target,float spCmd)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[5];
+	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, 5);
+        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, 5);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 5, 209);
+#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 into a message
+ * @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)
@@ -111,6 +122,20 @@ static inline uint16_t mavlink_msg_cmd_airspeed_chng_encode(uint8_t system_id, u
 }
 
 /**
+ * @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
  *
@@ -126,17 +151,25 @@ static inline uint16_t mavlink_msg_cmd_airspeed_chng_encode(uint8_t system_id, u
 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[5];
+	char buf[MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN];
 	_mav_put_float(buf, 0, spCmd);
 	_mav_put_uint8_t(buf, 4, target);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, buf, 5, 209);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, (const char *)&packet, 5, 209);
+#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
 }
 
@@ -181,6 +214,6 @@ static inline void mavlink_msg_cmd_airspeed_chng_decode(const mavlink_message_t*
 	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), 5);
+	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
index 32f46aadc..642f7aab9 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h
@@ -12,6 +12,9 @@ typedef struct __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 { \
@@ -37,26 +40,30 @@ static inline uint16_t mavlink_msg_filt_rot_vel_pack(uint8_t system_id, uint8_t
 						       const float *rotVel)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[12];
+	char buf[MAVLINK_MSG_ID_FILT_ROT_VEL_LEN];
 
 	_mav_put_float_array(buf, 0, rotVel, 3);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12);
+        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, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_FILT_ROT_VEL;
-	return mavlink_finalize_message(msg, system_id, component_id, 12, 79);
+#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 was sent over
+ * @param chan The MAVLink channel this message will be sent over
  * @param msg The MAVLink message to compress the data into
  * @param rotVel 
                 
@@ -68,23 +75,27 @@ static inline uint16_t mavlink_msg_filt_rot_vel_pack_chan(uint8_t system_id, uin
 						           const float *rotVel)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[12];
+	char buf[MAVLINK_MSG_ID_FILT_ROT_VEL_LEN];
 
 	_mav_put_float_array(buf, 0, rotVel, 3);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12);
+        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, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_FILT_ROT_VEL;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 79);
+#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 into a message
+ * @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)
@@ -97,6 +108,20 @@ static inline uint16_t mavlink_msg_filt_rot_vel_encode(uint8_t system_id, uint8_
 }
 
 /**
+ * @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
  *
@@ -109,15 +134,23 @@ static inline uint16_t mavlink_msg_filt_rot_vel_encode(uint8_t system_id, uint8_
 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[12];
+	char buf[MAVLINK_MSG_ID_FILT_ROT_VEL_LEN];
 
 	_mav_put_float_array(buf, 0, rotVel, 3);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, buf, 12, 79);
+#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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, (const char *)&packet, 12, 79);
+#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
 }
 
@@ -149,6 +182,6 @@ static inline void mavlink_msg_filt_rot_vel_decode(const mavlink_message_t* msg,
 #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), 12);
+	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
index 41a66adef..0f8369881 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h
@@ -15,6 +15,9 @@ typedef struct __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
 
@@ -45,28 +48,32 @@ static inline uint16_t mavlink_msg_llc_out_pack(uint8_t system_id, uint8_t compo
 						       const int16_t *servoOut, const int16_t *MotorOut)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[12];
+	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, 12);
+        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, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LLC_OUT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_LLC_OUT;
-	return mavlink_finalize_message(msg, system_id, component_id, 12, 5);
+#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 was sent over
+ * @param chan The MAVLink channel this message will be sent over
  * @param msg The MAVLink message to compress the data into
  * @param servoOut 
                 
@@ -81,25 +88,29 @@ static inline uint16_t mavlink_msg_llc_out_pack_chan(uint8_t system_id, uint8_t
 						           const int16_t *servoOut,const int16_t *MotorOut)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[12];
+	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, 12);
+        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, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LLC_OUT_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_LLC_OUT;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 5);
+#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 into a message
+ * @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)
@@ -112,6 +123,20 @@ static inline uint16_t mavlink_msg_llc_out_encode(uint8_t system_id, uint8_t com
 }
 
 /**
+ * @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
  *
@@ -127,17 +152,25 @@ static inline uint16_t mavlink_msg_llc_out_encode(uint8_t system_id, uint8_t com
 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[12];
+	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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, buf, 12, 5);
+#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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, (const char *)&packet, 12, 5);
+#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
 }
 
@@ -182,6 +215,6 @@ static inline void mavlink_msg_llc_out_decode(const mavlink_message_t* msg, mavl
 	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), 12);
+	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
index 3e08b5842..5d9382326 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h
@@ -12,6 +12,9 @@ typedef struct __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 { \
@@ -37,26 +40,30 @@ static inline uint16_t mavlink_msg_obs_air_temp_pack(uint8_t system_id, uint8_t
 						       float airT)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[4];
+	char buf[MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN];
 	_mav_put_float(buf, 0, airT);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 4);
+        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, 4);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_OBS_AIR_TEMP;
-	return mavlink_finalize_message(msg, system_id, component_id, 4, 248);
+#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 was sent over
+ * @param chan The MAVLink channel this message will be sent over
  * @param msg The MAVLink message to compress the data into
  * @param airT 
                 
@@ -68,23 +75,27 @@ static inline uint16_t mavlink_msg_obs_air_temp_pack_chan(uint8_t system_id, uin
 						           float airT)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[4];
+	char buf[MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN];
 	_mav_put_float(buf, 0, airT);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 4);
+        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, 4);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_OBS_AIR_TEMP;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 248);
+#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 into a message
+ * @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)
@@ -97,6 +108,20 @@ static inline uint16_t mavlink_msg_obs_air_temp_encode(uint8_t system_id, uint8_
 }
 
 /**
+ * @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
  *
@@ -109,15 +134,23 @@ static inline uint16_t mavlink_msg_obs_air_temp_encode(uint8_t system_id, uint8_
 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[4];
+	char buf[MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN];
 	_mav_put_float(buf, 0, airT);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, buf, 4, 248);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, (const char *)&packet, 4, 248);
+#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
 }
 
@@ -149,6 +182,6 @@ static inline void mavlink_msg_obs_air_temp_decode(const mavlink_message_t* msg,
 #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), 4);
+	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
index 2f7c6f9e9..35d813ca1 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h
@@ -18,6 +18,9 @@ typedef struct __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 { \
@@ -51,30 +54,34 @@ static inline uint16_t mavlink_msg_obs_air_velocity_pack(uint8_t system_id, uint
 						       float magnitude, float aoa, float slip)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[12];
+	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, 12);
+        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, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_OBS_AIR_VELOCITY;
-	return mavlink_finalize_message(msg, system_id, component_id, 12, 32);
+#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 was sent over
+ * @param chan The MAVLink channel this message will be sent over
  * @param msg The MAVLink message to compress the data into
  * @param magnitude 
                 
@@ -92,27 +99,31 @@ static inline uint16_t mavlink_msg_obs_air_velocity_pack_chan(uint8_t system_id,
 						           float magnitude,float aoa,float slip)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[12];
+	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, 12);
+        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, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_OBS_AIR_VELOCITY;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 32);
+#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 into a message
+ * @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)
@@ -125,6 +136,20 @@ static inline uint16_t mavlink_msg_obs_air_velocity_encode(uint8_t system_id, ui
 }
 
 /**
+ * @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
  *
@@ -143,19 +168,27 @@ static inline uint16_t mavlink_msg_obs_air_velocity_encode(uint8_t system_id, ui
 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[12];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, buf, 12, 32);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, (const char *)&packet, 12, 32);
+#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
 }
 
@@ -213,6 +246,6 @@ static inline void mavlink_msg_obs_air_velocity_decode(const mavlink_message_t*
 	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), 12);
+	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
index 3247392f6..9c80cd66e 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h
@@ -12,6 +12,9 @@ typedef struct __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 { \
@@ -37,26 +40,30 @@ static inline uint16_t mavlink_msg_obs_attitude_pack(uint8_t system_id, uint8_t
 						       const double *quat)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[32];
+	char buf[MAVLINK_MSG_ID_OBS_ATTITUDE_LEN];
 
 	_mav_put_double_array(buf, 0, quat, 4);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 32);
+        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, 32);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_OBS_ATTITUDE;
-	return mavlink_finalize_message(msg, system_id, component_id, 32, 146);
+#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 was sent over
+ * @param chan The MAVLink channel this message will be sent over
  * @param msg The MAVLink message to compress the data into
  * @param quat 
                 
@@ -68,23 +75,27 @@ static inline uint16_t mavlink_msg_obs_attitude_pack_chan(uint8_t system_id, uin
 						           const double *quat)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[32];
+	char buf[MAVLINK_MSG_ID_OBS_ATTITUDE_LEN];
 
 	_mav_put_double_array(buf, 0, quat, 4);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 32);
+        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, 32);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_OBS_ATTITUDE;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 146);
+#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 into a message
+ * @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)
@@ -97,6 +108,20 @@ static inline uint16_t mavlink_msg_obs_attitude_encode(uint8_t system_id, uint8_
 }
 
 /**
+ * @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
  *
@@ -109,15 +134,23 @@ static inline uint16_t mavlink_msg_obs_attitude_encode(uint8_t system_id, uint8_
 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[32];
+	char buf[MAVLINK_MSG_ID_OBS_ATTITUDE_LEN];
 
 	_mav_put_double_array(buf, 0, quat, 4);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, buf, 32, 146);
+#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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, (const char *)&packet, 32, 146);
+#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
 }
 
@@ -149,6 +182,6 @@ static inline void mavlink_msg_obs_attitude_decode(const mavlink_message_t* msg,
 #if MAVLINK_NEED_BYTE_SWAP
 	mavlink_msg_obs_attitude_get_quat(msg, obs_attitude->quat);
 #else
-	memcpy(obs_attitude, _MAV_PAYLOAD(msg), 32);
+	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
index 2a0d49f34..24dd43b57 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h
@@ -15,6 +15,9 @@ typedef struct __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
 
@@ -45,28 +48,32 @@ static inline uint16_t mavlink_msg_obs_bias_pack(uint8_t system_id, uint8_t comp
 						       const float *accBias, const float *gyroBias)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[24];
+	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, 24);
+        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, 24);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_BIAS_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_OBS_BIAS;
-	return mavlink_finalize_message(msg, system_id, component_id, 24, 159);
+#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 was sent over
+ * @param chan The MAVLink channel this message will be sent over
  * @param msg The MAVLink message to compress the data into
  * @param accBias 
                 
@@ -81,25 +88,29 @@ static inline uint16_t mavlink_msg_obs_bias_pack_chan(uint8_t system_id, uint8_t
 						           const float *accBias,const float *gyroBias)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[24];
+	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, 24);
+        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, 24);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_BIAS_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_OBS_BIAS;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 24, 159);
+#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 into a message
+ * @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)
@@ -112,6 +123,20 @@ static inline uint16_t mavlink_msg_obs_bias_encode(uint8_t system_id, uint8_t co
 }
 
 /**
+ * @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
  *
@@ -127,17 +152,25 @@ static inline uint16_t mavlink_msg_obs_bias_encode(uint8_t system_id, uint8_t co
 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[24];
+	char buf[MAVLINK_MSG_ID_OBS_BIAS_LEN];
 
 	_mav_put_float_array(buf, 0, accBias, 3);
 	_mav_put_float_array(buf, 12, gyroBias, 3);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, buf, 24, 159);
+#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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, (const char *)&packet, 24, 159);
+#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
 }
 
@@ -182,6 +215,6 @@ static inline void mavlink_msg_obs_bias_decode(const mavlink_message_t* msg, mav
 	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), 24);
+	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
index 0d89bcdb7..cfc2fe7e1 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h
@@ -18,6 +18,9 @@ typedef struct __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 { \
@@ -51,30 +54,34 @@ static inline uint16_t mavlink_msg_obs_position_pack(uint8_t system_id, uint8_t
 						       int32_t lon, int32_t lat, int32_t alt)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[12];
+	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, 12);
+        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, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_POSITION_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_OBS_POSITION;
-	return mavlink_finalize_message(msg, system_id, component_id, 12, 15);
+#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 was sent over
+ * @param chan The MAVLink channel this message will be sent over
  * @param msg The MAVLink message to compress the data into
  * @param lon 
                 
@@ -92,27 +99,31 @@ static inline uint16_t mavlink_msg_obs_position_pack_chan(uint8_t system_id, uin
 						           int32_t lon,int32_t lat,int32_t alt)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[12];
+	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, 12);
+        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, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_POSITION_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_OBS_POSITION;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 15);
+#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 into a message
+ * @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)
@@ -125,6 +136,20 @@ static inline uint16_t mavlink_msg_obs_position_encode(uint8_t system_id, uint8_
 }
 
 /**
+ * @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
  *
@@ -143,19 +168,27 @@ static inline uint16_t mavlink_msg_obs_position_encode(uint8_t system_id, uint8_
 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[12];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, buf, 12, 15);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, (const char *)&packet, 12, 15);
+#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
 }
 
@@ -213,6 +246,6 @@ static inline void mavlink_msg_obs_position_decode(const mavlink_message_t* 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), 12);
+	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
index 1f32eff40..24e272bf7 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h
@@ -12,6 +12,9 @@ typedef struct __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 { \
@@ -37,26 +40,30 @@ static inline uint16_t mavlink_msg_obs_qff_pack(uint8_t system_id, uint8_t compo
 						       float qff)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[4];
+	char buf[MAVLINK_MSG_ID_OBS_QFF_LEN];
 	_mav_put_float(buf, 0, qff);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 4);
+        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, 4);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_QFF_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_OBS_QFF;
-	return mavlink_finalize_message(msg, system_id, component_id, 4, 24);
+#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 was sent over
+ * @param chan The MAVLink channel this message will be sent over
  * @param msg The MAVLink message to compress the data into
  * @param qff 
                 
@@ -68,23 +75,27 @@ static inline uint16_t mavlink_msg_obs_qff_pack_chan(uint8_t system_id, uint8_t
 						           float qff)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[4];
+	char buf[MAVLINK_MSG_ID_OBS_QFF_LEN];
 	_mav_put_float(buf, 0, qff);
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 4);
+        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, 4);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_QFF_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_OBS_QFF;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 24);
+#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 into a message
+ * @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)
@@ -97,6 +108,20 @@ static inline uint16_t mavlink_msg_obs_qff_encode(uint8_t system_id, uint8_t com
 }
 
 /**
+ * @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
  *
@@ -109,15 +134,23 @@ static inline uint16_t mavlink_msg_obs_qff_encode(uint8_t system_id, uint8_t com
 static inline void mavlink_msg_obs_qff_send(mavlink_channel_t chan, float qff)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[4];
+	char buf[MAVLINK_MSG_ID_OBS_QFF_LEN];
 	_mav_put_float(buf, 0, qff);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, buf, 4, 24);
+#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;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, (const char *)&packet, 4, 24);
+#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
 }
 
@@ -149,6 +182,6 @@ static inline void mavlink_msg_obs_qff_decode(const mavlink_message_t* msg, mavl
 #if MAVLINK_NEED_BYTE_SWAP
 	obs_qff->qff = mavlink_msg_obs_qff_get_qff(msg);
 #else
-	memcpy(obs_qff, _MAV_PAYLOAD(msg), 4);
+	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
index d0cc15e8b..6e3776632 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h
@@ -12,6 +12,9 @@ typedef struct __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 { \
@@ -37,26 +40,30 @@ static inline uint16_t mavlink_msg_obs_velocity_pack(uint8_t system_id, uint8_t
 						       const float *vel)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[12];
+	char buf[MAVLINK_MSG_ID_OBS_VELOCITY_LEN];
 
 	_mav_put_float_array(buf, 0, vel, 3);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12);
+        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, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_VELOCITY_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_OBS_VELOCITY;
-	return mavlink_finalize_message(msg, system_id, component_id, 12, 108);
+#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 was sent over
+ * @param chan The MAVLink channel this message will be sent over
  * @param msg The MAVLink message to compress the data into
  * @param vel 
                 
@@ -68,23 +75,27 @@ static inline uint16_t mavlink_msg_obs_velocity_pack_chan(uint8_t system_id, uin
 						           const float *vel)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[12];
+	char buf[MAVLINK_MSG_ID_OBS_VELOCITY_LEN];
 
 	_mav_put_float_array(buf, 0, vel, 3);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12);
+        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, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_VELOCITY_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_OBS_VELOCITY;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 108);
+#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 into a message
+ * @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)
@@ -97,6 +108,20 @@ static inline uint16_t mavlink_msg_obs_velocity_encode(uint8_t system_id, uint8_
 }
 
 /**
+ * @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
  *
@@ -109,15 +134,23 @@ static inline uint16_t mavlink_msg_obs_velocity_encode(uint8_t system_id, uint8_
 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[12];
+	char buf[MAVLINK_MSG_ID_OBS_VELOCITY_LEN];
 
 	_mav_put_float_array(buf, 0, vel, 3);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, buf, 12, 108);
+#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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, (const char *)&packet, 12, 108);
+#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
 }
 
@@ -149,6 +182,6 @@ static inline void mavlink_msg_obs_velocity_decode(const mavlink_message_t* msg,
 #if MAVLINK_NEED_BYTE_SWAP
 	mavlink_msg_obs_velocity_get_vel(msg, obs_velocity->vel);
 #else
-	memcpy(obs_velocity, _MAV_PAYLOAD(msg), 12);
+	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
index d08d41ae9..55f7cb2ae 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h
@@ -12,6 +12,9 @@ typedef struct __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 { \
@@ -37,26 +40,30 @@ static inline uint16_t mavlink_msg_obs_wind_pack(uint8_t system_id, uint8_t comp
 						       const float *wind)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[12];
+	char buf[MAVLINK_MSG_ID_OBS_WIND_LEN];
 
 	_mav_put_float_array(buf, 0, wind, 3);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12);
+        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, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_WIND_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_OBS_WIND;
-	return mavlink_finalize_message(msg, system_id, component_id, 12, 16);
+#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 was sent over
+ * @param chan The MAVLink channel this message will be sent over
  * @param msg The MAVLink message to compress the data into
  * @param wind 
                 
@@ -68,23 +75,27 @@ static inline uint16_t mavlink_msg_obs_wind_pack_chan(uint8_t system_id, uint8_t
 						           const float *wind)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[12];
+	char buf[MAVLINK_MSG_ID_OBS_WIND_LEN];
 
 	_mav_put_float_array(buf, 0, wind, 3);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12);
+        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, 12);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_WIND_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_OBS_WIND;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 16);
+#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 into a message
+ * @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)
@@ -97,6 +108,20 @@ static inline uint16_t mavlink_msg_obs_wind_encode(uint8_t system_id, uint8_t co
 }
 
 /**
+ * @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
  *
@@ -109,15 +134,23 @@ static inline uint16_t mavlink_msg_obs_wind_encode(uint8_t system_id, uint8_t co
 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[12];
+	char buf[MAVLINK_MSG_ID_OBS_WIND_LEN];
 
 	_mav_put_float_array(buf, 0, wind, 3);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, buf, 12, 16);
+#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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, (const char *)&packet, 12, 16);
+#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
 }
 
@@ -149,6 +182,6 @@ static inline void mavlink_msg_obs_wind_decode(const mavlink_message_t* msg, mav
 #if MAVLINK_NEED_BYTE_SWAP
 	mavlink_msg_obs_wind_get_wind(msg, obs_wind->wind);
 #else
-	memcpy(obs_wind, _MAV_PAYLOAD(msg), 12);
+	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
index 5b7a4b2d6..e0963ece7 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h
@@ -18,6 +18,9 @@ typedef struct __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 { \
@@ -51,28 +54,32 @@ static inline uint16_t mavlink_msg_pm_elec_pack(uint8_t system_id, uint8_t compo
 						       float PwCons, float BatStat, const float *PwGen)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[20];
+	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, 20);
+        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, 20);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PM_ELEC_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_PM_ELEC;
-	return mavlink_finalize_message(msg, system_id, component_id, 20, 170);
+#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 was sent over
+ * @param chan The MAVLink channel this message will be sent over
  * @param msg The MAVLink message to compress the data into
  * @param PwCons 
                 
@@ -90,25 +97,29 @@ static inline uint16_t mavlink_msg_pm_elec_pack_chan(uint8_t system_id, uint8_t
 						           float PwCons,float BatStat,const float *PwGen)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[20];
+	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, 20);
+        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, 20);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PM_ELEC_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_PM_ELEC;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 170);
+#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 into a message
+ * @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)
@@ -121,6 +132,20 @@ static inline uint16_t mavlink_msg_pm_elec_encode(uint8_t system_id, uint8_t com
 }
 
 /**
+ * @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
  *
@@ -139,17 +164,25 @@ static inline uint16_t mavlink_msg_pm_elec_encode(uint8_t system_id, uint8_t com
 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[20];
+	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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, buf, 20, 170);
+#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);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, (const char *)&packet, 20, 170);
+#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
 }
 
@@ -207,6 +240,6 @@ static inline void mavlink_msg_pm_elec_decode(const mavlink_message_t* msg, mavl
 	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), 20);
+	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
index ab5d8f8a8..94f3e58bb 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h
@@ -21,6 +21,9 @@ typedef struct __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 { \
@@ -58,13 +61,13 @@ static inline uint16_t mavlink_msg_sys_stat_pack(uint8_t system_id, uint8_t comp
 						       uint8_t gps, uint8_t act, uint8_t mod, uint8_t commRssi)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[4];
+	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, 4);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SYS_Stat_LEN);
 #else
 	mavlink_sys_stat_t packet;
 	packet.gps = gps;
@@ -72,18 +75,22 @@ static inline uint16_t mavlink_msg_sys_stat_pack(uint8_t system_id, uint8_t comp
 	packet.mod = mod;
 	packet.commRssi = commRssi;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 4);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYS_Stat_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_SYS_Stat;
-	return mavlink_finalize_message(msg, system_id, component_id, 4, 157);
+#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 was sent over
+ * @param chan The MAVLink channel this message will be sent over
  * @param msg The MAVLink message to compress the data into
  * @param gps 
                 
@@ -104,13 +111,13 @@ static inline uint16_t mavlink_msg_sys_stat_pack_chan(uint8_t system_id, uint8_t
 						           uint8_t gps,uint8_t act,uint8_t mod,uint8_t commRssi)
 {
 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[4];
+	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, 4);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SYS_Stat_LEN);
 #else
 	mavlink_sys_stat_t packet;
 	packet.gps = gps;
@@ -118,15 +125,19 @@ static inline uint16_t mavlink_msg_sys_stat_pack_chan(uint8_t system_id, uint8_t
 	packet.mod = mod;
 	packet.commRssi = commRssi;
 
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 4);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYS_Stat_LEN);
 #endif
 
 	msg->msgid = MAVLINK_MSG_ID_SYS_Stat;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 157);
+#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 into a message
+ * @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)
@@ -139,6 +150,20 @@ static inline uint16_t mavlink_msg_sys_stat_encode(uint8_t system_id, uint8_t co
 }
 
 /**
+ * @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
  *
@@ -160,13 +185,17 @@ static inline uint16_t mavlink_msg_sys_stat_encode(uint8_t system_id, uint8_t co
 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[4];
+	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);
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_Stat, buf, 4, 157);
+#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;
@@ -174,7 +203,11 @@ static inline void mavlink_msg_sys_stat_send(mavlink_channel_t chan, uint8_t gps
 	packet.mod = mod;
 	packet.commRssi = commRssi;
 
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_Stat, (const char *)&packet, 4, 157);
+#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
 }
 
@@ -245,6 +278,6 @@ static inline void mavlink_msg_sys_stat_decode(const mavlink_message_t* msg, mav
 	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), 4);
+	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
index 84b296981..26666b0cc 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h
@@ -12,15 +12,15 @@ extern "C" {
 // MESSAGE LENGTHS AND CRCS
 
 #ifndef MAVLINK_MESSAGE_LENGTHS
-#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 0, 0, 0, 254, 249, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 33, 25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 12, 0, 12, 0, 32, 0, 12, 0, 12, 0, 24, 0, 4, 4, 12, 0, 12, 0, 20, 0, 4, 0, 5, 0, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0}
+#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 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, 0, 0, 0, 235, 93, 124, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 241, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 15, 0, 108, 0, 146, 0, 16, 0, 32, 0, 159, 0, 24, 248, 79, 0, 5, 0, 170, 0, 157, 0, 209, 0, 243, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}
+#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 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, {"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_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_POSITION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_VELOCITY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_ATTITUDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_WIND, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_AIR_VELOCITY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_BIAS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_QFF, MAVLINK_MESSAGE_INFO_OBS_AIR_TEMP, MAVLINK_MESSAGE_INFO_FILT_ROT_VEL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LLC_OUT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PM_ELEC, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SYS_Stat, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_CHNG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}}
+#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"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"
diff --git a/mavlink/include/mavlink/v1.0/sensesoar/testsuite.h b/mavlink/include/mavlink/v1.0/sensesoar/testsuite.h
index 4c8f73c5b..67ffca799 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/testsuite.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/testsuite.h
@@ -31,8 +31,8 @@ static void mavlink_test_obs_position(uint8_t system_id, uint8_t component_id, m
         uint16_t i;
 	mavlink_obs_position_t packet_in = {
 		963497464,
-	963497672,
-	963497880,
+	}963497672,
+	}963497880,
 	};
 	mavlink_obs_position_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -207,8 +207,8 @@ static void mavlink_test_obs_air_velocity(uint8_t system_id, uint8_t component_i
         uint16_t i;
 	mavlink_obs_air_velocity_t packet_in = {
 		17.0,
-	45.0,
-	73.0,
+	}45.0,
+	}73.0,
 	};
 	mavlink_obs_air_velocity_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -254,7 +254,7 @@ static void mavlink_test_obs_bias(uint8_t system_id, uint8_t component_id, mavli
         uint16_t i;
 	mavlink_obs_bias_t packet_in = {
 		{ 17.0, 18.0, 19.0 },
-	{ 101.0, 102.0, 103.0 },
+	}{ 101.0, 102.0, 103.0 },
 	};
 	mavlink_obs_bias_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -428,7 +428,7 @@ static void mavlink_test_llc_out(uint8_t system_id, uint8_t component_id, mavlin
         uint16_t i;
 	mavlink_llc_out_t packet_in = {
 		{ 17235, 17236, 17237, 17238 },
-	{ 17651, 17652 },
+	}{ 17651, 17652 },
 	};
 	mavlink_llc_out_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -473,8 +473,8 @@ static void mavlink_test_pm_elec(uint8_t system_id, uint8_t component_id, mavlin
         uint16_t i;
 	mavlink_pm_elec_t packet_in = {
 		17.0,
-	45.0,
-	{ 73.0, 74.0, 75.0 },
+	}45.0,
+	}{ 73.0, 74.0, 75.0 },
 	};
 	mavlink_pm_elec_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -520,9 +520,9 @@ static void mavlink_test_sys_stat(uint8_t system_id, uint8_t component_id, mavli
         uint16_t i;
 	mavlink_sys_stat_t packet_in = {
 		5,
-	72,
-	139,
-	206,
+	}72,
+	}139,
+	}206,
 	};
 	mavlink_sys_stat_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -569,7 +569,7 @@ static void mavlink_test_cmd_airspeed_chng(uint8_t system_id, uint8_t component_
         uint16_t i;
 	mavlink_cmd_airspeed_chng_t packet_in = {
 		17.0,
-	17,
+	}17,
 	};
 	mavlink_cmd_airspeed_chng_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
@@ -614,7 +614,7 @@ static void mavlink_test_cmd_airspeed_ack(uint8_t system_id, uint8_t component_i
         uint16_t i;
 	mavlink_cmd_airspeed_ack_t packet_in = {
 		17.0,
-	17,
+	}17,
 	};
 	mavlink_cmd_airspeed_ack_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
diff --git a/mavlink/include/mavlink/v1.0/sensesoar/version.h b/mavlink/include/mavlink/v1.0/sensesoar/version.h
index 18eb38f00..cdd683949 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/version.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/version.h
@@ -5,8 +5,8 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Wed Jan  9 16:19:38 2013"
+#define MAVLINK_BUILD_DATE "Tue Feb  4 15:28:27 2014"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
-#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254
+#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
  
 #endif // MAVLINK_VERSION_H
diff --git a/misc/tones/charge.txt b/misc/tones/charge.txt
new file mode 100644
index 000000000..cf33c58b3
--- /dev/null
+++ b/misc/tones/charge.txt
@@ -0,0 +1 @@
+MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4
\ No newline at end of file
diff --git a/misc/tones/cucuracha.txt b/misc/tones/cucuracha.txt
new file mode 100644
index 000000000..dce328c03
--- /dev/null
+++ b/misc/tones/cucuracha.txt
@@ -0,0 +1 @@
+MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8
\ No newline at end of file
diff --git a/misc/tones/daisy.txt b/misc/tones/daisy.txt
new file mode 100644
index 000000000..e39410621
--- /dev/null
+++ b/misc/tones/daisy.txt
@@ -0,0 +1 @@
+MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8
\ No newline at end of file
diff --git a/misc/tones/dixie.txt b/misc/tones/dixie.txt
new file mode 100644
index 000000000..f3ddd19ed
--- /dev/null
+++ b/misc/tones/dixie.txt
@@ -0,0 +1 @@
+MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16
\ No newline at end of file
diff --git a/misc/tones/tell.txt b/misc/tones/tell.txt
new file mode 100644
index 000000000..ea77d1a24
--- /dev/null
+++ b/misc/tones/tell.txt
@@ -0,0 +1 @@
+T200O2B4P8B16B16B4P8B16B16B8G+8E8G+8B8G+8B8O3E8O2B8G+8E8G+8B8G+8B8O3E8O2B4P8B16B16B4P8B16O2B16B4P8B16B16B4P8B16B16B8B16B16B8B8B8B16O2B16B8B8B8B16B16B8B8B8B16B16B8B8B2B2B8P8 P4P4P8O1B16B16B8B16B16B8B16B16O2E8F+8G+8 O1B16B16B8B16B16O2E8G+16G+16F+8D+8O1B8B16 O1B16B8B16B16B8B16B16O2E8F+8G+8E16G+16B4 O2B16A16G+16F+16E8G+8E8O3B16B16B8B16B16B8 O3B16B16O4E8F+8G+8O3B16B16B8B16B16O4E8G+16 O4G+16F+8D+8O3B8B16B16B8B16B16B8B16B16O4E8 O4F+8G+8E16G+16B4B16A16G+16F+16E8G+8E8O3G+16 O3G+16G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8 O4C+8O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8 O3G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8 O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8 O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8 O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16 O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8O2B16 O2B16B8F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8 O2E4G+8F+8F+8F+8O3F+16F+16F+8F+16F+16F+8 O3G+8A8F+4A8G+8E4G+8F+8O2B16B16B8O1B16B16 O1B8B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16 O1B16O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16 O1B8B16B16O2E8F+8G+8E16G+16B4B16A16G+16F+16 O2E8G+8E8O3B16B16B8B16B16B8B16B16O4E8F+8 O4G+8O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8 O3B16B16B8B16B16B8B16B16O4E8F+8G+8E16G+16 O4B4B16A16G+16F+16E8G+8E8O3E64F64G64A64B64 O4C64D64E8E16E16E8E8G+4.F+8E8D+8E8C+8O3B16 O4C+16O3B16O4C+16O3B16O4C+16D+16E16O3A16 O3B16A16B16A16B16O4C+16D+16O3G+16A16G+16 O3A16G+16A16B16O4C+16O3F+16G+16F+16G+16F+16 O3G+16F+16G+16F+16G+16F+16D+16O2B16O3B16 O4C+16D+16E8D+8E8C+8O3B16O4C+16O3B16O4C+16 O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16 O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16 O3G+16F+16G+16F+16A16F+16E16E8P8C+4C+16O2C16 O3C+16O2C16O3D+16C+16O2B16A16A16G+16E16C+16 O2C+16C+16C+16C+16E16D+16O1C16G+16G+16G+16 O1G+16G+16G+16O2C+16E16G+16O3C+16C+16C+16 O3C+16C+16O2C16O3C+16O2C16O3D+16C+16O2B16 O2A16A16G+16E16C+16C+16C+16C+16C+16E16D+16 O1C16G+16G+16G+16G+16G+16G+16O2C+16E16G+16 O3C+16E16D+16C+16D+16O2C16G+16G+16G+16O3G+16 O3E16C+16D+16O2C16G+16G+16G+16O3G+16E16C+16 O3D+16O2B16G+16G+16A+16G16D+16D+16G+16G16 O2G+16G16G+16A16G+16F+16E16O1B16A+16B16O2E16 O1B16O2F+16O1B16O2G+16E16D+16E16G+16E16A16 O2F+16B16O3G+16F+16E16D+16F+16E16C+16O2B16 O3C+16O2B16O3C+16D+16E16F+16G+16O2A16B16 O2A16B16O3C+16D+16E16F+16O2G+16A16G+16A16 O2C16O3C+16D+16E16O2F+16G+16F+16G+16F+16 O2G+16F+16G+16F+16G+16F+16D+16O1B16C16O2C+16 O2D+16E16O1B16A+16B16O2E16O1B16O2F+16O1B16 O2G+16E16D+16E16G+16E16A16F+16B16O3G+16F+16 O3E16D+16F+16E16C+16O2B16O3C+16O2B16O3C+16 O3D+16E16F+16G+16O2A16B16A16B16O3C+16D+16 O3E16F+16O2G+16A16G+16A16B16O3C+16D+16E16 O2F+16O3C+16O2C16O3C+16D+16C+16O2A16F+16 O2E16O3E16F+16G+16A16B16O4C+16D+16E8E16E16 O4E8E8G+4.F8E8D+8E8C+8O3B16O4C+16O3B16O4C+16 O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16 O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16 O3G+16F+16G+16F+16G+16F+16G+16F+16G+16F+16 O3D+16O2B16O3B16O4C+16D+16E8E16E16E8E8G+4." O4F+8E8D+8E8C+8O3B16O4C+16O3B16O4C+16O3B16 O4C+16D+16E16O3A16B16A16B16A16B16O4C+16D+16 O3G+16A16G+16A16G+16A16B16O4C+16O3F+16G+16 O3F+16G+16F+16A16G+16F+16E8O2B8O3E8G+16G+16 O3G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8 O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8G+16 O3G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8 O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8 O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8 O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16 O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8A+8 O3B8O2F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8 O2E4G+8F+8B8O1B8O2F+16F+16F+8F+16F+16F+8 O2G+8A8F+4A8G+8E4G+8F+8B16B16B8O1B16B16B8 O1B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16B16 O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16B8B16 O1B16O2E8F+8G+8E16G+16B4B16A16G+16F+16E8 O1B8O2E8O3B16B16B8B16B16B8B16B16O4E8F+8G+8 O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8B16 O3B16B8B16B16B8B16B16O4E8F+8G+8O3E16G+16 O3B4B16A16G+16F+16E16F+16G+16A16G+16A16B16 O4C+16O3B16O4C+16D+16E16D+16E16F+16G+16A16 O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16 O3B16O4A16O3B16O4A16O3B16O4A16O3B16E16F+16 O3G+16A16G+16A16B16O4C+16O3B16O4C+16D+16 O4E16D+16E16F+16G+16A16O3B16O4A16O3B16O4A16 O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16 O3B16O4A16O3B16P16G+16O4G+16O3G+16P16D+16 O4D+16O3D+16P16E16O4E16O3E16P16A16O4A16O3A16 P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16 P16O3E16O4E16O3E16P16A16O4A16O3A16O4G16O3G16 O4G16O3G16O4G16O3G16O4G16O3G16O4G8E8C8E8 O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+16 O3G+16O4G+8E8O3B8O4E8G+16O3G+16O4G+16O3G+16 O4G+16O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16 O3A+16O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16 O4A+8G8E8G8B8P16A+16P16A16P16G+16P16F+16 P16O4E16P16D+16P16C+16P16O3B16P16A+16P16 O3A16P16G+16P16F+16P16E16P16D+16P16F+16E16 O3F+16G+16A16G+16A16B16O4C+16O3B16O4C+16 O4D+16E16D+16E16F+16G+16A16O3B16O4A16O3B16 O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16 O4A16O3B16O4A16O3B16E16F+16G+16A16G+16A16 O3B16O4C+16O3B16O4C+16D+16E16D+16E16F+16 O4G+16A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16 O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16 P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16 P16O3E16O4E16O3E16P16A16O4A16O3A16P16G+16 O4G+16O3G+16P16D+16O4D+16O3D+16P16E16O4E16 O3E16P16A16O4A16O3A16O4G16O3G16O4G16O3G16 O4G16O3G16O4G16O3G16O4G8E8C8E8G+16O3G+16 O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+8 O4E8O3B8O4E8G+16O3G+16O4G+16O3G+16O4G+16 O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16O3A+16 O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16O4A+8 O4G8E8G8B8P16A+16P16A16P16G+16P16F+16P16 O4E16P16D+16P16C+16P16O3B16P16A+16P16A16 P16O3G+16P16F+16P16E16P16D+16P16F16E16D+16 O3E16D+16E8B16B16B8B16B16B8B16B16O4E8F+8 O4G+8O3B16B16B8B16B16B8B16B16O4G+8A8B8P8 O4E8F+8G+8P8O3G+8A8B8P8P2O2B16C16O3C+16D16 O3D+16E16F16F+16G16G+16A16A+16B16C16O4C+16 O4D+16E16D+16F+16D+16E16D+16F+16D+16E16D+16 O4F+16D+16E16D+16F+16D+16E16D+16F+16D+16 O4E16D+16F+16D+16E16D+16F+16D+16E16D+16F+16 O4D+16E8E16O3E16O4E16O3E16O4E16O3E16O4E8 O3B16O2B16O3B16O2B16O3B16O2B16O3B8G+16O2G+16 O3G+16O2G+16O3G+16O2G+16O3G8E16O2E16O3E16 O2E16O3E16O2E16O3E8E16E16E8E8E8O2B16B16B8 O2B8B8G+16G+16G+8G+8G+8E16E16E8E8E8O1B8O2E8 O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8 O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16 O2E2P64
\ No newline at end of file
diff --git a/misc/tones/yankee.txt b/misc/tones/yankee.txt
new file mode 100644
index 000000000..3677b39c1
--- /dev/null
+++ b/misc/tones/yankee.txt
@@ -0,0 +1 @@
+MNT150L8O2GGABGBADGGABL4GL8F+
\ No newline at end of file
diff --git a/nuttx-configs/px4fmu-v1/include/board.h b/nuttx-configs/px4fmu-v1/include/board.h
new file mode 100644
index 000000000..ff1c63424
--- /dev/null
+++ b/nuttx-configs/px4fmu-v1/include/board.h
@@ -0,0 +1,319 @@
+/************************************************************************************
+ * configs/stm32f4discovery/include/board.h
+ * include/arch/board/board.h
+ *
+ *   Copyright (C) 2012 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 __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H
+#define __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#ifndef __ASSEMBLY__
+# include <stdint.h>
+#endif
+
+#include "stm32_rcc.h"
+#include "stm32_sdio.h"
+#include "stm32.h"
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/* Clocking *************************************************************************/
+/* The PX4FMU uses a 24MHz crystal connected to the HSE.
+ *
+ * This is the canonical configuration:
+ *   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_PLLQ)
+ *   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
+//#define STM32_LSE_FREQUENCY     32768
+
+/* Main PLL Configuration.
+ *
+ * PLL source is HSE
+ * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN
+ *         = (8,000,000 / 8) * 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)
+
+/* LED definitions ******************************************************************/
+/* If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any
+ * way.  The following definitions are used to access individual LEDs.
+ */
+
+/* LED index values for use with stm32_setled() */
+
+#define BOARD_LED1        0
+#define BOARD_LED2        1
+#define BOARD_NLEDS       2
+
+#define BOARD_LED_BLUE    BOARD_LED1
+#define BOARD_LED_RED     BOARD_LED2
+
+/* LED bits for use with stm32_setleds() */
+
+#define BOARD_LED1_BIT    (1 << BOARD_LED1)
+#define BOARD_LED2_BIT    (1 << BOARD_LED2)
+
+/* If CONFIG_ARCH_LEDs is defined, then NuttX will control the 2 LEDs on board the
+ * px4fmu-v1.  The following definitions describe how NuttX controls the LEDs:
+ */
+
+#define LED_STARTED       0  /* LED1 */
+#define LED_HEAPALLOCATE  1  /* LED2 */
+#define LED_IRQSENABLED   2  /* LED1 */
+#define LED_STACKCREATED  3  /* LED1 + LED2 */
+#define LED_INIRQ         4  /* LED1 */
+#define LED_SIGNAL        5  /* LED2 */
+#define LED_ASSERTION     6  /* LED1 + LED2 */
+#define LED_PANIC         7  /* LED1 + LED2 */
+
+/* Alternate function pin selections ************************************************/
+
+/*
+ * UARTs.
+ *
+ * Note that UART5 has no optional pinout, so it is not listed here.
+ */
+#define GPIO_USART1_RX	GPIO_USART1_RX_2
+#define GPIO_USART1_TX	GPIO_USART1_TX_2
+
+#define GPIO_USART2_RX	GPIO_USART2_RX_1
+#define GPIO_USART2_TX	GPIO_USART2_TX_1
+#define GPIO_USART2_RTS	GPIO_USART2_RTS_1
+#define GPIO_USART2_CTS	GPIO_USART2_CTS_1
+
+#define GPIO_USART6_RX	GPIO_USART6_RX_1
+#define GPIO_USART6_TX	GPIO_USART6_TX_1
+
+/* UART DMA configuration for USART1/6 */
+#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2
+#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2
+ 
+/*
+ * CAN
+ *
+ * CAN2 is routed to the expansion connector.
+ */
+
+#define GPIO_CAN2_RX	GPIO_CAN2_RX_1
+#define GPIO_CAN2_TX	GPIO_CAN2_TX_1
+
+/*
+ * 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.
+ */
+#define GPIO_I2C1_SCL		GPIO_I2C1_SCL_2
+#define GPIO_I2C1_SDA		GPIO_I2C1_SDA_2
+#define GPIO_I2C1_SCL_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8)
+#define GPIO_I2C1_SDA_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9)
+
+#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)
+
+#define GPIO_I2C3_SCL		GPIO_I2C3_SCL_1
+#define GPIO_I2C3_SDA		GPIO_I2C3_SDA_1
+#define GPIO_I2C3_SCL_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN8)
+#define GPIO_I2C3_SDA_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN9)
+
+/*
+ * SPI
+ *
+ * There are sensors on SPI1, and SPI3 is connected to the microSD slot.
+ */
+#define GPIO_SPI1_MISO	(GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI1_MOSI	(GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI1_SCK	(GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz)
+
+#define GPIO_SPI3_MISO	(GPIO_SPI3_MISO_2|GPIO_SPEED_50MHz)
+#define GPIO_SPI3_MOSI	(GPIO_SPI3_MOSI_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI3_SCK	(GPIO_SPI3_SCK_2|GPIO_SPEED_50MHz)
+#define GPIO_SPI3_NSS	(GPIO_SPI3_NSS_2|GPIO_SPEED_50MHz)
+
+/* SPI DMA configuration for SPI3 (microSD) */
+#define DMACHAN_SPI3_RX DMAMAP_SPI3_RX_1
+#define DMACHAN_SPI3_TX DMAMAP_SPI3_TX_2
+/* XXX since we allocate the HP work stack from CCM RAM on normal system startup,
+   SPI1 will never run in DMA mode - so we can just give it a random config here.
+   What we really need to do is to make DMA configurable per channel, and always
+   disable it for SPI1. */
+#define DMACHAN_SPI1_RX DMAMAP_SPI1_RX_1
+#define DMACHAN_SPI1_TX DMAMAP_SPI1_TX_2
+
+/************************************************************************************
+ * 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);
+
+/************************************************************************************
+ * Name:  stm32_ledinit, stm32_setled, and stm32_setleds
+ *
+ * Description:
+ *   If CONFIG_ARCH_LEDS is defined, then NuttX will control the on-board LEDs.  If
+ *   CONFIG_ARCH_LEDS is not defined, then the following interfacesare available to
+ *   control the LEDs from user applications.
+ *
+ ************************************************************************************/
+
+#ifndef CONFIG_ARCH_LEDS
+EXTERN void stm32_ledinit(void);
+EXTERN void stm32_setled(int led, bool ledon);
+EXTERN void stm32_setleds(uint8_t ledset);
+#endif
+
+#undef EXTERN
+#if defined(__cplusplus)
+}
+#endif
+
+#endif /* __ASSEMBLY__ */
+#endif  /* __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H */
diff --git a/nuttx-configs/px4fmu-v1/include/nsh_romfsimg.h b/nuttx-configs/px4fmu-v1/include/nsh_romfsimg.h
new file mode 100644
index 000000000..15e4e7a8d
--- /dev/null
+++ b/nuttx-configs/px4fmu-v1/include/nsh_romfsimg.h
@@ -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.
+ *
+ ****************************************************************************/
+
+/**
+ * nsh_romfsetc.h
+ *
+ * This file is a stub for 'make export' purposes; the actual ROMFS
+ * must be supplied by the library client.
+ */
+
+extern unsigned char romfs_img[];
+extern unsigned int romfs_img_len;
diff --git a/nuttx-configs/px4fmu-v1/nsh/Make.defs b/nuttx-configs/px4fmu-v1/nsh/Make.defs
new file mode 100644
index 000000000..7b2ea703a
--- /dev/null
+++ b/nuttx-configs/px4fmu-v1/nsh/Make.defs
@@ -0,0 +1,179 @@
+############################################################################
+# configs/px4fmu-v1/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/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig
new file mode 100644
index 000000000..1dc96b3c3
--- /dev/null
+++ b/nuttx-configs/px4fmu-v1/nsh/defconfig
@@ -0,0 +1,983 @@
+#
+# Automatically generated file; DO NOT EDIT.
+# Nuttx/ Configuration
+#
+CONFIG_NUTTX_NEWCONFIG=y
+
+#
+# Build Setup
+#
+# CONFIG_EXPERIMENTAL is not set
+# CONFIG_HOST_LINUX is not set
+CONFIG_HOST_OSX=y
+# 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=y
+
+#
+# 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_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=y
+# 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 is not set
+# 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_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=y
+CONFIG_STM32_I2C2=y
+CONFIG_STM32_I2C3=y
+CONFIG_STM32_OTGFS=y
+# 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 is not set
+CONFIG_STM32_SPI3=y
+CONFIG_STM32_SYSCFG=y
+# CONFIG_STM32_TIM1 is not set
+# CONFIG_STM32_TIM2 is not set
+# CONFIG_STM32_TIM3 is not set
+CONFIG_STM32_TIM4=y
+CONFIG_STM32_TIM5=y
+CONFIG_STM32_TIM6=y
+CONFIG_STM32_TIM7=y
+# CONFIG_STM32_TIM8 is not set
+CONFIG_STM32_TIM9=y
+CONFIG_STM32_TIM10=y
+CONFIG_STM32_TIM11=y
+CONFIG_STM32_TIM12=y
+CONFIG_STM32_TIM13=y
+CONFIG_STM32_TIM14=y
+CONFIG_STM32_USART1=y
+CONFIG_STM32_USART2=y
+# CONFIG_STM32_USART3 is not set
+# CONFIG_STM32_UART4 is not set
+CONFIG_STM32_UART5=y
+CONFIG_STM32_USART6=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=y
+# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set
+# CONFIG_STM32_JTAG_SW_ENABLE is not set
+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_TIM4_PWM is not set
+# CONFIG_STM32_TIM5_PWM is not set
+# CONFIG_STM32_TIM9_PWM is not set
+# CONFIG_STM32_TIM10_PWM is not set
+# CONFIG_STM32_TIM11_PWM is not set
+# CONFIG_STM32_TIM12_PWM is not set
+# CONFIG_STM32_TIM13_PWM is not set
+# CONFIG_STM32_TIM14_PWM 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 is not set
+# CONFIG_USART2_RS485 is not set
+CONFIG_USART2_RXDMA=y
+# CONFIG_USART3_RXDMA is not set
+# CONFIG_UART4_RXDMA is not set
+# CONFIG_UART5_RS485 is not set
+CONFIG_UART5_RXDMA=y
+# CONFIG_USART6_RS485 is not set
+CONFIG_USART6_RXDMA=y
+# CONFIG_USART7_RXDMA is not set
+# CONFIG_USART8_RXDMA is not set
+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=196608
+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
+CONFIG_NSH_MMCSDSLOTNO=0
+CONFIG_NSH_MMCSDSPIPORTNO=3
+
+#
+# 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=y
+# CONFIG_DISABLE_CLOCK is not set
+# CONFIG_DISABLE_POSIX_TIMERS is not set
+# CONFIG_DISABLE_PTHREAD is not set
+# CONFIG_DISABLE_SIGNALS is not set
+# CONFIG_DISABLE_MQUEUE is not set
+# CONFIG_DISABLE_ENVIRON 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=32
+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=y
+CONFIG_MMCSD_NSLOTS=1
+# CONFIG_MMCSD_READONLY is not set
+# CONFIG_MMCSD_MULTIBLOCK_DISABLE is not set
+# CONFIG_MMCSD_MMCSUPPORT is not set
+# CONFIG_MMCSD_HAVECARDDETECT is not set
+CONFIG_MMCSD_SPI=y
+CONFIG_MMCSD_SPICLOCK=24000000
+# CONFIG_MMCSD_SDIO is not set
+CONFIG_MTD=y
+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_SERIAL_REMOVABLE=y
+# CONFIG_16550_UART is not set
+CONFIG_ARCH_HAVE_UART5=y
+CONFIG_ARCH_HAVE_USART1=y
+CONFIG_ARCH_HAVE_USART2=y
+CONFIG_ARCH_HAVE_USART6=y
+CONFIG_MCU_SERIAL=y
+CONFIG_STANDARD_SERIAL=y
+CONFIG_SERIAL_NPOLLWAITERS=2
+CONFIG_USART1_SERIAL_CONSOLE=y
+# CONFIG_USART2_SERIAL_CONSOLE is not set
+# CONFIG_UART5_SERIAL_CONSOLE is not set
+# CONFIG_USART6_SERIAL_CONSOLE is not set
+# CONFIG_NO_SERIAL_CONSOLE is not set
+
+#
+# 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 is not set
+# CONFIG_MTD_SST25 is not set
+# CONFIG_MTD_SST39FV is not set
+# CONFIG_MTD_W25 is not set
+
+#
+# USART1 Configuration
+#
+CONFIG_USART1_RXBUFSIZE=512
+CONFIG_USART1_TXBUFSIZE=512
+CONFIG_USART1_BAUD=57600
+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=57600
+CONFIG_USART2_BITS=8
+CONFIG_USART2_PARITY=0
+CONFIG_USART2_2STOP=0
+CONFIG_USART2_IFLOWCONTROL=y
+CONFIG_USART2_OFLOWCONTROL=y
+
+#
+# UART5 Configuration
+#
+CONFIG_UART5_RXBUFSIZE=512
+CONFIG_UART5_TXBUFSIZE=512
+CONFIG_UART5_BAUD=57600
+CONFIG_UART5_BITS=8
+CONFIG_UART5_PARITY=0
+CONFIG_UART5_2STOP=0
+# CONFIG_UART5_IFLOWCONTROL is not set
+# CONFIG_UART5_OFLOWCONTROL is not set
+
+#
+# USART6 Configuration
+#
+CONFIG_USART6_RXBUFSIZE=512
+CONFIG_USART6_TXBUFSIZE=512
+CONFIG_USART6_BAUD=57600
+CONFIG_USART6_BITS=8
+CONFIG_USART6_PARITY=0
+CONFIG_USART6_2STOP=0
+# CONFIG_USART6_IFLOWCONTROL is not set
+# CONFIG_USART6_OFLOWCONTROL is not set
+CONFIG_SERIAL_IFLOWCONTROL=y
+CONFIG_SERIAL_OFLOWCONTROL=y
+CONFIG_USBDEV=y
+
+#
+# USB Device Controller Driver Options
+#
+# CONFIG_USBDEV_ISOCHRONOUS is not set
+# CONFIG_USBDEV_DUALSPEED is not set
+# CONFIG_USBDEV_SELFPOWERED is not set
+CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_MAXPOWER=500
+# CONFIG_USBDEV_REMOTEWAKEUP is not set
+# CONFIG_USBDEV_DMA is not set
+# CONFIG_USBDEV_TRACE is not set
+
+#
+# USB Device Class Driver Options
+#
+# CONFIG_USBDEV_COMPOSITE is not set
+# CONFIG_PL2303 is not set
+CONFIG_CDCACM=y
+CONFIG_CDCACM_CONSOLE=n
+CONFIG_CDCACM_EP0MAXPACKET=64
+CONFIG_CDCACM_EPINTIN=1
+CONFIG_CDCACM_EPINTIN_FSSIZE=64
+CONFIG_CDCACM_EPINTIN_HSSIZE=64
+CONFIG_CDCACM_EPBULKOUT=3
+CONFIG_CDCACM_EPBULKOUT_FSSIZE=64
+CONFIG_CDCACM_EPBULKOUT_HSSIZE=512
+CONFIG_CDCACM_EPBULKIN=2
+CONFIG_CDCACM_EPBULKIN_FSSIZE=64
+CONFIG_CDCACM_EPBULKIN_HSSIZE=512
+CONFIG_CDCACM_NWRREQS=4
+CONFIG_CDCACM_NRDREQS=4
+CONFIG_CDCACM_BULKIN_REQLEN=96
+CONFIG_CDCACM_RXBUFSIZE=512
+CONFIG_CDCACM_TXBUFSIZE=2048
+CONFIG_CDCACM_VENDORID=0x26ac
+CONFIG_CDCACM_PRODUCTID=0x0010
+CONFIG_CDCACM_VENDORSTR="3D Robotics"
+CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v1.x"
+# CONFIG_USBMSC 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 is not set
+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=y
+CONFIG_SYSLOG_CHAR=y
+CONFIG_SYSLOG_DEVPATH="/dev/ttyS0"
+
+#
+# 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=y
+CONFIG_GRAN_INTR=y
+
+#
+# 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=256
+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=4000
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKPERIOD=50000
+CONFIG_SCHED_LPWORKSTACKSIZE=4000
+# 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_CDCACM=y
+# 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 is not set
+# 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_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
+# CONFIG_NSH_USBCONSOLE is not set
+
+#
+# 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 is not set
+
+#
+# FLASH Program Installation
+#
+# CONFIG_SYSTEM_INSTALL is not set
+
+#
+# FLASH Erase-all Command
+#
+
+#
+# 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/nuttx-configs/px4fmu-v1/nsh/setenv.sh b/nuttx-configs/px4fmu-v1/nsh/setenv.sh
new file mode 100755
index 000000000..db372217c
--- /dev/null
+++ b/nuttx-configs/px4fmu-v1/nsh/setenv.sh
@@ -0,0 +1,75 @@
+#!/bin/bash
+# configs/px4fmu-v1/usbnsh/setenv.sh
+#
+#   Copyright (C) 2013 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.
+#
+
+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 is 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 is 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"
+
+# These are the Cygwin paths to the locations where I installed the Atollic
+# toolchain under windows.  You will also have to edit this if you install
+# the Atollic toolchain in any other location.  /usr/bin is added before
+# the Atollic bin path because there is are binaries named gcc.exe and g++.exe
+# at those locations as well.
+#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for ARM Pro 2.3.0/ARMTools/bin"
+#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for STMicroelectronics STM32 Lite 2.3.0/ARMTools/bin"
+
+# This is the Cygwin path to the location where I build the buildroot
+# toolchain.
+#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin"
+
+# Add the path to the toolchain to the PATH varialble
+export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}"
+
+echo "PATH : ${PATH}"
diff --git a/nuttx-configs/px4fmu-v1/scripts/ld.script b/nuttx-configs/px4fmu-v1/scripts/ld.script
new file mode 100644
index 000000000..ced5b21b7
--- /dev/null
+++ b/nuttx-configs/px4fmu-v1/scripts/ld.script
@@ -0,0 +1,149 @@
+/****************************************************************************
+ * configs/px4fmu-v1/scripts/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 STM32F405RG has 1024Kb of FLASH beginning at address 0x0800:0000 and
+ * 192Kb 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 CCM 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 = 1008K
+    sram (rwx)   : ORIGIN = 0x20000000, LENGTH = 128K
+    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/nuttx-configs/px4fmu-v1/src/Makefile b/nuttx-configs/px4fmu-v1/src/Makefile
new file mode 100644
index 000000000..6ef8b7d6a
--- /dev/null
+++ b/nuttx-configs/px4fmu-v1/src/Makefile
@@ -0,0 +1,84 @@
+############################################################################
+# configs/px4fmu-v1/src/Makefile
+#
+#   Copyright (C) 2013 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)/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 $@
+
+depend: .depend
+
+clean:
+	$(call DELFILE, libboard$(LIBEXT))
+	$(call CLEAN)
+
+distclean: clean
+	$(call DELFILE, Make.dep)
+	$(call DELFILE, .depend)
+
+-include Make.dep
+
diff --git a/nuttx-configs/px4fmu-v1/src/empty.c b/nuttx-configs/px4fmu-v1/src/empty.c
new file mode 100644
index 000000000..ace900866
--- /dev/null
+++ b/nuttx-configs/px4fmu-v1/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-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h
new file mode 100755
index 000000000..3bede8a1f
--- /dev/null
+++ b/nuttx-configs/px4fmu-v2/include/board.h
@@ -0,0 +1,306 @@
+/************************************************************************************
+ * configs/px4fmu/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 PX4FMUV2 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
+//#define STM32_LSE_FREQUENCY     32768
+
+/* Main PLL Configuration.
+ *
+ * PLL source is HSE
+ * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN
+ *         = (25,000,000 / 25) * 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)
+
+/* SDIO dividers.  Note that slower clocking is required when DMA is disabled 
+ * in order to avoid RX overrun/TX underrun errors due to delayed responses
+ * to service FIFOs in interrupt driven mode.  These values have not been
+ * tuned!!!
+ *
+ * HCLK=72MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(178+2)=400 KHz
+ */
+  
+#define SDIO_INIT_CLKDIV        (178 << SDIO_CLKCR_CLKDIV_SHIFT)
+
+/* DMA ON:  HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(2+2)=18 MHz
+ * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz
+ */
+
+#ifdef CONFIG_SDIO_DMA
+#  define SDIO_MMCXFR_CLKDIV    (2 << SDIO_CLKCR_CLKDIV_SHIFT) 
+#else
+#  define SDIO_MMCXFR_CLKDIV    (3 << SDIO_CLKCR_CLKDIV_SHIFT) 
+#endif
+
+/* DMA ON:  HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(1+2)=24 MHz
+ * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz
+ */
+
+#ifdef CONFIG_SDIO_DMA
+#  define SDIO_SDXFR_CLKDIV     (1 << SDIO_CLKCR_CLKDIV_SHIFT)
+#else
+#  define SDIO_SDXFR_CLKDIV     (3 << SDIO_CLKCR_CLKDIV_SHIFT)
+#endif
+
+/* DMA Channl/Stream Selections *****************************************************/
+/* Stream selections are arbitrary for now but might become important in the future
+ * is we set aside more DMA channels/streams.
+ *
+ * SDIO DMA
+ *   DMAMAP_SDIO_1 = Channel 4, Stream 3 <- may later be used by SPI DMA
+ *   DMAMAP_SDIO_2 = Channel 4, Stream 6
+ */
+
+#define DMAMAP_SDIO DMAMAP_SDIO_1
+
+/* Alternate function pin selections ************************************************/
+
+/*
+ * UARTs.
+ */
+#define GPIO_USART1_RX	GPIO_USART1_RX_1	/* console in from IO */
+#define GPIO_USART1_TX	0			/* USART1 is RX-only */
+
+#define GPIO_USART2_RX	GPIO_USART2_RX_2
+#define GPIO_USART2_TX	GPIO_USART2_TX_2
+#define GPIO_USART2_RTS	GPIO_USART2_RTS_2
+#define GPIO_USART2_CTS	GPIO_USART2_CTS_2
+
+#define GPIO_USART3_RX	GPIO_USART3_RX_3
+#define GPIO_USART3_TX	GPIO_USART3_TX_3
+#define GPIO_USART3_RTS	GPIO_USART3_RTS_2
+#define GPIO_USART3_CTS	GPIO_USART3_CTS_2
+
+#define GPIO_UART4_RX	GPIO_UART4_RX_1
+#define GPIO_UART4_TX	GPIO_UART4_TX_1
+
+#define GPIO_USART6_RX	GPIO_USART6_RX_1
+#define GPIO_USART6_TX	GPIO_USART6_TX_1
+
+#define GPIO_UART7_RX	GPIO_UART7_RX_1
+#define GPIO_UART7_TX	GPIO_UART7_TX_1
+
+/* UART8 has no alternate pin config */
+
+/* UART RX DMA configurations */
+#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2
+#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2
+
+/* 
+ * CAN
+ *
+ * CAN1 is routed to the onboard transceiver. 
+ * CAN2 is routed to the expansion connector.
+ */
+#define GPIO_CAN1_RX	GPIO_CAN1_RX_3
+#define GPIO_CAN1_TX	GPIO_CAN1_TX_3
+#define GPIO_CAN2_RX	GPIO_CAN2_RX_1
+#define GPIO_CAN2_TX	GPIO_CAN2_TX_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.
+ */
+#define GPIO_I2C1_SCL		GPIO_I2C1_SCL_2
+#define GPIO_I2C1_SDA		GPIO_I2C1_SDA_2
+#define GPIO_I2C1_SCL_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8)
+#define GPIO_I2C1_SDA_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9)
+
+#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
+ *
+ * There are sensors on SPI1, and SPI2 is connected to the FRAM.
+ */
+#define GPIO_SPI1_MISO	(GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI1_MOSI	(GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI1_SCK	(GPIO_SPI1_SCK_1|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)
+#define GPIO_SPI2_SCK	(GPIO_SPI2_SCK_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/nuttx-configs/px4fmu-v2/include/nsh_romfsimg.h b/nuttx-configs/px4fmu-v2/include/nsh_romfsimg.h
new file mode 100644
index 000000000..15e4e7a8d
--- /dev/null
+++ b/nuttx-configs/px4fmu-v2/include/nsh_romfsimg.h
@@ -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.
+ *
+ ****************************************************************************/
+
+/**
+ * nsh_romfsetc.h
+ *
+ * This file is a stub for 'make export' purposes; the actual ROMFS
+ * must be supplied by the library client.
+ */
+
+extern unsigned char romfs_img[];
+extern unsigned int romfs_img_len;
diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs
new file mode 100644
index 000000000..e70320aaa
--- /dev/null
+++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs
@@ -0,0 +1,179 @@
+############################################################################
+# configs/px4fmu-v2/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/px4fmu-v2/nsh/appconfig b/nuttx-configs/px4fmu-v2/nsh/appconfig
new file mode 100644
index 000000000..0e18aa8ef
--- /dev/null
+++ b/nuttx-configs/px4fmu-v2/nsh/appconfig
@@ -0,0 +1,52 @@
+############################################################################
+# configs/px4fmu/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
+
+ifeq ($(CONFIG_CAN),y)
+#CONFIGURED_APPS += examples/can
+endif
+
+#ifeq ($(CONFIG_USBDEV),y)
+#ifeq ($(CONFIG_CDCACM),y)
+CONFIGURED_APPS += examples/cdcacm
+#endif
+#endif
diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig
new file mode 100644
index 000000000..2a734c27e
--- /dev/null
+++ b/nuttx-configs/px4fmu-v2/nsh/defconfig
@@ -0,0 +1,1061 @@
+#
+# Automatically generated file; DO NOT EDIT.
+# Nuttx/ Configuration
+#
+CONFIG_NUTTX_NEWCONFIG=y
+
+#
+# Build Setup
+#
+# CONFIG_EXPERIMENTAL is not set
+# CONFIG_HOST_LINUX is not set
+CONFIG_HOST_OSX=y
+# 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=n
+CONFIG_DEBUG_VERBOSE=n
+
+#
+# Subsystem Debug Options
+#
+# CONFIG_DEBUG_MM is not set
+# CONFIG_DEBUG_SCHED is not set
+# CONFIG_DEBUG_USB is not set
+CONFIG_DEBUG_FS=y
+# CONFIG_DEBUG_LIB is not set
+# CONFIG_DEBUG_BINFMT is not set
+# CONFIG_DEBUG_GRAPHICS is not set
+
+#
+# Driver Debug Options
+#
+# CONFIG_DEBUG_ANALOG is not set
+# CONFIG_DEBUG_I2C is not set
+# CONFIG_DEBUG_SPI is not set
+# CONFIG_DEBUG_SDIO is not set
+# CONFIG_DEBUG_GPIO is not set
+CONFIG_DEBUG_DMA=y
+# CONFIG_DEBUG_WATCHDOG is not set
+# CONFIG_DEBUG_AUDIO is not set
+CONFIG_DEBUG_SYMBOLS=y
+
+#
+# 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
+# CONFIG_DEBUG_HARDFAULT is not set
+
+#
+# ARMV7M Configuration Options
+#
+# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set
+CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y
+CONFIG_ARMV7M_STACKCHECK=y
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_SDIO_DMA=y
+CONFIG_SDIO_DMAPRIO=0x00010000
+# CONFIG_SDIO_WIDTH_D1_ONLY is not set
+
+#
+# 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=y
+CONFIG_STM32_I2C2=y
+# CONFIG_STM32_I2C3 is not set
+CONFIG_STM32_OTGFS=y
+# CONFIG_STM32_OTGHS is not set
+CONFIG_STM32_PWR=y
+# CONFIG_STM32_RNG is not set
+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_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 is not set
+# 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=y
+CONFIG_STM32_TIM11=y
+# 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=y
+# CONFIG_STM32_UART5 is not set
+CONFIG_STM32_USART6=y
+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_TIM9_PWM is not set
+# CONFIG_STM32_TIM10_PWM is not set
+# CONFIG_STM32_TIM11_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_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_UART4_RS485 is not set
+CONFIG_UART4_RXDMA=y
+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_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
+
+#
+# SDIO Configuration
+#
+CONFIG_SDIO_PRI=128
+
+#
+# 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
+CONFIG_NSH_MMCSDSLOTNO=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=32
+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=y
+CONFIG_MMCSD_NSLOTS=1
+# CONFIG_MMCSD_READONLY is not set
+CONFIG_MMCSD_MULTIBLOCK_DISABLE=y
+# CONFIG_MMCSD_MMCSUPPORT is not set
+# CONFIG_MMCSD_HAVECARDDETECT is not set
+# CONFIG_MMCSD_SPI is not set
+CONFIG_MMCSD_SDIO=y
+# CONFIG_SDIO_MUXBUS is not set
+# CONFIG_SDIO_BLOCKSETUP 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_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_SERIAL_REMOVABLE=y
+# CONFIG_16550_UART is not set
+CONFIG_ARCH_HAVE_UART4=y
+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_ARCH_HAVE_USART6=y
+CONFIG_MCU_SERIAL=y
+CONFIG_STANDARD_SERIAL=y
+CONFIG_SERIAL_NPOLLWAITERS=2
+# CONFIG_SERIAL_TIOCSERGSTRUCT is not set
+# CONFIG_USART1_SERIAL_CONSOLE is not set
+# CONFIG_USART2_SERIAL_CONSOLE is not set
+# 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_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=57600
+CONFIG_USART2_BITS=8
+CONFIG_USART2_PARITY=0
+CONFIG_USART2_2STOP=0
+CONFIG_USART2_IFLOWCONTROL=y
+CONFIG_USART2_OFLOWCONTROL=y
+
+#
+# USART3 Configuration
+#
+CONFIG_USART3_RXBUFSIZE=512
+CONFIG_USART3_TXBUFSIZE=512
+CONFIG_USART3_BAUD=57600
+CONFIG_USART3_BITS=8
+CONFIG_USART3_PARITY=0
+CONFIG_USART3_2STOP=0
+CONFIG_USART3_IFLOWCONTROL=y
+CONFIG_USART3_OFLOWCONTROL=y
+
+#
+# UART4 Configuration
+#
+CONFIG_UART4_RXBUFSIZE=512
+CONFIG_UART4_TXBUFSIZE=512
+CONFIG_UART4_BAUD=57600
+CONFIG_UART4_BITS=8
+CONFIG_UART4_PARITY=0
+CONFIG_UART4_2STOP=0
+# CONFIG_UART4_IFLOWCONTROL is not set
+# CONFIG_UART4_OFLOWCONTROL is not set
+
+#
+# USART6 Configuration
+#
+CONFIG_USART6_RXBUFSIZE=512
+CONFIG_USART6_TXBUFSIZE=512
+CONFIG_USART6_BAUD=57600
+CONFIG_USART6_BITS=8
+CONFIG_USART6_PARITY=0
+CONFIG_USART6_2STOP=0
+# CONFIG_USART6_IFLOWCONTROL is not set
+# CONFIG_USART6_OFLOWCONTROL is not set
+
+#
+# UART7 Configuration
+#
+CONFIG_UART7_RXBUFSIZE=512
+CONFIG_UART7_TXBUFSIZE=512
+CONFIG_UART7_BAUD=57600
+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=57600
+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=y
+CONFIG_SERIAL_OFLOWCONTROL=y
+CONFIG_USBDEV=y
+
+#
+# USB Device Controller Driver Options
+#
+# CONFIG_USBDEV_ISOCHRONOUS is not set
+# CONFIG_USBDEV_DUALSPEED is not set
+# CONFIG_USBDEV_SELFPOWERED is not set
+CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_MAXPOWER=500
+# CONFIG_USBDEV_DMA is not set
+# CONFIG_USBDEV_TRACE is not set
+
+#
+# USB Device Class Driver Options
+#
+# CONFIG_USBDEV_COMPOSITE is not set
+# CONFIG_PL2303 is not set
+CONFIG_CDCACM=y
+# CONFIG_CDCACM_CONSOLE is not set
+CONFIG_CDCACM_EP0MAXPACKET=64
+CONFIG_CDCACM_EPINTIN=1
+CONFIG_CDCACM_EPINTIN_FSSIZE=64
+CONFIG_CDCACM_EPINTIN_HSSIZE=64
+CONFIG_CDCACM_EPBULKOUT=3
+CONFIG_CDCACM_EPBULKOUT_FSSIZE=64
+CONFIG_CDCACM_EPBULKOUT_HSSIZE=512
+CONFIG_CDCACM_EPBULKIN=2
+CONFIG_CDCACM_EPBULKIN_FSSIZE=64
+CONFIG_CDCACM_EPBULKIN_HSSIZE=512
+CONFIG_CDCACM_NWRREQS=4
+CONFIG_CDCACM_NRDREQS=4
+CONFIG_CDCACM_BULKIN_REQLEN=96
+CONFIG_CDCACM_RXBUFSIZE=512
+CONFIG_CDCACM_TXBUFSIZE=2048
+CONFIG_CDCACM_VENDORID=0x26ac
+CONFIG_CDCACM_PRODUCTID=0x0011
+CONFIG_CDCACM_VENDORSTR="3D Robotics"
+CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2.x"
+# CONFIG_USBMSC 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
+# CONFIG_DEBUG_GRAN 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=4000
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKPERIOD=50000
+CONFIG_SCHED_LPWORKSTACKSIZE=4000
+# 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_CDCACM=y
+# 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 is not set
+# 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_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
+# CONFIG_NSH_USBCONSOLE is not set
+
+#
+# USB Trace Support
+#
+# CONFIG_NSH_USBDEV_TRACE is not set
+# 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 is not set
+
+#
+# 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/nuttx-configs/px4fmu-v2/nsh/setenv.sh b/nuttx-configs/px4fmu-v2/nsh/setenv.sh
new file mode 100755
index 000000000..265520997
--- /dev/null
+++ b/nuttx-configs/px4fmu-v2/nsh/setenv.sh
@@ -0,0 +1,67 @@
+#!/bin/bash
+# configs/stm3240g-eval/nsh/setenv.sh
+#
+#   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.
+#
+
+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"
+
+# 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"
+
+# Add the path to the toolchain to the PATH varialble
+export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}"
+
+echo "PATH : ${PATH}"
diff --git a/nuttx-configs/px4fmu-v2/scripts/ld.script b/nuttx-configs/px4fmu-v2/scripts/ld.script
new file mode 100644
index 000000000..1be39fb87
--- /dev/null
+++ b/nuttx-configs/px4fmu-v2/scripts/ld.script
@@ -0,0 +1,150 @@
+/****************************************************************************
+ * configs/px4fmu/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/nuttx-configs/px4fmu-v2/src/Makefile b/nuttx-configs/px4fmu-v2/src/Makefile
new file mode 100644
index 000000000..d4276f7fc
--- /dev/null
+++ b/nuttx-configs/px4fmu-v2/src/Makefile
@@ -0,0 +1,84 @@
+############################################################################
+# configs/px4fmu/src/Makefile
+#
+#   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)/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 $@
+
+depend: .depend
+
+clean:
+	$(call DELFILE, libboard$(LIBEXT))
+	$(call CLEAN)
+
+distclean: clean
+	$(call DELFILE, Make.dep)
+	$(call DELFILE, .depend)
+
+-include Make.dep
+
diff --git a/nuttx-configs/px4fmu-v2/src/empty.c b/nuttx-configs/px4fmu-v2/src/empty.c
new file mode 100644
index 000000000..ace900866
--- /dev/null
+++ b/nuttx-configs/px4fmu-v2/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/px4io-v1/include/README.txt b/nuttx-configs/px4io-v1/include/README.txt
new file mode 100755
index 000000000..2264a80aa
--- /dev/null
+++ b/nuttx-configs/px4io-v1/include/README.txt
@@ -0,0 +1 @@
+This directory contains header files unique to the PX4IO board.
diff --git a/nuttx-configs/px4io-v1/include/board.h b/nuttx-configs/px4io-v1/include/board.h
new file mode 100755
index 000000000..815079266
--- /dev/null
+++ b/nuttx-configs/px4io-v1/include/board.h
@@ -0,0 +1,152 @@
+/************************************************************************************
+ * configs/px4io/include/board.h
+ * include/arch/board/board.h
+ *
+ *   Copyright (C) 2009 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *   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.
+ *
+ ************************************************************************************/
+
+#ifndef __ARCH_BOARD_BOARD_H
+#define __ARCH_BOARD_BOARD_H
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+#ifndef __ASSEMBLY__
+# include <stdint.h>
+# include <stdbool.h>
+#endif
+#include <stm32_rcc.h>
+#include <stm32_sdio.h>
+#include <stm32.h>
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/* Clocking *************************************************************************/
+
+/* On-board crystal frequency is 24MHz (HSE) */
+
+#define STM32_BOARD_XTAL        24000000ul
+
+/* Use the HSE output as the system clock */
+
+#define STM32_SYSCLK_SW         RCC_CFGR_SW_HSE
+#define STM32_SYSCLK_SWS        RCC_CFGR_SWS_HSE
+#define STM32_SYSCLK_FREQUENCY  STM32_BOARD_XTAL
+
+/* AHB clock (HCLK) is SYSCLK (24MHz) */
+
+#define STM32_RCC_CFGR_HPRE     RCC_CFGR_HPRE_SYSCLK
+#define STM32_HCLK_FREQUENCY    STM32_SYSCLK_FREQUENCY
+#define STM32_BOARD_HCLK        STM32_HCLK_FREQUENCY    /* same as above, to satisfy compiler */
+
+/* APB2 clock (PCLK2) is HCLK (24MHz) */
+
+#define STM32_RCC_CFGR_PPRE2    RCC_CFGR_PPRE2_HCLK
+#define STM32_PCLK2_FREQUENCY   STM32_HCLK_FREQUENCY
+#define STM32_APB2_CLKIN        (STM32_PCLK2_FREQUENCY)   /* Timers 2-4 */
+
+/* APB2 timer 1 will receive PCLK2. */
+
+#define STM32_APB2_TIM1_CLKIN   (STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM8_CLKIN   (STM32_PCLK2_FREQUENCY)
+
+/* APB1 clock (PCLK1) is HCLK (24MHz) */
+
+#define STM32_RCC_CFGR_PPRE1    RCC_CFGR_PPRE1_HCLK
+#define STM32_PCLK1_FREQUENCY   (STM32_HCLK_FREQUENCY)
+
+/* All timers run off PCLK */
+
+#define STM32_APB1_TIM1_CLKIN   (STM32_PCLK2_FREQUENCY)
+#define STM32_APB1_TIM2_CLKIN   (STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM3_CLKIN   (STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM4_CLKIN   (STM32_PCLK1_FREQUENCY)
+
+/*
+ * Some of the USART pins are not available; override the GPIO
+ * definitions with an invalid pin configuration.
+ */
+#undef GPIO_USART2_CTS
+#define GPIO_USART2_CTS	0xffffffff
+#undef GPIO_USART2_RTS
+#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
+#define GPIO_USART3_CTS	0xffffffff
+#undef GPIO_USART3_RTS
+#define GPIO_USART3_RTS	0xffffffff
+
+/************************************************************************************
+ * 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);
+    
+#if defined(__cplusplus)
+}
+#endif
+
+#endif /* __ASSEMBLY__ */
+#endif  /* __ARCH_BOARD_BOARD_H */
diff --git a/nuttx-configs/px4io-v1/nsh/Make.defs b/nuttx-configs/px4io-v1/nsh/Make.defs
new file mode 100644
index 000000000..712631f47
--- /dev/null
+++ b/nuttx-configs/px4io-v1/nsh/Make.defs
@@ -0,0 +1,166 @@
+############################################################################
+# configs/px4io-v1/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-m3 \
+			   -mthumb \
+			   -march=armv7-m
+
+# 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)
+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/px4io-v1/nsh/appconfig b/nuttx-configs/px4io-v1/nsh/appconfig
new file mode 100644
index 000000000..48a41bcdb
--- /dev/null
+++ b/nuttx-configs/px4io-v1/nsh/appconfig
@@ -0,0 +1,32 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
diff --git a/nuttx-configs/px4io-v1/nsh/defconfig b/nuttx-configs/px4io-v1/nsh/defconfig
new file mode 100755
index 000000000..3785e0367
--- /dev/null
+++ b/nuttx-configs/px4io-v1/nsh/defconfig
@@ -0,0 +1,537 @@
+############################################################################
+# configs/px4io-v1/nsh/defconfig
+#
+#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#   Copyright (C) 2011-2012 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.
+#
+############################################################################
+#
+# architecture selection
+#
+# CONFIG_ARCH - identifies the arch subdirectory and, hence, the
+#   processor architecture.
+# CONFIG_ARCH_family - for use in C code.  This identifies the
+#   particular chip family that the architecture is implemented
+#   in.
+# CONFIG_ARCH_architecture - for use in C code.  This identifies the
+#   specific architecture within the chip family.
+# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory
+# CONFIG_ARCH_CHIP_name - For use in C code
+# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence,
+#   the board that supports the particular chip or SoC.
+# CONFIG_ARCH_BOARD_name - for use in C code
+# CONFIG_ENDIAN_BIG - define if big endian (default is little endian)
+# CONFIG_BOARD_LOOPSPERMSEC - for delay loops
+# CONFIG_DRAM_SIZE - Describes the installed DRAM.
+# CONFIG_DRAM_START - The start address of DRAM (physical)
+# CONFIG_ARCH_IRQPRIO - The ST32F100CB supports interrupt prioritization
+# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt
+#   stack. If defined, this symbol is the size of the interrupt
+#   stack in bytes.  If not defined, the user task stacks will be
+#   used during interrupt handling.
+# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions
+# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader.
+# CONFIG_ARCH_LEDS -  Use LEDs to show state. Unique to board architecture.
+# CONFIG_ARCH_BUTTONS -  Enable support for buttons. Unique to board architecture.
+# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that
+#   cause a 100 second delay during boot-up.  This 100 second delay
+#   serves no purpose other than it allows you to calibrate
+#   CONFIG_BOARD_LOOPSPERMSEC.  You simply use a stop watch to measure
+#   the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until
+#   the delay actually is 100 seconds.
+# CONFIG_ARCH_DMA - Support DMA initialization
+#
+CONFIG_ARCH="arm"
+CONFIG_ARCH_ARM=y
+CONFIG_ARCH_CORTEXM3=y
+CONFIG_ARCH_CHIP="stm32"
+CONFIG_ARCH_CHIP_STM32F100C8=y
+#
+# Board Selection
+#
+CONFIG_ARCH_BOARD_PX4IO_V1=y
+# CONFIG_ARCH_BOARD_CUSTOM is not set
+CONFIG_ARCH_BOARD="px4io-v1"
+CONFIG_BOARD_LOOPSPERMSEC=2000
+CONFIG_DRAM_SIZE=0x00002000
+CONFIG_DRAM_START=0x20000000
+CONFIG_ARCH_IRQPRIO=y
+CONFIG_ARCH_INTERRUPTSTACK=n
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARCH_BOOTLOADER=n
+CONFIG_ARCH_LEDS=n
+CONFIG_ARCH_BUTTONS=n
+CONFIG_ARCH_CALIBRATION=n
+CONFIG_ARCH_DMA=y
+CONFIG_ARCH_MATH_H=y
+
+CONFIG_ARMV7M_CMNVECTOR=y
+# CONFIG_ARMV7M_STACKCHECK is not set
+
+#
+# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled):
+#
+#   CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG
+#
+# JTAG Enable options:
+#
+#   CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP)
+#   CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP)
+#     but without JNTRST.
+#   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_NOJNTRST_ENABLE=n
+CONFIG_STM32_JTAG_SW_ENABLE=n
+
+#
+# Individual subsystems can be enabled:
+#
+# AHB: 
+CONFIG_STM32_DMA1=y
+CONFIG_STM32_DMA2=n
+CONFIG_STM32_CRC=n
+# APB1:
+# Timers 2,3 and 4 are owned by the PWM driver
+CONFIG_STM32_TIM2=n
+CONFIG_STM32_TIM3=n
+CONFIG_STM32_TIM4=n
+CONFIG_STM32_TIM5=n
+CONFIG_STM32_TIM6=n
+CONFIG_STM32_TIM7=n
+CONFIG_STM32_WWDG=n
+CONFIG_STM32_SPI2=n
+CONFIG_STM32_USART2=y
+CONFIG_STM32_USART3=y
+CONFIG_STM32_I2C1=y
+CONFIG_STM32_I2C2=n
+CONFIG_STM32_BKP=n
+CONFIG_STM32_PWR=n
+CONFIG_STM32_DAC=n
+# APB2:
+# We use our own ADC driver, but leave this on for clocking purposes.
+CONFIG_STM32_ADC1=y
+CONFIG_STM32_ADC2=n
+# TIM1 is owned by the HRT
+CONFIG_STM32_TIM1=n
+CONFIG_STM32_SPI1=n
+CONFIG_STM32_TIM8=n
+CONFIG_STM32_USART1=y
+CONFIG_STM32_ADC3=n
+
+
+#
+# STM32F100 specific serial device driver settings
+#
+# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the
+#   console and ttys0 (default is the USART1).
+# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received.
+#   This specific the size of the receive buffer
+# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before
+#   being sent.  This specific the size of the transmit buffer
+# CONFIG_USARTn_BAUD - The configure BAUD of the UART.  Must be
+# CONFIG_USARTn_BITS - The number of bits.  Must be either 7 or 8.
+# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity
+# CONFIG_USARTn_2STOP - Two stop bits
+#
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_STANDARD_SERIAL=y
+
+CONFIG_USART1_SERIAL_CONSOLE=y
+CONFIG_USART2_SERIAL_CONSOLE=n
+CONFIG_USART3_SERIAL_CONSOLE=n
+
+CONFIG_USART1_TXBUFSIZE=64
+CONFIG_USART2_TXBUFSIZE=64
+CONFIG_USART3_TXBUFSIZE=64
+
+CONFIG_USART1_RXBUFSIZE=64
+CONFIG_USART2_RXBUFSIZE=64
+CONFIG_USART3_RXBUFSIZE=64
+
+CONFIG_USART1_BAUD=115200
+CONFIG_USART2_BAUD=115200
+CONFIG_USART3_BAUD=115200
+
+CONFIG_USART1_BITS=8
+CONFIG_USART2_BITS=8
+CONFIG_USART3_BITS=8
+
+CONFIG_USART1_PARITY=0
+CONFIG_USART2_PARITY=0
+CONFIG_USART3_PARITY=0
+
+CONFIG_USART1_2STOP=0
+CONFIG_USART2_2STOP=0
+CONFIG_USART3_2STOP=0
+
+CONFIG_USART1_RXDMA=y
+SERIAL_HAVE_CONSOLE_DMA=y
+# Conflicts with I2C1 DMA
+CONFIG_USART2_RXDMA=n
+CONFIG_USART3_RXDMA=y
+
+#
+# General build options
+#
+# CONFIG_RRLOAD_BINARY - make the rrload binary format used with
+#   BSPs from www.ridgerun.com using the tools/mkimage.sh script
+# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format
+#   used with many different loaders using the GNU objcopy program
+#   Should not be selected if you are not using the GNU toolchain.
+# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format
+#   used with many different loaders using the GNU objcopy program
+#   Should not be selected if you are not using the GNU toolchain.
+# CONFIG_RAW_BINARY - make a raw binary format file used with many
+#   different loaders using the GNU objcopy program.  This option
+#   should not be selected if you are not using the GNU toolchain.
+# CONFIG_HAVE_LIBM - toolchain supports libm.a
+#
+CONFIG_RRLOAD_BINARY=n
+CONFIG_INTELHEX_BINARY=n
+CONFIG_MOTOROLA_SREC=n
+CONFIG_RAW_BINARY=y
+CONFIG_HAVE_LIBM=n
+
+#
+# General OS setup
+#
+# CONFIG_APPS_DIR - Identifies the relative path to the directory
+#   that builds the application to link with NuttX.  Default: ../apps
+# CONFIG_DEBUG - enables built-in debug options
+# CONFIG_DEBUG_VERBOSE - enables verbose debug output
+# CONFIG_DEBUG_SYMBOLS - build without optimization and with
+#   debug symbols (needed for use with a debugger).
+# CONFIG_HAVE_CXX - Enable support for C++
+# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support
+#   for initialization of static C++ instances for this architecture
+#   and for the selected toolchain (via up_cxxinitialize()).
+# CONFIG_MM_REGIONS - If the architecture includes multiple
+#   regions of memory to allocate from, this specifies the
+#   number of memory regions that the memory manager must
+#   handle and enables the API mm_addregion(start, end);
+# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot
+#   time console output
+# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz
+#   or MSEC_PER_TICK=10.  This setting may be defined to
+#   inform NuttX that the processor hardware is providing
+#   system timer interrupts at some interrupt interval other
+#   than 10 msec.
+# CONFIG_RR_INTERVAL - The round robin timeslice will be set
+#   this number of milliseconds;  Round robin scheduling can
+#   be disabled by setting this value to zero.
+# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in 
+#   scheduler to monitor system performance
+# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a
+#   task name to save in the TCB.  Useful if scheduler
+#   instrumentation is selected.  Set to zero to disable.
+# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY -
+#   Used to initialize the internal time logic.
+# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions.
+#   You would only need this if you are concerned about accurate
+#   time conversions in the past or in the distant future.
+# CONFIG_JULIAN_TIME - Enables Julian time conversions. You
+#   would only need this if you are concerned about accurate
+#   time conversion in the distand past.  You must also define
+#   CONFIG_GREGORIAN_TIME in order to use Julian time.
+# CONFIG_DEV_CONSOLE - Set if architecture-specific logic
+#   provides /dev/console.  Enables stdout, stderr, stdin.
+# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console
+#   driver (minimul support)
+# CONFIG_MUTEX_TYPES: Set to enable support for recursive and
+#   errorcheck mutexes. Enables pthread_mutexattr_settype().
+# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority
+#   inheritance on mutexes and semaphores. 
+# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority
+#   inheritance is enabled.  It defines the maximum number of
+#   different threads (minus one) that can take counts on a
+#   semaphore with priority inheritance support.  This may be 
+#   set to zero if priority inheritance is disabled OR if you
+#   are only using semaphores as mutexes (only one holder) OR
+#   if no more than two threads participate using a counting
+#   semaphore.
+# CONFIG_SEM_NNESTPRIO.  If priority inheritance is enabled,
+#   then this setting is the maximum number of higher priority
+#   threads (minus 1) than can be waiting for another thread
+#   to release a count on a semaphore.  This value may be set
+#   to zero if no more than one thread is expected to wait for
+#   a semaphore.
+# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors
+#   by task_create() when a new task is started.  If set, all
+#   files/drivers will appear to be closed in the new task.
+# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first
+#   three file descriptors (stdin, stdout, stderr) by task_create()
+#   when a new task is started. If set, all files/drivers will
+#   appear to be closed in the new task except for stdin, stdout,
+#   and stderr.
+# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket
+#   desciptors by task_create() when a new task is started. If
+#   set, all sockets will appear to be closed in the new task.
+# CONFIG_SCHED_WORKQUEUE.  Create a dedicated "worker" thread to
+#  handle delayed processing from interrupt handlers.  This feature
+#  is required for some drivers but, if there are not complaints,
+#  can be safely disabled.  The worker thread also performs
+#  garbage collection -- completing any delayed memory deallocations
+#  from interrupt handlers.  If the worker thread is disabled,
+#  then that clean will be performed by the IDLE thread instead
+#  (which runs at the lowest of priority and may not be appropriate
+#  if memory reclamation is of high priority).  If CONFIG_SCHED_WORKQUEUE
+#  is enabled, then the following options can also be used:
+# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker
+#  thread.  Default: 50
+# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for
+#  work in units of microseconds.  Default: 50*1000 (50 MS).
+# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker
+#  thread.  Default: CONFIG_IDLETHREAD_STACKSIZE.
+# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up
+#  the worker thread.  Default: 4
+# CONFIG_SCHED_WAITPID - Enable the waitpid() API
+# CONFIG_SCHED_ATEXIT - Enabled the atexit() API
+#
+CONFIG_USER_ENTRYPOINT="user_start"
+#CONFIG_APPS_DIR=
+CONFIG_DEBUG=n
+CONFIG_DEBUG_VERBOSE=n
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEBUG_FS=n
+CONFIG_DEBUG_GRAPHICS=n
+CONFIG_DEBUG_LCD=n
+CONFIG_DEBUG_USB=n
+CONFIG_DEBUG_NET=n
+CONFIG_DEBUG_RTC=n
+CONFIG_DEBUG_ANALOG=n
+CONFIG_DEBUG_PWM=n
+CONFIG_DEBUG_CAN=n
+CONFIG_DEBUG_I2C=n
+CONFIG_DEBUG_INPUT=n
+
+CONFIG_MSEC_PER_TICK=1
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_MM_REGIONS=1
+CONFIG_MM_SMALL=y
+CONFIG_ARCH_LOWPUTC=y
+CONFIG_RR_INTERVAL=0
+CONFIG_SCHED_INSTRUMENTATION=n
+CONFIG_TASK_NAME_SIZE=8
+CONFIG_START_YEAR=1970
+CONFIG_START_MONTH=1
+CONFIG_START_DAY=1
+CONFIG_GREGORIAN_TIME=n
+CONFIG_JULIAN_TIME=n
+# this eats ~1KiB of RAM ... work out why
+CONFIG_DEV_CONSOLE=y
+CONFIG_DEV_LOWCONSOLE=n
+CONFIG_MUTEX_TYPES=n
+CONFIG_PRIORITY_INHERITANCE=n
+CONFIG_SEM_PREALLOCHOLDERS=0
+CONFIG_SEM_NNESTPRIO=0
+CONFIG_FDCLONE_DISABLE=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SCHED_WORKQUEUE=n
+CONFIG_SCHED_WORKPRIORITY=50
+CONFIG_SCHED_WORKPERIOD=50000
+CONFIG_SCHED_WORKSTACKSIZE=1024
+CONFIG_SIG_SIGWORK=4
+CONFIG_SCHED_WAITPID=n
+CONFIG_SCHED_ATEXIT=n
+
+#
+# The following can be used to disable categories of
+# APIs supported by the OS.  If the compiler supports
+# weak functions, then it should not be necessary to
+# disable functions unless you want to restrict usage
+# of those APIs.
+#
+# There are certain dependency relationships in these
+# features.
+#
+# o mq_notify logic depends on signals to awaken tasks
+#   waiting for queues to become full or empty.
+# o pthread_condtimedwait() depends on signals to wake
+#   up waiting tasks.
+#
+CONFIG_DISABLE_CLOCK=n
+CONFIG_DISABLE_POSIX_TIMERS=y
+CONFIG_DISABLE_PTHREAD=y
+CONFIG_DISABLE_SIGNALS=y
+CONFIG_DISABLE_MQUEUE=y
+CONFIG_DISABLE_MOUNTPOINT=y
+CONFIG_DISABLE_ENVIRON=y
+CONFIG_DISABLE_POLL=y
+
+#
+# Misc libc settings
+#
+# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a
+#   little smaller if we do not support fieldwidthes
+#
+CONFIG_NOPRINTF_FIELDWIDTH=n
+
+#
+# Allow for architecture optimized implementations
+#
+# The architecture can provide optimized versions of the
+# following to improve system performance
+#
+CONFIG_ARCH_MEMCPY=n
+CONFIG_ARCH_MEMCMP=n
+CONFIG_ARCH_MEMMOVE=n
+CONFIG_ARCH_MEMSET=n
+CONFIG_ARCH_STRCMP=n
+CONFIG_ARCH_STRCPY=n
+CONFIG_ARCH_STRNCPY=n
+CONFIG_ARCH_STRLEN=n
+CONFIG_ARCH_STRNLEN=n
+CONFIG_ARCH_BZERO=n
+
+#
+# Sizes of configurable things (0 disables)
+#
+# CONFIG_MAX_TASKS - The maximum number of simultaneously
+#   active tasks.  This value must be a power of two.
+# CONFIG_MAX_TASK_ARGS - This controls the maximum number of
+#   of parameters that a task may receive (i.e., maxmum value
+#   of 'argc')
+# CONFIG_NPTHREAD_KEYS - The number of items of thread-
+#   specific data that can be retained
+# CONFIG_NFILE_DESCRIPTORS - The maximum number of file
+#   descriptors (one for each open)
+# CONFIG_NFILE_STREAMS - The maximum number of streams that
+#   can be fopen'ed
+# CONFIG_NAME_MAX - The maximum size of a file name.
+# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate
+#   on fopen. (Only if CONFIG_NFILE_STREAMS > 0)
+# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled
+#   (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added
+#   to force automatic, line-oriented flushing the output buffer
+#   for putc(), fputc(), putchar(), puts(), fputs(), printf(),
+#   fprintf(), and vfprintf().  When a newline is encountered in
+#   the output string, the output buffer will be flushed.  This
+#   (slightly) increases the NuttX footprint but supports the kind
+#   of behavior that people expect for printf().
+# CONFIG_NUNGET_CHARS - Number of characters that can be
+#   buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0)
+# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message
+#   structures.  The system manages a pool of preallocated
+#   message structures to minimize dynamic allocations
+# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with
+#   a fixed payload size given by this settin (does not include
+#   other message structure overhead.
+# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that
+#   can be passed to a watchdog handler
+# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog
+#   structures.  The system manages a pool of preallocated
+#   watchdog structures to minimize dynamic allocations
+# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX
+#   timer structures.  The system manages a pool of preallocated
+#   timer structures to minimize dynamic allocations.  Set to
+#   zero for all dynamic allocations.
+#
+CONFIG_MAX_TASKS=4
+CONFIG_MAX_TASK_ARGS=4
+CONFIG_NPTHREAD_KEYS=2
+CONFIG_NFILE_DESCRIPTORS=8
+CONFIG_NFILE_STREAMS=0
+CONFIG_NAME_MAX=12
+CONFIG_STDIO_BUFFER_SIZE=32
+CONFIG_STDIO_LINEBUFFER=n
+CONFIG_NUNGET_CHARS=2
+CONFIG_PREALLOC_MQ_MSGS=4
+CONFIG_MQ_MAXMSGSIZE=32
+CONFIG_MAX_WDOGPARMS=2
+CONFIG_PREALLOC_WDOGS=4
+CONFIG_PREALLOC_TIMERS=0
+
+
+#
+# Settings for apps/nshlib
+#
+# CONFIG_NSH_BUILTIN_APPS - Support external registered,
+#   "named" applications that can be executed from the NSH
+#   command line (see apps/README.txt for more information).
+# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer
+# CONFIG_NSH_STRERROR - Use strerror(errno)
+# CONFIG_NSH_LINELEN - Maximum length of one command line
+# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi
+# CONFIG_NSH_DISABLESCRIPT - Disable scripting support
+# CONFIG_NSH_DISABLEBG - Disable background commands
+# CONFIG_NSH_ROMFSETC - Use startup script in /etc
+# CONFIG_NSH_CONSOLE - Use serial console front end
+# CONFIG_NSH_TELNET - Use telnetd console front end
+# CONFIG_NSH_ARCHINIT - Platform provides architecture
+#   specific initialization (nsh_archinitialize()).
+#
+
+# Disable NSH completely
+CONFIG_NSH_CONSOLE=n
+
+#
+# Stack and heap information
+#
+# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP
+#   operation from FLASH but must copy initialized .data sections to RAM.
+#   (should also be =n for the STM3210E-EVAL which always runs from flash)
+# CONFIG_BOOT_COPYTORAM -  Some configurations boot in FLASH
+#   but copy themselves entirely into RAM for better performance.
+# CONFIG_CUSTOM_STACK - The up_ implementation will handle
+#   all stack operations outside of the nuttx model.
+# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only)
+# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack.
+#  This is the thread that (1) performs the inital boot of the system up
+#  to the point where user_start() is spawned, and (2) there after is the
+#  IDLE thread that executes only when there is no other thread ready to
+#  run.
+# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate
+#  for the main user thread that begins at the user_start() entry point.
+# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size
+# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size
+# CONFIG_HEAP_BASE - The beginning of the heap
+# CONFIG_HEAP_SIZE - The size of the heap
+#
+CONFIG_BOOT_RUNFROMFLASH=n
+CONFIG_BOOT_COPYTORAM=n
+CONFIG_CUSTOM_STACK=n
+CONFIG_STACK_POINTER=
+CONFIG_IDLETHREAD_STACKSIZE=1024
+CONFIG_USERMAIN_STACKSIZE=1200
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_PTHREAD_STACK_DEFAULT=1024
+CONFIG_HEAP_BASE=
+CONFIG_HEAP_SIZE=
+
+#
+# NSH Library
+#
+# CONFIG_NSH_LIBRARY is not set
+# CONFIG_NSH_BUILTIN_APPS is not set
diff --git a/nuttx-configs/px4io-v1/nsh/setenv.sh b/nuttx-configs/px4io-v1/nsh/setenv.sh
new file mode 100755
index 000000000..ff9a4bf8a
--- /dev/null
+++ b/nuttx-configs/px4io-v1/nsh/setenv.sh
@@ -0,0 +1,47 @@
+#!/bin/bash
+# configs/stm3210e-eval/dfu/setenv.sh
+#
+#   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.
+#
+
+if [ "$(basename $0)" = "setenv.sh" ] ; then
+  echo "You must source this script, not run it!" 1>&2
+  exit 1
+fi
+
+if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi
+
+WD=`pwd`
+export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin"
+export BUILDROOT_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin"
+export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}"
+
+echo "PATH : ${PATH}"
diff --git a/nuttx-configs/px4io-v1/scripts/ld.script b/nuttx-configs/px4io-v1/scripts/ld.script
new file mode 100755
index 000000000..69c2f9cb2
--- /dev/null
+++ b/nuttx-configs/px4io-v1/scripts/ld.script
@@ -0,0 +1,129 @@
+/****************************************************************************
+ * configs/stm3210e-eval/nsh/ld.script
+ *
+ *   Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * 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 STM32F100C8 has 64Kb of FLASH beginning at address 0x0800:0000 and
+ * 8Kb of SRAM beginning at address 0x2000: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.
+ */
+
+MEMORY
+{
+    flash (rx) : ORIGIN = 0x08001000, LENGTH = 60K
+    sram (rwx) : ORIGIN = 0x20000000, LENGTH = 8K
+}
+
+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.*)
+		*(.glue_7)
+		*(.glue_7t)
+		*(.got)
+		*(.gcc_except_table)
+		*(.gnu.linkonce.r.*)
+		_etext = ABSOLUTE(.);
+	} > flash
+
+	/*
+	 * Init functions (static constructors and the like)
+	 */
+        .init_section : {
+                _sinit = ABSOLUTE(.);
+                KEEP(*(.init_array .init_array.*))
+                _einit = ABSOLUTE(.);
+        } > flash
+
+	.ARM.extab : {
+		*(.ARM.extab*)
+	} > flash
+
+	__exidx_start = ABSOLUTE(.);
+	.ARM.exidx : {
+		*(.ARM.exidx*)
+	} > flash
+	__exidx_end = ABSOLUTE(.);
+
+	_eronly = ABSOLUTE(.);
+
+	/* The STM32F100CB has 8Kb of SRAM beginning at the following address */
+
+	.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/nuttx-configs/px4io-v1/src/Makefile b/nuttx-configs/px4io-v1/src/Makefile
new file mode 100644
index 000000000..bb9539d16
--- /dev/null
+++ b/nuttx-configs/px4io-v1/src/Makefile
@@ -0,0 +1,84 @@
+############################################################################
+# configs/stm3210e-eval/src/Makefile
+#
+#   Copyright (C) 2009-2010 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+#
+# 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)/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)
+	@$(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
+	@touch $@
+
+depend: .depend
+
+clean:
+	$(call DELFILE, libboard$(LIBEXT))
+	$(call CLEAN)
+
+distclean: clean
+	$(call DELFILE, Make.dep)
+	$(call DELFILE, .depend)
+
+-include Make.dep
diff --git a/nuttx-configs/px4io-v1/src/empty.c b/nuttx-configs/px4io-v1/src/empty.c
new file mode 100644
index 000000000..ace900866
--- /dev/null
+++ b/nuttx-configs/px4io-v1/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/px4io-v2/include/board.h b/nuttx-configs/px4io-v2/include/board.h
new file mode 100755
index 000000000..4b9c90638
--- /dev/null
+++ b/nuttx-configs/px4io-v2/include/board.h
@@ -0,0 +1,149 @@
+/************************************************************************************
+ * configs/px4io/include/board.h
+ * include/arch/board/board.h
+ *
+ *   Copyright (C) 2009 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *   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.
+ *
+ ************************************************************************************/
+
+#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 *************************************************************************/
+
+/* On-board crystal frequency is 24MHz (HSE) */
+
+#define STM32_BOARD_XTAL        24000000ul
+
+/* Use the HSE output as the system clock */
+
+#define STM32_SYSCLK_SW         RCC_CFGR_SW_HSE
+#define STM32_SYSCLK_SWS        RCC_CFGR_SWS_HSE
+#define STM32_SYSCLK_FREQUENCY  STM32_BOARD_XTAL
+
+/* AHB clock (HCLK) is SYSCLK (24MHz) */
+
+#define STM32_RCC_CFGR_HPRE     RCC_CFGR_HPRE_SYSCLK
+#define STM32_HCLK_FREQUENCY    STM32_SYSCLK_FREQUENCY
+#define STM32_BOARD_HCLK        STM32_HCLK_FREQUENCY    /* same as above, to satisfy compiler */
+
+/* APB2 clock (PCLK2) is HCLK (24MHz) */
+
+#define STM32_RCC_CFGR_PPRE2    RCC_CFGR_PPRE2_HCLK
+#define STM32_PCLK2_FREQUENCY   STM32_HCLK_FREQUENCY
+#define STM32_APB2_CLKIN        (STM32_PCLK2_FREQUENCY)   /* Timers 2-4 */
+
+/* APB2 timer 1 will receive PCLK2. */
+
+#define STM32_APB2_TIM1_CLKIN   (STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM8_CLKIN   (STM32_PCLK2_FREQUENCY)
+
+/* APB1 clock (PCLK1) is HCLK (24MHz) */
+
+#define STM32_RCC_CFGR_PPRE1    RCC_CFGR_PPRE1_HCLK
+#define STM32_PCLK1_FREQUENCY   (STM32_HCLK_FREQUENCY)
+
+/* All timers run off PCLK */
+
+#define STM32_APB1_TIM1_CLKIN   (STM32_PCLK2_FREQUENCY)
+#define STM32_APB1_TIM2_CLKIN   (STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM3_CLKIN   (STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM4_CLKIN   (STM32_PCLK1_FREQUENCY)
+
+/*
+ * Some of the USART pins are not available; override the GPIO
+ * definitions with an invalid pin configuration.
+ */
+#undef GPIO_USART2_CTS
+#define GPIO_USART2_CTS	0xffffffff
+#undef GPIO_USART2_RTS
+#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
+#define GPIO_USART3_CTS	0xffffffff
+#undef GPIO_USART3_RTS
+#define GPIO_USART3_RTS	0xffffffff
+
+/************************************************************************************
+ * 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);
+    
+#if defined(__cplusplus)
+}
+#endif
+
+#endif /* __ASSEMBLY__ */
+#endif  /* __ARCH_BOARD_BOARD_H */
diff --git a/nuttx-configs/px4io-v2/nsh/Make.defs b/nuttx-configs/px4io-v2/nsh/Make.defs
new file mode 100644
index 000000000..cd2d8eba3
--- /dev/null
+++ b/nuttx-configs/px4io-v2/nsh/Make.defs
@@ -0,0 +1,170 @@
+############################################################################
+# configs/px4io-v2/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-m3 \
+			   -mthumb \
+			   -march=armv7-m
+
+# enable precise stack overflow tracking
+#INSTRUMENTATIONDEFINES	 = -finstrument-functions \
+#			   -ffixed-r10
+
+# 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)
+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/px4io-v2/nsh/appconfig b/nuttx-configs/px4io-v2/nsh/appconfig
new file mode 100644
index 000000000..48a41bcdb
--- /dev/null
+++ b/nuttx-configs/px4io-v2/nsh/appconfig
@@ -0,0 +1,32 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig
new file mode 100755
index 000000000..4111e70eb
--- /dev/null
+++ b/nuttx-configs/px4io-v2/nsh/defconfig
@@ -0,0 +1,548 @@
+############################################################################
+# configs/px4io/nsh/defconfig
+#
+#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#   Copyright (C) 2011-2012 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.
+#
+############################################################################
+#
+# architecture selection
+#
+# CONFIG_ARCH - identifies the arch subdirectory and, hence, the
+#   processor architecture.
+# CONFIG_ARCH_family - for use in C code.  This identifies the
+#   particular chip family that the architecture is implemented
+#   in.
+# CONFIG_ARCH_architecture - for use in C code.  This identifies the
+#   specific architecture within the chip family.
+# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory
+# CONFIG_ARCH_CHIP_name - For use in C code
+# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence,
+#   the board that supports the particular chip or SoC.
+# CONFIG_ARCH_BOARD_name - for use in C code
+# CONFIG_ENDIAN_BIG - define if big endian (default is little endian)
+# CONFIG_BOARD_LOOPSPERMSEC - for delay loops
+# CONFIG_DRAM_SIZE - Describes the installed DRAM.
+# CONFIG_DRAM_START - The start address of DRAM (physical)
+# CONFIG_ARCH_IRQPRIO - The ST32F100CB supports interrupt prioritization
+# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt
+#   stack. If defined, this symbol is the size of the interrupt
+#   stack in bytes.  If not defined, the user task stacks will be
+#   used during interrupt handling.
+# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions
+# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader.
+# CONFIG_ARCH_LEDS -  Use LEDs to show state. Unique to board architecture.
+# CONFIG_ARCH_BUTTONS -  Enable support for buttons. Unique to board architecture.
+# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that
+#   cause a 100 second delay during boot-up.  This 100 second delay
+#   serves no purpose other than it allows you to calibrate
+#   CONFIG_BOARD_LOOPSPERMSEC.  You simply use a stop watch to measure
+#   the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until
+#   the delay actually is 100 seconds.
+# CONFIG_ARCH_DMA - Support DMA initialization
+#
+CONFIG_ARCH="arm"
+CONFIG_ARCH_ARM=y
+CONFIG_ARCH_CORTEXM3=y
+CONFIG_ARCH_CHIP="stm32"
+CONFIG_ARCH_CHIP_STM32F100C8=y
+CONFIG_ARCH_BOARD="px4io-v2"
+CONFIG_ARCH_BOARD_PX4IO_V2=y
+CONFIG_BOARD_LOOPSPERMSEC=2000
+CONFIG_DRAM_SIZE=0x00002000
+CONFIG_DRAM_START=0x20000000
+CONFIG_ARCH_IRQPRIO=y
+CONFIG_ARCH_INTERRUPTSTACK=n
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARCH_BOOTLOADER=n
+CONFIG_ARCH_LEDS=n
+CONFIG_ARCH_BUTTONS=n
+CONFIG_ARCH_CALIBRATION=n
+CONFIG_ARCH_DMA=y
+CONFIG_ARCH_MATH_H=y
+
+CONFIG_ARMV7M_CMNVECTOR=y
+
+#
+# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled):
+#
+#   CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG
+#
+# JTAG Enable options:
+#
+#   CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP)
+#   CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP)
+#     but without JNTRST.
+#   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_NOJNTRST_ENABLE=n
+CONFIG_STM32_JTAG_SW_ENABLE=n
+
+#
+# Individual subsystems can be enabled:
+#
+# AHB: 
+CONFIG_STM32_DMA1=y
+CONFIG_STM32_DMA2=n
+CONFIG_STM32_CRC=n
+# APB1:
+# Timers 2,3 and 4 are owned by the PWM driver
+CONFIG_STM32_TIM2=n
+CONFIG_STM32_TIM3=n
+CONFIG_STM32_TIM4=n
+CONFIG_STM32_TIM5=n
+CONFIG_STM32_TIM6=n
+CONFIG_STM32_TIM7=n
+CONFIG_STM32_WWDG=n
+CONFIG_STM32_SPI2=n
+CONFIG_STM32_USART2=y
+CONFIG_STM32_USART3=y
+CONFIG_STM32_I2C1=n
+CONFIG_STM32_I2C2=n
+CONFIG_STM32_BKP=n
+CONFIG_STM32_PWR=n
+CONFIG_STM32_DAC=n
+# APB2:
+# We use our own ADC driver, but leave this on for clocking purposes.
+CONFIG_STM32_ADC1=y
+CONFIG_STM32_ADC2=n
+# TIM1 is owned by the HRT
+CONFIG_STM32_TIM1=n
+CONFIG_STM32_SPI1=n
+CONFIG_STM32_TIM8=n
+CONFIG_STM32_USART1=y
+CONFIG_STM32_ADC3=n
+
+
+#
+# STM32F100 specific serial device driver settings
+#
+# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the
+#   console and ttys0 (default is the USART1).
+# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received.
+#   This specific the size of the receive buffer
+# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before
+#   being sent.  This specific the size of the transmit buffer
+# CONFIG_USARTn_BAUD - The configure BAUD of the UART.  Must be
+# CONFIG_USARTn_BITS - The number of bits.  Must be either 7 or 8.
+# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity
+# CONFIG_USARTn_2STOP - Two stop bits
+#
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_STANDARD_SERIAL=y
+
+CONFIG_USART1_SERIAL_CONSOLE=y
+CONFIG_USART2_SERIAL_CONSOLE=n
+CONFIG_USART3_SERIAL_CONSOLE=n
+
+CONFIG_USART1_TXBUFSIZE=64
+CONFIG_USART2_TXBUFSIZE=64
+CONFIG_USART3_TXBUFSIZE=64
+
+CONFIG_USART1_RXBUFSIZE=64
+CONFIG_USART2_RXBUFSIZE=64
+CONFIG_USART3_RXBUFSIZE=64
+
+CONFIG_USART1_BAUD=115200
+CONFIG_USART2_BAUD=115200
+CONFIG_USART3_BAUD=115200
+
+CONFIG_USART1_BITS=8
+CONFIG_USART2_BITS=8
+CONFIG_USART3_BITS=8
+
+CONFIG_USART1_PARITY=0
+CONFIG_USART2_PARITY=0
+CONFIG_USART3_PARITY=0
+
+CONFIG_USART1_2STOP=0
+CONFIG_USART2_2STOP=0
+CONFIG_USART3_2STOP=0
+
+CONFIG_USART1_RXDMA=y
+SERIAL_HAVE_CONSOLE_DMA=y
+# Conflicts with I2C1 DMA
+CONFIG_USART2_RXDMA=n
+CONFIG_USART3_RXDMA=y
+
+#
+# PX4IO specific driver settings
+#
+# CONFIG_HRT_TIMER
+#   Enables the high-resolution timer.  The board definition must
+#   set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/
+#   compare channels to be used.
+# CONFIG_HRT_PPM
+#   Enables R/C PPM input using the HRT.  The board definition must
+#   set HRT_PPM_CHANNEL to the timer capture/compare channel to be
+#   used, and define GPIO_PPM_IN to configure the appropriate timer
+#   GPIO.
+# CONFIG_PWM_SERVO
+#   Enables the PWM servo driver.  The driver configuration must be
+#   supplied by the board support at initialisation time.
+#   Note that USART2 must be disabled on the PX4 board for this to
+#   be available.
+#
+#
+CONFIG_HRT_TIMER=y
+CONFIG_HRT_PPM=y
+
+#
+# General build options
+#
+# CONFIG_RRLOAD_BINARY - make the rrload binary format used with
+#   BSPs from www.ridgerun.com using the tools/mkimage.sh script
+# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format
+#   used with many different loaders using the GNU objcopy program
+#   Should not be selected if you are not using the GNU toolchain.
+# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format
+#   used with many different loaders using the GNU objcopy program
+#   Should not be selected if you are not using the GNU toolchain.
+# CONFIG_RAW_BINARY - make a raw binary format file used with many
+#   different loaders using the GNU objcopy program.  This option
+#   should not be selected if you are not using the GNU toolchain.
+# CONFIG_HAVE_LIBM - toolchain supports libm.a
+#
+CONFIG_RRLOAD_BINARY=n
+CONFIG_INTELHEX_BINARY=n
+CONFIG_MOTOROLA_SREC=n
+CONFIG_RAW_BINARY=y
+CONFIG_HAVE_LIBM=n
+
+#
+# General OS setup
+#
+# CONFIG_APPS_DIR - Identifies the relative path to the directory
+#   that builds the application to link with NuttX.  Default: ../apps
+# CONFIG_DEBUG - enables built-in debug options
+# CONFIG_DEBUG_VERBOSE - enables verbose debug output
+# CONFIG_DEBUG_SYMBOLS - build without optimization and with
+#   debug symbols (needed for use with a debugger).
+# CONFIG_HAVE_CXX - Enable support for C++
+# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support
+#   for initialization of static C++ instances for this architecture
+#   and for the selected toolchain (via up_cxxinitialize()).
+# CONFIG_MM_REGIONS - If the architecture includes multiple
+#   regions of memory to allocate from, this specifies the
+#   number of memory regions that the memory manager must
+#   handle and enables the API mm_addregion(start, end);
+# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot
+#   time console output
+# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz
+#   or MSEC_PER_TICK=10.  This setting may be defined to
+#   inform NuttX that the processor hardware is providing
+#   system timer interrupts at some interrupt interval other
+#   than 10 msec.
+# CONFIG_RR_INTERVAL - The round robin timeslice will be set
+#   this number of milliseconds;  Round robin scheduling can
+#   be disabled by setting this value to zero.
+# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in 
+#   scheduler to monitor system performance
+# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a
+#   task name to save in the TCB.  Useful if scheduler
+#   instrumentation is selected.  Set to zero to disable.
+# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY -
+#   Used to initialize the internal time logic.
+# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions.
+#   You would only need this if you are concerned about accurate
+#   time conversions in the past or in the distant future.
+# CONFIG_JULIAN_TIME - Enables Julian time conversions. You
+#   would only need this if you are concerned about accurate
+#   time conversion in the distand past.  You must also define
+#   CONFIG_GREGORIAN_TIME in order to use Julian time.
+# CONFIG_DEV_CONSOLE - Set if architecture-specific logic
+#   provides /dev/console.  Enables stdout, stderr, stdin.
+# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console
+#   driver (minimul support)
+# CONFIG_MUTEX_TYPES: Set to enable support for recursive and
+#   errorcheck mutexes. Enables pthread_mutexattr_settype().
+# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority
+#   inheritance on mutexes and semaphores. 
+# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority
+#   inheritance is enabled.  It defines the maximum number of
+#   different threads (minus one) that can take counts on a
+#   semaphore with priority inheritance support.  This may be 
+#   set to zero if priority inheritance is disabled OR if you
+#   are only using semaphores as mutexes (only one holder) OR
+#   if no more than two threads participate using a counting
+#   semaphore.
+# CONFIG_SEM_NNESTPRIO.  If priority inheritance is enabled,
+#   then this setting is the maximum number of higher priority
+#   threads (minus 1) than can be waiting for another thread
+#   to release a count on a semaphore.  This value may be set
+#   to zero if no more than one thread is expected to wait for
+#   a semaphore.
+# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors
+#   by task_create() when a new task is started.  If set, all
+#   files/drivers will appear to be closed in the new task.
+# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first
+#   three file descriptors (stdin, stdout, stderr) by task_create()
+#   when a new task is started. If set, all files/drivers will
+#   appear to be closed in the new task except for stdin, stdout,
+#   and stderr.
+# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket
+#   desciptors by task_create() when a new task is started. If
+#   set, all sockets will appear to be closed in the new task.
+# CONFIG_SCHED_WORKQUEUE.  Create a dedicated "worker" thread to
+#  handle delayed processing from interrupt handlers.  This feature
+#  is required for some drivers but, if there are not complaints,
+#  can be safely disabled.  The worker thread also performs
+#  garbage collection -- completing any delayed memory deallocations
+#  from interrupt handlers.  If the worker thread is disabled,
+#  then that clean will be performed by the IDLE thread instead
+#  (which runs at the lowest of priority and may not be appropriate
+#  if memory reclamation is of high priority).  If CONFIG_SCHED_WORKQUEUE
+#  is enabled, then the following options can also be used:
+# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker
+#  thread.  Default: 50
+# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for
+#  work in units of microseconds.  Default: 50*1000 (50 MS).
+# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker
+#  thread.  Default: CONFIG_IDLETHREAD_STACKSIZE.
+# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up
+#  the worker thread.  Default: 4
+# CONFIG_SCHED_WAITPID - Enable the waitpid() API
+# CONFIG_SCHED_ATEXIT - Enabled the atexit() API
+#
+CONFIG_USER_ENTRYPOINT="user_start"
+#CONFIG_APPS_DIR=
+CONFIG_DEBUG=n
+CONFIG_DEBUG_VERBOSE=n
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEBUG_FS=n
+CONFIG_DEBUG_GRAPHICS=n
+CONFIG_DEBUG_LCD=n
+CONFIG_DEBUG_USB=n
+CONFIG_DEBUG_NET=n
+CONFIG_DEBUG_RTC=n
+CONFIG_DEBUG_ANALOG=n
+CONFIG_DEBUG_PWM=n
+CONFIG_DEBUG_CAN=n
+CONFIG_DEBUG_I2C=n
+CONFIG_DEBUG_INPUT=n
+
+CONFIG_MSEC_PER_TICK=1
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_MM_REGIONS=1
+CONFIG_MM_SMALL=y
+CONFIG_ARCH_LOWPUTC=y
+CONFIG_RR_INTERVAL=0
+CONFIG_SCHED_INSTRUMENTATION=n
+CONFIG_TASK_NAME_SIZE=8
+CONFIG_START_YEAR=1970
+CONFIG_START_MONTH=1
+CONFIG_START_DAY=1
+CONFIG_GREGORIAN_TIME=n
+CONFIG_JULIAN_TIME=n
+# this eats ~1KiB of RAM ... work out why
+CONFIG_DEV_CONSOLE=y
+CONFIG_DEV_LOWCONSOLE=n
+CONFIG_MUTEX_TYPES=n
+CONFIG_PRIORITY_INHERITANCE=n
+CONFIG_SEM_PREALLOCHOLDERS=0
+CONFIG_SEM_NNESTPRIO=0
+CONFIG_FDCLONE_DISABLE=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SCHED_WORKQUEUE=n
+CONFIG_SCHED_WORKPRIORITY=50
+CONFIG_SCHED_WORKPERIOD=50000
+CONFIG_SCHED_WORKSTACKSIZE=1024
+CONFIG_SIG_SIGWORK=4
+CONFIG_SCHED_WAITPID=n
+CONFIG_SCHED_ATEXIT=n
+
+#
+# The following can be used to disable categories of
+# APIs supported by the OS.  If the compiler supports
+# weak functions, then it should not be necessary to
+# disable functions unless you want to restrict usage
+# of those APIs.
+#
+# There are certain dependency relationships in these
+# features.
+#
+# o mq_notify logic depends on signals to awaken tasks
+#   waiting for queues to become full or empty.
+# o pthread_condtimedwait() depends on signals to wake
+#   up waiting tasks.
+#
+CONFIG_DISABLE_CLOCK=n
+CONFIG_DISABLE_POSIX_TIMERS=y
+CONFIG_DISABLE_PTHREAD=y
+CONFIG_DISABLE_SIGNALS=y
+CONFIG_DISABLE_MQUEUE=y
+CONFIG_DISABLE_MOUNTPOINT=y
+CONFIG_DISABLE_ENVIRON=y
+CONFIG_DISABLE_POLL=y
+
+#
+# Misc libc settings
+#
+# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a
+#   little smaller if we do not support fieldwidthes
+#
+CONFIG_NOPRINTF_FIELDWIDTH=n
+
+#
+# Allow for architecture optimized implementations
+#
+# The architecture can provide optimized versions of the
+# following to improve system performance
+#
+CONFIG_ARCH_MEMCPY=n
+CONFIG_ARCH_MEMCMP=n
+CONFIG_ARCH_MEMMOVE=n
+CONFIG_ARCH_MEMSET=n
+CONFIG_ARCH_STRCMP=n
+CONFIG_ARCH_STRCPY=n
+CONFIG_ARCH_STRNCPY=n
+CONFIG_ARCH_STRLEN=n
+CONFIG_ARCH_STRNLEN=n
+CONFIG_ARCH_BZERO=n
+
+#
+# Sizes of configurable things (0 disables)
+#
+# CONFIG_MAX_TASKS - The maximum number of simultaneously
+#   active tasks.  This value must be a power of two.
+# CONFIG_MAX_TASK_ARGS - This controls the maximum number of
+#   of parameters that a task may receive (i.e., maxmum value
+#   of 'argc')
+# CONFIG_NPTHREAD_KEYS - The number of items of thread-
+#   specific data that can be retained
+# CONFIG_NFILE_DESCRIPTORS - The maximum number of file
+#   descriptors (one for each open)
+# CONFIG_NFILE_STREAMS - The maximum number of streams that
+#   can be fopen'ed
+# CONFIG_NAME_MAX - The maximum size of a file name.
+# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate
+#   on fopen. (Only if CONFIG_NFILE_STREAMS > 0)
+# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled
+#   (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added
+#   to force automatic, line-oriented flushing the output buffer
+#   for putc(), fputc(), putchar(), puts(), fputs(), printf(),
+#   fprintf(), and vfprintf().  When a newline is encountered in
+#   the output string, the output buffer will be flushed.  This
+#   (slightly) increases the NuttX footprint but supports the kind
+#   of behavior that people expect for printf().
+# CONFIG_NUNGET_CHARS - Number of characters that can be
+#   buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0)
+# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message
+#   structures.  The system manages a pool of preallocated
+#   message structures to minimize dynamic allocations
+# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with
+#   a fixed payload size given by this settin (does not include
+#   other message structure overhead.
+# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that
+#   can be passed to a watchdog handler
+# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog
+#   structures.  The system manages a pool of preallocated
+#   watchdog structures to minimize dynamic allocations
+# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX
+#   timer structures.  The system manages a pool of preallocated
+#   timer structures to minimize dynamic allocations.  Set to
+#   zero for all dynamic allocations.
+#
+CONFIG_MAX_TASKS=4
+CONFIG_MAX_TASK_ARGS=4
+CONFIG_NPTHREAD_KEYS=2
+CONFIG_NFILE_DESCRIPTORS=8
+CONFIG_NFILE_STREAMS=0
+CONFIG_NAME_MAX=12
+CONFIG_STDIO_BUFFER_SIZE=32
+CONFIG_STDIO_LINEBUFFER=n
+CONFIG_NUNGET_CHARS=2
+CONFIG_PREALLOC_MQ_MSGS=4
+CONFIG_MQ_MAXMSGSIZE=32
+CONFIG_MAX_WDOGPARMS=2
+CONFIG_PREALLOC_WDOGS=4
+CONFIG_PREALLOC_TIMERS=0
+
+
+#
+# Settings for apps/nshlib
+#
+# CONFIG_NSH_BUILTIN_APPS - Support external registered,
+#   "named" applications that can be executed from the NSH
+#   command line (see apps/README.txt for more information).
+# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer
+# CONFIG_NSH_STRERROR - Use strerror(errno)
+# CONFIG_NSH_LINELEN - Maximum length of one command line
+# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi
+# CONFIG_NSH_DISABLESCRIPT - Disable scripting support
+# CONFIG_NSH_DISABLEBG - Disable background commands
+# CONFIG_NSH_ROMFSETC - Use startup script in /etc
+# CONFIG_NSH_CONSOLE - Use serial console front end
+# CONFIG_NSH_TELNET - Use telnetd console front end
+# CONFIG_NSH_ARCHINIT - Platform provides architecture
+#   specific initialization (nsh_archinitialize()).
+#
+
+# Disable NSH completely
+CONFIG_NSH_CONSOLE=n
+
+#
+# Stack and heap information
+#
+# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP
+#   operation from FLASH but must copy initialized .data sections to RAM.
+#   (should also be =n for the STM3210E-EVAL which always runs from flash)
+# CONFIG_BOOT_COPYTORAM -  Some configurations boot in FLASH
+#   but copy themselves entirely into RAM for better performance.
+# CONFIG_CUSTOM_STACK - The up_ implementation will handle
+#   all stack operations outside of the nuttx model.
+# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only)
+# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack.
+#  This is the thread that (1) performs the inital boot of the system up
+#  to the point where user_start() is spawned, and (2) there after is the
+#  IDLE thread that executes only when there is no other thread ready to
+#  run.
+# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate
+#  for the main user thread that begins at the user_start() entry point.
+# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size
+# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size
+# CONFIG_HEAP_BASE - The beginning of the heap
+# CONFIG_HEAP_SIZE - The size of the heap
+#
+CONFIG_BOOT_RUNFROMFLASH=n
+CONFIG_BOOT_COPYTORAM=n
+CONFIG_CUSTOM_STACK=n
+CONFIG_STACK_POINTER=
+CONFIG_IDLETHREAD_STACKSIZE=1024
+CONFIG_USERMAIN_STACKSIZE=1200
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_PTHREAD_STACK_DEFAULT=1024
+CONFIG_HEAP_BASE=
+CONFIG_HEAP_SIZE=
diff --git a/nuttx-configs/px4io-v2/nsh/setenv.sh b/nuttx-configs/px4io-v2/nsh/setenv.sh
new file mode 100755
index 000000000..ff9a4bf8a
--- /dev/null
+++ b/nuttx-configs/px4io-v2/nsh/setenv.sh
@@ -0,0 +1,47 @@
+#!/bin/bash
+# configs/stm3210e-eval/dfu/setenv.sh
+#
+#   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.
+#
+
+if [ "$(basename $0)" = "setenv.sh" ] ; then
+  echo "You must source this script, not run it!" 1>&2
+  exit 1
+fi
+
+if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi
+
+WD=`pwd`
+export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin"
+export BUILDROOT_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin"
+export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}"
+
+echo "PATH : ${PATH}"
diff --git a/nuttx-configs/px4io-v2/scripts/ld.script b/nuttx-configs/px4io-v2/scripts/ld.script
new file mode 100755
index 000000000..69c2f9cb2
--- /dev/null
+++ b/nuttx-configs/px4io-v2/scripts/ld.script
@@ -0,0 +1,129 @@
+/****************************************************************************
+ * configs/stm3210e-eval/nsh/ld.script
+ *
+ *   Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * 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 STM32F100C8 has 64Kb of FLASH beginning at address 0x0800:0000 and
+ * 8Kb of SRAM beginning at address 0x2000: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.
+ */
+
+MEMORY
+{
+    flash (rx) : ORIGIN = 0x08001000, LENGTH = 60K
+    sram (rwx) : ORIGIN = 0x20000000, LENGTH = 8K
+}
+
+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.*)
+		*(.glue_7)
+		*(.glue_7t)
+		*(.got)
+		*(.gcc_except_table)
+		*(.gnu.linkonce.r.*)
+		_etext = ABSOLUTE(.);
+	} > flash
+
+	/*
+	 * Init functions (static constructors and the like)
+	 */
+        .init_section : {
+                _sinit = ABSOLUTE(.);
+                KEEP(*(.init_array .init_array.*))
+                _einit = ABSOLUTE(.);
+        } > flash
+
+	.ARM.extab : {
+		*(.ARM.extab*)
+	} > flash
+
+	__exidx_start = ABSOLUTE(.);
+	.ARM.exidx : {
+		*(.ARM.exidx*)
+	} > flash
+	__exidx_end = ABSOLUTE(.);
+
+	_eronly = ABSOLUTE(.);
+
+	/* The STM32F100CB has 8Kb of SRAM beginning at the following address */
+
+	.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/nuttx-configs/px4io-v2/src/Makefile b/nuttx-configs/px4io-v2/src/Makefile
new file mode 100644
index 000000000..bb9539d16
--- /dev/null
+++ b/nuttx-configs/px4io-v2/src/Makefile
@@ -0,0 +1,84 @@
+############################################################################
+# configs/stm3210e-eval/src/Makefile
+#
+#   Copyright (C) 2009-2010 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+#
+# 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)/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)
+	@$(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
+	@touch $@
+
+depend: .depend
+
+clean:
+	$(call DELFILE, libboard$(LIBEXT))
+	$(call CLEAN)
+
+distclean: clean
+	$(call DELFILE, Make.dep)
+	$(call DELFILE, .depend)
+
+-include Make.dep
diff --git a/nuttx-configs/px4io-v2/src/empty.c b/nuttx-configs/px4io-v2/src/empty.c
new file mode 100644
index 000000000..ace900866
--- /dev/null
+++ b/nuttx-configs/px4io-v2/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/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
new file mode 100644
index 000000000..771f2128f
--- /dev/null
+++ b/src/drivers/airspeed/airspeed.cpp
@@ -0,0 +1,394 @@
+/****************************************************************************
+ *
+ *   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 ets_airspeed.cpp
+ * @author Simon Wilks
+ *
+ * Driver for the Eagle Tree Airspeed V3 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 <arch/board/board.h>
+
+#include <systemlib/airspeed.h>
+#include <systemlib/err.h>
+#include <systemlib/param/param.h>
+#include <systemlib/perf_counter.h>
+
+#include <drivers/drv_airspeed.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/device/ringbuffer.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/subsystem_info.h>
+
+#include <drivers/airspeed/airspeed.h>
+
+Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) :
+	I2C("Airspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000),
+	_reports(nullptr),
+	_buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")),
+	_max_differential_pressure_pa(0),
+	_sensor_ok(false),
+	_measure_ticks(0),
+	_collect_phase(false),
+	_diff_pres_offset(0.0f),
+	_airspeed_pub(-1),
+	_class_instance(-1),
+	_conversion_interval(conversion_interval),
+	_sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors"))
+{
+	// enable debug() calls
+	_debug_enabled = false;
+
+	// work_cancel in the dtor will explode if we don't do this...
+	memset(&_work, 0, sizeof(_work));
+}
+
+Airspeed::~Airspeed()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	if (_class_instance != -1)
+		unregister_class_devname(AIRSPEED_DEVICE_PATH, _class_instance);
+
+	/* free any existing reports */
+	if (_reports != nullptr)
+		delete _reports;
+
+	// free perf counters
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+	perf_free(_buffer_overflows);
+}
+
+int
+Airspeed::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(differential_pressure_s));
+	if (_reports == nullptr)
+		goto out;
+
+	/* register alternate interfaces if we have to */
+	_class_instance = register_class_devname(AIRSPEED_DEVICE_PATH);
+
+	/* publication init */
+	if (_class_instance == CLASS_DEVICE_PRIMARY) {
+
+		/* advertise sensor topic, measure manually to initialize valid report */
+		struct differential_pressure_s arp;
+		measure();
+		_reports->get(&arp);
+
+		/* measurement will have generated a report, publish */
+		_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp);
+
+		if (_airspeed_pub < 0)
+			warnx("failed to create airspeed sensor object. uORB started?");
+	}
+
+	ret = OK;
+	/* sensor is ok, but we don't really know if it is within range */
+	_sensor_ok = true;
+out:
+	return ret;
+}
+
+int
+Airspeed::probe()
+{
+	/* on initial power up the device needs more than one retry
+	   for detection. Once it is running then retries aren't
+	   needed 
+	*/
+	_retries = 4;
+	int ret = measure();
+	_retries = 2;
+	return ret;
+}
+
+int
+Airspeed::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(_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(_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 AIRSPEEDIOCSSCALE: {
+		struct airspeed_scale *s = (struct airspeed_scale*)arg;
+		_diff_pres_offset = s->offset_pa;
+		return OK;
+		}
+
+	case AIRSPEEDIOCGSCALE: {
+		struct airspeed_scale *s = (struct airspeed_scale*)arg;
+		s->offset_pa = _diff_pres_offset;
+		s->scale = 1.0f;
+		return OK;
+		}
+
+	default:
+		/* give it to the superclass */
+		return I2C::ioctl(filp, cmd, arg);
+	}
+}
+
+ssize_t
+Airspeed::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(differential_pressure_s);
+	differential_pressure_s *abuf = reinterpret_cast<differential_pressure_s *>(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(abuf)) {
+				ret += sizeof(*abuf);
+				abuf++;
+			}
+		}
+
+		/* 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(_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(abuf)) {
+			ret = sizeof(*abuf);
+		}
+
+	} while (0);
+
+	return ret;
+}
+
+void
+Airspeed::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)&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
+Airspeed::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+Airspeed::cycle_trampoline(void *arg)
+{
+	Airspeed *dev = (Airspeed *)arg;
+
+	dev->cycle();
+}
+
+void
+Airspeed::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	perf_print_counter(_buffer_overflows);
+	warnx("poll interval:  %u ticks", _measure_ticks);
+	_reports->print_info("report queue");
+}
+
+void
+Airspeed::new_report(const differential_pressure_s &report)
+{
+	if (!_reports->force(&report))
+		perf_count(_buffer_overflows);
+}
diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h
new file mode 100644
index 000000000..c27b1bcd8
--- /dev/null
+++ b/src/drivers/airspeed/airspeed.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 airspeed.h
+ * @author Simon Wilks
+ *
+ * Generic driver for airspeed sensors 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 <arch/board/board.h>
+
+#include <systemlib/airspeed.h>
+#include <systemlib/err.h>
+#include <systemlib/param/param.h>
+#include <systemlib/perf_counter.h>
+
+#include <drivers/drv_airspeed.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/device/ringbuffer.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/subsystem_info.h>
+
+/* Default I2C bus */
+#define PX4_I2C_BUS_DEFAULT		PX4_I2C_BUS_EXPANSION
+
+/* 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 __EXPORT Airspeed : public device::I2C
+{
+public:
+	Airspeed(int bus, int address, unsigned conversion_interval);
+	virtual ~Airspeed();
+
+	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.
+	 */
+	virtual void	print_info();
+
+private:
+	RingBuffer		*_reports;
+	perf_counter_t		_buffer_overflows;
+
+protected:
+	virtual int	probe();
+
+	/**
+	* Perform a poll cycle; collect from the previous measurement
+	* and start a new one.
+	*/
+	virtual void	cycle() = 0;
+	virtual int	measure() = 0;
+	virtual int	collect() = 0;
+
+	work_s			_work;
+	float			_max_differential_pressure_pa;
+	bool			_sensor_ok;
+	int			_measure_ticks;
+	bool			_collect_phase;
+	float			_diff_pres_offset;
+
+	orb_advert_t		_airspeed_pub;
+
+	int			_class_instance;
+
+	unsigned		_conversion_interval;
+
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_comms_errors;
+
+
+	/**
+	* 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();
+
+	/**
+	* 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);
+
+	/**
+	* add a new report to the reports queue
+	*
+	* @param report		differential_pressure_s report
+	*/
+	void	new_report(const differential_pressure_s &report);
+};
+
+
diff --git a/src/drivers/airspeed/module.mk b/src/drivers/airspeed/module.mk
new file mode 100644
index 000000000..4eef06161
--- /dev/null
+++ b/src/drivers/airspeed/module.mk
@@ -0,0 +1,38 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Makefile to build the generic airspeed driver.
+#
+
+SRCS			= airspeed.cpp
diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c
index 735bdb41a..b88f61ce8 100644
--- a/src/drivers/ardrone_interface/ardrone_interface.c
+++ b/src/drivers/ardrone_interface/ardrone_interface.c
@@ -53,8 +53,10 @@
 #include <sys/prctl.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/safety.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/vehicle_control_mode.h>
 
 #include <systemlib/systemlib.h>
 
@@ -243,16 +245,14 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	int led_counter = 0;
 
 	/* declare and safely initialize all structs */
-	struct vehicle_status_s state;
-	memset(&state, 0, sizeof(state));
 	struct actuator_controls_s actuator_controls;
 	memset(&actuator_controls, 0, sizeof(actuator_controls));
 	struct actuator_armed_s armed;
+	//XXX is this necessairy?
 	armed.armed = false;
 
 	/* subscribe to attitude, motor setpoints and system state */
 	int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
-	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
 	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 
 	printf("[ardrone_interface] Motors initialized - ready.\n");
@@ -322,8 +322,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 		} else {
 			/* MAIN OPERATION MODE */
 
-			/* get a local copy of the vehicle state */
-			orb_copy(ORB_ID(vehicle_status), state_sub, &state);
 			/* get a local copy of the actuator controls */
 			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
 			orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c
index ecd31a073..81f634992 100644
--- a/src/drivers/ardrone_interface/ardrone_motor_control.c
+++ b/src/drivers/ardrone_interface/ardrone_motor_control.c
@@ -41,11 +41,11 @@
 #include <stdio.h>
 #include <fcntl.h>
 #include <unistd.h>
+#include <math.h>
 #include <drivers/drv_gpio.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/actuator_controls_effective.h>
 #include <systemlib/err.h>
 
 #include "ardrone_motor_control.h"
@@ -109,7 +109,7 @@ int ar_multiplexing_init()
 {
 	int		fd;
 	
-	fd = open(GPIO_DEVICE_PATH, 0);
+	fd = open(PX4FMU_DEVICE_PATH, 0);
 
 	if (fd < 0) {
 		warn("GPIO: open fail");
@@ -369,11 +369,9 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
 	float yaw_control = actuators->control[2];
 	float motor_thrust = actuators->control[3];
 
-	//printf("AMO: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",roll_control, pitch_control, yaw_control, motor_thrust);
-
 	const float min_thrust = 0.02f;			/**< 2% minimum thrust */
 	const float max_thrust = 1.0f;			/**< 100% max thrust */
-	const float scaling = 500.0f;			/**< 100% thrust equals a value of 500 which works, 512 leads to cutoff */
+	const float scaling = 510.0f;			/**< 100% thrust equals a value of 510 which works, 512 leads to cutoff */
 	const float min_gas = min_thrust * scaling;	/**< value range sent to motors, minimum */
 	const float max_gas = max_thrust * scaling;	/**< value range sent to motors, maximum */
 
@@ -382,110 +380,77 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
 	float motor_calc[4] = {0};
 
 	float output_band = 0.0f;
-	float band_factor = 0.75f;
 	const float startpoint_full_control = 0.25f;	/**< start full control at 25% thrust */
-	float yaw_factor = 1.0f;
 
 	static bool initialized = false;
-	/* publish effective outputs */
-	static struct actuator_controls_effective_s actuator_controls_effective;
-	static orb_advert_t actuator_controls_effective_pub;
-
-	if (motor_thrust <= min_thrust) {
-		motor_thrust = min_thrust;
-		output_band = 0.0f;
-	} else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) {
-		output_band = band_factor * (motor_thrust - min_thrust);
-	} else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) {
-		output_band = band_factor * startpoint_full_control;
-	} else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) {
-		output_band = band_factor * (max_thrust - motor_thrust);
+
+	/* 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
+	} else {
+		output_band = 1.0f;
 	}
 
+	roll_control *= output_band;
+	pitch_control *= output_band;
+	yaw_control *= output_band;
+
+
 	//add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer
 
 	// FRONT (MOTOR 1)
 	motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control);
-
 	// RIGHT (MOTOR 2)
 	motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control);
-
 	// BACK (MOTOR 3)
 	motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control);
-
 	// LEFT (MOTOR 4)
 	motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control);
 
-	// if we are not in the output band
-	if (!(motor_calc[0] < motor_thrust + output_band && motor_calc[0] > motor_thrust - output_band
-	      && motor_calc[1] < motor_thrust + output_band && motor_calc[1] > motor_thrust - output_band
-	      && motor_calc[2] < motor_thrust + output_band && motor_calc[2] > motor_thrust - output_band
-	      && motor_calc[3] < motor_thrust + output_band && motor_calc[3] > motor_thrust - output_band)) {
+	/* if one motor is saturated, reduce throttle */
+	float saturation = fmaxf(fmaxf(motor_calc[0], motor_calc[1]),fmaxf(motor_calc[2], motor_calc[3])) - max_thrust;
+
+
+	if (saturation > 0.0f) {
+
+		/* reduce the motor thrust according to the saturation */
+		motor_thrust = motor_thrust - saturation;
 
-		yaw_factor = 0.5f;
-		yaw_control *= yaw_factor;
 		// FRONT (MOTOR 1)
 		motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control);
-
 		// RIGHT (MOTOR 2)
 		motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control);
-
 		// BACK (MOTOR 3)
 		motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control);
-
 		// LEFT (MOTOR 4)
 		motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control);
 	}
 
-	for (int i = 0; i < 4; i++) {
-		//check for limits
-		if (motor_calc[i] < motor_thrust - output_band) {
-			motor_calc[i] = motor_thrust - output_band;
-		}
-
-		if (motor_calc[i] > motor_thrust + output_band) {
-			motor_calc[i] = motor_thrust + output_band;
-		}
-	}
-
-	/* publish effective outputs */
-	actuator_controls_effective.control_effective[0] = roll_control;
-	actuator_controls_effective.control_effective[1] = pitch_control;
-	/* yaw output after limiting */
-	actuator_controls_effective.control_effective[2] = yaw_control;
-	/* possible motor thrust limiting */
-	actuator_controls_effective.control_effective[3] = (motor_calc[0] + motor_calc[1] + motor_calc[2] + motor_calc[3]) / 4.0f;
-
-	if (!initialized) {
-		/* advertise and publish */
-		actuator_controls_effective_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, &actuator_controls_effective);
-		initialized = true;
-	} else {
-		/* already initialized, just publishing */
-		orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_controls_effective_pub, &actuator_controls_effective);
-	}
-
 	/* set the motor values */
 
-	/* scale up from 0..1 to 10..512) */
+	/* scale up from 0..1 to 10..500) */
 	motor_pwm[0] = (uint16_t) (motor_calc[0] * ((float)max_gas - min_gas) + min_gas);
 	motor_pwm[1] = (uint16_t) (motor_calc[1] * ((float)max_gas - min_gas) + min_gas);
 	motor_pwm[2] = (uint16_t) (motor_calc[2] * ((float)max_gas - min_gas) + min_gas);
 	motor_pwm[3] = (uint16_t) (motor_calc[3] * ((float)max_gas - min_gas) + min_gas);
 
-	/* Keep motors spinning while armed and prevent overflows */
-
-	/* Failsafe logic - should never be necessary */
-	motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : 10;
-	motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : 10;
-	motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : 10;
-	motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10;
-
-	/* Failsafe logic - should never be necessary */
-	motor_pwm[0] = (motor_pwm[0] <= 511) ? motor_pwm[0] : 511;
-	motor_pwm[1] = (motor_pwm[1] <= 511) ? motor_pwm[1] : 511;
-	motor_pwm[2] = (motor_pwm[2] <= 511) ? motor_pwm[2] : 511;
-	motor_pwm[3] = (motor_pwm[3] <= 511) ? motor_pwm[3] : 511;
+	/* scale up from 0..1 to 10..500) */
+	motor_pwm[0] = (uint16_t) (motor_calc[0] * (float)((max_gas - min_gas) + min_gas));
+	motor_pwm[1] = (uint16_t) (motor_calc[1] * (float)((max_gas - min_gas) + min_gas));
+	motor_pwm[2] = (uint16_t) (motor_calc[2] * (float)((max_gas - min_gas) + min_gas));
+	motor_pwm[3] = (uint16_t) (motor_calc[3] * (float)((max_gas - min_gas) + min_gas));
+
+	/* Failsafe logic for min values - should never be necessary */
+	motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : min_gas;
+	motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : min_gas;
+	motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : min_gas;
+	motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : min_gas;
+
+	/* Failsafe logic for max values - should never be necessary */
+	motor_pwm[0] = (motor_pwm[0] <= max_gas) ? motor_pwm[0] : max_gas;
+	motor_pwm[1] = (motor_pwm[1] <= max_gas) ? motor_pwm[1] : max_gas;
+	motor_pwm[2] = (motor_pwm[2] <= max_gas) ? motor_pwm[2] : max_gas;
+	motor_pwm[3] = (motor_pwm[3] <= max_gas) ? motor_pwm[3] : max_gas;
 
 	/* send motors via UART */
 	ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp
index 3fabfd9a5..2361f4dd1 100644
--- a/src/drivers/blinkm/blinkm.cpp
+++ b/src/drivers/blinkm/blinkm.cpp
@@ -92,9 +92,6 @@
 
 #include <nuttx/config.h>
 
-#include <arch/board/board.h>
-#include <drivers/device/i2c.h>
-
 #include <sys/types.h>
 #include <stdint.h>
 #include <string.h>
@@ -104,18 +101,23 @@
 #include <unistd.h>
 #include <stdio.h>
 #include <ctype.h>
-
-#include <drivers/drv_blinkm.h>
+#include <poll.h>
 
 #include <nuttx/wqueue.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
-
 #include <systemlib/systemlib.h>
-#include <poll.h>
+
+#include <board_config.h>
+
+#include <drivers/device/i2c.h>
+#include <drivers/drv_blinkm.h>
+
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/vehicle_gps_position.h>
 
 static const float MAX_CELL_VOLTAGE	= 4.3f;
@@ -375,7 +377,9 @@ 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;
@@ -386,6 +390,8 @@ BlinkM::led()
 	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;
@@ -398,6 +404,12 @@ BlinkM::led()
 		vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
 		orb_set_interval(vehicle_status_sub_fd, 1000);
 
+		vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode));
+		orb_set_interval(vehicle_control_mode_sub_fd, 1000);
+
+		actuator_armed_sub_fd = orb_subscribe(ORB_ID(actuator_armed));
+		orb_set_interval(actuator_armed_sub_fd, 1000);
+
 		vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
 		orb_set_interval(vehicle_gps_position_sub_fd, 1000);
 
@@ -452,12 +464,16 @@ BlinkM::led()
 	if (led_thread_runcount == 15) {
 		/* obtained data for the first file descriptor */
 		struct vehicle_status_s vehicle_status_raw;
+		struct vehicle_control_mode_s vehicle_control_mode;
+		struct actuator_armed_s actuator_armed;
 		struct vehicle_gps_position_s vehicle_gps_position_raw;
 
 		memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
 		memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw));
 
 		bool new_data_vehicle_status;
+		bool new_data_vehicle_control_mode;
+		bool new_data_actuator_armed;
 		bool new_data_vehicle_gps_position;
 
 		orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
@@ -471,6 +487,28 @@ BlinkM::led()
 				no_data_vehicle_status = 3;
 		}
 
+		orb_check(vehicle_control_mode_sub_fd, &new_data_vehicle_control_mode);
+
+		if (new_data_vehicle_control_mode) {
+			orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub_fd, &vehicle_control_mode);
+			no_data_vehicle_control_mode = 0;
+		} else {
+			no_data_vehicle_control_mode++;
+			if(no_data_vehicle_control_mode >= 3)
+				no_data_vehicle_control_mode = 3;
+		}
+
+		orb_check(actuator_armed_sub_fd, &new_data_actuator_armed);
+
+		if (new_data_actuator_armed) {
+			orb_copy(ORB_ID(actuator_armed), actuator_armed_sub_fd, &actuator_armed);
+			no_data_actuator_armed = 0;
+		} else {
+			no_data_actuator_armed++;
+			if(no_data_actuator_armed >= 3)
+				no_data_actuator_armed = 3;
+		}
+
 		orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position);
 
 		if (new_data_vehicle_gps_position) {
@@ -486,24 +524,24 @@ BlinkM::led()
 
 		/* get number of used satellites in navigation */
 		num_of_used_sats = 0;
-		//for(int satloop=0; satloop<20; satloop++) {
-		for(int satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) {
+
+		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++;
 			}
 		}
 
-		if(new_data_vehicle_status || no_data_vehicle_status < 3){
-			if(num_of_cells == 0) {
+		if (new_data_vehicle_status || no_data_vehicle_status < 3) {
+			if (num_of_cells == 0) {
 				/* looking for lipo cells that are connected */
 				printf("<blinkm> checking cells\n");
 				for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) {
-					if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break;
+					if(vehicle_status_raw.battery_voltage < num_of_cells * MAX_CELL_VOLTAGE) break;
 				}
 				printf("<blinkm> cells found:%d\n", num_of_cells);
 
 			} else {
-				if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
+				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;
@@ -515,7 +553,7 @@ BlinkM::led()
 					led_color_8 = LED_YELLOW;
 					led_blink = LED_BLINK;
 
-				} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
+				} else 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;
@@ -530,7 +568,7 @@ BlinkM::led()
 				} else {
 					/* no battery warnings here */
 
-					if(vehicle_status_raw.flag_system_armed == false) {
+					if(actuator_armed.armed == false) {
 						/* system not armed */
 						led_color_1 = LED_RED;
 						led_color_2 = LED_RED;
@@ -554,27 +592,24 @@ BlinkM::led()
 						led_color_8 = LED_OFF;
 						led_blink = LED_BLINK;
 
-						/* handle 4th led - flightmode indicator */
-						switch((int)vehicle_status_raw.flight_mode) {
-							case VEHICLE_FLIGHT_MODE_MANUAL:
-								led_color_4 = LED_AMBER;
-								break;
-
-							case VEHICLE_FLIGHT_MODE_STAB:
-								led_color_4 = LED_YELLOW;
-								break;
-
-							case VEHICLE_FLIGHT_MODE_HOLD:
-								led_color_4 = LED_BLUE;
-								break;
+						if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
 
-							case VEHICLE_FLIGHT_MODE_AUTO:
+							//XXX please check
+							if (vehicle_control_mode.flag_control_position_enabled)
 								led_color_4 = LED_GREEN;
-								break;
+							else if (vehicle_control_mode.flag_control_velocity_enabled)
+								led_color_4 = LED_BLUE;
+							else if (vehicle_control_mode.flag_control_attitude_enabled)
+								led_color_4 = LED_YELLOW;
+							else if (vehicle_control_mode.flag_control_manual_enabled)
+								led_color_4 = LED_AMBER;
+							else 
+								led_color_4 = LED_OFF;
+								
 						}
 
 						if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
-							/* handling used sat´s */
+							/* handling used sat�s */
 							if(num_of_used_sats >= 7) {
 								led_color_1 = LED_OFF;
 								led_color_2 = LED_OFF;
@@ -831,11 +866,13 @@ BlinkM::get_firmware_version(uint8_t version[2])
 	return transfer(&msg, sizeof(msg), version, 2);
 }
 
+void blinkm_usage();
+
 void blinkm_usage() {
-	fprintf(stderr, "missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}\n");
-	fprintf(stderr, "options:\n");
-	fprintf(stderr, "\t-b --bus i2cbus (3)\n");
-	fprintf(stderr, "\t-a --blinkmaddr blinkmaddr (9)\n");
+	warnx("missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}");
+	warnx("options:");
+	warnx("\t-b --bus i2cbus (3)");
+	warnx("\t-a --blinkmaddr blinkmaddr (9)");
 }
 
 int
diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp
index 4409a8a9c..e43a34805 100644
--- a/src/drivers/bma180/bma180.cpp
+++ b/src/drivers/bma180/bma180.cpp
@@ -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
@@ -59,10 +59,11 @@
 #include <nuttx/clock.h>
 
 #include <drivers/drv_hrt.h>
-#include <arch/board/board.h>
+#include <board_config.h>
 
 #include <drivers/device/spi.h>
 #include <drivers/drv_accel.h>
+#include <drivers/device/ringbuffer.h>
 
 
 /* oddly, ERROR is not defined for c++ */
@@ -146,15 +147,13 @@ private:
 	struct hrt_call		_call;
 	unsigned		_call_interval;
 
-	unsigned		_num_reports;
-	volatile unsigned	_next_report;
-	volatile unsigned	_oldest_report;
-	struct accel_report	*_reports;
+	RingBuffer		*_reports;
 
 	struct accel_scale	_accel_scale;
 	float			_accel_range_scale;
 	float			_accel_range_m_s2;
 	orb_advert_t		_accel_topic;
+	int			_class_instance;
 
 	unsigned		_current_lowpass;
 	unsigned		_current_range;
@@ -233,20 +232,14 @@ private:
 	int			set_lowpass(unsigned frequency);
 };
 
-/* helper macro for handling report buffer indices */
-#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
-
-
 BMA180::BMA180(int bus, spi_dev_e device) :
 	SPI("BMA180", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 8000000),
 	_call_interval(0),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
 	_reports(nullptr),
 	_accel_range_scale(0.0f),
 	_accel_range_m_s2(0.0f),
 	_accel_topic(-1),
+	_class_instance(-1),
 	_current_lowpass(0),
 	_current_range(0),
 	_sample_perf(perf_alloc(PC_ELAPSED, "bma180_read"))
@@ -270,7 +263,7 @@ BMA180::~BMA180()
 
 	/* free any existing reports */
 	if (_reports != nullptr)
-		delete[] _reports;
+		delete _reports;
 
 	/* delete the perf counter */
 	perf_free(_sample_perf);
@@ -286,17 +279,11 @@ BMA180::init()
 		goto out;
 
 	/* allocate basic report buffers */
-	_num_reports = 2;
-	_oldest_report = _next_report = 0;
-	_reports = new struct accel_report[_num_reports];
+	_reports = new RingBuffer(2, sizeof(accel_report));
 
 	if (_reports == nullptr)
 		goto out;
 
-	/* advertise sensor topic */
-	memset(&_reports[0], 0, sizeof(_reports[0]));
-	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &_reports[0]);
-
 	/* perform soft reset (p48) */
 	write_reg(ADDR_RESET, SOFT_RESET);
 
@@ -332,6 +319,19 @@ BMA180::init()
 		ret = ERROR;
 	}
 
+	_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
+
+	/* advertise sensor topic, measure manually to initialize valid report */
+	measure();
+
+	if (_class_instance == CLASS_DEVICE_PRIMARY) {
+		struct accel_report arp;
+		_reports->get(&arp);
+
+		/* measurement will have generated a report, publish */
+		_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
+	}
+
 out:
 	return ret;
 }
@@ -352,6 +352,7 @@ ssize_t
 BMA180::read(struct file *filp, char *buffer, size_t buflen)
 {
 	unsigned count = buflen / sizeof(struct accel_report);
+	struct accel_report *arp = reinterpret_cast<struct accel_report *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
@@ -367,10 +368,9 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen)
 		 * we are careful to avoid racing with it.
 		 */
 		while (count--) {
-			if (_oldest_report != _next_report) {
-				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
-				ret += sizeof(_reports[0]);
-				INCREMENT(_oldest_report, _num_reports);
+			if (_reports->get(arp)) {
+				ret += sizeof(*arp);
+				arp++;
 			}
 		}
 
@@ -379,12 +379,12 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen)
 	}
 
 	/* manual measurement */
-	_oldest_report = _next_report = 0;
+	_reports->flush();
 	measure();
 
 	/* measurement will have generated a report, copy it out */
-	memcpy(buffer, _reports, sizeof(*_reports));
-	ret = sizeof(*_reports);
+	if (_reports->get(arp))
+		ret = sizeof(*arp);
 
 	return ret;
 }
@@ -449,31 +449,22 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return 1000000 / _call_interval;
 
 	case SENSORIOCSQUEUEDEPTH: {
-			/* account for sentinel in the ring */
-			arg++;
-
-			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 2) || (arg > 100))
-				return -EINVAL;
-
-			/* allocate new buffer */
-			struct accel_report *buf = new struct accel_report[arg];
-
-			if (nullptr == buf)
-				return -ENOMEM;
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete[] _reports;
-			_num_reports = arg;
-			_reports = buf;
-			start();
-
-			return OK;
+		/* lower bound is mandatory, upper bound is a sanity check */
+		if ((arg < 2) || (arg > 100))
+			return -EINVAL;
+		
+		irqstate_t flags = irqsave();
+		if (!_reports->resize(arg)) {
+			irqrestore(flags);
+			return -ENOMEM;
 		}
+		irqrestore(flags);
+		
+		return OK;
+	}
 
 	case SENSORIOCGQUEUEDEPTH:
-		return _num_reports - 1;
+		return _reports->size();
 
 	case SENSORIOCRESET:
 		/* XXX implement */
@@ -654,7 +645,7 @@ BMA180::start()
 	stop();
 
 	/* reset the report ring */
-	_oldest_report = _next_report = 0;
+	_reports->flush();
 
 	/* start polling at the specified rate */
 	hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&BMA180::measure_trampoline, this);
@@ -688,7 +679,7 @@ BMA180::measure()
 // 	} raw_report;
 // #pragma pack(pop)
 
-	accel_report		*report = &_reports[_next_report];
+	struct accel_report report;
 
 	/* start the performance counter */
 	perf_begin(_sample_perf);
@@ -708,45 +699,42 @@ BMA180::measure()
 	 * them before.  There is no good way to synchronise with the internal
 	 * measurement flow without using the external interrupt.
 	 */
-	_reports[_next_report].timestamp = hrt_absolute_time();
+	report.timestamp = hrt_absolute_time();
+        report.error_count = 0;
 	/*
 	 * y of board is x of sensor and x of board is -y of sensor
 	 * perform only the axis assignment here.
 	 * Two non-value bits are discarded directly
 	 */
-	report->y_raw  = read_reg(ADDR_ACC_X_LSB + 0);
-	report->y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8;
-	report->x_raw  = read_reg(ADDR_ACC_X_LSB + 2);
-	report->x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8;
-	report->z_raw  = read_reg(ADDR_ACC_X_LSB + 4);
-	report->z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8;
+	report.y_raw  = read_reg(ADDR_ACC_X_LSB + 0);
+	report.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8;
+	report.x_raw  = read_reg(ADDR_ACC_X_LSB + 2);
+	report.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8;
+	report.z_raw  = read_reg(ADDR_ACC_X_LSB + 4);
+	report.z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8;
 
 	/* discard two non-value bits in the 16 bit measurement */
-	report->x_raw = (report->x_raw / 4);
-	report->y_raw = (report->y_raw / 4);
-	report->z_raw = (report->z_raw / 4);
+	report.x_raw = (report.x_raw / 4);
+	report.y_raw = (report.y_raw / 4);
+	report.z_raw = (report.z_raw / 4);
 
 	/* invert y axis, due to 14 bit data no overflow can occur in the negation */
-	report->y_raw = -report->y_raw;
-
-	report->x = ((report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
-	report->y = ((report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
-	report->z = ((report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
-	report->scaling = _accel_range_scale;
-	report->range_m_s2 = _accel_range_m_s2;
+	report.y_raw = -report.y_raw;
 
-	/* post a report to the ring - note, not locked */
-	INCREMENT(_next_report, _num_reports);
+	report.x = ((report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
+	report.y = ((report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
+	report.z = ((report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
+	report.scaling = _accel_range_scale;
+	report.range_m_s2 = _accel_range_m_s2;
 
-	/* if we are running up against the oldest report, fix it */
-	if (_next_report == _oldest_report)
-		INCREMENT(_oldest_report, _num_reports);
+	_reports->force(&report);
 
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
 	/* publish for subscribers */
-	orb_publish(ORB_ID(sensor_accel), _accel_topic, report);
+	if (_accel_topic > 0 && !(_pub_blocked))
+		orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
 
 	/* stop the perf counter */
 	perf_end(_sample_perf);
@@ -756,8 +744,7 @@ void
 BMA180::print_info()
 {
 	perf_print_counter(_sample_perf);
-	printf("report queue:   %u (%u/%u @ %p)\n",
-	       _num_reports, _oldest_report, _next_report, _reports);
+	_reports->print_info("report queue");
 }
 
 /**
diff --git a/src/drivers/boards/px4fmu/px4fmu_internal.h b/src/drivers/boards/px4fmu-v1/board_config.h
index 383a046ff..02c26b5c0 100644
--- a/src/drivers/boards/px4fmu/px4fmu_internal.h
+++ b/src/drivers/boards/px4fmu-v1/board_config.h
@@ -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
@@ -32,9 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file px4fmu_internal.h
+ * @file board_config.h
  *
- * PX4FMU internal definitions
+ * PX4FMUv1 internal definitions
  */
 
 #pragma once
@@ -51,13 +51,17 @@ __BEGIN_DECLS
 
 /* these headers are not C++ safe */
 #include <stm32.h>
-
+#include <arch/board/board.h>
  
 /****************************************************************************************************
  * Definitions
  ****************************************************************************************************/
 /* Configuration ************************************************************************************/
 
+/* PX4IO connection configuration */
+#define PX4IO_SERIAL_DEVICE	"/dev/ttyS2"
+#define UDID_START		0x1FFF7A10
+
 //#ifdef CONFIG_STM32_SPI2
 //#  error "SPI2 is not supported on this board"
 //#endif
@@ -74,15 +78,47 @@ __BEGIN_DECLS
 
 /* External interrupts */
 #define GPIO_EXTI_COMPASS	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1)
-// XXX MPU6000 DRDY?
 
 /* SPI chip selects */
-
 #define GPIO_SPI_CS_GYRO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14)
 #define GPIO_SPI_CS_ACCEL	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
 #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)
 
+/*
+ * Use these in place of the spi_dev_e enumeration to
+ * select a specific SPI device on SPI1
+ */
+#define PX4_SPIDEV_GYRO		1
+#define PX4_SPIDEV_ACCEL	2
+#define PX4_SPIDEV_MPU		3
+
+/*
+ * Optional devices on IO's external port
+ */
+#define PX4_SPIDEV_ACCEL_MAG 2
+
+/*
+ * I2C busses
+ */
+#define PX4_I2C_BUS_ESC		1
+#define PX4_I2C_BUS_ONBOARD	2
+#define PX4_I2C_BUS_EXPANSION	3
+#define PX4_I2C_BUS_LED		3
+
+/*
+ * Devices on the onboard bus.
+ *
+ * Note that these are unshifted addresses.
+ */
+#define PX4_I2C_OBDEV_HMC5883	0x1e
+#define PX4_I2C_OBDEV_MS5611	0x76
+#define PX4_I2C_OBDEV_EEPROM	NOTDEFINED
+#define PX4_I2C_OBDEV_LED	0x55
+
+#define PX4_I2C_OBDEV_PX4IO_BL	0x18
+#define PX4_I2C_OBDEV_PX4IO	0x1a
+
 /* User GPIOs
  *
  * GPIO0-1 are the buffered high-power GPIOs.
@@ -107,31 +143,45 @@ __BEGIN_DECLS
 #define GPIO_GPIO7_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN12)
 #define GPIO_GPIO_DIR		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
 
+/*
+ * Tone alarm output
+ */
+#define TONE_ALARM_TIMER	3	/* timer 3 */
+#define TONE_ALARM_CHANNEL	3	/* channel 3 */
+#define GPIO_TONE_ALARM_IDLE	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN8)
+#define GPIO_TONE_ALARM		(GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN8)
+
+/*
+ * PWM
+ *
+ * Four PWM outputs can be configured on pins otherwise shared with
+ * USART2; two can take the flow control pins if they are not being used.
+ *
+ * Pins:
+ *
+ * CTS - PA0 - TIM2CH1
+ * RTS - PA1 - TIM2CH2
+ * TX  - PA2 - TIM2CH3
+ * RX  - PA3 - TIM2CH4
+ *
+ */
+#define GPIO_TIM2_CH1OUT	GPIO_TIM2_CH1OUT_1
+#define GPIO_TIM2_CH2OUT	GPIO_TIM2_CH2OUT_1
+#define GPIO_TIM2_CH3OUT	GPIO_TIM2_CH3OUT_1
+#define GPIO_TIM2_CH4OUT	GPIO_TIM2_CH4OUT_1
+
 /* USB OTG FS
  *
  * PA9  OTG_FS_VBUS VBUS sensing (also connected to the green LED)
  */
 #define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
 
-/* PWM
- *
- * The PX4FMU has five PWM outputs, of which only the output on
- * pin PC8 is fixed assigned to this function. The other four possible
- * pwm sources are the TX, RX, RTS and CTS pins of USART2
- *
- * Alternate function mapping:
- * PC8 - BUZZER - TIM8_CH3/SDIO_D0 /TIM3_CH3/ USART6_CK / DCMI_D2
+/* High-resolution timer
  */
-
-#ifdef CONFIG_PWM
-#  if defined(CONFIG_STM32_TIM3_PWM)
-#    define BUZZER_PWMCHANNEL 3
-#	 define BUZZER_PWMTIMER 3
-#  elif defined(CONFIG_STM32_TIM8_PWM)
-#    define BUZZER_PWMCHANNEL 3
-#    define BUZZER_PWMTIMER 8
-#  endif
-#endif
+#define HRT_TIMER		1	/* use timer1 for the HRT */
+#define HRT_TIMER_CHANNEL	1	/* use capture/compare channel */
+#define HRT_PPM_CHANNEL		3	/* use capture/compare channel 3 */
+#define GPIO_PPM_IN		(GPIO_ALT|GPIO_AF1|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10)
 
 /****************************************************************************************************
  * Public Types
diff --git a/src/drivers/boards/px4fmu/module.mk b/src/drivers/boards/px4fmu-v1/module.mk
index 66b776917..66b776917 100644
--- a/src/drivers/boards/px4fmu/module.mk
+++ b/src/drivers/boards/px4fmu-v1/module.mk
diff --git a/src/drivers/boards/px4fmu/px4fmu_can.c b/src/drivers/boards/px4fmu-v1/px4fmu_can.c
index 187689ff9..1e1f10040 100644
--- a/src/drivers/boards/px4fmu/px4fmu_can.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_can.c
@@ -54,7 +54,7 @@
 
 #include "stm32.h"
 #include "stm32_can.h"
-#include "px4fmu_internal.h"
+#include "board_config.h"
 
 #ifdef CONFIG_CAN
 
diff --git a/src/drivers/boards/px4fmu/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c
index 212a92cfa..4b12b75f9 100644
--- a/src/drivers/boards/px4fmu/px4fmu_init.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c
@@ -59,7 +59,7 @@
 #include <nuttx/analog/adc.h>
 
 #include "stm32.h"
-#include "px4fmu_internal.h"
+#include "board_config.h"
 #include "stm32_uart.h"
 
 #include <arch/board/board.h>
@@ -194,7 +194,7 @@ __EXPORT int nsh_archinitialize(void)
 	/* initial LED state */
 	drv_led_start();
 	led_off(LED_AMBER);
-	led_on(LED_BLUE);
+	led_off(LED_BLUE);
 
 
 	/* Configure SPI-based devices */
@@ -222,14 +222,9 @@ __EXPORT int nsh_archinitialize(void)
 	 * If SPI2 is enabled in the defconfig, we loose some ADC pins as chip selects.
 	 * Keep the SPI2 init optional and conditionally initialize the ADC pins
 	 */
-	spi2 = up_spiinitialize(2);
 
-	if (!spi2) {
-		message("[boot] Enabling IN12/13 instead of SPI2\n");
-		/* no SPI2, use pins for ADC */
-		stm32_configgpio(GPIO_ADC1_IN12);
-		stm32_configgpio(GPIO_ADC1_IN13);	// jumperable to MPU6000 DRDY on some boards
-	} else {
+	#ifdef CONFIG_STM32_SPI2
+		spi2 = up_spiinitialize(2);
 		/* Default SPI2 to 1MHz and de-assert the known chip selects. */
 		SPI_SETFREQUENCY(spi2, 10000000);
 		SPI_SETBITS(spi2, 8);
@@ -238,7 +233,13 @@ __EXPORT int nsh_archinitialize(void)
 		SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false);
 
 		message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n");
-	}
+	#else
+		spi2 = NULL;
+		message("[boot] Enabling IN12/13 instead of SPI2\n");
+		/* no SPI2, use pins for ADC */
+		stm32_configgpio(GPIO_ADC1_IN12);
+		stm32_configgpio(GPIO_ADC1_IN13);	// jumperable to MPU6000 DRDY on some boards
+	#endif
 
 	/* Get the SPI port for the microSD slot */
 
diff --git a/src/drivers/boards/px4fmu/px4fmu_led.c b/src/drivers/boards/px4fmu-v1/px4fmu_led.c
index 31b25984e..ea91f34ad 100644
--- a/src/drivers/boards/px4fmu/px4fmu_led.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_led.c
@@ -42,7 +42,7 @@
 #include <stdbool.h>
 
 #include "stm32.h"
-#include "px4fmu_internal.h"
+#include "board_config.h"
 
 #include <arch/board/board.h>
 
@@ -57,6 +57,7 @@ __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()
@@ -94,3 +95,21 @@ __EXPORT void led_off(int led)
 		stm32_gpiowrite(GPIO_LED2, true);
 	}
 }
+
+__EXPORT void led_toggle(int led)
+{
+	if (led == 0)
+	{
+		if (stm32_gpioread(GPIO_LED1))
+			stm32_gpiowrite(GPIO_LED1, false);
+		else
+			stm32_gpiowrite(GPIO_LED1, true);
+	}
+	if (led == 1)
+	{
+		if (stm32_gpioread(GPIO_LED2))
+			stm32_gpiowrite(GPIO_LED2, false);
+		else
+			stm32_gpiowrite(GPIO_LED2, true);
+	}
+}
diff --git a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu-v1/px4fmu_pwm_servo.c
index d85131dd8..848e21d79 100644
--- a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_pwm_servo.c
@@ -41,15 +41,15 @@
 
 #include <stdint.h>
 
-#include <drivers/stm32/drv_pwm_servo.h>
-
-#include <arch/board/board.h>
-#include <drivers/drv_pwm_output.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_TIM2_BASE,
diff --git a/src/drivers/boards/px4fmu/px4fmu_spi.c b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c
index e05ddecf3..17e6862f7 100644
--- a/src/drivers/boards/px4fmu/px4fmu_spi.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c
@@ -53,7 +53,7 @@
 #include "up_arch.h"
 #include "chip.h"
 #include "stm32.h"
-#include "px4fmu_internal.h"
+#include "board_config.h"
 
 /************************************************************************************
  * Public Functions
diff --git a/src/drivers/boards/px4fmu/px4fmu_usb.c b/src/drivers/boards/px4fmu-v1/px4fmu_usb.c
index 0be981c1e..0fc8569aa 100644
--- a/src/drivers/boards/px4fmu/px4fmu_usb.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_usb.c
@@ -53,7 +53,7 @@
 
 #include "up_arch.h"
 #include "stm32.h"
-#include "px4fmu_internal.h"
+#include "board_config.h"
 
 /************************************************************************************
  * Definitions
diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h
new file mode 100644
index 000000000..7cfca7656
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/board_config.h
@@ -0,0 +1,214 @@
+/****************************************************************************
+ *
+ *   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
+ *
+ * PX4FMUv2 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
+ ****************************************************************************************************/
+/* Configuration ************************************************************************************/
+
+/* PX4IO connection configuration */
+#define PX4IO_SERIAL_DEVICE	"/dev/ttyS4"
+#define PX4IO_SERIAL_TX_GPIO	GPIO_USART6_TX
+#define PX4IO_SERIAL_RX_GPIO	GPIO_USART6_RX
+#define PX4IO_SERIAL_BASE	STM32_USART6_BASE	/* hardwired on the board */
+#define PX4IO_SERIAL_VECTOR	STM32_IRQ_USART6
+#define PX4IO_SERIAL_TX_DMAMAP	DMAMAP_USART6_TX_2
+#define PX4IO_SERIAL_RX_DMAMAP	DMAMAP_USART6_RX_2
+#define PX4IO_SERIAL_CLOCK	STM32_PCLK2_FREQUENCY
+#define PX4IO_SERIAL_BITRATE	1500000			/* 1.5Mbps -> max rate for IO */
+
+
+/* PX4FMU GPIOs ***********************************************************************************/
+/* LEDs */
+
+#define GPIO_LED1		(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12)
+
+/* External interrupts */
+#define GPIO_EXTI_GYRO_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_EXTI_MAG_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1)
+#define GPIO_EXTI_ACCEL_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN4)
+#define GPIO_EXTI_MPU_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)
+
+/* Data ready pins off */
+#define GPIO_GYRO_DRDY_OFF	(GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_MAG_DRDY_OFF	(GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN1)
+#define GPIO_ACCEL_DRDY_OFF	(GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN4)
+#define GPIO_EXTI_MPU_DRDY_OFF	(GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)
+
+/* SPI1 off */
+#define GPIO_SPI1_SCK_OFF	(GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN5)
+#define GPIO_SPI1_MISO_OFF	(GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN6)
+#define GPIO_SPI1_MOSI_OFF	(GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN7)
+
+/* SPI1 chip selects off */
+#define GPIO_SPI_CS_GYRO_OFF		(GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN13)
+#define GPIO_SPI_CS_ACCEL_MAG_OFF	(GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN15)
+#define GPIO_SPI_CS_BARO_OFF		(GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTD|GPIO_PIN7)
+#define GPIO_SPI_CS_MPU_OFF		(GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2)
+
+/* SPI chip selects */
+#define GPIO_SPI_CS_GYRO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
+#define GPIO_SPI_CS_ACCEL_MAG	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
+#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)
+
+/* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */
+#define PX4_SPIDEV_GYRO		1
+#define PX4_SPIDEV_ACCEL_MAG	2
+#define PX4_SPIDEV_BARO		3
+#define PX4_SPIDEV_MPU		4
+
+/* I2C busses */
+#define PX4_I2C_BUS_EXPANSION	1
+#define PX4_I2C_BUS_LED		2
+
+/* Devices on the onboard bus.
+ *
+ * Note that these are unshifted addresses.
+ */
+#define PX4_I2C_OBDEV_LED	0x55
+#define PX4_I2C_OBDEV_HMC5883	0x1e
+
+/* User GPIOs
+ *
+ * GPIO0-5 are the PWM servo outputs.
+ */
+#define GPIO_GPIO0_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN14)
+#define GPIO_GPIO1_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN13)
+#define GPIO_GPIO2_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11)
+#define GPIO_GPIO3_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN9)
+#define GPIO_GPIO4_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_GPIO5_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14)
+#define GPIO_GPIO0_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14)
+#define GPIO_GPIO1_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13)
+#define GPIO_GPIO2_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11)
+#define GPIO_GPIO3_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9)
+#define GPIO_GPIO4_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_GPIO5_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14)
+
+/* Power supply control and monitoring GPIOs */
+#define GPIO_VDD_5V_PERIPH_EN	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8)
+#define GPIO_VDD_BRICK_VALID	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
+#define GPIO_VDD_SERVO_VALID	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN7)
+#define GPIO_VDD_3V3_SENSORS_EN	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3)
+#define GPIO_VDD_5V_HIPOWER_OC	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN10)
+#define GPIO_VDD_5V_PERIPH_OC	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15)
+
+/* Tone alarm output */
+#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_PIN15)
+#define GPIO_TONE_ALARM		(GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15)
+
+/* PWM
+ *
+ * Six PWM outputs are configured.
+ *
+ * Pins:
+ *
+ * CH1 : PE14 : TIM1_CH4
+ * CH2 : PE13 : TIM1_CH3
+ * CH3 : PE11 : TIM1_CH2
+ * CH4 : PE9  : TIM1_CH1
+ * CH5 : PD13 : TIM4_CH2
+ * CH6 : PD14 : TIM4_CH3
+ */
+#define GPIO_TIM1_CH1OUT	GPIO_TIM1_CH1OUT_2
+#define GPIO_TIM1_CH2OUT	GPIO_TIM1_CH2OUT_2
+#define GPIO_TIM1_CH3OUT	GPIO_TIM1_CH3OUT_2
+#define GPIO_TIM1_CH4OUT	GPIO_TIM1_CH4OUT_2
+#define GPIO_TIM4_CH2OUT	GPIO_TIM4_CH2OUT_2
+#define GPIO_TIM4_CH3OUT	GPIO_TIM4_CH3OUT_2
+
+/* USB OTG FS
+ *
+ * PA9  OTG_FS_VBUS VBUS sensing (also connected to the green LED)
+ */
+#define GPIO_OTGFS_VBUS		(GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
+
+/* High-resolution timer */
+#define HRT_TIMER		8	/* use timer8 for the HRT */
+#define HRT_TIMER_CHANNEL	1	/* use capture/compare channel */
+
+/****************************************************************************************************
+ * 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/px4fmu-v2/module.mk b/src/drivers/boards/px4fmu-v2/module.mk
new file mode 100644
index 000000000..99d37eeca
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/module.mk
@@ -0,0 +1,10 @@
+#
+# Board-specific startup code for the PX4FMUv2
+#
+
+SRCS		 = px4fmu_can.c \
+		   px4fmu2_init.c \
+		   px4fmu_pwm_servo.c \
+		   px4fmu_spi.c \
+		   px4fmu_usb.c \
+		   px4fmu2_led.c
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
new file mode 100644
index 000000000..71414d62c
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
@@ -0,0 +1,332 @@
+/****************************************************************************
+ *
+ *   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 px4fmu_init.c
+ *
+ * PX4FMU-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/sdio.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 *spi1;
+static struct spi_dev_s *spi2;
+static struct sdio_dev_s *sdio;
+
+#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_IN2);	/* BATT_VOLTAGE_SENS */
+	stm32_configgpio(GPIO_ADC1_IN3);	/* BATT_CURRENT_SENS */
+	stm32_configgpio(GPIO_ADC1_IN4);	/* VDD_5V_SENS */
+	// stm32_configgpio(GPIO_ADC1_IN10);	/* used by VBUS valid */
+	// stm32_configgpio(GPIO_ADC1_IN11);	/* unused */
+	// stm32_configgpio(GPIO_ADC1_IN12);	/* used by MPU6000 CS */
+	stm32_configgpio(GPIO_ADC1_IN13);	/* FMU_AUX_ADC_1 */
+	stm32_configgpio(GPIO_ADC1_IN14);	/* FMU_AUX_ADC_2 */
+	stm32_configgpio(GPIO_ADC1_IN15);	/* PRESSURE_SENS */
+
+	/* configure power supply control/sense pins */
+	stm32_configgpio(GPIO_VDD_5V_PERIPH_EN);
+	stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
+	stm32_configgpio(GPIO_VDD_BRICK_VALID);
+	stm32_configgpio(GPIO_VDD_SERVO_VALID);
+	stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC);
+	stm32_configgpio(GPIO_VDD_5V_PERIPH_OC);
+
+	/* 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 SPI-based devices */
+
+	spi1 = up_spiinitialize(1);
+
+	if (!spi1) {
+		message("[boot] FAILED to initialize SPI port 1\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	/* Default SPI1 to 1MHz and de-assert the known chip selects. */
+	SPI_SETFREQUENCY(spi1, 10000000);
+	SPI_SETBITS(spi1, 8);
+	SPI_SETMODE(spi1, SPIDEV_MODE3);
+	SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
+	SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false);
+	SPI_SELECT(spi1, PX4_SPIDEV_BARO, false);
+	SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
+	up_udelay(20);
+
+	message("[boot] Initialized SPI port 1 (SENSORS)\n");
+
+	/* Get the SPI port for the FRAM */
+
+	spi2 = up_spiinitialize(2);
+
+	if (!spi2) {
+		message("[boot] FAILED to initialize SPI port 2\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	/* Default SPI2 to 37.5 MHz (40 MHz rounded to nearest valid divider, F4 max)
+	 * and de-assert the known chip selects. */
+
+	// XXX start with 10.4 MHz in FRAM usage and go up to 37.5 once validated
+	SPI_SETFREQUENCY(spi2, 12 * 1000 * 1000);
+	SPI_SETBITS(spi2, 8);
+	SPI_SETMODE(spi2, SPIDEV_MODE3);
+	SPI_SELECT(spi2, SPIDEV_FLASH, false);
+
+	message("[boot] Initialized SPI port 2 (RAMTRON FRAM)\n");
+
+	#ifdef CONFIG_MMCSD
+	/* First, get an instance of the SDIO interface */
+
+	sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO);
+	if (!sdio) {
+		message("[boot] Failed to initialize SDIO slot %d\n",
+			CONFIG_NSH_MMCSDSLOTNO);
+		return -ENODEV;
+	}
+
+	/* Now bind the SDIO interface to the MMC/SD driver */
+	int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio);
+	if (ret != OK) {
+		message("[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret);
+		return ret;
+	}
+
+	/* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */
+	sdio_mediachange(sdio, true);
+
+	message("[boot] Initialized SDIO\n");
+	#endif
+
+	return OK;
+}
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c
new file mode 100644
index 000000000..a856ccb02
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c
@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   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 px4fmu2_led.c
+ *
+ * PX4FMU 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()
+{
+	/* Configure LED1 GPIO for output */
+
+	stm32_configgpio(GPIO_LED1);
+}
+
+__EXPORT void led_on(int led)
+{
+	if (led == 1)
+	{
+		/* Pull down to switch on */
+		stm32_gpiowrite(GPIO_LED1, false);
+	}
+}
+
+__EXPORT void led_off(int led)
+{
+	if (led == 1)
+	{
+		/* Pull up to switch off */
+		stm32_gpiowrite(GPIO_LED1, true);
+	}
+}
+
+__EXPORT void led_toggle(int led)
+{
+	if (led == 1)
+	{
+		if (stm32_gpioread(GPIO_LED1))
+			stm32_gpiowrite(GPIO_LED1, false);
+		else
+			stm32_gpiowrite(GPIO_LED1, true);
+	}
+}
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_can.c b/src/drivers/boards/px4fmu-v2/px4fmu_can.c
new file mode 100644
index 000000000..f66c7cd79
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu_can.c
@@ -0,0 +1,144 @@
+/****************************************************************************
+ *
+ *   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 px4fmu_can.c
+ *
+ * Board-specific CAN functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/can.h>
+#include <arch/board/board.h>
+
+#include "chip.h"
+#include "up_arch.h"
+
+#include "stm32.h"
+#include "stm32_can.h"
+#include "board_config.h"
+
+#ifdef CONFIG_CAN
+
+/************************************************************************************
+ * Pre-processor Definitions
+ ************************************************************************************/
+/* Configuration ********************************************************************/
+
+#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2)
+#  warning "Both CAN1 and CAN2 are enabled.  Assuming only CAN1."
+#  undef CONFIG_STM32_CAN2
+#endif
+
+#ifdef CONFIG_STM32_CAN1
+#  define CAN_PORT 1
+#else
+#  define CAN_PORT 2
+#endif
+
+/* Debug ***************************************************************************/
+/* Non-standard debug that may be enabled just for testing CAN */
+
+#ifdef CONFIG_DEBUG_CAN
+#  define candbg    dbg
+#  define canvdbg   vdbg
+#  define canlldbg  lldbg
+#  define canllvdbg llvdbg
+#else
+#  define candbg(x...)
+#  define canvdbg(x...)
+#  define canlldbg(x...)
+#  define canllvdbg(x...)
+#endif
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: can_devinit
+ *
+ * Description:
+ *   All STM32 architectures must provide the following interface to work with
+ *   examples/can.
+ *
+ ************************************************************************************/
+
+int can_devinit(void)
+{
+	static bool initialized = false;
+	struct can_dev_s *can;
+	int ret;
+
+	/* Check if we have already initialized */
+
+	if (!initialized) {
+		/* Call stm32_caninitialize() to get an instance of the CAN interface */
+
+		can = stm32_caninitialize(CAN_PORT);
+
+		if (can == NULL) {
+			candbg("ERROR:  Failed to get CAN interface\n");
+			return -ENODEV;
+		}
+
+		/* Register the CAN driver at "/dev/can0" */
+
+		ret = can_register("/dev/can0", can);
+
+		if (ret < 0) {
+			candbg("ERROR: can_register failed: %d\n", ret);
+			return ret;
+		}
+
+		/* Now we are initialized */
+
+		initialized = true;
+	}
+
+	return OK;
+}
+
+#endif
\ No newline at end of file
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c
new file mode 100644
index 000000000..600dfef41
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c
@@ -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 px4fmu_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_TIM4_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM4EN,
+		.clock_freq = STM32_APB1_TIM4_CLKIN
+	}
+};
+
+__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
+	{
+		.gpio = GPIO_TIM1_CH4OUT,
+		.timer_index = 0,
+		.timer_channel = 4,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM1_CH3OUT,
+		.timer_index = 0,
+		.timer_channel = 3,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM1_CH2OUT,
+		.timer_index = 0,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM1_CH1OUT,
+		.timer_index = 0,
+		.timer_channel = 1,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM4_CH2OUT,
+		.timer_index = 1,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM4_CH3OUT,
+		.timer_index = 1,
+		.timer_channel = 3,
+		.default_value = 1000,
+	}
+};
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
new file mode 100644
index 000000000..c66c490a7
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
@@ -0,0 +1,159 @@
+/****************************************************************************
+ *
+ *   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 px4fmu_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_SPI_CS_GYRO);
+	stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
+	stm32_configgpio(GPIO_SPI_CS_BARO);
+	stm32_configgpio(GPIO_SPI_CS_MPU);
+
+	/* 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_gpiowrite(GPIO_SPI_CS_MPU, 1);
+
+	stm32_configgpio(GPIO_EXTI_GYRO_DRDY);
+	stm32_configgpio(GPIO_EXTI_MAG_DRDY);
+	stm32_configgpio(GPIO_EXTI_ACCEL_DRDY);
+	stm32_configgpio(GPIO_EXTI_MPU_DRDY);
+#endif
+
+#ifdef CONFIG_STM32_SPI2
+	stm32_configgpio(GPIO_SPI_CS_FRAM);
+	stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1);
+#endif
+}
+
+__EXPORT void stm32_spi1select(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);
+		stm32_gpiowrite(GPIO_SPI_CS_MPU, 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);
+		stm32_gpiowrite(GPIO_SPI_CS_MPU, 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);
+		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+		break;
+
+	case PX4_SPIDEV_MPU:
+		/* 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, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected);
+		break;
+
+	default:
+		break;
+	}
+}
+
+__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	return SPI_STATUS_PRESENT;
+}
+
+
+#ifdef CONFIG_STM32_SPI2
+__EXPORT void stm32_spi2select(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_SPI_CS_FRAM, !selected);
+}
+
+__EXPORT uint8_t stm32_spi2status(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/px4fmu-v2/px4fmu_usb.c b/src/drivers/boards/px4fmu-v2/px4fmu_usb.c
new file mode 100644
index 000000000..f329e06ff
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu_usb.c
@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ *   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 px4fmu_usb.c
+ *
+ * Board-specific USB functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/usb/usbdev.h>
+#include <nuttx/usb/usbdev_trace.h>
+
+#include <up_arch.h>
+#include <stm32.h>
+#include "board_config.h"
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_usbinitialize
+ *
+ * Description:
+ *   Called to setup USB-related GPIO pins for the PX4FMU board.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbinitialize(void)
+{
+	/* The OTG FS has an internal soft pull-up */
+
+	/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
+
+#ifdef CONFIG_STM32_OTGFS
+	stm32_configgpio(GPIO_OTGFS_VBUS);
+	/* XXX We only support device mode
+	stm32_configgpio(GPIO_OTGFS_PWRON);
+	stm32_configgpio(GPIO_OTGFS_OVER);
+	*/
+#endif
+}
+
+/************************************************************************************
+ * Name:  stm32_usbsuspend
+ *
+ * Description:
+ *   Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is
+ *   used.  This function is called whenever the USB enters or leaves suspend mode.
+ *   This is an opportunity for the board logic to shutdown clocks, power, etc.
+ *   while the USB is suspended.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
+{
+	ulldbg("resume: %d\n", resume);
+}
+
diff --git a/src/drivers/boards/px4io/px4io_internal.h b/src/drivers/boards/px4io-v1/board_config.h
index 6638e715e..1be4877ba 100644
--- a/src/drivers/boards/px4io/px4io_internal.h
+++ b/src/drivers/boards/px4io-v1/board_config.h
@@ -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
@@ -32,7 +32,7 @@
  ****************************************************************************/
  
 /**
- * @file px4io_internal.h
+ * @file board_config.h
  * 
  * PX4IO hardware definitions.
  */
@@ -47,7 +47,9 @@
 #include <nuttx/compiler.h>
 #include <stdint.h>
 
+/* these headers are not C++ safe */
 #include <stm32.h>
+#include <arch/board/board.h>
 
 /************************************************************************************
  * Definitions
@@ -56,11 +58,11 @@
 /* PX4IO GPIOs **********************************************************************/
 /* LEDs */
 
-#define GPIO_LED1       (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
+#define GPIO_LED1       (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\
                          GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14)
-#define GPIO_LED2       (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
+#define GPIO_LED2       (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\
                          GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15)
-#define GPIO_LED3       (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
+#define GPIO_LED3       (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\
                          GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
 
 /* Safety switch button *************************************************************/
@@ -83,3 +85,11 @@
 
 #define GPIO_ADC_VBATT	(GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4)
 #define GPIO_ADC_IN5	(GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5)
+
+/* 
+ * High-resolution timer
+ */
+#define HRT_TIMER		1	/* use timer1 for the HRT */
+#define HRT_TIMER_CHANNEL	2	/* use capture/compare channel 2 */
+#define HRT_PPM_CHANNEL		1	/* use capture/compare channel 1 */
+#define GPIO_PPM_IN		(GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_PORTE|GPIO_PIN9)
diff --git a/src/drivers/boards/px4io/module.mk b/src/drivers/boards/px4io-v1/module.mk
index 2601a1c15..2601a1c15 100644
--- a/src/drivers/boards/px4io/module.mk
+++ b/src/drivers/boards/px4io-v1/module.mk
diff --git a/src/drivers/boards/px4io/px4io_init.c b/src/drivers/boards/px4io-v1/px4io_init.c
index 15c59e423..8292da9e1 100644
--- a/src/drivers/boards/px4io/px4io_init.c
+++ b/src/drivers/boards/px4io-v1/px4io_init.c
@@ -55,7 +55,7 @@
 #include <nuttx/arch.h>
 
 #include "stm32.h"
-#include "px4io_internal.h"
+#include "board_config.h"
 #include "stm32_uart.h"
 
 #include <arch/board/board.h>
diff --git a/src/drivers/boards/px4io/px4io_pwm_servo.c b/src/drivers/boards/px4io-v1/px4io_pwm_servo.c
index 6df470da6..6df470da6 100644
--- a/src/drivers/boards/px4io/px4io_pwm_servo.c
+++ b/src/drivers/boards/px4io-v1/px4io_pwm_servo.c
diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h
new file mode 100644
index 000000000..ef9bb5cad
--- /dev/null
+++ b/src/drivers/boards/px4io-v2/board_config.h
@@ -0,0 +1,138 @@
+/****************************************************************************
+ *
+ *   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
+ *
+ * PX4IOV2 internal definitions
+ */
+
+#pragma once
+
+/******************************************************************************
+ * Included Files
+ ******************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+/* these headers are not C++ safe */
+#include <stm32.h>
+#include <arch/board/board.h>
+ 
+/******************************************************************************
+ * Definitions
+ ******************************************************************************/
+/* Configuration **************************************************************/
+
+/******************************************************************************
+ * Serial
+ ******************************************************************************/
+#define PX4FMU_SERIAL_BASE	STM32_USART2_BASE
+#define PX4FMU_SERIAL_VECTOR	STM32_IRQ_USART2
+#define PX4FMU_SERIAL_TX_GPIO	GPIO_USART2_TX
+#define PX4FMU_SERIAL_RX_GPIO	GPIO_USART2_RX
+#define PX4FMU_SERIAL_TX_DMA	DMACHAN_USART2_TX
+#define PX4FMU_SERIAL_RX_DMA	DMACHAN_USART2_RX
+#define PX4FMU_SERIAL_CLOCK	STM32_PCLK1_FREQUENCY
+#define PX4FMU_SERIAL_BITRATE	1500000
+
+/******************************************************************************
+ * GPIOS
+ ******************************************************************************/
+
+/* LEDS  **********************************************************************/
+
+#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
+#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
+#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
+
+/* Safety switch button *******************************************************/
+
+#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5)
+
+/* Power switch controls ******************************************************/
+
+#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
+
+#define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15)
+
+/* Analog inputs **************************************************************/
+
+#define GPIO_ADC_VSERVO (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4)
+
+/* the same rssi signal goes to both an adc and a timer input */
+#define GPIO_ADC_RSSI   (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5)
+#define GPIO_TIM_RSSI   (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN12)
+
+/* PWM pins  **************************************************************/
+
+#define GPIO_PPM (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN8)
+
+#define GPIO_PWM1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
+#define GPIO_PWM2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_PWM3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8)
+#define GPIO_PWM4 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9)
+#define GPIO_PWM5 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6)
+#define GPIO_PWM6 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
+#define GPIO_PWM7 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_PWM8 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1)
+
+/* SBUS pins  *************************************************************/
+
+/* XXX these should be UART pins */
+#define GPIO_SBUS_INPUT   (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11)
+#define GPIO_SBUS_OUTPUT  (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
+#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4)
+
+/* 
+ * High-resolution timer
+ */
+#define HRT_TIMER		1	/* use timer1 for the HRT */
+#define HRT_TIMER_CHANNEL	2	/* use capture/compare channel 2 */
+#define HRT_PPM_CHANNEL		1	/* use capture/compare channel 1 */
+#define GPIO_PPM_IN		(GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_PORTE|GPIO_PIN9)
+
+/* LED definitions ******************************************************************/
+/* PX4 has two LEDs that we will encode as: */
+
+#define LED_STARTED       0  /* LED? */
+#define LED_HEAPALLOCATE  1  /* LED? */
+#define LED_IRQSENABLED   2  /* LED? + LED? */
+#define LED_STACKCREATED  3  /* LED? */
+#define LED_INIRQ         4  /* LED? + LED? */
+#define LED_SIGNAL        5  /* LED? + LED? */
+#define LED_ASSERTION     6  /* LED? + LED? + LED? */
+#define LED_PANIC         7  /* N/C  + N/C  + N/C + LED? */
+
diff --git a/src/drivers/boards/px4io-v2/module.mk b/src/drivers/boards/px4io-v2/module.mk
new file mode 100644
index 000000000..85f94e8be
--- /dev/null
+++ b/src/drivers/boards/px4io-v2/module.mk
@@ -0,0 +1,6 @@
+#
+# Board-specific startup code for the PX4IOv2
+#
+
+SRCS		= px4iov2_init.c \
+		  px4iov2_pwm_servo.c
diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c
new file mode 100644
index 000000000..9f8c0eeb2
--- /dev/null
+++ b/src/drivers/boards/px4io-v2/px4iov2_init.c
@@ -0,0 +1,158 @@
+/****************************************************************************
+ *
+ *   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 px4iov2_init.c
+ *
+ * PX4FMU-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 <stm32.h>
+#include "board_config.h"
+
+#include <arch/board/board.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
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/************************************************************************************
+ * 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 GPIOs */
+
+	/* LEDS - default to off */
+	stm32_configgpio(GPIO_LED1);
+	stm32_configgpio(GPIO_LED2);
+	stm32_configgpio(GPIO_LED3);
+
+	stm32_configgpio(GPIO_BTN_SAFETY);
+
+	/* spektrum power enable is active high - enable it by default */
+	stm32_configgpio(GPIO_SPEKTRUM_PWR_EN);
+
+	stm32_configgpio(GPIO_SERVO_FAULT_DETECT);
+
+	/* RSSI inputs */
+	stm32_configgpio(GPIO_TIM_RSSI); /* xxx alternate function */
+	stm32_configgpio(GPIO_ADC_RSSI);
+
+	/* servo rail voltage */
+	stm32_configgpio(GPIO_ADC_VSERVO);
+
+	stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */
+	stm32_configgpio(GPIO_SBUS_OUTPUT);
+	
+	/* sbus output enable is active low - disable it by default */
+	stm32_gpiowrite(GPIO_SBUS_OENABLE, true);
+	stm32_configgpio(GPIO_SBUS_OENABLE);
+
+	stm32_configgpio(GPIO_PPM); /* xxx alternate function */
+
+	stm32_gpiowrite(GPIO_PWM1, false);
+	stm32_configgpio(GPIO_PWM1);
+
+	stm32_gpiowrite(GPIO_PWM2, false);
+	stm32_configgpio(GPIO_PWM2);
+
+	stm32_gpiowrite(GPIO_PWM3, false);
+	stm32_configgpio(GPIO_PWM3);
+
+	stm32_gpiowrite(GPIO_PWM4, false);
+	stm32_configgpio(GPIO_PWM4);
+
+	stm32_gpiowrite(GPIO_PWM5, false);
+	stm32_configgpio(GPIO_PWM5);
+
+	stm32_gpiowrite(GPIO_PWM6, false);
+	stm32_configgpio(GPIO_PWM6);
+
+	stm32_gpiowrite(GPIO_PWM7, false);
+	stm32_configgpio(GPIO_PWM7);
+
+	stm32_gpiowrite(GPIO_PWM8, false);
+	stm32_configgpio(GPIO_PWM8);
+}
diff --git a/src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c b/src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c
new file mode 100644
index 000000000..4f1b9487c
--- /dev/null
+++ b/src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c
@@ -0,0 +1,123 @@
+/****************************************************************************
+ *
+ *  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 px4iov2_pwm_servo.c
+ *
+ * Configuration data for the stm32 pwm_servo driver.
+ *
+ * Note that these arrays must always be fully-sized.
+ */
+
+#include <stdint.h>
+
+#include <drivers/stm32/drv_pwm_servo.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_pwm_output.h>
+
+#include <stm32.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
+
+__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
+	{
+		.base = STM32_TIM2_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM2EN,
+		.clock_freq = STM32_APB1_TIM2_CLKIN
+	},
+	{
+		.base = STM32_TIM3_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM3EN,
+		.clock_freq = STM32_APB1_TIM3_CLKIN
+	},
+	{
+		.base = STM32_TIM4_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM4EN,
+		.clock_freq = STM32_APB1_TIM4_CLKIN
+	}
+};
+
+__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
+	{
+		.gpio = GPIO_TIM2_CH1OUT,
+		.timer_index = 0,
+		.timer_channel = 1,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM2_CH2OUT,
+		.timer_index = 0,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM4_CH3OUT,
+		.timer_index = 2,
+		.timer_channel = 3,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM4_CH4OUT,
+		.timer_index = 2,
+		.timer_channel = 4,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM3_CH1OUT,
+		.timer_index = 1,
+		.timer_channel = 1,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM3_CH2OUT,
+		.timer_index = 1,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM3_CH3OUT,
+		.timer_index = 1,
+		.timer_channel = 3,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM3_CH4OUT,
+		.timer_index = 1,
+		.timer_channel = 4,
+		.default_value = 1000,
+	}
+};
diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp
index 422321850..b157b3f18 100644
--- a/src/drivers/device/cdev.cpp
+++ b/src/drivers/device/cdev.cpp
@@ -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
@@ -38,6 +38,7 @@
  */
 
 #include "device.h"
+#include "drivers/drv_device.h"
 
 #include <sys/ioctl.h>
 #include <arch/irq.h>
@@ -93,6 +94,7 @@ CDev::CDev(const char *name,
 	Device(name, irq),
 	// public
 	// protected
+	_pub_blocked(false),
 	// private
 	_devname(devname),
 	_registered(false),
@@ -109,23 +111,59 @@ CDev::~CDev()
 }
 
 int
-CDev::init()
+CDev::register_class_devname(const char *class_devname)
 {
-	int ret = OK;
+	if (class_devname == nullptr) {
+		return -EINVAL;
+	}
+	int class_instance = 0;
+	int ret = -ENOSPC;
+	while (class_instance < 4) {
+		if (class_instance == 0) {
+			ret = register_driver(class_devname, &fops, 0666, (void *)this);
+			if (ret == OK) break;
+		} else {
+			char name[32];
+			snprintf(name, sizeof(name), "%s%u", class_devname, class_instance);
+			ret = register_driver(name, &fops, 0666, (void *)this);
+			if (ret == OK) break;
+		}
+		class_instance++;
+	}
+	if (class_instance == 4)
+		return ret;
+	return class_instance;
+}
 
+int
+CDev::unregister_class_devname(const char *class_devname, unsigned class_instance)
+{
+	if (class_instance > 0) {
+		char name[32];
+		snprintf(name, sizeof(name), "%s%u", class_devname, class_instance);
+		return unregister_driver(name);
+	}
+	return unregister_driver(class_devname);
+}
+
+int
+CDev::init()
+{
 	// base class init first
-	ret = Device::init();
+	int ret = Device::init();
 
 	if (ret != OK)
 		goto out;
 
 	// now register the driver
-	ret = register_driver(_devname, &fops, 0666, (void *)this);
+	if (_devname != nullptr) {
+		ret = register_driver(_devname, &fops, 0666, (void *)this);
 
-	if (ret != OK)
-		goto out;
+		if (ret != OK)
+			goto out;
 
-	_registered = true;
+		_registered = true;
+	}
 
 out:
 	return ret;
@@ -220,6 +258,14 @@ CDev::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case DIOC_GETPRIV:
 		*(void **)(uintptr_t)arg = (void *)this;
 		return OK;
+		break;
+	case DEVIOCSPUBBLOCK:
+		_pub_blocked = (arg != 0);
+		return OK;
+		break;
+	case DEVIOCGPUBBLOCK:
+		return _pub_blocked;
+		break;
 	}
 
 	return -ENOTTY;
@@ -395,4 +441,4 @@ cdev_poll(struct file *filp, struct pollfd *fds, bool setup)
 	return cdev->poll(filp, fds, setup);
 }
 
-} // namespace device
\ No newline at end of file
+} // namespace device
diff --git a/src/drivers/device/device.cpp b/src/drivers/device/device.cpp
index 04a5222c3..683455149 100644
--- a/src/drivers/device/device.cpp
+++ b/src/drivers/device/device.cpp
@@ -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
@@ -223,5 +223,22 @@ interrupt(int irq, void *context)
 	return OK;
 }
 
+int
+Device::read(unsigned offset, void *data, unsigned count)
+{
+	return -ENODEV;
+}
+
+int
+Device::write(unsigned offset, void *data, unsigned count)
+{
+	return -ENODEV;
+}
+
+int
+Device::ioctl(unsigned operation, unsigned &arg)
+{
+	return -ENODEV;
+}
 
-} // namespace device
\ No newline at end of file
+} // namespace device
diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h
index 7d375aab9..d99f22922 100644
--- a/src/drivers/device/device.h
+++ b/src/drivers/device/device.h
@@ -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
@@ -69,10 +69,61 @@ class __EXPORT Device
 {
 public:
 	/**
+	 * Destructor.
+	 *
+	 * Public so that anonymous devices can be destroyed.
+	 */
+	virtual ~Device();
+
+	/**
 	 * Interrupt handler.
 	 */
 	virtual void	interrupt(void *ctx);	/**< interrupt handler */
 
+	/*
+	 * Direct access methods.
+	 */
+
+	/**
+	 * Initialise the driver and make it ready for use.
+	 *
+	 * @return	OK if the driver initialized OK, negative errno otherwise;
+	 */
+	virtual int	init();
+
+	/**
+	 * Read directly from the device.
+	 *
+	 * The actual size of each unit quantity is device-specific.
+	 *
+	 * @param offset	The device address at which to start reading
+	 * @param data		The buffer into which the read values should be placed.
+	 * @param count		The number of items to read.
+	 * @return		The number of items read on success, negative errno otherwise.
+	 */
+	virtual int	read(unsigned address, void *data, unsigned count);
+
+	/**
+	 * Write directly to the device.
+	 *
+	 * The actual size of each unit quantity is device-specific.
+	 *
+	 * @param address	The device address at which to start writing.
+	 * @param data		The buffer from which values should be read.
+	 * @param count		The number of items to write.
+	 * @return		The number of items written on success, negative errno otherwise.
+	 */	 
+	virtual int	write(unsigned address, void *data, unsigned count);
+
+	/**
+	 * Perform a device-specific operation.
+	 *
+	 * @param operation	The operation to perform.
+	 * @param arg		An argument to the operation.
+	 * @return		Negative errno on error, OK or positive value on success.
+	 */
+	virtual int	ioctl(unsigned operation, unsigned &arg);
+
 protected:
 	const char	*_name;			/**< driver name */
 	bool		_debug_enabled;		/**< if true, debug messages are printed */
@@ -85,14 +136,6 @@ protected:
 	 */
 	Device(const char *name,
 	       int irq = 0);
-	~Device();
-
-	/**
-	 * Initialise the driver and make it ready for use.
-	 *
-	 * @return	OK if the driver initialised OK.
-	 */
-	virtual int	init();
 
 	/**
 	 * Enable the device interrupt
@@ -189,7 +232,7 @@ public:
 	/**
 	 * Destructor
 	 */
-	~CDev();
+	virtual ~CDev();
 
 	virtual int	init();
 
@@ -282,6 +325,7 @@ public:
 	 * Test whether the device is currently open.
 	 *
 	 * This can be used to avoid tearing down a device that is still active.
+	 * Note - not virtual, cannot be overridden by a subclass.
 	 *
 	 * @return              True if the device is currently open.
 	 */
@@ -352,6 +396,27 @@ protected:
 	 */
 	virtual int	close_last(struct file *filp);
 
+        /**
+	 * Register a class device name, automatically adding device
+	 * class instance suffix if need be.
+	 *
+	 * @param class_devname   Device class name
+	 * @return class_instamce Class instance created, or -errno on failure
+	 */
+	virtual int register_class_devname(const char *class_devname);
+
+        /**
+	 * Register a class device name, automatically adding device
+	 * class instance suffix if need be.
+	 *
+	 * @param class_devname   Device class name
+	 * @param class_instance  Device class instance from register_class_devname()
+	 * @return		  OK on success, -errno otherwise
+	 */
+	virtual int unregister_class_devname(const char *class_devname, unsigned class_instance);
+
+	bool		_pub_blocked;		/**< true if publishing should be blocked */
+
 private:
 	static const unsigned _max_pollwaiters = 8;
 
@@ -396,9 +461,9 @@ public:
 	    const char *devname,
 	    uint32_t base,
 	    int irq = 0);
-	~PIO();
+	virtual ~PIO();
 
-	int		init();
+	virtual int	init();
 
 protected:
 
@@ -407,7 +472,7 @@ protected:
 	 *
 	 * @param offset	Register offset in bytes from the base address.
 	 */
-	uint32_t reg(uint32_t offset) {
+	uint32_t	reg(uint32_t offset) {
 		return *(volatile uint32_t *)(_base + offset);
 	}
 
@@ -444,4 +509,7 @@ private:
 
 } // namespace device
 
-#endif /* _DEVICE_DEVICE_H */
\ No newline at end of file
+// class instance for primary driver of each class
+#define CLASS_DEVICE_PRIMARY 0
+
+#endif /* _DEVICE_DEVICE_H */
diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h
index b4a9cdd53..549879352 100644
--- a/src/drivers/device/i2c.h
+++ b/src/drivers/device/i2c.h
@@ -55,13 +55,11 @@ class __EXPORT I2C : public CDev
 
 public:
 
-    /**
-     * Get the address
-     */
-    uint16_t get_address() {
-      return _address;
-    }
-
+	/**
+	 * Get the address
+	 */
+	int16_t		get_address() { return _address; }
+	
 protected:
 	/**
 	 * The number of times a read or write operation will be retried on
@@ -90,7 +88,7 @@ protected:
 	    uint16_t address,
 	    uint32_t frequency,
 	    int irq = 0);
-	~I2C();
+	virtual ~I2C();
 
 	virtual int	init();
 
diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h
new file mode 100644
index 000000000..a9e22eaa6
--- /dev/null
+++ b/src/drivers/device/ringbuffer.h
@@ -0,0 +1,509 @@
+/****************************************************************************
+ *
+ *   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 ringbuffer.h
+ *
+ * A flexible ringbuffer class.
+ */
+
+#pragma once
+
+class RingBuffer {
+public:
+	RingBuffer(unsigned ring_size, size_t entry_size);
+	virtual ~RingBuffer();
+
+	/**
+	 * Put an item into the buffer.
+	 *
+	 * @param val		Item to put
+	 * @return		true if the item was put, false if the buffer is full
+	 */
+	bool			put(const void *val, size_t val_size = 0);
+
+	bool			put(int8_t val);
+	bool			put(uint8_t val);
+	bool			put(int16_t val);
+	bool			put(uint16_t val);
+	bool			put(int32_t val);
+	bool			put(uint32_t val);
+	bool			put(int64_t val);
+	bool			put(uint64_t val);
+	bool			put(float val);
+	bool			put(double val);
+
+	/**
+	 * Force an item into the buffer, discarding an older item if there is not space.
+	 *
+	 * @param val		Item to put
+	 * @return		true if an item was discarded to make space
+	 */
+	bool			force(const void *val, size_t val_size = 0);
+
+	bool			force(int8_t val);
+	bool			force(uint8_t val);
+	bool			force(int16_t val);
+	bool			force(uint16_t val);
+	bool			force(int32_t val);
+	bool			force(uint32_t val);
+	bool			force(int64_t val);
+	bool			force(uint64_t val);
+	bool			force(float val);
+	bool			force(double val);
+
+	/**
+	 * Get an item from the buffer.
+	 *
+	 * @param val		Item that was gotten
+	 * @return		true if an item was got, false if the buffer was empty.
+	 */
+	bool			get(void *val, size_t val_size = 0);
+
+	bool			get(int8_t &val);
+	bool			get(uint8_t &val);
+	bool			get(int16_t &val);
+	bool			get(uint16_t &val);
+	bool			get(int32_t &val);
+	bool			get(uint32_t &val);
+	bool			get(int64_t &val);
+	bool			get(uint64_t &val);
+	bool			get(float &val);
+	bool			get(double &val);
+
+	/*
+	 * Get the number of slots free in the buffer.
+	 *
+	 * @return		The number of items that can be put into the buffer before
+	 *			it becomes full.
+	 */
+	unsigned		space(void);
+
+	/*
+	 * Get the number of items in the buffer.
+	 *
+	 * @return		The number of items that can be got from the buffer before
+	 *			it becomes empty.
+	 */
+	unsigned		count(void);
+
+	/*
+	 * Returns true if the buffer is empty.
+	 */
+	bool			empty();
+
+	/*
+	 * Returns true if the buffer is full.
+	 */
+	bool			full();
+
+	/*
+	 * Returns the capacity of the buffer, or zero if the buffer could
+	 * not be allocated.
+	 */
+	unsigned		size();
+
+	/*
+	 * Empties the buffer.
+	 */
+	void			flush();
+
+	/*
+	 * resize the buffer. This is unsafe to be called while
+	 * a producer or consuming is running. Caller is responsible
+	 * for any locking needed
+	 *
+	 * @param new_size	new size for buffer
+	 * @return		true if the resize succeeds, false if
+	 * 			not (allocation error)
+	 */
+	bool			resize(unsigned new_size);
+
+	/*
+	 * printf() some info on the buffer
+	 */
+	void			print_info(const char *name);
+
+private:
+	unsigned		_num_items;
+	const size_t		_item_size;
+	char			*_buf;	
+	volatile unsigned	_head;	/**< insertion point in _item_size units */
+	volatile unsigned	_tail;	/**< removal point in _item_size units */
+
+	unsigned		_next(unsigned index);
+};
+
+RingBuffer::RingBuffer(unsigned num_items, size_t item_size) :
+	_num_items(num_items),
+	_item_size(item_size),
+	_buf(new char[(_num_items+1) * item_size]),
+ 	_head(_num_items),
+ 	_tail(_num_items)
+{}
+
+RingBuffer::~RingBuffer()
+{
+	if (_buf != nullptr)
+		delete[] _buf;
+}
+
+unsigned
+RingBuffer::_next(unsigned index)
+{
+	return (0 == index) ? _num_items : (index - 1); 
+}
+
+bool
+RingBuffer::empty()
+{
+	return _tail == _head; 
+}
+
+bool
+RingBuffer::full()
+{
+	return _next(_head) == _tail; 
+}
+
+unsigned
+RingBuffer::size()
+{
+	return (_buf != nullptr) ? _num_items : 0; 
+}
+
+void
+RingBuffer::flush()
+{
+	while (!empty())
+		get(NULL);
+}
+
+bool
+RingBuffer::put(const void *val, size_t val_size) 
+{
+	unsigned next = _next(_head);
+	if (next != _tail) {
+		if ((val_size == 0) || (val_size > _item_size))
+			val_size = _item_size;
+		memcpy(&_buf[_head * _item_size], val, val_size);
+		_head = next;
+		return true;
+	} else {
+		return false;
+	}
+}
+
+bool
+RingBuffer::put(int8_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(uint8_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(int16_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(uint16_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(int32_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(uint32_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(int64_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(uint64_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(float val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(double val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(const void *val, size_t val_size)
+{
+	bool overwrote = false;
+
+	for (;;) {
+		if (put(val, val_size))
+			break;
+		get(NULL);
+		overwrote = true;
+	}
+	return overwrote;
+}
+
+bool
+RingBuffer::force(int8_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(uint8_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(int16_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(uint16_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(int32_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(uint32_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(int64_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(uint64_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(float val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(double val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(void *val, size_t val_size) 
+{
+	if (_tail != _head) {
+		unsigned candidate;
+		unsigned next;
+
+		if ((val_size == 0) || (val_size > _item_size))
+			val_size = _item_size;
+
+		do {
+			/* decide which element we think we're going to read */
+			candidate = _tail;
+
+			/* and what the corresponding next index will be */
+			next = _next(candidate);
+
+			/* go ahead and read from this index */
+			if (val != NULL)
+				memcpy(val, &_buf[candidate * _item_size], val_size);
+
+			/* if the tail pointer didn't change, we got our item */
+		} while (!__sync_bool_compare_and_swap(&_tail, candidate, next));
+
+		return true;
+	} else {
+		return false;
+	}
+}
+
+bool
+RingBuffer::get(int8_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(uint8_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(int16_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(uint16_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(int32_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(uint32_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(int64_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(uint64_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(float &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(double &val)
+{
+	return get(&val, sizeof(val));
+}
+
+unsigned
+RingBuffer::space(void) 
+{
+	unsigned tail, head;
+
+	/*
+	 * Make a copy of the head/tail pointers in a fashion that
+	 * may err on the side of under-estimating the free space
+	 * in the buffer in the case that the buffer is being updated
+	 * asynchronously with our check.
+	 * If the head pointer changes (reducing space) while copying,
+	 * re-try the copy.
+	 */
+	do {
+		head = _head;
+		tail = _tail;
+	} while (head != _head);
+
+	return (tail >= head) ? (_num_items - (tail - head)) : (head - tail - 1);
+}
+
+unsigned
+RingBuffer::count(void) 
+{
+	/*
+	 * Note that due to the conservative nature of space(), this may
+	 * over-estimate the number of items in the buffer.
+	 */
+	return _num_items - space();
+}
+
+bool
+RingBuffer::resize(unsigned new_size) 
+{
+	char *old_buffer;
+	char *new_buffer = new char [(new_size+1) * _item_size];
+	if (new_buffer == nullptr) {
+		return false;
+	}
+	old_buffer = _buf;
+	_buf = new_buffer;
+	_num_items = new_size;
+	_head = new_size;
+	_tail = new_size;
+	delete[] old_buffer;
+	return true;
+}
+
+void
+RingBuffer::print_info(const char *name) 
+{
+	printf("%s	%u/%u (%u/%u @ %p)\n",
+	       name, 
+	       _num_items, 
+	       _num_items * _item_size, 
+	       _head, 
+	       _tail, 
+	       _buf);
+}
diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp
index 8fffd60cb..fed6c312c 100644
--- a/src/drivers/device/spi.cpp
+++ b/src/drivers/device/spi.cpp
@@ -67,6 +67,7 @@ SPI::SPI(const char *name,
 	CDev(name, devname, irq),
 	// public
 	// protected
+	locking_mode(LOCK_PREEMPTION),
 	// private
 	_bus(bus),
 	_device(device),
@@ -132,13 +133,25 @@ SPI::probe()
 int
 SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
 {
+	irqstate_t	state;
 
 	if ((send == nullptr) && (recv == nullptr))
 		return -EINVAL;
 
-	/* do common setup */
-	if (!up_interrupt_context())
-		SPI_LOCK(_dev, true);
+	/* 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;
+		}
+	}
 
 	SPI_SETFREQUENCY(_dev, _frequency);
 	SPI_SETMODE(_dev, _mode);
@@ -151,10 +164,27 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
 	/* and clean up */
 	SPI_SELECT(_dev, _device, false);
 
-	if (!up_interrupt_context())
-		SPI_LOCK(_dev, 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 d2d01efb3..299575dc5 100644
--- a/src/drivers/device/spi.h
+++ b/src/drivers/device/spi.h
@@ -71,7 +71,7 @@ protected:
 	    enum spi_mode_e mode,
 	    uint32_t frequency,
 	    int irq = 0);
-	~SPI();
+	virtual ~SPI();
 
 	virtual int	init();
 
@@ -101,6 +101,28 @@ protected:
 	 */
 	int		transfer(uint8_t *send, uint8_t *recv, unsigned len);
 
+	/**
+	 * Set the SPI bus frequency
+	 * This is used to change frequency on the fly. Some sensors
+	 * (such as the MPU6000) need a lower frequency for setup
+	 * registers and can handle higher frequency for sensor 
+	 * value registers
+	 *
+	 * @param frequency	Frequency to set (Hz)
+	 */
+        void		set_frequency(uint32_t frequency);
+
+	/**
+	 * Locking modes supported by the driver.
+	 */
+	enum LockMode {
+		LOCK_PREEMPTION,	/**< the default; lock against all forms of preemption. */
+		LOCK_THREADS,		/**< lock only against other threads, using SPI_LOCK */
+		LOCK_NONE		/**< perform no locking, only safe if the bus is entirely private */
+	};
+
+	LockMode	locking_mode;	/**< selected locking mode */
+
 private:
 	int			_bus;
 	enum spi_dev_e		_device;
diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h
index 794de584b..7d93ed938 100644
--- a/src/drivers/drv_accel.h
+++ b/src/drivers/drv_accel.h
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Accelerometer driver interface.
+ * @file drv_accel.h
+ *
+ * Accelerometer driver interface.
  */
 
 #ifndef _DRV_ACCEL_H
@@ -52,6 +54,7 @@
  */
 struct accel_report {
 	uint64_t timestamp;
+	uint64_t error_count;
 	float x;		/**< acceleration in the NED X board axis in m/s^2 */
 	float y;		/**< acceleration in the NED Y board axis in m/s^2 */
 	float z;		/**< acceleration in the NED Z board axis in m/s^2 */
@@ -65,7 +68,7 @@ struct accel_report {
 	int16_t temperature_raw;
 };
 
-/** accel scaling factors; Vout = (Vin * Vscale) + Voffset */
+/** accel scaling factors; Vout = Vscale * (Vin + Voffset) */
 struct accel_scale {
 	float	x_offset;
 	float	x_scale;
diff --git a/src/drivers/drv_airspeed.h b/src/drivers/drv_airspeed.h
index bffc35c62..78f31495d 100644
--- a/src/drivers/drv_airspeed.h
+++ b/src/drivers/drv_airspeed.h
@@ -32,7 +32,10 @@
  ****************************************************************************/
 
 /**
- * @file Airspeed driver interface.
+ * @file drv_airspeed.h
+ *
+ * Airspeed driver interface.
+ * 
  * @author Simon Wilks
  */
 
@@ -57,5 +60,14 @@
 #define _AIRSPEEDIOCBASE		(0x7700)
 #define __AIRSPEEDIOC(_n)		(_IOC(_AIRSPEEDIOCBASE, _n))
 
+#define AIRSPEEDIOCSSCALE		__AIRSPEEDIOC(0)
+#define AIRSPEEDIOCGSCALE		__AIRSPEEDIOC(1)
+
+
+/** airspeed scaling factors; out = (in * Vscale) + offset */
+struct airspeed_scale {
+	float	offset_pa;
+	float	scale;
+};
 
 #endif /* _DRV_AIRSPEED_H */
diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h
index 176f798c0..e2f0137ae 100644
--- a/src/drivers/drv_baro.h
+++ b/src/drivers/drv_baro.h
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Barometric pressure sensor driver interface.
+ * @file drv_baro.h
+ *
+ * Barometric pressure sensor driver interface.
  */
 
 #ifndef _DRV_BARO_H
@@ -55,6 +57,7 @@ struct baro_report {
 	float altitude;
 	float temperature;
 	uint64_t timestamp;
+	uint64_t error_count;
 };
 
 /*
diff --git a/src/drivers/drv_device.h b/src/drivers/drv_device.h
new file mode 100644
index 000000000..b310beb74
--- /dev/null
+++ b/src/drivers/drv_device.h
@@ -0,0 +1,62 @@
+/****************************************************************************
+ *
+ *   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_device.h
+ *
+ * Generic device / sensor interface.
+ */
+
+#ifndef _DRV_DEVICE_H
+#define _DRV_DEVICE_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_sensor.h"
+#include "drv_orb_dev.h"
+
+/*
+ * ioctl() definitions
+ */
+
+#define _DEVICEIOCBASE		(0x100)
+#define _DEVICEIOC(_n)		(_IOC(_DEVICEIOCBASE, _n))
+
+/** ask device to stop publishing */
+#define DEVIOCSPUBBLOCK	_DEVICEIOC(0)
+
+/** check publication block status */
+#define DEVIOCGPUBBLOCK	_DEVICEIOC(1)
+
+#endif /* _DRV_DEVICE_H */
diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h
index 7e3998fca..f60964c2b 100644
--- a/src/drivers/drv_gpio.h
+++ b/src/drivers/drv_gpio.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   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
@@ -46,7 +46,7 @@
 /*
  * PX4FMU GPIO numbers.
  *
- * For shared pins, alternate function 1 selects the non-GPIO mode 
+ * For shared pins, alternate function 1 selects the non-GPIO mode
  * (USART2, CAN2, etc.)
  */
 # define GPIO_EXT_1		(1<<0)		/**< high-power GPIO 1 */
@@ -59,48 +59,47 @@
 # define GPIO_CAN_RX		(1<<7)		/**< CAN2 RX */
 
 /**
- * Default GPIO device - other devices may also support this protocol if
- * they also export GPIO-like things.  This is always the GPIOs on the
- * main board.
+ * Device paths for things that support the GPIO ioctl protocol.
  */
-# define GPIO_DEVICE_PATH	"/dev/px4fmu"
+# define PX4FMU_DEVICE_PATH	"/dev/px4fmu"
+# define PX4IO_DEVICE_PATH	"/dev/px4io"
 
 #endif
 
-#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
 /*
- * PX4IO GPIO numbers.
+ * PX4FMUv2 GPIO numbers.
  *
- * XXX note that these are here for reference/future use; currently
- * there is no good way to wire these up without a common STM32 GPIO
- * driver, which isn't implemented yet.
+ * There are no alternate functions on this board.
  */
-/* outputs */
-# define GPIO_ACC1_POWER	(1<<0)		/**< accessory power 1 */
-# define GPIO_ACC2_POWER	(1<<1)		/**< accessory power 2 */
-# define GPIO_SERVO_POWER	(1<<2)		/**< servo power */
-# define GPIO_RELAY1		(1<<3)		/**< relay 1 */
-# define GPIO_RELAY2		(1<<4)		/**< relay 2 */
-# define GPIO_LED_BLUE		(1<<5)		/**< blue LED */
-# define GPIO_LED_AMBER		(1<<6)		/**< amber/red LED */
-# define GPIO_LED_SAFETY	(1<<7)		/**< safety LED */
-
-/* inputs */
-# define GPIO_ACC_OVERCURRENT	(1<<8)		/**< accessory 1/2 overcurrent detect */
-# define GPIO_SERVO_OVERCURRENT	(1<<9)		/**< servo overcurrent detect */
-# define GPIO_SAFETY_BUTTON	(1<<10)		/**< safety button pressed */
+# define GPIO_SERVO_1		(1<<0)		/**< servo 1 output */
+# define GPIO_SERVO_2		(1<<1)		/**< servo 2 output */
+# define GPIO_SERVO_3		(1<<2)		/**< servo 3 output */
+# define GPIO_SERVO_4		(1<<3)		/**< servo 4 output */
+# define GPIO_SERVO_5		(1<<4)		/**< servo 5 output */
+# define GPIO_SERVO_6		(1<<5)		/**< servo 6 output */
+
+# define GPIO_5V_PERIPH_EN	(1<<6)		/**< PA8 - !VDD_5V_PERIPH_EN */
+# define GPIO_3V3_SENSORS_EN	(1<<7)		/**< PE3 - VDD_3V3_SENSORS_EN */
+# define GPIO_BRICK_VALID	(1<<8)		/**< PB5 - !VDD_BRICK_VALID */
+# define GPIO_SERVO_VALID	(1<<9)		/**< PB7 - !VDD_SERVO_VALID */
+# define GPIO_5V_HIPOWER_OC	(1<<10)		/**< PE10 - !VDD_5V_HIPOWER_OC */
+# define GPIO_5V_PERIPH_OC	(1<<11)		/**< PE10 - !VDD_5V_PERIPH_OC */
 
 /**
- * Default GPIO device - other devices may also support this protocol if
- * they also export GPIO-like things.  This is always the GPIOs on the
- * main board.
+ * Device paths for things that support the GPIO ioctl protocol.
  */
-# define GPIO_DEVICE_PATH	"/dev/px4io"
+# define PX4FMU_DEVICE_PATH	"/dev/px4fmu"
+# define PX4IO_DEVICE_PATH	"/dev/px4io"
 
 #endif
 
-#ifndef GPIO_DEVICE_PATH
-#  error No GPIO support for this board.
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
+/* no GPIO driver on the PX4IOv1 board */
+#endif
+
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+/* no GPIO driver on the PX4IOv2 board */
 #endif
 
 /*
@@ -145,4 +144,6 @@
 /** read all the GPIOs and return their values in *(uint32_t *)arg */
 #define GPIO_GET	GPIOC(12)
 
+#define GPIO_SENSOR_RAIL_RESET	GPIOC(13)
+
 #endif /* _DRV_GPIO_H */
\ No newline at end of file
diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h
index 1dda8ef0b..06e3535b3 100644
--- a/src/drivers/drv_gps.h
+++ b/src/drivers/drv_gps.h
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file GPS driver interface.
+ * @file drv_gps.h
+ *
+ * GPS driver interface.
  */
 
 #ifndef _DRV_GPS_H
@@ -51,8 +53,7 @@
 typedef enum {
 	GPS_DRIVER_MODE_NONE = 0,
 	GPS_DRIVER_MODE_UBX,
-	GPS_DRIVER_MODE_MTK,
-	GPS_DRIVER_MODE_NMEA,
+	GPS_DRIVER_MODE_MTK
 } gps_driver_mode_t;
 
 
diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h
index 1d0fef6fc..2ae8c7058 100644
--- a/src/drivers/drv_gyro.h
+++ b/src/drivers/drv_gyro.h
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Gyroscope driver interface.
+ * @file drv_gyro.h
+ *
+ * Gyroscope driver interface.
  */
 
 #ifndef _DRV_GYRO_H
@@ -52,6 +54,7 @@
  */
 struct gyro_report {
 	uint64_t timestamp;
+	uint64_t error_count;
 	float x;		/**< angular velocity in the NED X board axis in rad/s */
 	float y;		/**< angular velocity in the NED Y board axis in rad/s */
 	float z;		/**< angular velocity in the NED Z board axis in rad/s */
diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h
index 8a99eeca7..d130d68b3 100644
--- a/src/drivers/drv_hrt.h
+++ b/src/drivers/drv_hrt.h
@@ -142,6 +142,20 @@ __EXPORT extern bool	hrt_called(struct hrt_call *entry);
 __EXPORT extern void	hrt_cancel(struct hrt_call *entry);
 
 /*
+ * initialise a hrt_call structure
+ */
+__EXPORT extern void	hrt_call_init(struct hrt_call *entry);
+
+/*
+ * delay a hrt_call_every() periodic call by the given number of
+ * microseconds. This should be called from within the callout to
+ * cause the callout to be re-scheduled for a later time. The periodic
+ * callouts will then continue from that new base time at the
+ * previously specified period.
+ */
+__EXPORT extern void	hrt_call_delay(struct hrt_call *entry, hrt_abstime delay);
+
+/*
  * Initialise the HRT.
  */
 __EXPORT extern void	hrt_init(void);
diff --git a/src/drivers/drv_led.h b/src/drivers/drv_led.h
index 21044f620..4ce04696e 100644
--- a/src/drivers/drv_led.h
+++ b/src/drivers/drv_led.h
@@ -54,12 +54,13 @@
 
 #define LED_ON			_IOC(_LED_BASE, 0)
 #define LED_OFF			_IOC(_LED_BASE, 1)
+#define LED_TOGGLE		_IOC(_LED_BASE, 2)
 
 __BEGIN_DECLS
 
 /*
  * Initialise the LED driver.
  */
-__EXPORT extern void drv_led_start(void);
+__EXPORT void drv_led_start(void);
 
 __END_DECLS
diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h
index 9aab995a1..06107bd3d 100644
--- a/src/drivers/drv_mag.h
+++ b/src/drivers/drv_mag.h
@@ -54,6 +54,7 @@
  */
 struct mag_report {
 	uint64_t timestamp;
+	uint64_t error_count;
 	float x;
 	float y;
 	float z;
@@ -90,25 +91,37 @@ ORB_DECLARE(sensor_mag);
 /** set the mag internal sample rate to at least (arg) Hz */
 #define MAGIOCSSAMPLERATE	_MAGIOC(0)
 
+/** return the mag internal sample rate in Hz */
+#define MAGIOCGSAMPLERATE	_MAGIOC(1)
+
 /** set the mag internal lowpass filter to no lower than (arg) Hz */
-#define MAGIOCSLOWPASS		_MAGIOC(1)
+#define MAGIOCSLOWPASS		_MAGIOC(2)
+
+/** return the mag internal lowpass filter in Hz */
+#define MAGIOCGLOWPASS		_MAGIOC(3)
 
 /** set the mag scaling constants to the structure pointed to by (arg) */
-#define MAGIOCSSCALE		_MAGIOC(2)
+#define MAGIOCSSCALE		_MAGIOC(4)
 
 /** copy the mag scaling constants to the structure pointed to by (arg) */
-#define MAGIOCGSCALE		_MAGIOC(3)
+#define MAGIOCGSCALE		_MAGIOC(5)
 
 /** set the measurement range to handle (at least) arg Gauss */
-#define MAGIOCSRANGE		_MAGIOC(4)
+#define MAGIOCSRANGE		_MAGIOC(6)
+
+/** return the current mag measurement range in Gauss */
+#define MAGIOCGRANGE		_MAGIOC(7)
 
 /** perform self-calibration, update scale factors to canonical units */
-#define MAGIOCCALIBRATE		_MAGIOC(5)
+#define MAGIOCCALIBRATE		_MAGIOC(8)
 
 /** excite strap */
-#define MAGIOCEXSTRAP		_MAGIOC(6)
+#define MAGIOCEXSTRAP		_MAGIOC(9)
 
 /** perform self test and report status */
-#define MAGIOCSELFTEST		_MAGIOC(7)
+#define MAGIOCSELFTEST		_MAGIOC(10)
+
+/** determine if mag is external or onboard */
+#define MAGIOCGEXTERNAL		_MAGIOC(11)
 
 #endif /* _DRV_MAG_H */
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 56af71059..7a93e513e 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -65,6 +65,36 @@ __BEGIN_DECLS
 #define PWM_OUTPUT_MAX_CHANNELS	16
 
 /**
+ * Lowest minimum PWM in us
+ */
+#define PWM_LOWEST_MIN 900
+
+/**
+ * Default minimum PWM in us
+ */
+#define PWM_DEFAULT_MIN 1000
+
+/**
+ * Highest PWM allowed as the minimum PWM
+ */
+#define PWM_HIGHEST_MIN 1300
+
+/**
+ * Highest maximum PWM in us
+ */
+#define PWM_HIGHEST_MAX 2100
+
+/**
+ * Default maximum PWM in us
+ */
+#define PWM_DEFAULT_MAX 2000
+
+/**
+ * Lowest PWM allowed as the maximum PWM
+ */
+#define PWM_LOWEST_MAX 1700
+
+/**
  * Servo output signal type, value is actual servo output pulse
  * width in microseconds.
  */
@@ -79,6 +109,7 @@ typedef uint16_t	servo_position_t;
 struct pwm_output_values {
 	/** desired pulse widths for each of the supported channels */
 	servo_position_t	values[PWM_OUTPUT_MAX_CHANNELS];
+	unsigned			channel_count;
 };
 
 /*
@@ -100,20 +131,81 @@ ORB_DECLARE(output_pwm);
 /** disarm all servo outputs (stop generating pulses) */
 #define PWM_SERVO_DISARM	_IOC(_PWM_SERVO_BASE, 1)
 
+/** get default servo update rate */
+#define PWM_SERVO_GET_DEFAULT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2)
+
 /** set alternate servo update rate */
-#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2)
+#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 3)
+
+/** get alternate servo update rate */
+#define PWM_SERVO_GET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4)
 
 /** get the number of servos in *(unsigned *)arg */
-#define PWM_SERVO_GET_COUNT	_IOC(_PWM_SERVO_BASE, 3)
+#define PWM_SERVO_GET_COUNT	_IOC(_PWM_SERVO_BASE, 5)
 
 /** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */
-#define PWM_SERVO_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4)
+#define PWM_SERVO_SET_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 6)
+
+/** check the selected update rates */
+#define PWM_SERVO_GET_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 7)
 
 /** set the 'ARM ok' bit, which activates the safety switch */
-#define PWM_SERVO_SET_ARM_OK	_IOC(_PWM_SERVO_BASE, 5)
+#define PWM_SERVO_SET_ARM_OK	_IOC(_PWM_SERVO_BASE, 8)
 
 /** clear the 'ARM ok' bit, which deactivates the safety switch */
-#define PWM_SERVO_CLEAR_ARM_OK	_IOC(_PWM_SERVO_BASE, 6)
+#define PWM_SERVO_CLEAR_ARM_OK	_IOC(_PWM_SERVO_BASE, 9)
+
+/** start DSM bind */
+#define DSM_BIND_START	_IOC(_PWM_SERVO_BASE, 10)
+
+#define DSM2_BIND_PULSES 3	/* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */
+#define DSMX_BIND_PULSES 7	/* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */
+#define DSMX8_BIND_PULSES 10 	/* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */
+
+/** power up DSM receiver */
+#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11)
+
+/** set the PWM value for failsafe */
+#define PWM_SERVO_SET_FAILSAFE_PWM	_IOC(_PWM_SERVO_BASE, 12)
+
+/** get the PWM value for failsafe */
+#define PWM_SERVO_GET_FAILSAFE_PWM	_IOC(_PWM_SERVO_BASE, 13)
+
+/** set the PWM value when disarmed - should be no PWM (zero) by default */
+#define PWM_SERVO_SET_DISARMED_PWM	_IOC(_PWM_SERVO_BASE, 14)
+
+/** get the PWM value when disarmed */
+#define PWM_SERVO_GET_DISARMED_PWM	_IOC(_PWM_SERVO_BASE, 15)
+
+/** set the minimum PWM value the output will send */
+#define PWM_SERVO_SET_MIN_PWM	_IOC(_PWM_SERVO_BASE, 16)
+
+/** get the minimum PWM value the output will send */
+#define PWM_SERVO_GET_MIN_PWM	_IOC(_PWM_SERVO_BASE, 17)
+
+/** set the maximum PWM value the output will send */
+#define PWM_SERVO_SET_MAX_PWM	_IOC(_PWM_SERVO_BASE, 18)
+
+/** get the maximum PWM value the output will send */
+#define PWM_SERVO_GET_MAX_PWM	_IOC(_PWM_SERVO_BASE, 19)
+
+/** set the number of servos in (unsigned)arg - allows change of
+ * split between servos and GPIO */
+#define PWM_SERVO_SET_COUNT	_IOC(_PWM_SERVO_BASE, 20)
+
+/** set the lockdown override flag to enable outputs in HIL */
+#define PWM_SERVO_SET_DISABLE_LOCKDOWN		_IOC(_PWM_SERVO_BASE, 21)
+
+/** get the lockdown override flag to enable outputs in HIL */
+#define PWM_SERVO_GET_DISABLE_LOCKDOWN		_IOC(_PWM_SERVO_BASE, 22)
+
+/*
+ *
+ *
+ * WARNING WARNING WARNING! DO NOT EXCEED 31 IN IOC INDICES HERE!
+ *
+ *
+ */
 
 /** set a single servo to a specific value */
 #define PWM_SERVO_SET(_servo)	_IOC(_PWM_SERVO_BASE, 0x20 + _servo)
diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h
index 03a82ec5d..cf91f7030 100644
--- a/src/drivers/drv_range_finder.h
+++ b/src/drivers/drv_range_finder.h
@@ -52,6 +52,7 @@
  */
 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 */
 };
diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h
index 4decc324e..20763e265 100644
--- a/src/drivers/drv_rc_input.h
+++ b/src/drivers/drv_rc_input.h
@@ -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
@@ -32,8 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file R/C input interface.
+ * @file drv_rc_input.h
  *
+ * R/C input interface.
  */
 
 #ifndef _DRV_RC_INPUT_H
@@ -62,6 +63,11 @@
 #define RC_INPUT_MAX_CHANNELS	18
 
 /**
+ * Maximum RSSI value
+ */
+#define RC_INPUT_RSSI_MAX	255
+
+/**
  * Input signal type, value is a control position from zero to 100
  * percent.
  */
@@ -82,12 +88,52 @@ enum RC_INPUT_SOURCE {
  * on the board involved.
  */
 struct rc_input_values {
-	/** decoding time */
-	uint64_t		timestamp;
+	/** publication time */
+	uint64_t		timestamp_publication;
+
+	/** last valid reception time */
+	uint64_t		timestamp_last_signal;
 
 	/** number of channels actually being seen */
 	uint32_t		channel_count;
 
+	/** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 255: full reception */
+	int32_t			rssi;
+
+	/**
+	 * explicit failsafe flag: true on TX failure or TX out of range , false otherwise.
+	 * Only the true state is reliable, as there are some (PPM) receivers on the market going
+	 * into failsafe without telling us explicitly.
+	 * */
+	bool			rc_failsafe;
+
+	/**
+	 * RC receiver connection status: True,if no frame has arrived in the expected time, false otherwise.
+	 * True usally means that the receiver has been disconnected, but can also indicate a radio link loss on "stupid" systems.
+	 * Will remain false, if a RX with failsafe option continues to transmit frames after a link loss.
+	 * */
+	bool			rc_lost;
+
+	/**
+	 * Number of lost RC frames.
+	 * Note: intended purpose: observe the radio link quality if RSSI is not available
+	 * This value must not be used to trigger any failsafe-alike funtionality.
+	 * */
+	uint16_t		rc_lost_frame_count;
+
+	/**
+	 * Number of total RC frames.
+	 * Note: intended purpose: observe the radio link quality if RSSI is not available
+	 * This value must not be used to trigger any failsafe-alike funtionality.
+	 * */
+	uint16_t		rc_total_frame_count;
+
+	/**
+	 * Length of a single PPM frame.
+	 * Zero for non-PPM systems
+	 */
+	uint16_t		rc_ppm_frame_length;
+
 	/** Input source */
 	enum RC_INPUT_SOURCE 	input_source;
 
@@ -103,8 +149,12 @@ ORB_DECLARE(input_rc);
 #define _RC_INPUT_BASE		0x2b00
 
 /** Fetch R/C input values into (rc_input_values *)arg */
-
 #define RC_INPUT_GET			_IOC(_RC_INPUT_BASE, 0)
 
+/** Enable RSSI input via ADC */
+#define RC_INPUT_ENABLE_RSSI_ANALOG	_IOC(_RC_INPUT_BASE, 1)
+
+/** Enable RSSI input via PWM signal */
+#define RC_INPUT_ENABLE_RSSI_PWM	_IOC(_RC_INPUT_BASE, 2)
 
 #endif /* _DRV_RC_INPUT_H */
diff --git a/src/drivers/drv_rgbled.h b/src/drivers/drv_rgbled.h
new file mode 100644
index 000000000..07c6186dd
--- /dev/null
+++ b/src/drivers/drv_rgbled.h
@@ -0,0 +1,128 @@
+/****************************************************************************
+ *
+ *   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 drv_rgbled.h
+ *
+ * RGB led device API
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+/* more devices will be 1, 2, etc */
+#define RGBLED_DEVICE_PATH "/dev/rgbled0"
+
+/*
+ * ioctl() definitions
+ */
+
+#define _RGBLEDIOCBASE		(0x2900)
+#define _RGBLEDIOC(_n)		(_IOC(_RGBLEDIOCBASE, _n))
+
+/** play the named script in *(char *)arg, repeating forever */
+#define RGBLED_PLAY_SCRIPT_NAMED	_RGBLEDIOC(1)
+
+/** play the numbered script in (arg), repeating forever */
+#define RGBLED_PLAY_SCRIPT		_RGBLEDIOC(2)
+
+/** 
+ * Set the user script; (arg) is a pointer to an array of script lines,
+ * where each line is an array of four bytes giving <duration>, <command>, arg[0-2]
+ *
+ * The script is terminated by a zero command.
+ */
+#define RGBLED_SET_USER_SCRIPT		_RGBLEDIOC(3)
+
+/** set constant RGB values */
+#define RGBLED_SET_RGB			_RGBLEDIOC(4)
+
+/** set color */
+#define RGBLED_SET_COLOR		_RGBLEDIOC(5)
+
+/** set blink speed */
+#define RGBLED_SET_MODE			_RGBLEDIOC(6)
+
+/** set pattern */
+#define RGBLED_SET_PATTERN		_RGBLEDIOC(7)
+
+
+/* 
+  structure passed to RGBLED_SET_RGB ioctl()
+  Note that the driver scales the brightness to 0 to 255, regardless
+  of the hardware scaling
+ */
+typedef struct {
+	uint8_t red;
+	uint8_t green;
+	uint8_t blue;
+} rgbled_rgbset_t;
+
+/* enum passed to RGBLED_SET_COLOR ioctl()*/
+typedef enum {
+	RGBLED_COLOR_OFF,
+	RGBLED_COLOR_RED,
+	RGBLED_COLOR_YELLOW,
+	RGBLED_COLOR_PURPLE,
+	RGBLED_COLOR_GREEN,
+	RGBLED_COLOR_BLUE,
+	RGBLED_COLOR_WHITE,
+	RGBLED_COLOR_AMBER,
+	RGBLED_COLOR_DIM_RED,
+	RGBLED_COLOR_DIM_YELLOW,
+	RGBLED_COLOR_DIM_PURPLE,
+	RGBLED_COLOR_DIM_GREEN,
+	RGBLED_COLOR_DIM_BLUE,
+	RGBLED_COLOR_DIM_WHITE,
+	RGBLED_COLOR_DIM_AMBER
+} rgbled_color_t;
+
+/* enum passed to RGBLED_SET_MODE ioctl()*/
+typedef enum {
+	RGBLED_MODE_OFF,
+	RGBLED_MODE_ON,
+	RGBLED_MODE_BLINK_SLOW,
+	RGBLED_MODE_BLINK_NORMAL,
+	RGBLED_MODE_BLINK_FAST,
+	RGBLED_MODE_BREATHE,
+	RGBLED_MODE_PATTERN
+} rgbled_mode_t;
+
+#define RGBLED_PATTERN_LENGTH 20
+
+typedef struct {
+	rgbled_color_t color[RGBLED_PATTERN_LENGTH];
+	unsigned duration[RGBLED_PATTERN_LENGTH];
+} rgbled_pattern_t;
diff --git a/src/drivers/drv_sbus.h b/src/drivers/drv_sbus.h
new file mode 100644
index 000000000..927c904ec
--- /dev/null
+++ b/src/drivers/drv_sbus.h
@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ *   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_sbus.h
+ *
+ * Futaba S.BUS / S.BUS 2 compatible interface.
+ */
+
+#ifndef _DRV_SBUS_H
+#define _DRV_SBUS_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_orb_dev.h"
+
+/**
+ * Path for the default S.BUS device
+ */
+#define SBUS_DEVICE_PATH	"/dev/sbus"
+
+#define _SBUS_BASE		0x2c00
+
+/** Enable S.BUS version 1 / 2 output (0 to disable) */
+#define SBUS_SET_PROTO_VERSION		_IOC(_SBUS_BASE, 0)
+
+#endif /* _DRV_SBUS_H */
diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h
index 3a89cab9d..8e8b2c1b9 100644
--- a/src/drivers/drv_sensor.h
+++ b/src/drivers/drv_sensor.h
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Common sensor API and ioctl definitions.
+ * @file drv_sensor.h
+ *
+ * Common sensor API and ioctl definitions.
  */
 
 #ifndef _DRV_SENSOR_H
diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h
index 0c6afc64c..cb5fd53a5 100644
--- a/src/drivers/drv_tone_alarm.h
+++ b/src/drivers/drv_tone_alarm.h
@@ -31,7 +31,9 @@
  *
  ****************************************************************************/
 
-/*
+/**
+ * @file drv_tone_alarm.h
+ *
  * Driver for the PX4 audio alarm port, /dev/tone_alarm.
  *
  * The tone_alarm driver supports a set of predefined "alarm"
@@ -60,6 +62,8 @@
 
 #include <sys/ioctl.h>
 
+#define TONEALARM_DEVICE_PATH "/dev/tone_alarm"
+
 #define _TONE_ALARM_BASE	0x7400
 #define TONE_SET_ALARM		_IOC(_TONE_ALARM_BASE, 1)
 
@@ -125,4 +129,25 @@ enum tone_pitch {
 	TONE_NOTE_MAX
 };
 
+enum {
+	TONE_STOP_TUNE = 0,
+	TONE_STARTUP_TUNE,
+	TONE_ERROR_TUNE,
+	TONE_NOTIFY_POSITIVE_TUNE,
+	TONE_NOTIFY_NEUTRAL_TUNE,
+	TONE_NOTIFY_NEGATIVE_TUNE,
+	/* Do not include these unused tunes
+	TONE_CHARGE_TUNE,
+	TONE_DIXIE_TUNE,
+	TONE_CUCURACHA_TUNE,
+	TONE_YANKEE_TUNE,
+	TONE_DAISY_TUNE,
+	TONE_WILLIAM_TELL_TUNE, */
+	TONE_ARMING_WARNING_TUNE,
+	TONE_BATTERY_WARNING_SLOW_TUNE,
+	TONE_BATTERY_WARNING_FAST_TUNE,
+	TONE_GPS_WARNING_TUNE,
+	TONE_NUMBER_OF_TUNES
+};
+
 #endif /* DRV_TONE_ALARM_H_ */
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index e50395e47..8bbef5cfa 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.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
@@ -37,7 +37,7 @@
  *
  * Driver for the Eagle Tree Airspeed V3 connected via I2C.
  */
-	 
+
 #include <nuttx/config.h>
 
 #include <drivers/device/i2c.h>
@@ -59,7 +59,7 @@
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/board.h>
+#include <board_config.h>
 
 #include <systemlib/airspeed.h>
 #include <systemlib/err.h>
@@ -68,357 +68,54 @@
 
 #include <drivers/drv_airspeed.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/device/ringbuffer.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/subsystem_info.h>
-
-/* Default I2C bus */
-#define PX4_I2C_BUS_DEFAULT		PX4_I2C_BUS_EXPANSION
+#include <drivers/airspeed/airspeed.h>
 
 /* I2C bus address */
-#define I2C_ADDRESS		0x75	/* 7-bit address. 8-bit address is 0xEA */
+#define I2C_ADDRESS	0x75	/* 7-bit address. 8-bit address is 0xEA */
 
 /* Register address */
-#define READ_CMD		0x07		/* Read the data */
-	 
+#define READ_CMD	0x07	/* Read the data */
+
 /**
- * The Eagle Tree Airspeed V3 cannot provide accurate reading below speeds of 15km/h. 
+ * The Eagle Tree Airspeed V3 cannot provide accurate reading below speeds of 15km/h.
+ * You can set this value to 12 if you want a zero reading below 15km/h.
  */
-#define MIN_ACCURATE_DIFF_PRES_PA 12
+#define MIN_ACCURATE_DIFF_PRES_PA 0
 
 /* Measurement rate is 100Hz */
 #define CONVERSION_INTERVAL	(1000000 / 100)	/* microseconds */
 
-/* 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 ETSAirspeed : public device::I2C
+class ETSAirspeed : public Airspeed
 {
 public:
 	ETSAirspeed(int bus, int address = I2C_ADDRESS);
-	virtual ~ETSAirspeed();
-	
-	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;
-	unsigned					_num_reports;
-	volatile unsigned			_next_report;
-	volatile unsigned			_oldest_report;
-	differential_pressure_s		*_reports;
-	bool						_sensor_ok;
-	int							_measure_ticks;
-	bool						_collect_phase;
-	int 						_diff_pres_offset;
-	
-	orb_advert_t				_airspeed_pub;
-
-	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();
+	virtual void	cycle();
+	virtual int	measure();
+	virtual 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);
-	
-	
 };
 
-/* helper macro for handling report buffer indices */
-#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
-
 /*
  * Driver 'main' command.
  */
 extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]);
 
-ETSAirspeed::ETSAirspeed(int bus, int address) :
-	I2C("ETSAirspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
-	_reports(nullptr),
-	_sensor_ok(false),
-	_measure_ticks(0),
-	_collect_phase(false),
-	_diff_pres_offset(0),
-	_airspeed_pub(-1),
-	_sample_perf(perf_alloc(PC_ELAPSED, "ets_airspeed_read")),
-	_comms_errors(perf_alloc(PC_COUNT, "ets_airspeed_comms_errors")),
-	_buffer_overflows(perf_alloc(PC_COUNT, "ets_airspeed_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));
-}
-
-ETSAirspeed::~ETSAirspeed()
-{
-	/* make sure we are truly inactive */
-	stop();
-
-	/* free any existing reports */
-	if (_reports != nullptr)
-		delete[] _reports;
-}
-
-int
-ETSAirspeed::init()
-{
-	int ret = ERROR;
-
-	/* do I2C init (and probe) first */
-	if (I2C::init() != OK)
-		goto out;
-
-	/* allocate basic report buffers */
-	_num_reports = 2;
-	_reports = new struct differential_pressure_s[_num_reports];
-	for (unsigned i = 0; i < _num_reports; i++)
-		_reports[i].max_differential_pressure_pa = 0;
-
-	if (_reports == nullptr)
-		goto out;
-
-	_oldest_report = _next_report = 0;
-
-	/* get a publish handle on the airspeed topic */
-	memset(&_reports[0], 0, sizeof(_reports[0]));
-	_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_reports[0]);
-
-	if (_airspeed_pub < 0)
-		debug("failed to create airspeed sensor 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
-ETSAirspeed::probe()
-{
-	return measure();
-}
-
-int
-ETSAirspeed::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(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(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: {
-			/* add one to account for the sentinel in the ring */
-			arg++;
-
-			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 2) || (arg > 100))
-				return -EINVAL;
-
-			/* allocate new buffer */
-			struct differential_pressure_s *buf = new struct differential_pressure_s[arg];
-
-			if (nullptr == buf)
-				return -ENOMEM;
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete[] _reports;
-			_num_reports = arg;
-			_reports = buf;
-			start();
-
-			return OK;
-		}
-
-	case SENSORIOCGQUEUEDEPTH:
-		return _num_reports - 1;
-		
-	case SENSORIOCRESET:
-		/* XXX implement this */
-		return -EINVAL;
-	
-	default:
-		/* give it to the superclass */
-		return I2C::ioctl(filp, cmd, arg);
-	}
-}
-
-ssize_t
-ETSAirspeed::read(struct file *filp, char *buffer, size_t buflen)
+ETSAirspeed::ETSAirspeed(int bus, int address) : Airspeed(bus, address,
+	CONVERSION_INTERVAL)
 {
-	unsigned count = buflen / sizeof(struct differential_pressure_s);
-	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 (_oldest_report != _next_report) {
-				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
-				ret += sizeof(_reports[0]);
-				INCREMENT(_oldest_report, _num_reports);
-			}
-		}
-
-		/* if there was no data, warn the caller */
-		return ret ? ret : -EAGAIN;
-	}
-
-	/* manual measurement - run one conversion */
-	/* XXX really it'd be nice to lock against other readers here */
-	do {
-		_oldest_report = _next_report = 0;
-
-		/* trigger a measurement */
-		if (OK != measure()) {
-			ret = -EIO;
-			break;
-		}
-
-		/* wait for it to complete */
-		usleep(CONVERSION_INTERVAL);
-
-		/* run the collection phase */
-		if (OK != collect()) {
-			ret = -EIO;
-			break;
-		}
 
-		/* state machine will have generated a report, copy it out */
-		memcpy(buffer, _reports, sizeof(*_reports));
-		ret = sizeof(*_reports);
-
-	} while (0);
-
-	return ret;
 }
 
 int
@@ -432,14 +129,11 @@ ETSAirspeed::measure()
 	uint8_t cmd = READ_CMD;
 	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;
 }
 
@@ -447,98 +141,64 @@ int
 ETSAirspeed::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) {
-		log("error reading from sensor: %d", ret);
+		perf_count(_comms_errors);
 		return ret;
 	}
-	
+
 	uint16_t diff_pres_pa = val[1] << 8 | val[0];
+        if (diff_pres_pa == 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
+		// average
+		perf_count(_comms_errors);
+		log("zero value from sensor"); 
+		return -1;
+        }
 
-	param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset);
-	
-	if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) { 
+	if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
 		diff_pres_pa = 0;
 	} else {
-		diff_pres_pa -= _diff_pres_offset;	
+		diff_pres_pa -= _diff_pres_offset;
 	}
 
-    // XXX we may want to smooth out the readings to remove noise.
-
-	_reports[_next_report].timestamp = hrt_absolute_time();
-	_reports[_next_report].differential_pressure_pa = diff_pres_pa;
-
 	// Track maximum differential pressure measured (so we can work out top speed).
-	if (diff_pres_pa > _reports[_next_report].max_differential_pressure_pa) {
-		_reports[_next_report].max_differential_pressure_pa = diff_pres_pa;
+	if (diff_pres_pa > _max_differential_pressure_pa) {
+		_max_differential_pressure_pa = diff_pres_pa;
 	}
 
-	/* announce the airspeed if needed, just publish else */
-	orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]);
-
-	/* post a report to the ring - note, not locked */
-	INCREMENT(_next_report, _num_reports);
-
-	/* if we are running up against the oldest report, toss it */
-	if (_next_report == _oldest_report) {
-		perf_count(_buffer_overflows);
-		INCREMENT(_oldest_report, _num_reports);
+	// 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;
+	report.max_differential_pressure_pa = _max_differential_pressure_pa;
+
+	if (_airspeed_pub > 0 && !(_pub_blocked)) {
+		/* publish it */
+		orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
 	}
 
+	new_report(report);
+
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
 	ret = OK;
 
 	perf_end(_sample_perf);
-	
-	return ret;
-}
-
-void
-ETSAirspeed::start()
-{
-	/* reset the report ring and state machine */
-	_collect_phase = false;
-	_oldest_report = _next_report = 0;
-
-	/* schedule a cycle to start things */
-	work_queue(HPWORK, &_work, (worker_t)&ETSAirspeed::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
-ETSAirspeed::stop()
-{
-	work_cancel(HPWORK, &_work);
-}
-
-void
-ETSAirspeed::cycle_trampoline(void *arg)
-{
-	ETSAirspeed *dev = (ETSAirspeed *)arg;
-
-	dev->cycle();
+	return ret;
 }
 
 void
@@ -549,7 +209,7 @@ ETSAirspeed::cycle()
 
 		/* perform collection */
 		if (OK != collect()) {
-			log("collection error");
+			perf_count(_comms_errors);
 			/* restart the measurement state machine */
 			start();
 			return;
@@ -566,7 +226,7 @@ ETSAirspeed::cycle()
 			/* schedule a fresh cycle call when we are ready to measure again */
 			work_queue(HPWORK,
 				   &_work,
-				   (worker_t)&ETSAirspeed::cycle_trampoline,
+				   (worker_t)&Airspeed::cycle_trampoline,
 				   this,
 				   _measure_ticks - USEC2TICK(CONVERSION_INTERVAL));
 
@@ -584,22 +244,11 @@ ETSAirspeed::cycle()
 	/* schedule a fresh cycle call when the measurement is done */
 	work_queue(HPWORK,
 		   &_work,
-		   (worker_t)&ETSAirspeed::cycle_trampoline,
+		   (worker_t)&Airspeed::cycle_trampoline,
 		   this,
 		   USEC2TICK(CONVERSION_INTERVAL));
 }
 
-void
-ETSAirspeed::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);
-	printf("report queue:   %u (%u/%u @ %p)\n",
-	       _num_reports, _oldest_report, _next_report, _reports);
-}
-
 /**
  * Local functions in support of the shell command.
  */
@@ -637,7 +286,7 @@ start(int i2c_bus)
 	if (g_dev == nullptr)
 		goto fail;
 
-	if (OK != g_dev->init())
+	if (OK != g_dev->Airspeed::init())
 		goto fail;
 
 	/* set the poll rate to default, starts automatic data collection */
@@ -653,8 +302,7 @@ start(int i2c_bus)
 
 fail:
 
-	if (g_dev != nullptr) 
-	{
+	if (g_dev != nullptr) {
 		delete g_dev;
 		g_dev = nullptr;
 	}
@@ -668,15 +316,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);
 }
 
@@ -732,6 +379,10 @@ test()
 		warnx("diff pressure: %d pa", report.differential_pressure_pa);
 	}
 
+	/* reset the sensor polling to its default rate */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT))
+		errx(1, "failed to set default rate");
+
 	errx(0, "PASS");
 }
 
@@ -773,14 +424,14 @@ info()
 } // namespace
 
 
-static void 
-ets_airspeed_usage() 
+static void
+ets_airspeed_usage()
 {
-	fprintf(stderr, "usage: ets_airspeed [options] command\n");
-	fprintf(stderr, "options:\n");
-	fprintf(stderr, "\t-b --bus i2cbus (%d)\n", PX4_I2C_BUS_DEFAULT);
-	fprintf(stderr, "command:\n");
-	fprintf(stderr, "\tstart|stop|reset|test|info\n");
+	warnx("usage: ets_airspeed command [options]");
+	warnx("options:");
+	warnx("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
+	warnx("command:");
+	warnx("\tstart|stop|reset|test|info");
 }
 
 int
@@ -789,6 +440,7 @@ ets_airspeed_main(int argc, char *argv[])
 	int i2c_bus = PX4_I2C_BUS_DEFAULT;
 
 	int i;
+
 	for (i = 1; i < argc; i++) {
 		if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) {
 			if (argc > i + 1) {
@@ -802,12 +454,12 @@ ets_airspeed_main(int argc, char *argv[])
 	 */
 	if (!strcmp(argv[1], "start"))
 		ets_airspeed::start(i2c_bus);
-	
-	 /*
-	  * Stop the driver
-	  */
-	 if (!strcmp(argv[1], "stop"))
-		 ets_airspeed::stop();
+
+	/*
+	 * Stop the driver
+	 */
+	if (!strcmp(argv[1], "stop"))
+		ets_airspeed::stop();
 
 	/*
 	 * Test the driver/device.
diff --git a/src/drivers/ets_airspeed/module.mk b/src/drivers/ets_airspeed/module.mk
index cb5d3b1ed..15346c5c5 100644
--- a/src/drivers/ets_airspeed/module.mk
+++ b/src/drivers/ets_airspeed/module.mk
@@ -36,6 +36,6 @@
 #
 
 MODULE_COMMAND		= ets_airspeed
-MODULE_STACKSIZE	= 1024
+MODULE_STACKSIZE	= 2048
 
 SRCS			= ets_airspeed.cpp
diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c
new file mode 100644
index 000000000..63b2d2d29
--- /dev/null
+++ b/src/drivers/frsky_telemetry/frsky_data.c
@@ -0,0 +1,289 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *   Author: Stefan Rado <px4@sradonia.net>
+ *
+ * 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 frsky_data.c
+ * @author Stefan Rado <px4@sradonia.net>
+ *
+ * FrSky telemetry implementation.
+ *
+ */
+
+#include "frsky_data.h"
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <arch/math.h>
+#include <geo/geo.h>
+
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_status.h>
+
+/* FrSky sensor hub data IDs */
+#define FRSKY_ID_GPS_ALT_BP     0x01
+#define FRSKY_ID_TEMP1          0x02
+#define FRSKY_ID_RPM            0x03
+#define FRSKY_ID_FUEL           0x04
+#define FRSKY_ID_TEMP2          0x05
+#define FRSKY_ID_VOLTS          0x06
+#define FRSKY_ID_GPS_ALT_AP     0x09
+#define FRSKY_ID_BARO_ALT_BP    0x10
+#define FRSKY_ID_GPS_SPEED_BP   0x11
+#define FRSKY_ID_GPS_LONG_BP    0x12
+#define FRSKY_ID_GPS_LAT_BP     0x13
+#define FRSKY_ID_GPS_COURS_BP   0x14
+#define FRSKY_ID_GPS_DAY_MONTH  0x15
+#define FRSKY_ID_GPS_YEAR       0x16
+#define FRSKY_ID_GPS_HOUR_MIN   0x17
+#define FRSKY_ID_GPS_SEC        0x18
+#define FRSKY_ID_GPS_SPEED_AP   0x19
+#define FRSKY_ID_GPS_LONG_AP    0x1A
+#define FRSKY_ID_GPS_LAT_AP     0x1B
+#define FRSKY_ID_GPS_COURS_AP   0x1C
+#define FRSKY_ID_BARO_ALT_AP    0x21
+#define FRSKY_ID_GPS_LONG_EW    0x22
+#define FRSKY_ID_GPS_LAT_NS     0x23
+#define FRSKY_ID_ACCEL_X        0x24
+#define FRSKY_ID_ACCEL_Y        0x25
+#define FRSKY_ID_ACCEL_Z        0x26
+#define FRSKY_ID_CURRENT        0x28
+#define FRSKY_ID_VARIO          0x30
+#define FRSKY_ID_VFAS           0x39
+#define FRSKY_ID_VOLTS_BP       0x3A
+#define FRSKY_ID_VOLTS_AP       0x3B
+
+#define frac(f) (f - (int)f)
+
+static int battery_sub = -1;
+static int sensor_sub = -1;
+static int global_position_sub = -1;
+static int vehicle_status_sub = -1;
+
+/**
+ * Initializes the uORB subscriptions.
+ */
+void frsky_init()
+{
+	battery_sub = orb_subscribe(ORB_ID(battery_status));
+	global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+}
+
+/**
+ * Sends a 0x5E start/stop byte.
+ */
+static void frsky_send_startstop(int uart)
+{
+	static const uint8_t c = 0x5E;
+	write(uart, &c, sizeof(c));
+}
+
+/**
+ * Sends one byte, performing byte-stuffing if necessary.
+ */
+static void frsky_send_byte(int uart, uint8_t value)
+{
+	const uint8_t x5E[] = { 0x5D, 0x3E };
+	const uint8_t x5D[] = { 0x5D, 0x3D };
+
+	switch (value) {
+	case 0x5E:
+		write(uart, x5E, sizeof(x5E));
+		break;
+
+	case 0x5D:
+		write(uart, x5D, sizeof(x5D));
+		break;
+
+	default:
+		write(uart, &value, sizeof(value));
+		break;
+	}
+}
+
+/**
+ * Sends one data id/value pair.
+ */
+static void frsky_send_data(int uart, uint8_t id, int16_t data)
+{
+	/* Cast data to unsigned, because signed shift might behave incorrectly */
+	uint16_t udata = data;
+
+	frsky_send_startstop(uart);
+
+	frsky_send_byte(uart, id);
+	frsky_send_byte(uart, udata);      /* LSB */
+	frsky_send_byte(uart, udata >> 8); /* MSB */
+}
+
+/**
+ * Sends frame 1 (every 200ms):
+ *   acceleration values, barometer altitude, temperature, battery voltage & current
+ */
+void frsky_send_frame1(int uart)
+{
+	/* get a local copy of the current sensor values */
+	struct sensor_combined_s raw;
+	memset(&raw, 0, sizeof(raw));
+	orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
+
+	/* get a local copy of the battery data */
+	struct battery_status_s battery;
+	memset(&battery, 0, sizeof(battery));
+	orb_copy(ORB_ID(battery_status), battery_sub, &battery);
+
+	/* send formatted frame */
+	frsky_send_data(uart, FRSKY_ID_ACCEL_X,
+			roundf(raw.accelerometer_m_s2[0] * 1000.0f));
+	frsky_send_data(uart, FRSKY_ID_ACCEL_Y,
+			roundf(raw.accelerometer_m_s2[1] * 1000.0f));
+	frsky_send_data(uart, FRSKY_ID_ACCEL_Z,
+			roundf(raw.accelerometer_m_s2[2] * 1000.0f));
+
+	frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP,
+			raw.baro_alt_meter);
+	frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP,
+			roundf(frac(raw.baro_alt_meter) * 100.0f));
+
+	frsky_send_data(uart, FRSKY_ID_TEMP1,
+			roundf(raw.baro_temp_celcius));
+
+	frsky_send_data(uart, FRSKY_ID_VFAS,
+			roundf(battery.voltage_v * 10.0f));
+	frsky_send_data(uart, FRSKY_ID_CURRENT,
+			(battery.current_a < 0) ? 0 : roundf(battery.current_a * 10.0f));
+
+	frsky_send_startstop(uart);
+}
+
+/**
+ * Formats the decimal latitude/longitude to the required degrees/minutes/seconds.
+ */
+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);
+}
+
+/**
+ * Sends frame 2 (every 1000ms):
+ *   GPS course, latitude, longitude, ground speed, GPS altitude, remaining battery level
+ */
+void frsky_send_frame2(int uart)
+{
+	/* get a local copy of the global position data */
+	struct vehicle_global_position_s global_pos;
+	memset(&global_pos, 0, sizeof(global_pos));
+	orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
+
+	/* get a local copy of the vehicle status data */
+	struct vehicle_status_s vehicle_status;
+	memset(&vehicle_status, 0, sizeof(vehicle_status));
+	orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
+
+	/* send formatted frame */
+	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) {
+		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_ns = (global_pos.lat < 0) ? 'S' : 'N';
+		lon    = frsky_format_gps(abs(global_pos.lon) / 10000000.0f);
+		lon_ew = (global_pos.lon < 0) ? 'W' : 'E';
+		speed  = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy)
+				* 25.0f / 46.0f;
+		alt    = global_pos.alt;
+		sec    = tm_gps->tm_sec;
+	}
+
+	frsky_send_data(uart, FRSKY_ID_GPS_COURS_BP, course);
+	frsky_send_data(uart, FRSKY_ID_GPS_COURS_AP, frac(course) * 1000.0f);
+
+	frsky_send_data(uart, FRSKY_ID_GPS_LAT_BP, lat);
+	frsky_send_data(uart, FRSKY_ID_GPS_LAT_AP, frac(lat) * 10000.0f);
+	frsky_send_data(uart, FRSKY_ID_GPS_LAT_NS, lat_ns);
+
+	frsky_send_data(uart, FRSKY_ID_GPS_LONG_BP, lon);
+	frsky_send_data(uart, FRSKY_ID_GPS_LONG_AP, frac(lon) * 10000.0f);
+	frsky_send_data(uart, FRSKY_ID_GPS_LONG_EW, lon_ew);
+
+	frsky_send_data(uart, FRSKY_ID_GPS_SPEED_BP, speed);
+	frsky_send_data(uart, FRSKY_ID_GPS_SPEED_AP, frac(speed) * 100.0f);
+
+	frsky_send_data(uart, FRSKY_ID_GPS_ALT_BP, alt);
+	frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, frac(alt) * 100.0f);
+
+	frsky_send_data(uart, FRSKY_ID_FUEL,
+			roundf(vehicle_status.battery_remaining * 100.0f));
+
+	frsky_send_data(uart, FRSKY_ID_GPS_SEC, sec);
+
+	frsky_send_startstop(uart);
+}
+
+/**
+ * Sends frame 3 (every 5000ms):
+ *   GPS date & time
+ */
+void frsky_send_frame3(int uart)
+{
+	/* get a local copy of the battery data */
+	struct vehicle_global_position_s global_pos;
+	memset(&global_pos, 0, sizeof(global_pos));
+	orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
+
+	/* send formatted frame */
+	time_t time_gps = global_pos.time_gps_usec / 1000000;
+	struct tm *tm_gps = gmtime(&time_gps);
+	uint16_t hour_min = (tm_gps->tm_min << 8) | (tm_gps->tm_hour & 0xff);
+	frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, tm_gps->tm_mday);
+	frsky_send_data(uart, FRSKY_ID_GPS_YEAR, tm_gps->tm_year);
+	frsky_send_data(uart, FRSKY_ID_GPS_HOUR_MIN, hour_min);
+	frsky_send_data(uart, FRSKY_ID_GPS_SEC, tm_gps->tm_sec);
+
+	frsky_send_startstop(uart);
+}
diff --git a/src/drivers/frsky_telemetry/frsky_data.h b/src/drivers/frsky_telemetry/frsky_data.h
new file mode 100644
index 000000000..a7d9eee53
--- /dev/null
+++ b/src/drivers/frsky_telemetry/frsky_data.h
@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *   Author: Stefan Rado <px4@sradonia.net>
+ *
+ * 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 frsky_data.h
+ * @author Stefan Rado <px4@sradonia.net>
+ *
+ * FrSky telemetry implementation.
+ *
+ */
+#ifndef _FRSKY_DATA_H
+#define _FRSKY_DATA_H
+
+// Public functions
+void frsky_init(void);
+void frsky_send_frame1(int uart);
+void frsky_send_frame2(int uart);
+void frsky_send_frame3(int uart);
+
+#endif /* _FRSKY_TELEMETRY_H */
diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c
new file mode 100644
index 000000000..7b08ca69e
--- /dev/null
+++ b/src/drivers/frsky_telemetry/frsky_telemetry.c
@@ -0,0 +1,266 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *   Author: Stefan Rado <px4@sradonia.net>
+ *
+ * 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 frsky_telemetry.c
+ * @author Stefan Rado <px4@sradonia.net>
+ *
+ * FrSky telemetry implementation.
+ *
+ * This daemon emulates an FrSky sensor hub by periodically sending data
+ * packets to an attached FrSky receiver.
+ *
+ */
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <string.h>
+#include <sys/types.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+#include <termios.h>
+
+#include "frsky_data.h"
+
+
+/* thread state */
+static volatile bool thread_should_exit = false;
+static volatile bool thread_running = false;
+static int frsky_task;
+
+/* functions */
+static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original);
+static void usage(void);
+static int frsky_telemetry_thread_main(int argc, char *argv[]);
+__EXPORT int frsky_telemetry_main(int argc, char *argv[]);
+
+
+/**
+ * Opens the UART device and sets all required serial parameters.
+ */
+static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original)
+{
+	/* Open UART */
+	const int uart = open(uart_name, O_WRONLY | O_NOCTTY);
+
+	if (uart < 0) {
+		err(1, "Error opening port: %s", uart_name);
+	}
+
+	/* Back up the original UART configuration to restore it after exit */
+	int termios_state;
+	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 */
+	struct termios uart_config;
+	tcgetattr(uart, &uart_config);
+
+	/* Disable output post-processing */
+	uart_config.c_oflag &= ~OPOST;
+
+	/* Set baud rate */
+	static const speed_t speed = B9600;
+
+	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;
+}
+
+/**
+ * Print command usage information
+ */
+static void usage()
+{
+	fprintf(stderr,
+			"usage: frsky_telemetry start [-d <devicename>]\n"
+			"       frsky_telemetry stop\n"
+			"       frsky_telemetry status\n");
+	exit(1);
+}
+
+/**
+ * The daemon thread.
+ */
+static int frsky_telemetry_thread_main(int argc, char *argv[])
+{
+	/* Default values for arguments */
+	char *device_name = "/dev/ttyS1"; /* USART2 */
+
+	/* Work around some stupidity in task_create's argv handling */
+	argc -= 2;
+	argv += 2;
+
+	int ch;
+	while ((ch = getopt(argc, argv, "d:")) != EOF) {
+		switch (ch) {
+		case 'd':
+			device_name = optarg;
+			break;
+
+		default:
+			usage();
+			break;
+		}
+	}
+
+	/* Print welcome text */
+	warnx("FrSky telemetry interface starting...");
+
+	/* Open UART */
+	struct termios uart_config_original;
+	const int uart = frsky_open_uart(device_name, &uart_config_original);
+
+	if (uart < 0)
+		err(1, "could not open %s", device_name);
+
+	/* Subscribe to topics */
+	frsky_init();
+
+	thread_running = true;
+
+	/* Main thread loop */
+	unsigned int iteration = 0;
+	while (!thread_should_exit) {
+
+		/* Sleep 200 ms */
+		usleep(200000);
+
+		/* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */
+		frsky_send_frame1(uart);
+
+		/* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */
+		if (iteration % 5 == 0)
+		{
+			frsky_send_frame2(uart);
+		}
+
+		/* Send frame 3 (every 5000ms): date, time */
+		if (iteration % 25 == 0)
+		{
+			frsky_send_frame3(uart);
+
+			iteration = 0;
+		}
+
+		iteration++;
+	}
+
+	/* Reset the UART flags to original state */
+	tcsetattr(uart, TCSANOW, &uart_config_original);
+	close(uart);
+
+	thread_running = false;
+	return 0;
+}
+
+/**
+ * The main command function.
+ * Processes command line arguments and starts the daemon.
+ */
+int frsky_telemetry_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, "frsky_telemetry already running");
+
+		thread_should_exit = false;
+		frsky_task = task_spawn_cmd("frsky_telemetry",
+					  SCHED_DEFAULT,
+					  SCHED_PRIORITY_DEFAULT,
+					  2048,
+					  frsky_telemetry_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, "frsky_telemetry 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/drivers/frsky_telemetry/module.mk b/src/drivers/frsky_telemetry/module.mk
new file mode 100644
index 000000000..1632c74f7
--- /dev/null
+++ b/src/drivers/frsky_telemetry/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# FrSky telemetry application.
+#
+
+MODULE_COMMAND	= frsky_telemetry
+
+SRCS		= frsky_data.c \
+              frsky_telemetry.c
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 38835418b..a736cbdf6 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -85,7 +85,7 @@ static const int ERROR = -1;
 class GPS : public device::CDev
 {
 public:
-	GPS(const char* uart_path);
+	GPS(const char *uart_path, bool fake_gps);
 	virtual ~GPS();
 
 	virtual int			init();
@@ -112,6 +112,7 @@ private:
 	struct vehicle_gps_position_s 	_report;					///< uORB topic for gps position
 	orb_advert_t			_report_pub;					///< uORB pub for gps position
 	float				_rate;						///< position update rate
+	bool				_fake_gps;					///< fake gps output
 
 
 	/**
@@ -156,7 +157,7 @@ GPS	*g_dev;
 }
 
 
-GPS::GPS(const char* uart_path) :
+GPS::GPS(const char *uart_path, bool fake_gps) :
 	CDev("gps", GPS_DEVICE_PATH),
 	_task_should_exit(false),
 	_healthy(false),
@@ -164,7 +165,8 @@ GPS::GPS(const char* uart_path) :
 	_mode(GPS_DRIVER_MODE_UBX),
 	_Helper(nullptr),
 	_report_pub(-1),
-	_rate(0.0f)
+	_rate(0.0f),
+	_fake_gps(fake_gps)
 {
 	/* store port name */
 	strncpy(_port, uart_path, sizeof(_port));
@@ -192,6 +194,7 @@ GPS::~GPS()
 	/* well, kill it anyway, though this will probably crash */
 	if (_task != -1)
 		task_delete(_task);
+
 	g_dev = nullptr;
 
 }
@@ -263,84 +266,143 @@ GPS::task_main()
 	/* loop handling received serial bytes and also configuring in between */
 	while (!_task_should_exit) {
 
-		if (_Helper != nullptr) {
-			delete(_Helper);
-			/* set to zero to ensure parser is not used while not instantiated */
-			_Helper = nullptr;
-		}
+		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;
+
+			//no time and satellite information simulated
+
+			if (!(_pub_blocked)) {
+				if (_report_pub > 0) {
+					orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
+
+				} else {
+					_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
+				}
+			}
+
+			usleep(2e5);
+
+		} else {
+
+			if (_Helper != nullptr) {
+				delete(_Helper);
+				/* set to zero to ensure parser is not used while not instantiated */
+				_Helper = nullptr;
+			}
 
-		switch (_mode) {
+			switch (_mode) {
 			case GPS_DRIVER_MODE_UBX:
 				_Helper = new UBX(_serial_fd, &_report);
 				break;
+
 			case GPS_DRIVER_MODE_MTK:
 				_Helper = new MTK(_serial_fd, &_report);
 				break;
-			case GPS_DRIVER_MODE_NMEA:
-				//_Helper = new NMEA(); //TODO: add NMEA
-				break;
+
 			default:
 				break;
-		}
-		unlock();
-		if (_Helper->configure(_baudrate) == 0) {
+			}
+
 			unlock();
 
-			// GPS is obviously detected successfully, reset statistics
-			_Helper->reset_update_rates();
+			if (_Helper->configure(_baudrate) == 0) {
+				unlock();
 
-			while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
-//				lock();
-				/* opportunistic publishing - else invalid data would end up on the bus */
-				if (_report_pub > 0) {
-					orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
-				} else {
-					_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
-				}
+				// GPS is obviously detected successfully, reset statistics
+				_Helper->reset_update_rates();
+
+				while (_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);
 
-				last_rate_count++;
+						} else {
+							_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
+						}
+					}
 
-				/* measure update rate every 5 seconds */
-				if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
-					_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
-					last_rate_measurement = hrt_absolute_time();
-					last_rate_count = 0;
-					_Helper->store_update_rates();
-					_Helper->reset_update_rates();
+					last_rate_count++;
+
+					/* measure update rate every 5 seconds */
+					if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
+						_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
+						last_rate_measurement = hrt_absolute_time();
+						last_rate_count = 0;
+						_Helper->store_update_rates();
+						_Helper->reset_update_rates();
+					}
+
+					if (!_healthy) {
+						char *mode_str = "unknown";
+
+						switch (_mode) {
+						case GPS_DRIVER_MODE_UBX:
+							mode_str = "UBX";
+							break;
+
+						case GPS_DRIVER_MODE_MTK:
+							mode_str = "MTK";
+							break;
+
+						default:
+							break;
+						}
+
+						warnx("module found: %s", mode_str);
+						_healthy = true;
+					}
 				}
 
-				if (!_healthy) {
-					warnx("module found");
-					_healthy = true;
+				if (_healthy) {
+					warnx("module lost");
+					_healthy = false;
+					_rate = 0.0f;
 				}
-			}
-			if (_healthy) {
-				warnx("module lost");
-				_healthy = false;
-				_rate = 0.0f;
+
+				lock();
 			}
 
 			lock();
-		}
-		lock();
 
-		/* select next mode */
-		switch (_mode) {
+			/* select next mode */
+			switch (_mode) {
 			case GPS_DRIVER_MODE_UBX:
 				_mode = GPS_DRIVER_MODE_MTK;
 				break;
+
 			case GPS_DRIVER_MODE_MTK:
 				_mode = GPS_DRIVER_MODE_UBX;
 				break;
-		//				case GPS_DRIVER_MODE_NMEA:
-		//					_mode = GPS_DRIVER_MODE_UBX;
-		//					break;
+
 			default:
 				break;
+			}
 		}
 
 	}
-	debug("exiting");
+
+	warnx("exiting");
 
 	::close(_serial_fd);
 
@@ -361,23 +423,25 @@ void
 GPS::print_info()
 {
 	switch (_mode) {
-		case GPS_DRIVER_MODE_UBX:
-			warnx("protocol: UBX");
-			break;
-		case GPS_DRIVER_MODE_MTK:
-			warnx("protocol: MTK");
-			break;
-		case GPS_DRIVER_MODE_NMEA:
-			warnx("protocol: NMEA");
-			break;
-		default:
-			break;
+	case GPS_DRIVER_MODE_UBX:
+		warnx("protocol: UBX");
+		break;
+
+	case GPS_DRIVER_MODE_MTK:
+		warnx("protocol: MTK");
+		break;
+
+	default:
+		break;
 	}
+
 	warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
+
 	if (_report.timestamp_position != 0) {
-		warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type,
-			(double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f));
+		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("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);
@@ -395,7 +459,7 @@ namespace gps
 
 GPS	*g_dev;
 
-void	start(const char *uart_path);
+void	start(const char *uart_path, bool fake_gps);
 void	stop();
 void	test();
 void	reset();
@@ -405,7 +469,7 @@ void	info();
  * Start the driver.
  */
 void
-start(const char *uart_path)
+start(const char *uart_path, bool fake_gps)
 {
 	int fd;
 
@@ -413,7 +477,7 @@ start(const char *uart_path)
 		errx(1, "already started");
 
 	/* create the driver */
-	g_dev = new GPS(uart_path);
+	g_dev = new GPS(uart_path, fake_gps);
 
 	if (g_dev == nullptr)
 		goto fail;
@@ -428,6 +492,7 @@ start(const char *uart_path)
 		errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH);
 		goto fail;
 	}
+
 	exit(0);
 
 fail:
@@ -503,7 +568,8 @@ gps_main(int argc, char *argv[])
 {
 
 	/* set to default */
-	char* device_name = GPS_DEFAULT_UART_PORT;
+	char *device_name = GPS_DEFAULT_UART_PORT;
+	bool fake_gps = false;
 
 	/*
 	 * Start/load the driver.
@@ -513,15 +579,24 @@ gps_main(int argc, char *argv[])
 		if (argc > 3) {
 			if (!strcmp(argv[2], "-d")) {
 				device_name = argv[3];
+
 			} else {
 				goto out;
 			}
 		}
-		gps::start(device_name);
+
+		/* Detect fake gps option */
+		for (int i = 2; i < argc; i++) {
+			if (!strcmp(argv[i], "-f"))
+				fake_gps = true;
+		}
+
+		gps::start(device_name, fake_gps);
 	}
 
 	if (!strcmp(argv[1], "stop"))
 		gps::stop();
+
 	/*
 	 * Test the driver/device.
 	 */
@@ -541,5 +616,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]");
+	errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f]");
 }
diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp
index ba86d370a..2360ff39b 100644
--- a/src/drivers/gps/gps_helper.cpp
+++ b/src/drivers/gps/gps_helper.cpp
@@ -1,8 +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>
+ *   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
@@ -41,6 +39,9 @@
 
 /**
  * @file gps_helper.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
 float
@@ -87,13 +88,15 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
 
 	case 115200: speed = B115200; break;
 
-	warnx("try baudrate: %d\n", speed);
+		warnx("try baudrate: %d\n", speed);
 
 	default:
 		warnx("ERROR: Unsupported baudrate: %d\n", baud);
 		return -EINVAL;
 	}
+
 	struct termios uart_config;
+
 	int termios_state;
 
 	/* fill the struct for the new configuration */
@@ -109,14 +112,17 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
 		warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state);
 		return -1;
 	}
+
 	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
 		warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state);
 		return -1;
 	}
+
 	if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
 		warnx("ERROR setting baudrate (tcsetattr)\n");
 		return -1;
 	}
+
 	/* XXX if resetting the parser here, ensure it does exist (check for null pointer) */
 	return 0;
 }
diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h
index defc1a074..cfb9e0d43 100644
--- a/src/drivers/gps/gps_helper.h
+++ b/src/drivers/gps/gps_helper.h
@@ -1,8 +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>
+ *   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,8 +31,10 @@
  *
  ****************************************************************************/
 
-/** 
+/**
  * @file gps_helper.h
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
 #ifndef GPS_HELPER_H
diff --git a/src/drivers/gps/module.mk b/src/drivers/gps/module.mk
index 097db2abf..82c67d40a 100644
--- a/src/drivers/gps/module.mk
+++ b/src/drivers/gps/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
diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp
index 62941d74b..c90ecbe28 100644
--- a/src/drivers/gps/mtk.cpp
+++ b/src/drivers/gps/mtk.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) 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
@@ -33,7 +31,12 @@
  *
  ****************************************************************************/
 
-/* @file mkt.cpp */
+/**
+ * @file mtk.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ */
 
 #include <unistd.h>
 #include <stdio.h>
@@ -48,9 +51,9 @@
 
 
 MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) :
-_fd(fd),
-_gps_position(gps_position),
-_mtk_revision(0)
+	_fd(fd),
+	_gps_position(gps_position),
+	_mtk_revision(0)
 {
 	decode_init();
 }
@@ -73,24 +76,28 @@ MTK::configure(unsigned &baudrate)
 		warnx("mtk: config write failed");
 		return -1;
 	}
+
 	usleep(10000);
 
 	if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) {
 		warnx("mtk: config write failed");
 		return -1;
 	}
+
 	usleep(10000);
 
 	if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) {
 		warnx("mtk: config write failed");
 		return -1;
 	}
+
 	usleep(10000);
 
 	if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) {
 		warnx("mtk: config write failed");
 		return -1;
 	}
+
 	usleep(10000);
 
 	if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
@@ -128,12 +135,15 @@ MTK::receive(unsigned timeout)
 					handle_message(packet);
 					return 1;
 				}
+
 				/* in case we keep trying but only get crap from GPS */
-				if (time_started + timeout*1000 < hrt_absolute_time() ) {
+				if (time_started + timeout * 1000 < hrt_absolute_time()) {
 					return -1;
 				}
+
 				j++;
 			}
+
 			/* everything is read */
 			j = count = 0;
 		}
@@ -181,6 +191,7 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
 		if (b == MTK_SYNC1_V16) {
 			_decode_state = MTK_DECODE_GOT_CK_A;
 			_mtk_revision = 16;
+
 		} else if (b == MTK_SYNC1_V19) {
 			_decode_state = MTK_DECODE_GOT_CK_A;
 			_mtk_revision = 19;
@@ -201,7 +212,7 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
 			add_byte_to_checksum(b);
 
 		// Fill packet buffer
-		((uint8_t*)(&packet))[_rx_count] = b;
+		((uint8_t *)(&packet))[_rx_count] = b;
 		_rx_count++;
 
 		/* Packet size minus checksum, XXX ? */
@@ -209,14 +220,17 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
 			/* Compare checksum */
 			if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) {
 				ret = 1;
+
 			} else {
 				warnx("MTK Checksum invalid");
 				ret = -1;
 			}
+
 			// Reset state machine to decode next packet
 			decode_init();
 		}
 	}
+
 	return ret;
 }
 
@@ -226,19 +240,22 @@ MTK::handle_message(gps_mtk_packet_t &packet)
 	if (_mtk_revision == 16) {
 		_gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7
 		_gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7
+
 	} else if (_mtk_revision == 19) {
 		_gps_position->lat = packet.latitude; // both degrees*1e7
 		_gps_position->lon = packet.longitude; // both degrees*1e7
+
 	} else {
 		warnx("mtk: unknown revision");
 		_gps_position->lat = 0;
 		_gps_position->lon = 0;
 	}
+
 	_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->vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s
+	_gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s
 	_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
 	_gps_position->satellites_visible = packet.satellites;
 
diff --git a/src/drivers/gps/mtk.h b/src/drivers/gps/mtk.h
index b5cfbf0a6..a2d5e27bb 100644
--- a/src/drivers/gps/mtk.h
+++ b/src/drivers/gps/mtk.h
@@ -1,8 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
+ *   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
@@ -33,7 +31,12 @@
  *
  ****************************************************************************/
 
-/* @file mtk.h */
+/**
+ * @file mtk.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ */
 
 #ifndef MTK_H_
 #define MTK_H_
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index b3093b0f6..8a2afecb7 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.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) 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
@@ -39,15 +37,21 @@
  * U-Blox protocol implementation. Following u-blox 6/7 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>
+ *
  * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf
  */
 
-#include <unistd.h>
-#include <stdio.h>
-#include <poll.h>
+#include <assert.h>
 #include <math.h>
+#include <poll.h>
+#include <stdio.h>
 #include <string.h>
-#include <assert.h>
+#include <time.h>
+#include <unistd.h>
+
 #include <systemlib/err.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
@@ -55,13 +59,17 @@
 
 #include "ubx.h"
 
-#define UBX_CONFIG_TIMEOUT 100
+#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
 
 UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
-_fd(fd),
-_gps_position(gps_position),
-_waiting_for_ack(false),
-_disable_cmd_counter(0)
+	_fd(fd),
+	_gps_position(gps_position),
+	_configured(false),
+	_waiting_for_ack(false),
+	_disable_cmd_last(0)
 {
 	decode_init();
 }
@@ -73,12 +81,13 @@ UBX::~UBX()
 int
 UBX::configure(unsigned &baudrate)
 {
-	_waiting_for_ack = true;
-
+	_configured = false;
 	/* try different baudrates */
 	const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200};
 
-	for (int baud_i = 0; baud_i < 5; baud_i++) {
+	int baud_i;
+
+	for (baud_i = 0; baud_i < 5; baud_i++) {
 		baudrate = baudrates_to_try[baud_i];
 		set_baudrate(_fd, baudrate);
 
@@ -89,8 +98,8 @@ UBX::configure(unsigned &baudrate)
 		/* Set everything else of the packet to 0, otherwise the module wont accept it */
 		memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet));
 
-		_clsID_needed = UBX_CLASS_CFG;
-		_msgID_needed = UBX_MESSAGE_CFG_PRT;
+		_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;
@@ -102,9 +111,9 @@ UBX::configure(unsigned &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_config_packet(_fd, (uint8_t *)&cfg_prt_packet, sizeof(cfg_prt_packet));
 
-		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+		if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
 			/* try next baudrate */
 			continue;
 		}
@@ -120,100 +129,125 @@ UBX::configure(unsigned &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_config_packet(_fd, (uint8_t *)&cfg_prt_packet, sizeof(cfg_prt_packet));
+
 		/* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */
-		receive(UBX_CONFIG_TIMEOUT);
-		
+		wait_for_ack(UBX_CONFIG_TIMEOUT);
+
 		if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) {
 			set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE);
 			baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE;
 		}
 
-		/* send a CFT-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));
+		/* at this point we have correct baudrate on both ends */
+		break;
+	}
 
-		_clsID_needed = UBX_CLASS_CFG;
-		_msgID_needed = UBX_MESSAGE_CFG_RATE;
+	if (baud_i >= 5) {
+		return 1;
+	}
 
-		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 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_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet));
-		if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-			/* try next baudrate */
-			continue;
-		}
+	_message_class_needed = UBX_CLASS_CFG;
+	_message_id_needed = UBX_MESSAGE_CFG_RATE;
 
-		/* 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));
+	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;
 
-		_clsID_needed = UBX_CLASS_CFG;
-		_msgID_needed = UBX_MESSAGE_CFG_NAV5;
+	send_config_packet(_fd, (uint8_t *)&cfg_rate_packet, sizeof(cfg_rate_packet));
 
-		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;
+	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
+		warnx("ubx: configuration failed: RATE");
+		return 1;
+	}
 
-		send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
-		if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-			/* try next baudrate */
-			continue;
-		}
+	/* 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;
+
+	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_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");
+		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;
+	}
+
+	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1);
+
+	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
+		warnx("ubx: msg rate configuration failed: NAV TIMEUTC");
+		return 1;
+	}
 
-		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH,
-			UBX_CFG_MSG_PAYLOAD_RATE1_5HZ);
-		/* insist of receiving the ACK for this packet */
-		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
-		// 	continue;
-		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC,
-			1);
-		// /* insist of receiving the ACK for this packet */
-		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
-		// 	continue;
-		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL,
-			1);
-		// /* insist of receiving the ACK for this packet */
-		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
-		// 	continue;
-		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED,
-			1);
-		// /* insist of receiving the ACK for this packet */
-		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
-		// 	continue;
-		// configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_DOP,
-		// 	0);
-		// /* insist of receiving the ACK for this packet */
-		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
-		// 	continue;
-		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO,
-			0);
-		// /* insist of receiving the ACK for this packet */
-		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
-		// 	continue;
-
-		_waiting_for_ack = false;
-		return 0;
+	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1);
+
+	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
+		warnx("ubx: msg rate configuration failed: NAV SOL");
+		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");
+		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");
+		return 1;
 	}
-	return -1;
+
+	_configured = true;
+	return 0;
 }
 
 int
 UBX::wait_for_ack(unsigned timeout)
 {
 	_waiting_for_ack = true;
-	int ret = receive(timeout);
-	_waiting_for_ack = false;
-	return ret;
+	uint64_t time_started = hrt_absolute_time();
+
+	while (hrt_absolute_time() < time_started + timeout * 1000) {
+		if (receive(timeout) > 0) {
+			if (!_waiting_for_ack) {
+				return 1;
+			}
+
+		} else {
+			return -1;	// timeout or error receiving, or NAK
+		}
+	}
+
+	return -1;	// timeout
 }
 
 int
@@ -224,56 +258,61 @@ UBX::receive(unsigned timeout)
 	fds[0].fd = _fd;
 	fds[0].events = POLLIN;
 
-	uint8_t buf[32];
+	uint8_t buf[128];
 
 	/* timeout additional to poll */
 	uint64_t time_started = hrt_absolute_time();
 
-	int j = 0;
 	ssize_t count = 0;
 
-	while (true) {
+	bool handled = false;
 
-		/* pass received bytes to the packet decoder */
-		while (j < count) {
-			if (parse_char(buf[j]) > 0) {
-				/* return to configure during configuration or to the gps driver during normal work
-				 * if a packet has arrived */
-				 if (handle_message() > 0)
-					return 1;
-			}
-			/* in case we keep trying but only get crap from GPS */
-			if (time_started + timeout*1000 < hrt_absolute_time() ) {
-				return -1;
-			}
-			j++;
-		}
-
-		/* everything is read */
-		j = count = 0;
+	while (true) {
 
-		/* then poll for new data */
-		int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout);
+		/* 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);
 
 		if (ret < 0) {
 			/* something went wrong when polling */
+			warnx("ubx: poll error");
 			return -1;
 
 		} else if (ret == 0) {
-			/* Timeout */
-			return -1;
+			/* return success after short delay after receiving a packet or timeout after long delay */
+			if (handled) {
+				return 1;
+
+			} else {
+				return -1;
+			}
 
 		} else if (ret > 0) {
 			/* if we have new data from GPS, go handle it */
 			if (fds[0].revents & POLLIN) {
 				/*
 				 * We are here because poll says there is some data, so this
-				 * won't block even on a blocking device.  If more bytes are
-				 * available, we'll go back to poll() again...
+				 * won't block even on a blocking device. But don't read immediately
+				 * by 1-2 bytes, wait for some more data to save expensive read() calls.
+				 * If more bytes are available, we'll go back to poll() again.
 				 */
-				count = ::read(_fd, buf, sizeof(buf));
+				usleep(UBX_WAIT_BEFORE_READ * 1000);
+				count = read(_fd, buf, sizeof(buf));
+
+				/* 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;
+					}
+				}
 			}
 		}
+
+		/* abort after timeout if no useful packets received */
+		if (time_started + timeout * 1000 < hrt_absolute_time()) {
+			warnx("ubx: timeout - no useful messages");
+			return -1;
+		}
 	}
 }
 
@@ -282,410 +321,304 @@ UBX::parse_char(uint8_t b)
 {
 	switch (_decode_state) {
 		/* First, look for sync1 */
-		case UBX_DECODE_UNINIT:
-			if (b == UBX_SYNC1) {
-				_decode_state = UBX_DECODE_GOT_SYNC1;
-			}
-			break;
-		/* Second, look for sync2 */
-		case UBX_DECODE_GOT_SYNC1:
-			if (b == UBX_SYNC2) {
-				_decode_state = UBX_DECODE_GOT_SYNC2;
-			} else {
-				/* Second start symbol was wrong, reset state machine */
-				decode_init();
-			}
-			break;
-		/* Now look for class */
-		case UBX_DECODE_GOT_SYNC2:
-			/* everything except sync1 and sync2 needs to be added to the checksum */
-			add_byte_to_checksum(b);
-			/* check for known class */
-			switch (b) {
-				case UBX_CLASS_ACK:
-					_decode_state = UBX_DECODE_GOT_CLASS;
-					_message_class = ACK;
-					break;
+	case UBX_DECODE_UNINIT:
+		if (b == UBX_SYNC1) {
+			_decode_state = UBX_DECODE_GOT_SYNC1;
+		}
 
-				case UBX_CLASS_NAV:
-					_decode_state = UBX_DECODE_GOT_CLASS;
-					_message_class = NAV;
-					break;
+		break;
 
-//				case UBX_CLASS_RXM:
-//					_decode_state = UBX_DECODE_GOT_CLASS;
-//					_message_class = RXM;
-//					break;
+		/* Second, look for sync2 */
+	case UBX_DECODE_GOT_SYNC1:
+		if (b == UBX_SYNC2) {
+			_decode_state = UBX_DECODE_GOT_SYNC2;
+
+		} else {
+			/* Second start symbol was wrong, reset state machine */
+			decode_init();
+			/* don't return error, it can be just false sync1 */
+		}
 
-				case UBX_CLASS_CFG:
-					_decode_state = UBX_DECODE_GOT_CLASS;
-					_message_class = CFG;
-					break;
-				default: //unknown class: reset state machine
-					decode_init();
-					break;
-			}
-			break;
-		case UBX_DECODE_GOT_CLASS:
-			add_byte_to_checksum(b);
-			switch (_message_class) {
-				case NAV:
-					switch (b) {
-					case UBX_MESSAGE_NAV_POSLLH:
-						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-						_message_id = NAV_POSLLH;
-						break;
-
-					case UBX_MESSAGE_NAV_SOL:
-						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-						_message_id = NAV_SOL;
-						break;
-
-					case UBX_MESSAGE_NAV_TIMEUTC:
-						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-						_message_id = NAV_TIMEUTC;
-						break;
-
-//					case UBX_MESSAGE_NAV_DOP:
-//						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-//						_message_id = NAV_DOP;
-//						break;
-
-					case UBX_MESSAGE_NAV_SVINFO:
-						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-						_message_id = NAV_SVINFO;
-						break;
-
-					case UBX_MESSAGE_NAV_VELNED:
-						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-						_message_id = NAV_VELNED;
-						break;
-
-					default: //unknown class: reset state machine, should not happen
-						decode_init();
-						break;
-					}
-					break;
-//				case RXM:
-//					switch (b) {
-//					case UBX_MESSAGE_RXM_SVSI:
-//						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-//						_message_id = RXM_SVSI;
-//						break;
-//
-//					default: //unknown class: reset state machine, should not happen
-//						decode_init();
-//						break;
-//					}
-//					break;
-
-				case CFG:
-					switch (b) {
-					case UBX_MESSAGE_CFG_NAV5:
-						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-						_message_id = CFG_NAV5;
-						break;
-
-					default: //unknown class: reset state machine, should not happen
-						decode_init();
-						break;
-					}
-					break;
+		break;
 
-				case ACK:
-					switch (b) {
-					case UBX_MESSAGE_ACK_ACK:
-						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-						_message_id = ACK_ACK;
-						break;
-					case UBX_MESSAGE_ACK_NAK:
-						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-						_message_id = ACK_NAK;
-						break;
-					default: //unknown class: reset state machine, should not happen
-						decode_init();
-						break;
-					}
-					break;
-				default: //should not happen because we set the class
-					warnx("UBX Error, we set a class that we don't know");
-					decode_init();
-//					config_needed = true;
-					break;
-				}
-				break;
-			case UBX_DECODE_GOT_MESSAGEID:
-				add_byte_to_checksum(b);
-				_payload_size = b; //this is the first length byte
-				_decode_state = UBX_DECODE_GOT_LENGTH1;
-				break;
-			case UBX_DECODE_GOT_LENGTH1:
-				add_byte_to_checksum(b);
-				_payload_size += b << 8; // here comes the second byte of length
-				_decode_state = UBX_DECODE_GOT_LENGTH2;
-				break;
-			case UBX_DECODE_GOT_LENGTH2:
-				/* Add to checksum if not yet at checksum byte */
-				if (_rx_count < _payload_size)
-					add_byte_to_checksum(b);
-					_rx_buffer[_rx_count] = b;
-					/* 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]) {
-							return 1;
-						} else {
-							decode_init();
-							return -1;
-							warnx("ubx: Checksum wrong");
-						}
+		/* Now look for class */
+	case UBX_DECODE_GOT_SYNC2:
+		/* everything except sync1 and sync2 needs to be added to the checksum */
+		add_byte_to_checksum(b);
+		_message_class = b;
+		_decode_state = UBX_DECODE_GOT_CLASS;
+		break;
 
-						return 1;
-					} else if (_rx_count < RECV_BUFFER_SIZE) {
-						_rx_count++;
-					} else {
-						warnx("ubx: buffer full");
-						decode_init();
-						return -1;
-					}
-				break;
-		default:
-			break;
-	}
-	return 0; //XXX ?
-}
+	case UBX_DECODE_GOT_CLASS:
+		add_byte_to_checksum(b);
+		_message_id = b;
+		_decode_state = UBX_DECODE_GOT_MESSAGEID;
+		break;
 
+	case UBX_DECODE_GOT_MESSAGEID:
+		add_byte_to_checksum(b);
+		_payload_size = b; //this is the first length byte
+		_decode_state = UBX_DECODE_GOT_LENGTH1;
+		break;
 
-int
-UBX::handle_message()
-{
-	int ret = 0;
+	case UBX_DECODE_GOT_LENGTH1:
+		add_byte_to_checksum(b);
+		_payload_size += b << 8; // here comes the second byte of length
+		_decode_state = UBX_DECODE_GOT_LENGTH2;
+		break;
 
-	switch (_message_id) { //this enum is unique for all ids --> no need to check the class
-		case NAV_POSLLH: {
-//			printf("GOT NAV_POSLLH MESSAGE\n");
-			if (!_waiting_for_ack) {
-				gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer;
+	case UBX_DECODE_GOT_LENGTH2:
 
-				_gps_position->lat = packet->lat;
-				_gps_position->lon = packet->lon;
-				_gps_position->alt = packet->height_msl;
+		/* Add to checksum if not yet at checksum byte */
+		if (_rx_count < _payload_size)
+			add_byte_to_checksum(b);
 
-				_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
+		_rx_buffer[_rx_count] = b;
 
-				_rate_count_lat_lon++;
+		/* 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
 
-				/* Add timestamp to finish the report */
-				_gps_position->timestamp_position = hrt_absolute_time();
-				/* only return 1 when new position is available */
-				ret = 1;
+			} else {
+				warnx("ubx: checksum wrong");
+				decode_init();
+				return -1;
 			}
-			break;
+
+		} else if (_rx_count < RECV_BUFFER_SIZE) {
+			_rx_count++;
+
+		} else {
+			warnx("ubx: buffer full");
+			decode_init();
+			return -1;
 		}
 
-		case NAV_SOL: {
-//			printf("GOT NAV_SOL MESSAGE\n");
-			if (!_waiting_for_ack) {
-				gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer;
+		break;
 
-				_gps_position->fix_type = packet->gpsFix;
-				_gps_position->s_variance_m_s = packet->sAcc;
-				_gps_position->p_variance_m = packet->pAcc;
+	default:
+		break;
+	}
 
-				_gps_position->timestamp_variance = hrt_absolute_time();
-			}
-			break;
-		}
+	return 0;	// message decoding in progress
+}
 
-//		case NAV_DOP: {
-////		printf("GOT NAV_DOP MESSAGE\n");
-//			gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer;
-//
-//			_gps_position->eph_m =  packet->hDOP;
-//			_gps_position->epv =  packet->vDOP;
-//
-//			_gps_position->timestamp_posdilution = hrt_absolute_time();
-//
-//			_new_nav_dop = true;
-//
-//			break;
-//		}
-
-		case NAV_TIMEUTC: {
-//			printf("GOT NAV_TIMEUTC MESSAGE\n");
 
-			if (!_waiting_for_ack) {
-				gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer;
 
-				//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;
+int
+UBX::handle_message()
+{
+	int ret = 0;
 
-				time_t epoch = mktime(&timeinfo);
+	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->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
-				_gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
+					_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();
 
-				_gps_position->timestamp_time = hrt_absolute_time();
-			}
-			break;
-		}
+					_rate_count_lat_lon++;
 
-		case NAV_SVINFO: {
-//			printf("GOT NAV_SVINFO MESSAGE\n");
+					ret = 1;
+					break;
+				}
 
-			if (!_waiting_for_ack) {
-				//this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message
-				const int length_part1 = 8;
-				char _rx_buffer_part1[length_part1];
-				memcpy(_rx_buffer_part1, _rx_buffer, length_part1);
-				gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer_part1;
+			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;
 
-				//read checksum
-				const int length_part3 = 2;
-				char _rx_buffer_part3[length_part3];
-				memcpy(_rx_buffer_part3, &(_rx_buffer[_rx_count - 1]), length_part3);
+					_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();
 
-				//definitions needed to read numCh elements from the buffer:
-				const int length_part2 = 12;
-				gps_bin_nav_svinfo_part2_packet_t *packet_part2;
-				char _rx_buffer_part2[length_part2]; //for temporal storage
+					ret = 1;
+					break;
+				}
 
-				uint8_t satellites_used = 0;
-				int i;
+			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;
+
+					/* 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);
+
+#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);
+#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();
 
-				for (i = 0; i < packet_part1->numCh; i++) { //for each channel
+					ret = 1;
+					break;
+				}
 
-					/* Get satellite information from the buffer */
-					memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2);
-					packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2;
+			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;
 
-					/* Write satellite information in the global storage */
-					_gps_position->satellite_prn[i] = packet_part2->svid;
+					//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]);
 
-					//if satellite information is healthy store the data
-					uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield
+						/* write satellite information to global storage */
+						uint8_t sv_used = packet_part2->flags & 0x01;
 
-					if (!unhealthy) {
-						if ((packet_part2->flags) & 1) { //flags is a bitfield
-							_gps_position->satellite_used[i] = 1;
+						if (sv_used) {
+							/* count SVs used for NAV */
 							satellites_used++;
-
-						} else {
-							_gps_position->satellite_used[i] = 0;
 						}
 
+						/* 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);
+					}
 
-					} else {
+					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();
 
-				for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused
-					/* 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;
+					ret = 1;
+					break;
 				}
-				_gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones
 
-				/* set timestamp if any sat info is available */
-				if (packet_part1->numCh > 0) {
-					_gps_position->satellite_info_available = true;
-				} else {
-					_gps_position->satellite_info_available = false;
+			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;
+
+					_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();
+
+					_rate_count_vel++;
+
+					ret = 1;
+					break;
 				}
-				_gps_position->timestamp_satellites = hrt_absolute_time();
+
+			default:
+				break;
 			}
 
 			break;
-		}
 
-		case NAV_VELNED: {
-
-			if (!_waiting_for_ack) {
-				/* 35.15 NAV-VELNED (0x01 0x12) message (page 181 / 210 of reference manual */
-				gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer;
-
-				_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();
-
-				_rate_count_vel++;
+		case UBX_CLASS_ACK: {
+				/* ignore ACK when already configured */
+				ret = 1;
+				break;
 			}
 
+		default:
 			break;
 		}
 
-//		case RXM_SVSI: {
-//				printf("GOT RXM_SVSI MESSAGE\n");
-//				const int length_part1 = 7;
-//				char _rx_buffer_part1[length_part1];
-//				memcpy(_rx_buffer_part1, _rx_buffer, length_part1);
-//				gps_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) _rx_buffer_part1;
-//
-//				_gps_position->satellites_visible = packet->numVis;
-//				_gps_position->counter++;
-//				_last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time();
-//
-//			break;
-//		}
-		case 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 == _clsID_needed && packet->msgID == _msgID_needed) {
-					ret = 1;
-				}
+		if (ret == 0) {
+			/* message not handled */
+			warnx("ubx: unknown message received: 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id);
+
+			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;
+				warnx("ubx: disabling message 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id);
+				configure_message_rate(_message_class, _message_id, 0);
 			}
 		}
-		break;
 
-		case ACK_NAK: {
-//			printf("GOT ACK_NAK\n");
-			warnx("UBX: Received: Not Acknowledged");
-			/* configuration obviously not successful */
-			ret = -1;
-			break;
-		}
+	} 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;
+						}
+					}
 
-		default: //we don't know the message
-			warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id);
-			if (_disable_cmd_counter++ == 0) {
-				// Don't attempt for every message to disable, some might not be disabled */
-				warnx("Disabling message 0x%02x 0x%02x", (unsigned)_message_class, (unsigned)_message_id);
-				configure_message_rate(_message_class, _message_id, 0);
+					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;
+				}
+
+			default:
+				break;
 			}
-		return ret;
-			ret = -1;
-			break;
 		}
-	// end if _rx_count high enough
+	}
+
 	decode_init();
-	return ret; //XXX?
+	return ret;
 }
 
 void
@@ -695,9 +628,8 @@ UBX::decode_init(void)
 	_rx_ck_b = 0;
 	_rx_count = 0;
 	_decode_state = UBX_DECODE_UNINIT;
-	_message_class = CLASS_UNKNOWN;
-	_message_id = ID_UNKNOWN;
 	_payload_size = 0;
+	/* don't reset _message_class, _message_id, _rx_buffer leave it for message handler */
 }
 
 void
@@ -708,23 +640,24 @@ UBX::add_byte_to_checksum(uint8_t b)
 }
 
 void
-UBX::add_checksum_to_message(uint8_t* message, const unsigned length)
+UBX::add_checksum_to_message(uint8_t *message, const unsigned length)
 {
 	uint8_t ck_a = 0;
 	uint8_t ck_b = 0;
 	unsigned i;
 
-	for (i = 0; i < length-2; i++) {
+	for (i = 0; i < length - 2; i++) {
 		ck_a = ck_a + message[i];
 		ck_b = ck_b + ck_a;
 	}
-	/* The checksum is written to the last to bytes of a message */
-	message[length-2] = ck_a;
-	message[length-1] = ck_b;
+
+	/* 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::add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b)
 {
 	for (unsigned i = 0; i < length; i++) {
 		ck_a = ck_a + message[i];
@@ -735,11 +668,11 @@ UBX::add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_
 void
 UBX::configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate)
 {
-    struct ubx_cfg_msg_rate msg;
-    msg.msg_class	= msg_class;
-    msg.msg_id		= msg_id;
-    msg.rate		= rate;
-    send_message(CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg));
+	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));
 }
 
 void
@@ -747,36 +680,36 @@ UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
 {
 	ssize_t ret = 0;
 
-	/* Calculate the checksum now */
+	/* calculate the checksum now */
 	add_checksum_to_message(packet, length);
 
 	const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2};
 
-	/* Start with the two sync bytes */
+	/* start with the two sync bytes */
 	ret += write(fd, sync_bytes, sizeof(sync_bytes));
 	ret += write(fd, packet, length);
 
 	if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning?
-		warnx("ubx: config write fail");
+		warnx("ubx: configuration write fail");
 }
 
 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;
+	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 *)&header.msg_class, sizeof(header) - 2, ck_a, ck_b);
 	add_checksum((uint8_t *)msg, size, ck_a, ck_b);
 
-	// Configure receive check
-	_clsID_needed = msg_class;
-	_msgID_needed = msg_id;
+	/* 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);
diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h
index 5a433642c..79a904f4a 100644
--- a/src/drivers/gps/ubx.h
+++ b/src/drivers/gps/ubx.h
@@ -1,8 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
+ *   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
@@ -33,7 +31,17 @@
  *
  ****************************************************************************/
 
-/* @file U-Blox protocol definitions */
+/**
+ * @file ubx.h
+ *
+ * U-Blox protocol definition. Following u-blox 6/7 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>
+ *
+ */
 
 #ifndef UBX_H_
 #define UBX_H_
@@ -51,23 +59,23 @@
 
 /* MessageIDs (the ones that are used) */
 #define UBX_MESSAGE_NAV_POSLLH 0x02
-#define UBX_MESSAGE_NAV_SOL 0x06
-#define UBX_MESSAGE_NAV_TIMEUTC 0x21
 //#define UBX_MESSAGE_NAV_DOP 0x04
-#define UBX_MESSAGE_NAV_SVINFO 0x30
+#define UBX_MESSAGE_NAV_SOL 0x06
 #define UBX_MESSAGE_NAV_VELNED 0x12
 //#define UBX_MESSAGE_RXM_SVSI 0x20
-#define UBX_MESSAGE_ACK_ACK 0x01
+#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_NAV5 0x24
 #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		/**< always choose 38400 as GPS baudrate */
+#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 */
 
@@ -299,44 +307,6 @@ struct ubx_cfg_msg_rate {
 // ************
 
 typedef enum {
-	UBX_CONFIG_STATE_PRT = 0,
-	UBX_CONFIG_STATE_PRT_NEW_BAUDRATE,
-	UBX_CONFIG_STATE_RATE,
-	UBX_CONFIG_STATE_NAV5,
-	UBX_CONFIG_STATE_MSG_NAV_POSLLH,
-	UBX_CONFIG_STATE_MSG_NAV_TIMEUTC,
-	UBX_CONFIG_STATE_MSG_NAV_DOP,
-	UBX_CONFIG_STATE_MSG_NAV_SVINFO,
-	UBX_CONFIG_STATE_MSG_NAV_SOL,
-	UBX_CONFIG_STATE_MSG_NAV_VELNED,
-//	UBX_CONFIG_STATE_MSG_RXM_SVSI,
-	UBX_CONFIG_STATE_CONFIGURED
-} ubx_config_state_t;
-
-typedef enum {
-	CLASS_UNKNOWN = 0,
-	NAV = 1,
-	RXM = 2,
-	ACK = 3,
-	CFG = 4
-} ubx_message_class_t;
-
-typedef enum {
-	//these numbers do NOT correspond to the message id numbers of the ubx protocol
-	ID_UNKNOWN = 0,
-	NAV_POSLLH,
-	NAV_SOL,
-	NAV_TIMEUTC,
-//	NAV_DOP,
-	NAV_SVINFO,
-	NAV_VELNED,
-//	RXM_SVSI,
-	CFG_NAV5,
-	ACK_ACK,
-	ACK_NAK,
-} ubx_message_id_t;
-
-typedef enum {
 	UBX_DECODE_UNINIT = 0,
 	UBX_DECODE_GOT_SYNC1,
 	UBX_DECODE_GOT_SYNC2,
@@ -384,7 +354,7 @@ private:
 	/**
 	 * Add the two checksum bytes to an outgoing message
 	 */
-	void			add_checksum_to_message(uint8_t* message, const unsigned length);
+	void			add_checksum_to_message(uint8_t *message, const unsigned length);
 
 	/**
 	 * Helper to send a config packet
@@ -395,25 +365,25 @@ private:
 
 	void			send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size);
 
-	void			add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b);
+	void			add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b);
 
 	int			wait_for_ack(unsigned timeout);
 
 	int			_fd;
 	struct vehicle_gps_position_s *_gps_position;
-	ubx_config_state_t	_config_state;
+	bool			_configured;
 	bool			_waiting_for_ack;
-	uint8_t			_clsID_needed;
-	uint8_t			_msgID_needed;
+	uint8_t			_message_class_needed;
+	uint8_t			_message_id_needed;
 	ubx_decode_state_t	_decode_state;
 	uint8_t			_rx_buffer[RECV_BUFFER_SIZE];
 	unsigned		_rx_count;
 	uint8_t			_rx_ck_a;
 	uint8_t			_rx_ck_b;
-	ubx_message_class_t	_message_class;
-	ubx_message_id_t	_message_id;
+	uint8_t			_message_class;
+	uint8_t			_message_id;
 	unsigned		_payload_size;
-	uint8_t			_disable_cmd_counter;
+	uint8_t			_disable_cmd_last;
 };
 
 #endif /* UBX_H_ */
diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp
index bd027ce0b..0a047f38f 100644
--- a/src/drivers/hil/hil.cpp
+++ b/src/drivers/hil/hil.cpp
@@ -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
@@ -75,6 +75,7 @@
 #include <systemlib/mixer/mixer.h>
 
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_outputs.h>
 
 #include <systemlib/err.h>
@@ -192,9 +193,10 @@ HIL::~HIL()
 		} while (_task != -1);
 	}
 
-	/* clean up the alternate device node */
-	if (_primary_pwm_device)
-		unregister_driver(PWM_OUTPUT_DEVICE_PATH);
+	// XXX already claimed with CDEV
+	// /* clean up the alternate device node */
+	// if (_primary_pwm_device)
+	// 	unregister_driver(PWM_OUTPUT_DEVICE_PATH);
 
 	g_hil = nullptr;
 }
@@ -403,7 +405,7 @@ HIL::task_main()
 				for (unsigned i = 0; i < num_outputs; i++) {
 
 					/* last resort: catch NaN, INF and out-of-band errors */
-					if (i < (unsigned)outputs.noutputs &&
+					if (i < outputs.noutputs &&
 						isfinite(outputs.output[i]) &&
 						outputs.output[i] >= -1.0f &&
 						outputs.output[i] <= 1.0f) {
@@ -516,7 +518,7 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		g_hil->set_pwm_rate(arg);
 		break;
 
-	case PWM_SERVO_SELECT_UPDATE_RATE:
+	case PWM_SERVO_SET_SELECT_UPDATE_RATE:
 		// HIL always outputs at the alternate (usually faster) rate 
 		break;
 
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 59e90d86c..4c85c0cda 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -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
@@ -58,13 +58,14 @@
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/board.h>
+#include <board_config.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
 
 #include <drivers/drv_mag.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/device/ringbuffer.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/subsystem_info.h>
@@ -76,9 +77,10 @@
  */
 
 #define HMC5883L_ADDRESS		PX4_I2C_OBDEV_HMC5883
+#define HMC5883L_DEVICE_PATH		"/dev/hmc5883"
 
-/* Max measurement rate is 160Hz */
-#define HMC5883_CONVERSION_INTERVAL	(1000000 / 160)	/* microseconds */
+/* 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 */
 
 #define ADDR_CONF_A			0x00
 #define ADDR_CONF_B			0x01
@@ -148,14 +150,12 @@ private:
 	work_s			_work;
 	unsigned		_measure_ticks;
 
-	unsigned		_num_reports;
-	volatile unsigned	_next_report;
-	volatile unsigned	_oldest_report;
-	mag_report		*_reports;
+	RingBuffer		*_reports;
 	mag_scale		_scale;
 	float 			_range_scale;
 	float 			_range_ga;
 	bool			_collect_phase;
+	int			_class_instance;
 
 	orb_advert_t		_mag_topic;
 
@@ -167,6 +167,8 @@ private:
 	bool			_sensor_ok;		/**< sensor was found and reports ok */
 	bool			_calibrated;		/**< the calibration is valid */
 
+	int			_bus;			/**< the bus the device is connected to */
+
 	/**
 	 * Test whether the device supported by the driver is present at a
 	 * specific address.
@@ -190,6 +192,11 @@ private:
 	void			stop();
 
 	/**
+	 * Reset the device
+	 */
+	int			reset();
+
+	/**
 	 * Perform the on-sensor scale calibration routine.
 	 *
 	 * @note The sensor will continue to provide measurements, these
@@ -303,9 +310,6 @@ private:
 
 };
 
-/* helper macro for handling report buffer indices */
-#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
-
 /*
  * Driver 'main' command.
  */
@@ -313,20 +317,19 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]);
 
 
 HMC5883::HMC5883(int bus) :
-	I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000),
+	I2C("HMC5883", HMC5883L_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000),
 	_measure_ticks(0),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
 	_reports(nullptr),
 	_range_scale(0), /* default range scale from counts to gauss */
 	_range_ga(1.3f),
 	_mag_topic(-1),
+	_class_instance(-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")),
 	_sensor_ok(false),
-	_calibrated(false)
+	_calibrated(false),
+	_bus(bus)
 {
 	// enable debug() calls
 	_debug_enabled = false;
@@ -348,9 +351,16 @@ HMC5883::~HMC5883()
 	/* make sure we are truly inactive */
 	stop();
 
-	/* free any existing reports */
 	if (_reports != nullptr)
-		delete[] _reports;
+		delete _reports;
+
+	if (_class_instance != -1)
+		unregister_class_devname(MAG_DEVICE_PATH, _class_instance);
+
+	// free perf counters
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+	perf_free(_buffer_overflows);
 }
 
 int
@@ -363,23 +373,14 @@ HMC5883::init()
 		goto out;
 
 	/* allocate basic report buffers */
-	_num_reports = 2;
-	_reports = new struct mag_report[_num_reports];
-
+	_reports = new RingBuffer(2, sizeof(mag_report));
 	if (_reports == nullptr)
 		goto out;
 
-	_oldest_report = _next_report = 0;
-
-	/* get a publish handle on the mag topic */
-	memset(&_reports[0], 0, sizeof(_reports[0]));
-	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_reports[0]);
-
-	if (_mag_topic < 0)
-		debug("failed to create sensor_mag object");
+	/* reset the device configuration */
+	reset();
 
-	/* set range */
-	set_range(_range_ga);
+	_class_instance = register_class_devname(MAG_DEVICE_PATH);
 
 	ret = OK;
 	/* sensor is ok, but not calibrated */
@@ -480,6 +481,7 @@ ssize_t
 HMC5883::read(struct file *filp, char *buffer, size_t buflen)
 {
 	unsigned count = buflen / sizeof(struct mag_report);
+	struct mag_report *mag_buf = reinterpret_cast<struct mag_report *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
@@ -488,17 +490,15 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
 
 	/* 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 (_oldest_report != _next_report) {
-				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
-				ret += sizeof(_reports[0]);
-				INCREMENT(_oldest_report, _num_reports);
+			if (_reports->get(mag_buf)) {
+				ret += sizeof(struct mag_report);
+				mag_buf++;
 			}
 		}
 
@@ -509,7 +509,7 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
 	/* manual measurement - run one conversion */
 	/* XXX really it'd be nice to lock against other readers here */
 	do {
-		_oldest_report = _next_report = 0;
+		_reports->flush();
 
 		/* trigger a measurement */
 		if (OK != measure()) {
@@ -526,10 +526,9 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
 			break;
 		}
 
-		/* state machine will have generated a report, copy it out */
-		memcpy(buffer, _reports, sizeof(*_reports));
-		ret = sizeof(*_reports);
-
+		if (_reports->get(mag_buf)) {
+			ret = sizeof(struct mag_report);
+		}
 	} while (0);
 
 	return ret;
@@ -539,108 +538,105 @@ int
 HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
 	switch (cmd) {
-
 	case SENSORIOCSPOLLRATE: {
-			switch (arg) {
+		switch (arg) {
 
-				/* switching to manual polling */
-			case SENSOR_POLLRATE_MANUAL:
-				stop();
-				_measure_ticks = 0;
-				return OK;
+			/* switching to manual polling */
+		case SENSOR_POLLRATE_MANUAL:
+			stop();
+			_measure_ticks = 0;
+			return OK;
 
-				/* external signalling (DRDY) not supported */
-			case SENSOR_POLLRATE_EXTERNAL:
+			/* external signalling (DRDY) not supported */
+		case SENSOR_POLLRATE_EXTERNAL:
 
-				/* zero would be bad */
-			case 0:
-				return -EINVAL;
+			/* 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 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(HMC5883_CONVERSION_INTERVAL);
+				/* set interval for next measurement to minimum legal value */
+				_measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL);
 
-					/* if we need to start the poll state machine, do it */
-					if (want_start)
-						start();
+				/* if we need to start the poll state machine, do it */
+				if (want_start)
+					start();
 
-					return OK;
-				}
+				return OK;
+			}
 
-				/* adjust to a legal polling interval in Hz */
-			default: {
-					/* do we need to start internal polling? */
-					bool want_start = (_measure_ticks == 0);
+			/* 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);
+				/* convert hz to tick interval via microseconds */
+				unsigned ticks = USEC2TICK(1000000 / arg);
 
-					/* check against maximum rate */
-					if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
-						return -EINVAL;
+				/* check against maximum rate */
+				if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
+					return -EINVAL;
 
-					/* update interval for next measurement */
-					_measure_ticks = ticks;
+				/* update interval for next measurement */
+				_measure_ticks = ticks;
 
-					/* if we need to start the poll state machine, do it */
-					if (want_start)
-						start();
+				/* if we need to start the poll state machine, do it */
+				if (want_start)
+					start();
 
-					return OK;
-				}
+				return OK;
 			}
 		}
+	}
 
 	case SENSORIOCGPOLLRATE:
 		if (_measure_ticks == 0)
 			return SENSOR_POLLRATE_MANUAL;
 
-		return (1000 / _measure_ticks);
+		return 1000000/TICK2USEC(_measure_ticks);
 
 	case SENSORIOCSQUEUEDEPTH: {
-			/* add one to account for the sentinel in the ring */
-			arg++;
-
 			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 2) || (arg > 100))
+			if ((arg < 1) || (arg > 100))
 				return -EINVAL;
 
-			/* allocate new buffer */
-			struct mag_report *buf = new struct mag_report[arg];
-
-			if (nullptr == buf)
+			irqstate_t flags = irqsave();
+			if (!_reports->resize(arg)) {
+				irqrestore(flags);
 				return -ENOMEM;
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete[] _reports;
-			_num_reports = arg;
-			_reports = buf;
-			start();
+			}
+			irqrestore(flags);
 
 			return OK;
 		}
 
 	case SENSORIOCGQUEUEDEPTH:
-		return _num_reports - 1;
+		return _reports->size();
 
 	case SENSORIOCRESET:
-		/* XXX implement this */
-		return -EINVAL;
+		return reset();
 
 	case MAGIOCSSAMPLERATE:
-		/* not supported, always 1 sample per poll */
-		return -EINVAL;
+		/* same as pollrate because device is in single measurement mode*/
+		return ioctl(filp, SENSORIOCSPOLLRATE, arg);
+
+	case MAGIOCGSAMPLERATE:
+		/* same as pollrate because device is in single measurement mode*/
+		return 1000000/TICK2USEC(_measure_ticks);
 
 	case MAGIOCSRANGE:
 		return set_range(arg);
 
+	case MAGIOCGRANGE:
+		return _range_ga;
+
 	case MAGIOCSLOWPASS:
+	case MAGIOCGLOWPASS:
 		/* not supported, no internal filtering */
 		return -EINVAL;
 
@@ -665,6 +661,12 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case MAGIOCSELFTEST:
 		return check_calibration();
 
+	case MAGIOCGEXTERNAL:
+		if (_bus == PX4_I2C_BUS_EXPANSION)
+			return 1;
+		else
+			return 0;
+
 	default:
 		/* give it to the superclass */
 		return I2C::ioctl(filp, cmd, arg);
@@ -676,7 +678,7 @@ HMC5883::start()
 {
 	/* reset the report ring and state machine */
 	_collect_phase = false;
-	_oldest_report = _next_report = 0;
+	_reports->flush();
 
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&HMC5883::cycle_trampoline, this, 1);
@@ -688,6 +690,13 @@ HMC5883::stop()
 	work_cancel(HPWORK, &_work);
 }
 
+int
+HMC5883::reset()
+{
+	/* set range */
+	return set_range(_range_ga);
+}
+
 void
 HMC5883::cycle_trampoline(void *arg)
 {
@@ -778,9 +787,11 @@ HMC5883::collect()
 
 
 	perf_begin(_sample_perf);
+	struct mag_report new_report;
 
 	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
-	_reports[_next_report].timestamp = hrt_absolute_time();
+	new_report.timestamp = hrt_absolute_time();
+        new_report.error_count = perf_event_count(_comms_errors);
 
 	/*
 	 * @note  We could read the status register here, which could tell us that
@@ -810,8 +821,10 @@ HMC5883::collect()
 	 */
 	if ((abs(report.x) > 2048) ||
 	    (abs(report.y) > 2048) ||
-	    (abs(report.z) > 2048))
+	    (abs(report.z) > 2048)) {
+		perf_count(_comms_errors);
 		goto out;
+	}
 
 	/*
 	 * RAW outputs
@@ -819,58 +832,47 @@ HMC5883::collect()
 	 * to align the sensor axes with the board, x and y need to be flipped
 	 * and y needs to be negated
 	 */
-	_reports[_next_report].x_raw = report.y;
-	_reports[_next_report].y_raw = ((report.x == -32768) ? 32767 : -report.x);
+	new_report.x_raw = report.y;
+	new_report.y_raw = -report.x;
 	/* z remains z */
-	_reports[_next_report].z_raw = report.z;
+	new_report.z_raw = report.z;
 
 	/* scale values for output */
 
-	/*
-	 * 1) Scale raw value to SI units using scaling from datasheet.
-	 * 2) Subtract static offset (in SI units)
-	 * 3) Scale the statically calibrated values with a linear
-	 *    dynamically obtained factor
-	 *
-	 * Note: the static sensor offset is the number the sensor outputs
-	 * 	 at a nominally 'zero' input. Therefore the offset has to
-	 * 	 be subtracted.
-	 *
-	 *	 Example: A gyro outputs a value of 74 at zero angular rate
-	 *	 	  the offset is 74 from the origin and subtracting
-	 *		  74 from all measurements centers them around zero.
-	 */
-
 #ifdef PX4_I2C_BUS_ONBOARD
 	if (_bus == PX4_I2C_BUS_ONBOARD) {
-		/* to align the sensor axes with the board, x and y need to be flipped */
-		_reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
-		/* flip axes and negate value for y */
-		_reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale;
-		/* z remains z */
-		_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
-	} else {
-#endif
-		/* XXX axis assignment of external sensor is yet unknown */
-		_reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
-		/* flip axes and negate value for y */
-		_reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale;
-		/* z remains z */
-		_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
-#ifdef PX4_I2C_BUS_ONBOARD
-	}
+		// convert onboard so it matches offboard for the
+		// scaling below
+		report.y = -report.y;
+		report.x = -report.x;
+        }
 #endif
 
-	/* publish it */
-	orb_publish(ORB_ID(sensor_mag), _mag_topic, &_reports[_next_report]);
+	/* the standard external mag by 3DR has x pointing to the
+	 * right, y pointing backwards, and z down, therefore switch x
+	 * and y and invert y */
+	new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
+	/* flip axes and negate value for y */
+	new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
+	/* z remains z */
+	new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
 
-	/* post a report to the ring - note, not locked */
-	INCREMENT(_next_report, _num_reports);
+	if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
 
-	/* if we are running up against the oldest report, toss it */
-	if (_next_report == _oldest_report) {
+		if (_mag_topic != -1) {
+			/* publish it */
+			orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
+		} else {
+			_mag_topic = orb_advertise(ORB_ID(sensor_mag), &new_report);
+
+			if (_mag_topic < 0)
+				debug("failed to create sensor_mag publication");
+		}
+	}
+
+	/* post a report to the ring */
+	if (_reports->force(&new_report)) {
 		perf_count(_buffer_overflows);
-		INCREMENT(_oldest_report, _num_reports);
 	}
 
 	/* notify anyone waiting for data */
@@ -888,6 +890,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 	struct mag_report report;
 	ssize_t sz;
 	int ret = 1;
+	uint8_t good_count = 0;
 
 	// XXX do something smarter here
 	int fd = (int)enable;
@@ -910,30 +913,16 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 		1.0f,
 	};
 
-	float avg_excited[3] = {0.0f, 0.0f, 0.0f};
-	unsigned i;
+	float sum_excited[3] = {0.0f, 0.0f, 0.0f};
 
-	warnx("starting mag scale calibration");
+	/* expected axis scaling. The datasheet says that 766 will
+	 * be places in the X and Y axes and 713 in the Z
+	 * axis. Experiments show that in fact 766 is placed in X,
+	 * and 713 in Y and Z. This is relative to a base of 660
+	 * LSM/Ga, giving 1.16 and 1.08 */
+	float expected_cal[3] = { 1.16f, 1.08f, 1.08f };
 
-	/* do a simple demand read */
-	sz = read(filp, (char *)&report, sizeof(report));
-
-	if (sz != sizeof(report)) {
-		warn("immediate read failed");
-		ret = 1;
-		goto out;
-	}
-
-	warnx("current measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
-	warnx("time:        %lld", report.timestamp);
-	warnx("sampling 500 samples for scaling offset");
-
-	/* set the queue depth to 10 */
-	if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) {
-		warn("failed to set queue depth");
-		ret = 1;
-		goto out;
-	}
+	warnx("starting mag scale calibration");
 
 	/* start the sensor polling at 50 Hz */
 	if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
@@ -942,8 +931,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 		goto out;
 	}
 
-	/* Set to 2.5 Gauss */
-	if (OK != ioctl(filp, MAGIOCSRANGE, 2)) {
+	/* Set to 2.5 Gauss. We ask for 3 to get the right part of
+         * the chained if statement above. */
+	if (OK != ioctl(filp, MAGIOCSRANGE, 3)) {
 		warnx("failed to set 2.5 Ga range");
 		ret = 1;
 		goto out;
@@ -967,8 +957,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 		goto out;
 	}
 
-	/* read the sensor 10x and report each value */
-	for (i = 0; i < 500; i++) {
+	// discard 10 samples to let the sensor settle
+	for (uint8_t i = 0; i < 10; i++) {
 		struct pollfd fds;
 
 		/* wait for data to be ready */
@@ -986,32 +976,69 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 
 		if (sz != sizeof(report)) {
 			warn("periodic read failed");
+			ret = -EIO;
 			goto out;
+		}
+	}
 
-		} else {
-			avg_excited[0] += report.x;
-			avg_excited[1] += report.y;
-			avg_excited[2] += report.z;
+	/* read the sensor up to 50x, stopping when we have 10 good values */
+	for (uint8_t i = 0; i < 50 && good_count < 10; i++) {
+		struct pollfd fds;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		ret = ::poll(&fds, 1, 2000);
+
+		if (ret != 1) {
+			warn("timed out waiting for sensor data");
+			goto out;
+		}
+
+		/* now go get it */
+		sz = ::read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report)) {
+			warn("periodic read failed");
+			ret = -EIO;
+			goto out;
+		}
+		float cal[3] = {fabsf(expected_cal[0] / report.x), 
+				fabsf(expected_cal[1] / report.y), 
+				fabsf(expected_cal[2] / report.z)};
+
+		if (cal[0] > 0.7f && cal[0] < 1.35f &&
+		    cal[1] > 0.7f && cal[1] < 1.35f &&
+		    cal[2] > 0.7f && cal[2] < 1.35f) {
+			good_count++;
+			sum_excited[0] += cal[0];
+			sum_excited[1] += cal[1];
+			sum_excited[2] += cal[2];
 		}
 
 		//warnx("periodic read %u", i);
 		//warnx("measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
+		//warnx("cal: %.6f  %.6f  %.6f", (double)cal[0], (double)cal[1], (double)cal[2]);
 	}
 
-	avg_excited[0] /= i;
-	avg_excited[1] /= i;
-	avg_excited[2] /= i;
+	if (good_count < 5) {
+		warn("failed calibration");
+		ret = -EIO;
+		goto out;
+	}
 
-	warnx("done. Performed %u reads", i);
-	warnx("measurement avg: %.6f  %.6f  %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]);
+#if 0
+	warnx("measurement avg: %.6f  %.6f  %.6f", 
+	      (double)sum_excited[0]/good_count, 
+	      (double)sum_excited[1]/good_count, 
+	      (double)sum_excited[2]/good_count);
+#endif
 
 	float scaling[3];
 
-	/* calculate axis scaling */
-	scaling[0] = fabsf(1.16f / avg_excited[0]);
-	/* second axis inverted */
-	scaling[1] = fabsf(1.16f / -avg_excited[1]);
-	scaling[2] = fabsf(1.08f / avg_excited[2]);
+	scaling[0] = sum_excited[0] / good_count;
+	scaling[1] = sum_excited[1] / good_count;
+	scaling[2] = sum_excited[2] / good_count;
 
 	warnx("axes scaling: %.6f  %.6f  %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
 
@@ -1111,10 +1138,12 @@ int HMC5883::check_calibration()
 			SUBSYSTEM_TYPE_MAG};
 		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);
+		if (!(_pub_blocked)) {
+			if (pub > 0) {
+				orb_publish(ORB_ID(subsystem_info), pub, &info);
+			} else {
+				pub = orb_advertise(ORB_ID(subsystem_info), &info);
+			}
 		}
 	}
 
@@ -1142,6 +1171,8 @@ int HMC5883::set_excitement(unsigned enable)
 		conf_reg &= ~0x03;
 	}
 
+        // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)conf_reg);
+
 	ret = write_reg(ADDR_CONF_A, conf_reg);
 
 	if (OK != ret)
@@ -1150,6 +1181,8 @@ int HMC5883::set_excitement(unsigned enable)
 	uint8_t conf_reg_ret;
 	read_reg(ADDR_CONF_A, conf_reg_ret);
 
+	//print_info();
+
 	return !(conf_reg == conf_reg_ret);
 }
 
@@ -1188,8 +1221,11 @@ HMC5883::print_info()
 	perf_print_counter(_comms_errors);
 	perf_print_counter(_buffer_overflows);
 	printf("poll interval:  %u ticks\n", _measure_ticks);
-	printf("report queue:   %u (%u/%u @ %p)\n",
-	       _num_reports, _oldest_report, _next_report, _reports);
+	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);
+	_reports->print_info("report queue");
 }
 
 /**
@@ -1221,7 +1257,8 @@ start()
 	int fd;
 
 	if (g_dev != nullptr)
-		errx(1, "already started");
+		/* 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);
@@ -1245,7 +1282,7 @@ start()
 		goto fail;
 
 	/* set the poll rate to default, starts automatic data collection */
-	fd = open(MAG_DEVICE_PATH, O_RDONLY);
+	fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0)
 		goto fail;
@@ -1277,10 +1314,10 @@ test()
 	ssize_t sz;
 	int ret;
 
-	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+	int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0)
-		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
+		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH);
 
 	/* do a simple demand read */
 	sz = read(fd, &report, sizeof(report));
@@ -1292,7 +1329,12 @@ test()
 	warnx("measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
 	warnx("time:        %lld", report.timestamp);
 
-	/* set the queue depth to 10 */
+	/* check if mag is onboard or external */
+	if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0)
+		errx(1, "failed to get if mag is onboard or external");
+	warnx("device active: %s", ret ? "external" : "onboard");
+
+	/* set the queue depth to 5 */
 	if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10))
 		errx(1, "failed to set queue depth");
 
@@ -1372,10 +1414,10 @@ int calibrate()
 {
 	int ret;
 
-	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+	int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0)
-		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
+		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH);
 
 	if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) {
 		warnx("failed to enable sensor calibration mode");
@@ -1397,7 +1439,7 @@ int calibrate()
 void
 reset()
 {
-	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+	int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0)
 		err(1, "failed ");
diff --git a/src/drivers/hott/comms.cpp b/src/drivers/hott/comms.cpp
new file mode 100644
index 000000000..cb8bbba37
--- /dev/null
+++ b/src/drivers/hott/comms.cpp
@@ -0,0 +1,92 @@
+/****************************************************************************
+ *
+ *   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 comms.c
+ * @author Simon Wilks <sjwilks@gmail.com>
+ *
+ */
+
+#include "comms.h"
+
+#include <fcntl.h>
+#include <stdio.h>
+#include <sys/ioctl.h>
+#include <systemlib/err.h>
+#include <termios.h>
+
+int
+open_uart(const char *device)
+{
+	/* baud rate */
+	static const speed_t speed = B19200;
+
+	/* open uart */
+	const int uart = open(device, O_RDWR | O_NOCTTY);
+
+	if (uart < 0) {
+		err(1, "Error opening port: %s", device);
+	}
+	
+	/* Back up the original uart configuration to restore it after exit */	
+	int termios_state;
+	struct termios uart_config_original;
+	if ((termios_state = tcgetattr(uart, &uart_config_original)) < 0) {
+		close(uart);
+		err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state);
+	}
+
+	/* Fill the struct for the new configuration */
+	struct termios uart_config;
+	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) {
+		close(uart);
+		err(1, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)",
+			 device, termios_state);
+	}
+
+	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
+		close(uart);
+		err(1, "Error setting baudrate / termios config for %s (tcsetattr)", device);
+	}
+
+	/* Activate single wire mode */
+	ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
+
+	return uart;
+}
diff --git a/src/drivers/hott/comms.h b/src/drivers/hott/comms.h
new file mode 100644
index 000000000..f5608122f
--- /dev/null
+++ b/src/drivers/hott/comms.h
@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ *   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 comms.h
+ * @author Simon Wilks <sjwilks@gmail.com>
+ *
+ */
+
+
+#ifndef COMMS_H_
+#define COMMS_H
+
+int open_uart(const char *device);
+
+#endif /* COMMS_H_ */
diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp
new file mode 100644
index 000000000..a3d3a3933
--- /dev/null
+++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp
@@ -0,0 +1,238 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@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 hott_sensors.c
+ * @author Simon Wilks <sjwilks@gmail.com>
+ *
+ * Graupner HoTT sensor driver implementation.
+ *
+ * Poll any sensors connected to the PX4 via the telemetry wire.
+ */
+
+#include <fcntl.h>
+#include <nuttx/config.h>
+#include <poll.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <unistd.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+#include "../comms.h"
+#include "../messages.h"
+
+#define DEFAULT_UART "/dev/ttyS0";		/**< USART1 */
+
+/* Oddly, ERROR is not defined for C++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+static int thread_should_exit = false;		/**< Deamon exit flag */
+static int thread_running = false;		/**< Deamon status flag */
+static int deamon_task;				/**< Handle of deamon task / thread */
+static const char daemon_name[] = "hott_sensors";
+static const char commandline_usage[] = "usage: hott_sensors start|status|stop [-d <device>]";
+
+/**
+ * Deamon management function.
+ */
+extern "C" __EXPORT int hott_sensors_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of daemon.
+ */
+int hott_sensors_thread_main(int argc, char *argv[]);
+
+static int recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id);
+static int send_poll(int uart, uint8_t *buffer, size_t size);
+
+int
+send_poll(int uart, uint8_t *buffer, size_t size)
+{
+	for (size_t i = 0; i < size; i++) {
+		write(uart, &buffer[i], sizeof(buffer[i]));
+
+		/* Sleep before sending the next byte. */
+		usleep(POST_WRITE_DELAY_IN_USECS);
+	}
+
+	/* A hack the reads out what was written so the next read from the receiver doesn't get it. */
+	/* TODO: Fix this!! */
+	uint8_t dummy[size];
+	read(uart, &dummy, size);
+
+	return OK;
+}
+
+int
+recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id)
+{
+	static const int timeout_ms = 1000;
+	
+	struct pollfd fds;
+	fds.fd = uart;
+	fds.events = POLLIN;
+	
+	// XXX should this poll be inside the while loop???
+	if (poll(&fds, 1, timeout_ms) > 0) {
+		int i = 0;
+		bool stop_byte_read = false;
+		while (true)  {
+			read(uart, &buffer[i], sizeof(buffer[i]));
+
+			if (stop_byte_read) {
+				// XXX process checksum
+				*size = ++i;
+				return OK;
+			}
+			// XXX can some other field not have the STOP BYTE value?
+			if (buffer[i] == STOP_BYTE) {
+				*id = buffer[1];
+				stop_byte_read = true;
+			}
+			i++;
+		}
+	}
+	return ERROR;
+}
+
+int
+hott_sensors_thread_main(int argc, char *argv[])
+{
+	warnx("starting");
+
+	thread_running = true;
+
+	const char *device = DEFAULT_UART;
+
+	/* read commandline arguments */
+	for (int i = 0; i < argc && argv[i]; i++) {
+		if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set
+			if (argc > i + 1) {
+				device = argv[i + 1];
+
+			} else {
+				thread_running = false;
+				errx(1, "missing parameter to -d\n%s", commandline_usage);
+			}
+		}
+	}
+
+	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
+	const int uart = open_uart(device);
+	if (uart < 0) {
+		errx(1, "Failed opening HoTT UART, exiting.");
+		thread_running = false;
+	}
+
+	init_pub_messages();
+
+	uint8_t buffer[MAX_MESSAGE_BUFFER_SIZE];
+	size_t size = 0;
+	uint8_t id = 0;
+	while (!thread_should_exit) {
+		// Currently we only support a General Air Module sensor.
+		build_gam_request(&buffer[0], &size);
+		send_poll(uart, buffer, size);
+
+		// The sensor will need a little time before it starts sending.
+		usleep(5000);
+
+		recv_data(uart, &buffer[0], &size, &id);
+
+		// Determine which moduel sent it and process accordingly.
+		if (id == GAM_SENSOR_ID) {
+			publish_gam_message(buffer);
+		} else {
+			warnx("Unknown sensor ID: %d", id);
+		}
+	}
+
+	warnx("exiting");
+	close(uart);
+	thread_running = false;
+
+	return 0;
+}
+
+/**
+ * Process command line arguments and start the daemon.
+ */
+int
+hott_sensors_main(int argc, char *argv[])
+{
+	if (argc < 1) {
+		errx(1, "missing command\n%s", commandline_usage);
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("deamon already running");
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		deamon_task = task_spawn_cmd(daemon_name,
+					     SCHED_DEFAULT,
+					     SCHED_PRIORITY_DEFAULT,
+					     1024,
+					     hott_sensors_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("daemon is running");
+
+		} else {
+			warnx("daemon not started");
+		}
+
+		exit(0);
+	}
+
+	errx(1, "unrecognized command\n%s", commandline_usage);
+}
diff --git a/src/drivers/hott/hott_sensors/module.mk b/src/drivers/hott/hott_sensors/module.mk
new file mode 100644
index 000000000..b5f5762ba
--- /dev/null
+++ b/src/drivers/hott/hott_sensors/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Graupner HoTT Sensors application.
+#
+
+MODULE_COMMAND	= hott_sensors
+
+SRCS		= hott_sensors.cpp \
+		  ../messages.cpp \
+		  ../comms.cpp
diff --git a/src/drivers/hott_telemetry/hott_telemetry_main.c b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp
index 60f3f3e96..d293f9954 100644
--- a/src/drivers/hott_telemetry/hott_telemetry_main.c
+++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp
@@ -41,7 +41,6 @@
  * The HoTT receiver polls each device at a regular interval at which point
  * a data packet can be returned if necessary.
  *
- * TODO: Add support for at least the vario and GPS sensor data.
  */
 
 #include <fcntl.h>
@@ -50,124 +49,78 @@
 #include <stdlib.h>
 #include <stdio.h>
 #include <string.h>
-#include <termios.h>
 #include <sys/ioctl.h>
 #include <unistd.h>
+#include <systemlib/err.h>
 #include <systemlib/systemlib.h>
 
-#include "messages.h"
+#include "../comms.h"
+#include "../messages.h"
+
+#define DEFAULT_UART "/dev/ttyS0";		/**< USART1 */
+
+/* Oddly, ERROR is not defined for C++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
 
 static int thread_should_exit = false;		/**< Deamon exit flag */
 static int thread_running = false;		/**< Deamon status flag */
 static int deamon_task;				/**< Handle of deamon task / thread */
-static char *daemon_name = "hott_telemetry";
-static char *commandline_usage = "usage: hott_telemetry start|status|stop [-d <device>]";
-
+static const char daemon_name[] = "hott_telemetry";
+static const char commandline_usage[] = "usage: hott_telemetry start|status|stop [-d <device>]";
 
-/* A little console messaging experiment - console helper macro */
-#define FATAL_MSG(_msg)		fprintf(stderr, "[%s] %s\n", daemon_name, _msg); exit(1);
-#define ERROR_MSG(_msg)		fprintf(stderr, "[%s] %s\n", daemon_name, _msg);
-#define INFO_MSG(_msg)		printf("[%s] %s\n", daemon_name, _msg);
 /**
  * Deamon management function.
  */
-__EXPORT int hott_telemetry_main(int argc, char *argv[]);
+extern "C" __EXPORT int hott_telemetry_main(int argc, char *argv[]);
 
 /**
- * Mainloop of deamon.
+ * Mainloop of daemon.
  */
 int hott_telemetry_thread_main(int argc, char *argv[]);
 
-static int read_data(int uart, int *id);
-static int send_data(int uart, uint8_t *buffer, int size);
+static int recv_req_id(int uart, uint8_t *id);
+static int send_data(int uart, uint8_t *buffer, size_t size);
 
-static int open_uart(const char *device, struct termios *uart_config_original)
+int
+recv_req_id(int uart, uint8_t *id)
 {
-	/* baud rate */
-	int speed = B19200;
-	int uart;
-
-	/* open uart */
-	uart = open(device, O_RDWR | O_NOCTTY);
-
-	if (uart < 0) {
-		char msg[80];
-		sprintf(msg, "Error opening port: %s\n", device);
-		FATAL_MSG(msg);
-	}
-
-	/* Try to set baud rate */
-	struct termios uart_config;
-	int termios_state;
-
-	/* Back up the original uart configuration to restore it after exit */
-	char msg[80];
+	static const int timeout_ms = 1000;  // TODO make it a define
 
-	if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
-		sprintf(msg, "Error getting baudrate / termios config for %s: %d\n", device, termios_state);
-		close(uart);
-		FATAL_MSG(msg);
-	}
-
-	/* 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) {
-		sprintf(msg, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n",
-			device, termios_state);
-		close(uart);
-		FATAL_MSG(msg);
-	}
+	uint8_t mode;
+	
+	struct pollfd fds;
+	fds.fd = uart;
+	fds.events = POLLIN;
 
-	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
-		sprintf(msg, "Error setting baudrate / termios config for %s (tcsetattr)\n", device);
-		close(uart);
-		FATAL_MSG(msg);
-	}
-
-	/* Activate single wire mode */
-	ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
-
-	return uart;
-}
-
-int read_data(int uart, int *id)
-{
-	const int timeout = 1000;
-	struct pollfd fds[] = { { .fd = uart, .events = POLLIN } };
-
-	char mode;
-
-	if (poll(fds, 1, timeout) > 0) {
+	if (poll(&fds, 1, timeout_ms) > 0) {
 		/* Get the mode: binary or text  */
-		read(uart, &mode, 1);
-		/* Read the device ID being polled */
-		read(uart, id, 1);
+		read(uart, &mode, sizeof(mode));
 
 		/* if we have a binary mode request */
 		if (mode != BINARY_MODE_REQUEST_ID) {
 			return ERROR;
 		}
 
+		/* Read the device ID being polled */
+		read(uart, id, sizeof(*id));
 	} else {
-		ERROR_MSG("UART timeout on TX/RX port");
+		warnx("UART timeout on TX/RX port");
 		return ERROR;
 	}
 
 	return OK;
 }
 
-int send_data(int uart, uint8_t *buffer, int size)
+int
+send_data(int uart, uint8_t *buffer, size_t size)
 {
 	usleep(POST_READ_DELAY_IN_USECS);
 
 	uint16_t checksum = 0;
-
-	for (int i = 0; i < size; i++) {
+	for (size_t i = 0; i < size; i++) {
 		if (i == size - 1) {
 			/* Set the checksum: the first uint8_t is taken as the checksum. */
 			buffer[i] = checksum & 0xff;
@@ -176,7 +129,7 @@ int send_data(int uart, uint8_t *buffer, int size)
 			checksum += buffer[i];
 		}
 
-		write(uart, &buffer[i], 1);
+		write(uart, &buffer[i], sizeof(buffer[i]));
 
 		/* Sleep before sending the next byte. */
 		usleep(POST_WRITE_DELAY_IN_USECS);
@@ -190,13 +143,14 @@ int send_data(int uart, uint8_t *buffer, int size)
 	return OK;
 }
 
-int hott_telemetry_thread_main(int argc, char *argv[])
+int
+hott_telemetry_thread_main(int argc, char *argv[])
 {
-	INFO_MSG("starting");
+	warnx("starting");
 
 	thread_running = true;
 
-	char *device = "/dev/ttyS1";		/**< Default telemetry port: USART2 */
+	const char *device = DEFAULT_UART;
 
 	/* read commandline arguments */
 	for (int i = 0; i < argc && argv[i]; i++) {
@@ -206,45 +160,55 @@ int hott_telemetry_thread_main(int argc, char *argv[])
 
 			} else {
 				thread_running = false;
-				ERROR_MSG("missing parameter to -d");
-				ERROR_MSG(commandline_usage);
-				exit(1);
+				errx(1, "missing parameter to -d\n%s", commandline_usage);
 			}
 		}
 	}
 
 	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
-	struct termios uart_config_original;
-	int uart = open_uart(device, &uart_config_original);
-
+	const int uart = open_uart(device);
 	if (uart < 0) {
-		ERROR_MSG("Failed opening HoTT UART, exiting.");
+		errx(1, "Failed opening HoTT UART, exiting.");
 		thread_running = false;
-		exit(ERROR);
 	}
 
-	messages_init();
-
-	uint8_t buffer[MESSAGE_BUFFER_SIZE];
-	int size = 0;
-	int id = 0;
+	init_sub_messages();
 
+	uint8_t buffer[MAX_MESSAGE_BUFFER_SIZE];
+	size_t size = 0;
+	uint8_t id = 0;
+	bool connected = true;
 	while (!thread_should_exit) {
-		if (read_data(uart, &id) == OK) {
+		// Listen for and serve poll from the receiver.
+		if (recv_req_id(uart, &id) == OK) {
+			if (!connected) {
+				connected = true;
+				warnx("OK");
+			}
+
 			switch (id) {
-			case ELECTRIC_AIR_MODULE:
+			case EAM_SENSOR_ID:
 				build_eam_response(buffer, &size);
 				break;
+			case GAM_SENSOR_ID:
+				build_gam_response(buffer, &size);
+				break;
+			case GPS_SENSOR_ID:
+				build_gps_response(buffer, &size);
+				break;
 
 			default:
 				continue;	// Not a module we support.
 			}
 
 			send_data(uart, buffer, size);
+		} else {
+			connected = false;
+			warnx("syncing");
 		}
 	}
 
-	INFO_MSG("exiting");
+	warnx("exiting");
 
 	close(uart);
 
@@ -254,27 +218,26 @@ int hott_telemetry_thread_main(int argc, char *argv[])
 }
 
 /**
- * Process command line arguments and tart the daemon.
+ * Process command line arguments and start the daemon.
  */
-int hott_telemetry_main(int argc, char *argv[])
+int
+hott_telemetry_main(int argc, char *argv[])
 {
 	if (argc < 1) {
-		ERROR_MSG("missing command");
-		ERROR_MSG(commandline_usage);
-		exit(1);
+		errx(1, "missing command\n%s", commandline_usage);
 	}
 
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			INFO_MSG("deamon already running");
+			warnx("deamon already running");
 			exit(0);
 		}
 
 		thread_should_exit = false;
-		deamon_task = task_spawn_cmd("hott_telemetry",
+		deamon_task = task_spawn_cmd(daemon_name,
 					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 40,
+					 SCHED_PRIORITY_DEFAULT,
 					 2048,
 					 hott_telemetry_thread_main,
 					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
@@ -288,19 +251,14 @@ int hott_telemetry_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			INFO_MSG("daemon is running");
+			warnx("daemon is running");
 
 		} else {
-			INFO_MSG("daemon not started");
+			warnx("daemon not started");
 		}
 
 		exit(0);
 	}
 
-	ERROR_MSG("unrecognized command");
-	ERROR_MSG(commandline_usage);
-	exit(1);
+	errx(1, "unrecognized command\n%s", commandline_usage);
 }
-
-
-
diff --git a/src/drivers/hott_telemetry/module.mk b/src/drivers/hott/hott_telemetry/module.mk
index def1d59e9..b19cbd14c 100644
--- a/src/drivers/hott_telemetry/module.mk
+++ b/src/drivers/hott/hott_telemetry/module.mk
@@ -32,10 +32,11 @@
 ############################################################################
 
 #
-# Graupner HoTT Telemetry application.
+# Graupner HoTT Telemetry applications.
 #
 
 MODULE_COMMAND	= hott_telemetry
 
-SRCS		= hott_telemetry_main.c \
-		  messages.c
+SRCS		= hott_telemetry.cpp \
+		  ../messages.cpp \
+		  ../comms.cpp
diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp
new file mode 100644
index 000000000..bb8d45bea
--- /dev/null
+++ b/src/drivers/hott/messages.cpp
@@ -0,0 +1,330 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Simon Wilks <sjwilks@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 messages.c
+ *
+ */
+
+#include "messages.h"
+
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+#include <geo/geo.h>
+#include <unistd.h>
+#include <uORB/topics/airspeed.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/esc_status.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_gps_position.h>
+
+/* The board is very roughly 5 deg warmer than the surrounding air */
+#define BOARD_TEMP_OFFSET_DEG 5
+
+static int _battery_sub = -1;
+static int _gps_sub = -1;
+static int _home_sub = -1;
+static int _sensor_sub = -1;
+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;
+static double _home_lon = 0.0d;
+
+void 
+init_sub_messages(void)
+{
+	_battery_sub = orb_subscribe(ORB_ID(battery_status));
+	_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	_home_sub = orb_subscribe(ORB_ID(home_position));
+	_sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
+	_esc_sub = orb_subscribe(ORB_ID(esc_status));
+}
+
+void 
+init_pub_messages(void)
+{
+	memset(&_esc, 0, sizeof(_esc));
+	_esc_pub = orb_advertise(ORB_ID(esc_status), &_esc);
+}
+
+void
+build_gam_request(uint8_t *buffer, size_t *size)
+{
+	struct gam_module_poll_msg msg;
+	*size = sizeof(msg);
+	memset(&msg, 0, *size);
+
+	msg.mode = BINARY_MODE_REQUEST_ID;
+	msg.id = GAM_SENSOR_ID;
+
+	memcpy(buffer, &msg, *size);
+}
+
+void
+publish_gam_message(const uint8_t *buffer)
+{
+	struct gam_module_msg msg;
+	size_t size = sizeof(msg);
+	memset(&msg, 0, size);
+	memcpy(&msg, buffer, size);
+
+	/* announce the esc if needed, just publish else */
+	if (_esc_pub > 0) {
+		orb_publish(ORB_ID(esc_status), _esc_pub, &_esc);
+	} else {
+		_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 
+build_eam_response(uint8_t *buffer, size_t *size)
+{
+	/* get a local copy of the current sensor values */
+	struct sensor_combined_s raw;
+	memset(&raw, 0, sizeof(raw));
+	orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw);
+
+	/* get a local copy of the battery data */
+	struct battery_status_s battery;
+	memset(&battery, 0, sizeof(battery));
+	orb_copy(ORB_ID(battery_status), _battery_sub, &battery);
+
+	struct eam_module_msg msg;
+	*size = sizeof(msg);
+	memset(&msg, 0, *size);
+
+	msg.start = START_BYTE;
+	msg.eam_sensor_id = EAM_SENSOR_ID;
+	msg.sensor_text_id = EAM_SENSOR_TEXT_ID;
+	
+	msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20);
+	msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG;
+
+	msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10);
+
+	uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500);
+	msg.altitude_L = (uint8_t)alt & 0xff;
+	msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
+
+	/* get a local copy of the airspeed data */
+	struct airspeed_s airspeed;
+	memset(&airspeed, 0, sizeof(airspeed));
+	orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed);
+
+	uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6f);
+	msg.speed_L = (uint8_t)speed & 0xff;
+	msg.speed_H = (uint8_t)(speed >> 8) & 0xff;
+
+	msg.stop = STOP_BYTE;
+	memcpy(buffer, &msg, *size);
+}
+
+void 
+build_gam_response(uint8_t *buffer, size_t *size)
+{
+	/* get a local copy of the ESC Status values */
+	struct esc_status_s esc;
+	memset(&esc, 0, sizeof(esc));
+	orb_copy(ORB_ID(esc_status), _esc_sub, &esc);
+
+	struct gam_module_msg msg;
+	*size = sizeof(msg);
+	memset(&msg, 0, *size);
+
+	msg.start = START_BYTE;
+	msg.gam_sensor_id = GAM_SENSOR_ID;
+	msg.sensor_text_id = GAM_SENSOR_TEXT_ID;
+	
+	msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20);
+	msg.temperature2 = 20;  // 0 deg. C.
+
+	uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage);
+	msg.main_voltage_L = (uint8_t)voltage & 0xff;
+	msg.main_voltage_H = (uint8_t)(voltage >> 8) & 0xff;
+
+	uint16_t current = (uint16_t)(esc.esc[0].esc_current);
+	msg.current_L = (uint8_t)current & 0xff;
+	msg.current_H = (uint8_t)(current >> 8) & 0xff;
+
+	uint16_t rpm = (uint16_t)(esc.esc[0].esc_rpm * 0.1f);
+	msg.rpm_L = (uint8_t)rpm & 0xff;
+	msg.rpm_H = (uint8_t)(rpm >> 8) & 0xff;
+
+	msg.stop = STOP_BYTE;
+	memcpy(buffer, &msg, *size);
+}
+
+void 
+build_gps_response(uint8_t *buffer, size_t *size)
+{
+	/* get a local copy of the current sensor values */
+	struct sensor_combined_s raw;
+	memset(&raw, 0, sizeof(raw));
+	orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw);
+
+ 	/* get a local copy of the battery data */
+	struct vehicle_gps_position_s gps;
+	memset(&gps, 0, sizeof(gps));
+	orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps);
+
+	struct gps_module_msg msg;
+	*size = sizeof(msg);
+	memset(&msg, 0, *size);
+
+	msg.start = START_BYTE;
+	msg.sensor_id = GPS_SENSOR_ID;
+	msg.sensor_text_id = GPS_SENSOR_TEXT_ID;
+
+	msg.gps_num_sat = gps.satellites_visible;
+
+	/* The GPS fix type: 0 = none, 2 = 2D, 3 = 3D */
+	msg.gps_fix_char = (uint8_t)(gps.fix_type + 48);
+	msg.gps_fix = (uint8_t)(gps.fix_type + 48);
+
+	/* No point collecting more data if we don't have a 3D fix yet */
+	if (gps.fix_type > 2) {
+		/* Current flight direction */
+		msg.flight_direction = (uint8_t)(gps.cog_rad * M_RAD_TO_DEG_F);
+
+		/* GPS speed */
+		uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6f);
+		msg.gps_speed_L = (uint8_t)speed & 0xff;
+		msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff;
+		
+		/* Get latitude in degrees, minutes and seconds */
+		double lat = ((double)(gps.lat))*1e-7d;
+
+		/* Set the N or S specifier */		
+		msg.latitude_ns = 0;
+		if (lat < 0) {
+			msg.latitude_ns = 1;
+			lat = abs(lat);
+		}
+
+		int deg;
+		int min;
+		int sec;
+		convert_to_degrees_minutes_seconds(lat, &deg, &min, &sec);
+
+		uint16_t lat_min = (uint16_t)(deg * 100 + min);
+		msg.latitude_min_L = (uint8_t)lat_min & 0xff;
+		msg.latitude_min_H = (uint8_t)(lat_min >> 8) & 0xff;
+		uint16_t lat_sec = (uint16_t)(sec);
+		msg.latitude_sec_L = (uint8_t)lat_sec & 0xff;
+		msg.latitude_sec_H = (uint8_t)(lat_sec >> 8) & 0xff;
+
+		/* Get longitude in degrees, minutes and seconds */
+		double lon = ((double)(gps.lon))*1e-7d;
+
+		/* Set the E or W specifier */
+		msg.longitude_ew = 0;
+		if (lon < 0) {
+			msg.longitude_ew = 1;
+			lon = abs(lon);
+		}
+
+		convert_to_degrees_minutes_seconds(lon, &deg, &min, &sec);
+		
+		uint16_t lon_min = (uint16_t)(deg * 100 + min);
+		msg.longitude_min_L = (uint8_t)lon_min & 0xff;
+		msg.longitude_min_H = (uint8_t)(lon_min >> 8) & 0xff;
+		uint16_t lon_sec = (uint16_t)(sec);
+		msg.longitude_sec_L = (uint8_t)lon_sec & 0xff;
+		msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff;
+		
+		/* Altitude */
+		uint16_t alt = (uint16_t)(gps.alt*1e-3f + 500.0f);
+		msg.altitude_L = (uint8_t)alt & 0xff;
+		msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
+
+		/* Get any (and probably only ever one) _home_sub postion report */
+		bool updated;
+		orb_check(_home_sub, &updated);
+		if (updated) {
+			/* get a local copy of the home position data */
+			struct home_position_s home;
+			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_position_set = true;
+		}
+
+		/* Distance from home */
+		if (_home_position_set) {
+			uint16_t dist = (uint16_t)get_distance_to_next_waypoint(_home_lat, _home_lon, lat, lon);
+
+			msg.distance_L = (uint8_t)dist & 0xff;
+			msg.distance_H = (uint8_t)(dist >> 8) & 0xff;
+
+			/* Direction back to home */
+			uint16_t bearing = (uint16_t)(get_bearing_to_next_waypoint(_home_lat, _home_lon, lat, lon) * M_RAD_TO_DEG_F);
+			msg.home_direction = (uint8_t)bearing >> 1;
+		}
+	}
+
+	msg.stop = STOP_BYTE;
+	memcpy(buffer, &msg, *size);
+}
+
+void
+convert_to_degrees_minutes_seconds(double val, int *deg, int *min, int *sec)
+{
+	*deg = (int)val;
+
+	double delta = val - *deg;
+	const double min_d = delta * 60.0d;
+	*min = (int)min_d;
+	delta = min_d - *min;
+	*sec = (int)(delta * 10000.0d);
+}
diff --git a/src/drivers/hott/messages.h b/src/drivers/hott/messages.h
new file mode 100644
index 000000000..224f8fc56
--- /dev/null
+++ b/src/drivers/hott/messages.h
@@ -0,0 +1,249 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@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 messages.h
+ * @author Simon Wilks <sjwilks@gmail.com>
+ *
+ * Graupner HoTT Telemetry message generation.
+ *
+ */
+#ifndef MESSAGES_H_
+#define MESSAGES_H
+
+#include <stdlib.h>
+
+/* The HoTT receiver demands a minimum 5ms period of silence after delivering its request.
+ * Note that the value specified here is lower than 5000 (5ms) as time is lost constucting
+ * the message after the read which takes some milliseconds.
+ */
+#define POST_READ_DELAY_IN_USECS	4000
+/* A pause of 3ms is required between each uint8_t sent back to the HoTT receiver. Much lower
+ * values can be used in practise though.
+ */
+#define POST_WRITE_DELAY_IN_USECS	2000
+
+// Protocol constants.
+#define BINARY_MODE_REQUEST_ID	0x80	// Binary mode request.
+#define START_BYTE		0x7c
+#define STOP_BYTE		0x7d
+#define TEMP_ZERO_CELSIUS	0x14
+
+/* The GAM Module poll message. */
+struct gam_module_poll_msg {
+	uint8_t mode;
+	uint8_t id;
+};
+
+/* Electric Air Module (EAM) constants. */
+#define EAM_SENSOR_ID			0x8e
+#define EAM_SENSOR_TEXT_ID		0xe0
+
+/* The Electric Air Module message. */
+struct eam_module_msg {
+	uint8_t start;			/**< Start byte */
+	uint8_t eam_sensor_id;		/**< EAM sensor	*/
+	uint8_t warning;
+	uint8_t sensor_text_id;
+	uint8_t alarm_inverse1;
+	uint8_t alarm_inverse2;
+	uint8_t cell1_L;		/**< Lipo cell voltages. Not supported.	*/
+	uint8_t cell2_L;
+	uint8_t cell3_L;
+	uint8_t cell4_L;
+	uint8_t cell5_L;
+	uint8_t cell6_L;
+	uint8_t cell7_L;
+	uint8_t cell1_H;
+	uint8_t cell2_H;
+	uint8_t cell3_H;
+	uint8_t cell4_H;
+	uint8_t cell5_H;
+	uint8_t cell6_H;
+	uint8_t cell7_H;
+	uint8_t batt1_voltage_L;	/**< Battery 1 voltage, lower 8-bits in steps of 0.02V 	*/
+	uint8_t batt1_voltage_H;
+	uint8_t batt2_voltage_L;	/**< Battery 2 voltage, lower 8-bits in steps of 0.02V 	*/
+	uint8_t batt2_voltage_H;
+	uint8_t temperature1;		/**< Temperature sensor 1. 20 = 0 degrees */
+	uint8_t temperature2;
+	uint8_t altitude_L;		/**< Attitude (meters) lower 8-bits. 500 = 0 meters */
+	uint8_t altitude_H;
+	uint8_t current_L;		/**< Current (mAh) lower 8-bits in steps of 0.1V */
+	uint8_t current_H;
+	uint8_t main_voltage_L;		/**< Main power voltage lower 8-bits in steps of 0.1V */
+	uint8_t main_voltage_H;
+	uint8_t battery_capacity_L;	/**< Used battery capacity in steps of 10mAh */
+	uint8_t battery_capacity_H;
+	uint8_t climbrate_L;		/**< Climb rate in 0.01m/s. 0m/s = 30000 */
+	uint8_t climbrate_H;
+	uint8_t climbrate_3s;		/**< Climb rate in m/3sec. 0m/3sec = 120 */
+	uint8_t rpm_L;			/**< RPM Lower 8-bits In steps of 10 U/min */
+	uint8_t rpm_H;
+	uint8_t electric_min;		/**< Flight time in minutes. */
+	uint8_t electric_sec;		/**< Flight time in seconds. */
+	uint8_t speed_L;		/**< Airspeed in km/h in steps of 1 km/h */
+	uint8_t speed_H;
+	uint8_t stop;			/**< Stop byte */
+	uint8_t checksum;		/**< Lower 8-bits of all bytes summed. */
+};
+
+
+/* General Air Module (GAM) constants. */
+#define GAM_SENSOR_ID			0x8d
+#define GAM_SENSOR_TEXT_ID		0xd0
+
+struct gam_module_msg {
+        uint8_t start;			/**< Start byte	*/
+        uint8_t gam_sensor_id;		/**< GAM sensor id */
+        uint8_t warning_beeps;
+        uint8_t sensor_text_id;
+        uint8_t alarm_invers1;
+        uint8_t alarm_invers2;                                                              
+        uint8_t cell1;			/**< Lipo cell voltages. Not supported.	*/
+        uint8_t cell2;
+        uint8_t cell3;
+        uint8_t cell4;
+        uint8_t cell5;
+        uint8_t cell6;
+        uint8_t batt1_L;		/**< Battery 1 voltage LSB value. 0.1V steps. 50 = 5.5V	*/
+        uint8_t batt1_H;
+        uint8_t batt2_L;		/**< Battery 2 voltage LSB value. 0.1V steps. 50 = 5.5V	*/
+        uint8_t batt2_H;
+        uint8_t temperature1;		/**< Temperature 1. offset of 20. a value of 20 = 0°C */
+        uint8_t temperature2;		/**< Temperature 2. offset of 20. a value of 20 = 0°C */
+        uint8_t fuel_procent;		/**< Fuel capacity in %. Values 0 - 100 */
+					/**< Graphical display ranges: 0 25% 50% 75% 100% */
+        uint8_t fuel_ml_L;		/**< Fuel in ml scale. Full = 65535 */
+        uint8_t fuel_ml_H;
+        uint8_t rpm_L;			/**< RPM in 10 RPM steps. 300 = 3000rpm */
+        uint8_t rpm_H;
+        uint8_t altitude_L;		/**< Altitude in meters. offset of 500, 500 = 0m */
+        uint8_t altitude_H;
+        uint8_t climbrate_L;		/**< Climb rate in 0.01m/s. Value of 30000 = 0.00 m/s */
+        uint8_t climbrate_H;
+        uint8_t climbrate3s;		/**< Climb rate in m/3sec. Value of 120 = 0m/3sec */
+        uint8_t current_L;		/**< Current in 0.1A steps */
+        uint8_t current_H;
+        uint8_t main_voltage_L;		/**< Main power voltage using 0.1V steps */
+        uint8_t main_voltage_H;
+        uint8_t batt_cap_L;		/**< Used battery capacity in 10mAh steps */
+        uint8_t batt_cap_H;
+        uint8_t speed_L;		/**< Speed in km/h */
+        uint8_t speed_H;
+        uint8_t min_cell_volt;		/**< Minimum cell voltage in 2mV steps. 124 = 2,48V */
+        uint8_t min_cell_volt_num;	/**< Number of the cell with the lowest voltage */
+        uint8_t rpm2_L;			/**< RPM in 10 RPM steps. 300 = 3000rpm */
+        uint8_t rpm2_H;
+        uint8_t general_error_number;	/**< Voice error == 12. TODO: more docu */
+        uint8_t pressure;		/**< Pressure up to 16bar. 0,1bar scale. 20 = 2bar */
+        uint8_t version;
+        uint8_t stop;			/**< Stop byte */
+        uint8_t checksum;		/**< Lower 8-bits of all bytes summed */
+};
+
+/* GPS sensor constants. */
+#define GPS_SENSOR_ID		0x8a
+#define GPS_SENSOR_TEXT_ID	0xa0
+
+/** 
+ * The GPS sensor message
+ * Struct based on: https://code.google.com/p/diy-hott-gps/downloads 
+ */
+struct gps_module_msg { 
+	uint8_t start;			/**< Start byte */
+	uint8_t sensor_id;		/**< GPS sensor ID*/
+	uint8_t warning;		/**< 0…= warning beeps */
+	uint8_t sensor_text_id;		/**< GPS Sensor text mode ID */
+	uint8_t alarm_inverse1;		/**< 01 inverse status */
+	uint8_t alarm_inverse2;		/**< 00 inverse status status 1 = no GPS Signal */
+	uint8_t flight_direction;	/**< 119 = Flightdir./dir. 1 = 2°; 0° (North), 9 0° (East), 180° (South), 270° (West) */
+	uint8_t gps_speed_L;		/**< 8 = /GPS speed low byte 8km/h */
+	uint8_t gps_speed_H;		/**< 0 = /GPS speed high byte */
+	  
+	uint8_t latitude_ns;		/**< 000 = N = 48°39’988 */
+	uint8_t latitude_min_L;		/**< 231 0xE7 = 0x12E7 = 4839 */
+	uint8_t latitude_min_H;		/**< 018 18 = 0x12 */
+	uint8_t latitude_sec_L;		/**< 171 220 = 0xDC = 0x03DC =0988 */
+	uint8_t latitude_sec_H;		/**< 016 3 = 0x03 */
+	 
+	uint8_t longitude_ew;		/**< 000  = E= 9° 25’9360 */
+	uint8_t longitude_min_L;	/**< 150 157 = 0x9D = 0x039D = 0925 */
+	uint8_t longitude_min_H;	/**< 003 3 = 0x03 */
+	uint8_t longitude_sec_L;	/**< 056 144 = 0x90 0x2490 = 9360 */
+	uint8_t longitude_sec_H;	/**< 004 36 = 0x24 */
+	  
+	uint8_t distance_L;		/**< 027 123 = /distance low byte 6 = 6 m */
+	uint8_t distance_H;		/**< 036 35 = /distance high byte */
+	uint8_t altitude_L;		/**< 243 244 = /Altitude low byte 500 = 0m */
+	uint8_t altitude_H;		/**< 001 1 = /Altitude high byte */
+	uint8_t resolution_L;		/**< 48 = Low Byte m/s resolution 0.01m 48 = 30000 = 0.00m/s (1=0.01m/s) */
+	uint8_t resolution_H;		/**< 117 = High Byte m/s resolution 0.01m */
+	uint8_t unknown1;		/**< 120 = 0m/3s */
+	uint8_t gps_num_sat;		/**< GPS.Satellites (number of satelites) (1 byte) */
+	uint8_t gps_fix_char;		/**< GPS.FixChar. (GPS fix character. display, if DGPS, 2D oder 3D) (1 byte) */
+	uint8_t home_direction;		/**< HomeDirection (direction from starting point to Model position) (1 byte) */
+	uint8_t angle_x_direction;	/**< angle x-direction (1 byte) */
+	uint8_t angle_y_direction;	/**< angle y-direction (1 byte) */
+	uint8_t angle_z_direction;	/**< angle z-direction (1 byte) */
+	uint8_t gyro_x_L;		/**< gyro x low byte (2 bytes) */
+	uint8_t gyro_x_H;		/**< gyro x high byte */
+	uint8_t gyro_y_L;		/**< gyro y low byte (2 bytes) */
+	uint8_t gyro_y_H;		/**< gyro y high byte */
+	uint8_t gyro_z_L;		/**< gyro z low byte (2 bytes) */
+	uint8_t gyro_z_H;		/**< gyro z high byte */
+	uint8_t vibration;		/**< vibration (1 bytes) */
+	uint8_t ascii4;			/**< 00 ASCII Free Character [4] */
+	uint8_t ascii5;			/**< 00 ASCII Free Character [5] */
+	uint8_t gps_fix;		/**< 00 ASCII Free Character [6], we use it for GPS FIX */
+	uint8_t version;
+	uint8_t stop;			/**< Stop byte */
+	uint8_t checksum;		/**< Lower 8-bits of all bytes summed */
+};
+
+// The maximum size of a message.
+#define MAX_MESSAGE_BUFFER_SIZE 45
+
+void init_sub_messages(void);
+void init_pub_messages(void);
+void build_gam_request(uint8_t *buffer, size_t *size);
+void publish_gam_message(const uint8_t *buffer);
+void build_eam_response(uint8_t *buffer, size_t *size);
+void build_gam_response(uint8_t *buffer, size_t *size);
+void build_gps_response(uint8_t *buffer, size_t *size);
+float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
+void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec);
+
+
+#endif /* MESSAGES_H_ */
diff --git a/src/drivers/hott_telemetry/messages.c b/src/drivers/hott_telemetry/messages.c
deleted file mode 100644
index f0f32d244..000000000
--- a/src/drivers/hott_telemetry/messages.c
+++ /dev/null
@@ -1,99 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
- *   Author: Simon Wilks <sjwilks@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 messages.c
- * @author Simon Wilks <sjwilks@gmail.com>
- */
-
-#include "messages.h"
-
-#include <string.h>
-#include <systemlib/systemlib.h>
-#include <unistd.h>
-#include <uORB/topics/airspeed.h>
-#include <uORB/topics/battery_status.h>
-#include <uORB/topics/sensor_combined.h>
-
-static int airspeed_sub = -1;
-static int battery_sub = -1;
-static int sensor_sub = -1;
-
-void messages_init(void)
-{
-	battery_sub = orb_subscribe(ORB_ID(battery_status));
-	sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	airspeed_sub = orb_subscribe(ORB_ID(airspeed));
-}
-
-void build_eam_response(uint8_t *buffer, int *size)
-{
-	/* get a local copy of the current sensor values */
-	struct sensor_combined_s raw;
-	memset(&raw, 0, sizeof(raw));
-	orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
-
-	/* get a local copy of the battery data */
-	struct battery_status_s battery;
-	memset(&battery, 0, sizeof(battery));
-	orb_copy(ORB_ID(battery_status), battery_sub, &battery);
-
-	struct eam_module_msg msg;
-	*size = sizeof(msg);
-	memset(&msg, 0, *size);
-
-	msg.start = START_BYTE;
-	msg.eam_sensor_id = ELECTRIC_AIR_MODULE;
-	msg.sensor_id = EAM_SENSOR_ID;
-	msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20);
-	msg.temperature2 = TEMP_ZERO_CELSIUS;
-	msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10);
-
-	uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500);
-	msg.altitude_L = (uint8_t)alt & 0xff;
-	msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
-
-	/* get a local copy of the current sensor values */
-	struct airspeed_s airspeed;
-	memset(&airspeed, 0, sizeof(airspeed));
-	orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
-
-	uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6);
-	msg.speed_L = (uint8_t)speed & 0xff;
-	msg.speed_H = (uint8_t)(speed >> 8) & 0xff;
-
-	msg.stop = STOP_BYTE;
-
-	memcpy(buffer, &msg, *size);
-}
\ No newline at end of file
diff --git a/src/drivers/hott_telemetry/messages.h b/src/drivers/hott_telemetry/messages.h
deleted file mode 100644
index dd38075fa..000000000
--- a/src/drivers/hott_telemetry/messages.h
+++ /dev/null
@@ -1,124 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
- *   Author: Simon Wilks <sjwilks@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 messages.h
- * @author Simon Wilks <sjwilks@gmail.com>
- *
- * Graupner HoTT Telemetry message generation.
- *
- */
-#ifndef MESSAGES_H_
-#define MESSAGES_H
-
-#include <stdlib.h>
-
-/* The buffer size used to store messages. This must be at least as big as the number of
- * fields in the largest message struct.
- */
-#define MESSAGE_BUFFER_SIZE 50
-
-/* The HoTT receiver demands a minimum 5ms period of silence after delivering its request.
- * Note that the value specified here is lower than 5000 (5ms) as time is lost constucting
- * the message after the read which takes some milliseconds.
- */
-#define POST_READ_DELAY_IN_USECS	4000
-/* A pause of 3ms is required between each uint8_t sent back to the HoTT receiver. Much lower
- * values can be used in practise though.
- */
-#define POST_WRITE_DELAY_IN_USECS	2000
-
-// Protocol constants.
-#define BINARY_MODE_REQUEST_ID	0x80	// Binary mode request.
-#define START_BYTE		0x7c
-#define STOP_BYTE		0x7d
-#define TEMP_ZERO_CELSIUS	0x14
-
-/* Electric Air Module (EAM) constants. */
-#define ELECTRIC_AIR_MODULE	0x8e
-#define EAM_SENSOR_ID		0xe0
-
-/* The Electric Air Module message. */
-struct eam_module_msg {
-	uint8_t start;			/**< Start byte   				*/
-	uint8_t eam_sensor_id;		/**< EAM sensor ID 				*/
-	uint8_t warning;
-	uint8_t sensor_id;		/**< Sensor ID, why different? 			*/
-	uint8_t alarm_inverse1;
-	uint8_t alarm_inverse2;
-	uint8_t cell1_L;		/**< Lipo cell voltages. Not supported. 	*/
-	uint8_t cell2_L;
-	uint8_t cell3_L;
-	uint8_t cell4_L;
-	uint8_t cell5_L;
-	uint8_t cell6_L;
-	uint8_t cell7_L;
-	uint8_t cell1_H;
-	uint8_t cell2_H;
-	uint8_t cell3_H;
-	uint8_t cell4_H;
-	uint8_t cell5_H;
-	uint8_t cell6_H;
-	uint8_t cell7_H;
-	uint8_t batt1_voltage_L;	/**< Battery 1 voltage, lower 8-bits in steps of 0.02V 	*/
-	uint8_t batt1_voltage_H;
-	uint8_t batt2_voltage_L;	/**< Battery 2 voltage, lower 8-bits in steps of 0.02V 	*/
-	uint8_t batt2_voltage_H;
-	uint8_t temperature1;		/**< Temperature sensor 1. 20 = 0 degrees 		*/
-	uint8_t temperature2;
-	uint8_t altitude_L;		/**< Attitude (meters) lower 8-bits. 500 = 0 meters 	*/
-	uint8_t altitude_H;
-	uint8_t current_L;		/**< Current (mAh) lower 8-bits in steps of 0.1V 	*/
-	uint8_t current_H;
-	uint8_t main_voltage_L;		/**< Main power voltage lower 8-bits in steps of 0.1V 	*/
-	uint8_t main_voltage_H;
-	uint8_t battery_capacity_L;	/**< Used battery capacity in steps of 10mAh 		*/
-	uint8_t battery_capacity_H;
-	uint8_t climbrate_L;		/**< Climb rate in 0.01m/s. 0m/s = 30000 		*/
-	uint8_t climbrate_H;
-	uint8_t climbrate_3s;		/**< Climb rate in m/3sec. 0m/3sec = 120 		*/
-	uint8_t rpm_L;			/**< RPM Lower 8-bits In steps of 10 U/min 		*/
-	uint8_t rpm_H;
-	uint8_t electric_min;		/**< Flight time in minutes. 				*/
-	uint8_t electric_sec;		/**< Flight time in seconds. 				*/
-	uint8_t speed_L;		/**< Airspeed in km/h in steps of 1 km/h 		*/
-	uint8_t speed_H;
-	uint8_t stop;			/**< Stop byte 						*/
-	uint8_t checksum;		/**< Lower 8-bits of all bytes summed. 			*/
-};
-
-void messages_init(void);
-void build_eam_response(uint8_t *buffer, int *size);
-
-#endif /* MESSAGES_H_ */
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 98098c83b..90c3db9ae 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -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
@@ -59,11 +59,14 @@
 #include <nuttx/clock.h>
 
 #include <drivers/drv_hrt.h>
-#include <arch/board/board.h>
-
 #include <drivers/device/spi.h>
 #include <drivers/drv_gyro.h>
+#include <drivers/device/ringbuffer.h>
+
+#include <board_config.h>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
 
+#define L3GD20_DEVICE_PATH "/dev/l3gd20"
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -71,6 +74,12 @@
 #endif
 static const int ERROR = -1;
 
+/* Orientation on board */
+#define SENSOR_BOARD_ROTATION_000_DEG	0
+#define SENSOR_BOARD_ROTATION_090_DEG	1
+#define SENSOR_BOARD_ROTATION_180_DEG	2
+#define SENSOR_BOARD_ROTATION_270_DEG	3
+
 /* SPI protocol address bits */
 #define DIR_READ				(1<<7)
 #define DIR_WRITE				(0<<7)
@@ -78,15 +87,24 @@ static const int ERROR = -1;
 
 /* register addresses */
 #define ADDR_WHO_AM_I			0x0F
+#define WHO_I_AM_H 				0xD7
 #define WHO_I_AM				0xD4
 
 #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) | (1<<5) | (1<<4))
-#define RATE_380HZ_LP_30HZ		((1<<7) | (0<<6) | (1<<5) | (1<<4))
-#define RATE_760HZ_LP_30HZ		((1<<7) | (1<<6) | (1<<5) | (1<<4))
+#define RATE_190HZ_LP_25HZ		((0<<7) | (1<<6) | (0<<5) | (1<<4))
+#define RATE_190HZ_LP_50HZ		((0<<7) | (1<<6) | (1<<5) | (0<<4))
+#define RATE_190HZ_LP_70HZ		((0<<7) | (1<<6) | (1<<5) | (1<<4))
+#define RATE_380HZ_LP_20HZ		((1<<7) | (0<<6) | (1<<5) | (0<<4))
+#define RATE_380HZ_LP_25HZ		((1<<7) | (0<<6) | (0<<5) | (1<<4))
+#define RATE_380HZ_LP_50HZ		((1<<7) | (0<<6) | (1<<5) | (0<<4))
+#define RATE_380HZ_LP_100HZ		((1<<7) | (0<<6) | (1<<5) | (1<<4))
+#define RATE_760HZ_LP_30HZ		((1<<7) | (1<<6) | (0<<5) | (0<<4))
+#define RATE_760HZ_LP_35HZ		((1<<7) | (1<<6) | (0<<5) | (1<<4))
+#define RATE_760HZ_LP_50HZ		((1<<7) | (1<<6) | (1<<5) | (0<<4))
+#define RATE_760HZ_LP_100HZ		((1<<7) | (1<<6) | (1<<5) | (1<<4))
 
 #define ADDR_CTRL_REG2			0x21
 #define ADDR_CTRL_REG3			0x22
@@ -147,6 +165,10 @@ static const int ERROR = -1;
 #define FIFO_CTRL_STREAM_TO_FIFO_MODE		(3<<5)
 #define FIFO_CTRL_BYPASS_TO_STREAM_MODE		(1<<7)
 
+#define L3GD20_DEFAULT_RATE			760
+#define L3GD20_DEFAULT_RANGE_DPS		2000
+#define L3GD20_DEFAULT_FILTER_FREQ		30
+
 extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }
 
 class L3GD20 : public device::SPI
@@ -172,21 +194,27 @@ private:
 
 	struct hrt_call		_call;
 	unsigned		_call_interval;
-
-	unsigned		_num_reports;
-	volatile unsigned	_next_report;
-	volatile unsigned	_oldest_report;
-	struct gyro_report	*_reports;
+	
+	RingBuffer		*_reports;
 
 	struct gyro_scale	_gyro_scale;
 	float			_gyro_range_scale;
 	float			_gyro_range_rad_s;
 	orb_advert_t		_gyro_topic;
+	int			_class_instance;
 
 	unsigned		_current_rate;
-	unsigned		_current_range;
+	unsigned		_orientation;
+
+	unsigned		_read;
 
 	perf_counter_t		_sample_perf;
+	perf_counter_t		_reschedules;
+	perf_counter_t		_errors;
+
+	math::LowPassFilter2p	_gyro_filter_x;
+	math::LowPassFilter2p	_gyro_filter_y;
+	math::LowPassFilter2p	_gyro_filter_z;
 
 	/**
 	 * Start automatic measurement.
@@ -199,6 +227,16 @@ private:
 	void			stop();
 
 	/**
+	 * Reset the driver
+	 */
+	void			reset();
+
+	/**
+	 * disable I2C on the chip
+	 */
+	void			disable_i2c();
+
+	/**
 	 * Static trampoline from the hrt_call context; because we don't have a
 	 * generic hrt wrapper yet.
 	 *
@@ -260,25 +298,40 @@ private:
 	 * @return		OK if the value can be supported.
 	 */
 	int			set_samplerate(unsigned frequency);
-};
 
-/* helper macro for handling report buffer indices */
-#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
+	/**
+	 * Set the lowpass filter of the driver
+	 *
+	 * @param samplerate	The current samplerate
+	 * @param frequency	The cutoff frequency for the lowpass filter
+	 */
+	void			set_driver_lowpass_filter(float samplerate, float bandwidth);
 
+	/**
+	 * Self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	 int 			self_test();
+};
 
 L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
-	SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 8000000),
+	SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */),
 	_call_interval(0),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
 	_reports(nullptr),
 	_gyro_range_scale(0.0f),
 	_gyro_range_rad_s(0.0f),
 	_gyro_topic(-1),
+	_class_instance(-1),
 	_current_rate(0),
-	_current_range(0),
-	_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read"))
+	_orientation(SENSOR_BOARD_ROTATION_270_DEG),
+	_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)
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -299,10 +352,15 @@ L3GD20::~L3GD20()
 
 	/* free any existing reports */
 	if (_reports != nullptr)
-		delete[] _reports;
+		delete _reports;
+
+	if (_class_instance != -1)
+		unregister_class_devname(GYRO_DEVICE_PATH, _class_instance);
 
 	/* delete the perf counter */
 	perf_free(_sample_perf);
+	perf_free(_reschedules);
+	perf_free(_errors);
 }
 
 int
@@ -315,29 +373,29 @@ L3GD20::init()
 		goto out;
 
 	/* allocate basic report buffers */
-	_num_reports = 2;
-	_oldest_report = _next_report = 0;
-	_reports = new struct gyro_report[_num_reports];
+	_reports = new RingBuffer(2, sizeof(gyro_report));
 
 	if (_reports == nullptr)
 		goto out;
 
-	/* advertise sensor topic */
-	memset(&_reports[0], 0, sizeof(_reports[0]));
-	_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_reports[0]);
+	_class_instance = register_class_devname(GYRO_DEVICE_PATH);
 
-	/* set default configuration */
-	write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
-	write_reg(ADDR_CTRL_REG2, 0);		/* disable high-pass filters */
-	write_reg(ADDR_CTRL_REG3, 0);		/* no interrupts - we don't use them */
-	write_reg(ADDR_CTRL_REG4, REG4_BDU);
-	write_reg(ADDR_CTRL_REG5, 0);
+	reset();
+
+	measure();
 
-	write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE);	  /* disable wake-on-interrupt */
-	write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */
+	if (_class_instance == CLASS_DEVICE_PRIMARY) {
 
-	set_range(500);				/* default to 500dps */
-	set_samplerate(0);			/* max sample rate */
+		/* advertise sensor topic, measure manually to initialize valid report */
+		struct gyro_report grp;
+		_reports->get(&grp);
+
+		_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp);
+
+		if (_gyro_topic < 0)
+			debug("failed to create sensor_gyro publication");
+
+	}
 
 	ret = OK;
 out:
@@ -350,8 +408,29 @@ L3GD20::probe()
 	/* read dummy value to void to clear SPI statemachine on sensor */
 	(void)read_reg(ADDR_WHO_AM_I);
 
-	/* verify that the device is attached and functioning */
-	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM)
+	bool success = false;
+
+	/* 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
+
+		success = true;
+	}
+
+
+	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) {
+		_orientation = SENSOR_BOARD_ROTATION_180_DEG;
+		success = true;
+	}
+
+	if (success)
 		return OK;
 
 	return -EIO;
@@ -361,6 +440,7 @@ ssize_t
 L3GD20::read(struct file *filp, char *buffer, size_t buflen)
 {
 	unsigned count = buflen / sizeof(struct gyro_report);
+	struct gyro_report *gbuf = reinterpret_cast<struct gyro_report *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
@@ -376,10 +456,9 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen)
 		 * we are careful to avoid racing with it.
 		 */
 		while (count--) {
-			if (_oldest_report != _next_report) {
-				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
-				ret += sizeof(_reports[0]);
-				INCREMENT(_oldest_report, _num_reports);
+			if (_reports->get(gbuf)) {
+				ret += sizeof(*gbuf);
+				gbuf++;
 			}
 		}
 
@@ -388,12 +467,13 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen)
 	}
 
 	/* manual measurement */
-	_oldest_report = _next_report = 0;
+	_reports->flush();
 	measure();
 
 	/* measurement will have generated a report, copy it out */
-	memcpy(buffer, _reports, sizeof(*_reports));
-	ret = sizeof(*_reports);
+	if (_reports->get(gbuf)) {
+		ret = sizeof(*gbuf);
+	}
 
 	return ret;
 }
@@ -422,8 +502,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 				/* set default/max polling rate */
 			case SENSOR_POLLRATE_MAX:
 			case SENSOR_POLLRATE_DEFAULT:
-				/* With internal low pass filters enabled, 250 Hz is sufficient */
-				return ioctl(filp, SENSORIOCSPOLLRATE, 250);
+				return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_DEFAULT_RATE);
 
 				/* adjust to a legal polling interval in Hz */
 			default: {
@@ -441,6 +520,11 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 					/* XXX this is a bit shady, but no other way to adjust... */
 					_call.period = _call_interval = ticks;
 
+					/* adjust filters */
+					float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq();
+					float sample_rate = 1.0e6f/ticks;
+					set_driver_lowpass_filter(sample_rate, cutoff_freq_hz);
+
 					/* if we need to start the poll state machine, do it */
 					if (want_start)
 						start();
@@ -457,35 +541,26 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return 1000000 / _call_interval;
 
 	case SENSORIOCSQUEUEDEPTH: {
-			/* account for sentinel in the ring */
-			arg++;
-
-			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 2) || (arg > 100))
-				return -EINVAL;
-
-			/* allocate new buffer */
-			struct gyro_report *buf = new struct gyro_report[arg];
-
-			if (nullptr == buf)
-				return -ENOMEM;
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete[] _reports;
-			_num_reports = arg;
-			_reports = buf;
-			start();
-
-			return OK;
+		/* 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 _num_reports - 1;
+		return _reports->size();
 
 	case SENSORIOCRESET:
-		/* XXX implement */
-		return -EINVAL;
+		reset();
+		return OK;
 
 	case GYROIOCSSAMPLERATE:
 		return set_samplerate(arg);
@@ -493,10 +568,16 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case GYROIOCGSAMPLERATE:
 		return _current_rate;
 
-	case GYROIOCSLOWPASS:
+	case GYROIOCSLOWPASS: {
+		float cutoff_freq_hz = arg;
+		float sample_rate = 1.0e6f / _call_interval;
+		set_driver_lowpass_filter(sample_rate, cutoff_freq_hz);
+
+		return OK;
+	}
+
 	case GYROIOCGLOWPASS:
-		/* XXX not implemented due to wacky interaction with samplerate */
-		return -EINVAL;
+		return _gyro_filter_x.get_cutoff_freq();
 
 	case GYROIOCSSCALE:
 		/* copy scale in */
@@ -509,10 +590,15 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return OK;
 
 	case GYROIOCSRANGE:
+		/* arg should be in dps */
 		return set_range(arg);
 
 	case GYROIOCGRANGE:
-		return _current_range;
+		/* convert to dps and round */
+		return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f);
+
+	case GYROIOCSELFTEST:
+		return self_test();
 
 	default:
 		/* give it to the superclass */
@@ -526,6 +612,7 @@ L3GD20::read_reg(unsigned reg)
 	uint8_t cmd[2];
 
 	cmd[0] = reg | DIR_READ;
+	cmd[1] = 0;
 
 	transfer(cmd, cmd, sizeof(cmd));
 
@@ -558,28 +645,33 @@ int
 L3GD20::set_range(unsigned max_dps)
 {
 	uint8_t bits = REG4_BDU;
+	float new_range_scale_dps_digit;
+	float new_range;
 
-	if (max_dps == 0)
+	if (max_dps == 0) {
 		max_dps = 2000;
-
+	}
 	if (max_dps <= 250) {
-		_current_range = 250;
+		new_range = 250;
 		bits |= RANGE_250DPS;
+		new_range_scale_dps_digit = 8.75e-3f;
 
 	} else if (max_dps <= 500) {
-		_current_range = 500;
+		new_range = 500;
 		bits |= RANGE_500DPS;
+		new_range_scale_dps_digit = 17.5e-3f;
 
 	} else if (max_dps <= 2000) {
-		_current_range = 2000;
+		new_range = 2000;
 		bits |= RANGE_2000DPS;
+		new_range_scale_dps_digit = 70e-3f;
 
 	} else {
 		return -EINVAL;
 	}
 
-	_gyro_range_rad_s = _current_range / 180.0f * M_PI_F;
-	_gyro_range_scale = _gyro_range_rad_s / 32768.0f;
+	_gyro_range_rad_s = new_range / 180.0f * M_PI_F;
+	_gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F;
 	write_reg(ADDR_CTRL_REG4, bits);
 
 	return OK;
@@ -593,22 +685,22 @@ L3GD20::set_samplerate(unsigned frequency)
 	if (frequency == 0)
 		frequency = 760;
 
-	if (frequency <= 95) {
+	/* use limits good for H or non-H models */
+	if (frequency <= 100) {
 		_current_rate = 95;
 		bits |= RATE_95HZ_LP_25HZ;
 
-	} else if (frequency <= 190) {
+	} else if (frequency <= 200) {
 		_current_rate = 190;
-		bits |= RATE_190HZ_LP_25HZ;
+		bits |= RATE_190HZ_LP_50HZ;
 
-	} else if (frequency <= 380) {
+	} else if (frequency <= 400) {
 		_current_rate = 380;
-		bits |= RATE_380HZ_LP_30HZ;
+		bits |= RATE_380HZ_LP_50HZ;
 
-	} else if (frequency <= 760) {
+	} else if (frequency <= 800) {
 		_current_rate = 760;
-		bits |= RATE_760HZ_LP_30HZ;
-
+		bits |= RATE_760HZ_LP_50HZ;
 	} else {
 		return -EINVAL;
 	}
@@ -619,13 +711,21 @@ L3GD20::set_samplerate(unsigned frequency)
 }
 
 void
+L3GD20::set_driver_lowpass_filter(float samplerate, float bandwidth)
+{
+	_gyro_filter_x.set_cutoff_frequency(samplerate, bandwidth);
+	_gyro_filter_y.set_cutoff_frequency(samplerate, bandwidth);
+	_gyro_filter_z.set_cutoff_frequency(samplerate, bandwidth);
+}
+
+void
 L3GD20::start()
 {
 	/* make sure we are stopped first */
 	stop();
 
 	/* reset the report ring */
-	_oldest_report = _next_report = 0;
+	_reports->flush();
 
 	/* start polling at the specified rate */
 	hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&L3GD20::measure_trampoline, this);
@@ -638,6 +738,48 @@ L3GD20::stop()
 }
 
 void
+L3GD20::disable_i2c(void)
+{
+	uint8_t retries = 10;
+	while (retries--) {
+		// add retries
+		uint8_t a = read_reg(0x05);
+		write_reg(0x05, (0x20 | a));
+		if (read_reg(0x05) == (a | 0x20)) {
+			return;
+		}
+	}
+	debug("FAILED TO DISABLE I2C");
+}
+
+void
+L3GD20::reset()
+{
+	// ensure the chip doesn't interpret any other bus traffic as I2C
+	disable_i2c();
+
+	/* set default configuration */
+	write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
+	write_reg(ADDR_CTRL_REG2, 0);		/* disable high-pass filters */
+	write_reg(ADDR_CTRL_REG3, 0x08);        /* DRDY enable */
+	write_reg(ADDR_CTRL_REG4, REG4_BDU);
+	write_reg(ADDR_CTRL_REG5, 0);
+
+	write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE);		/* disable wake-on-interrupt */
+
+	/* disable FIFO. This makes things simpler and ensures we
+	 * aren't getting stale data. It means we must run the hrt
+	 * callback fast enough to not miss data. */
+	write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
+
+	set_samplerate(0); // 760Hz
+	set_range(L3GD20_DEFAULT_RANGE_DPS);
+	set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ);
+
+	_read = 0;
+}
+
+void
 L3GD20::measure_trampoline(void *arg)
 {
 	L3GD20 *dev = (L3GD20 *)arg;
@@ -646,9 +788,26 @@ L3GD20::measure_trampoline(void *arg)
 	dev->measure();
 }
 
+#ifdef GPIO_EXTI_GYRO_DRDY
+# define L3GD20_USE_DRDY 1
+#else
+# define L3GD20_USE_DRDY 0
+#endif
+
 void
 L3GD20::measure()
 {
+#if L3GD20_USE_DRDY
+	// 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) {
+		perf_count(_reschedules);
+		hrt_call_delay(&_call, 100);
+		return;
+	}
+#endif
+
 	/* status register and data as read back from the device */
 #pragma pack(push, 1)
 	struct {
@@ -661,15 +820,26 @@ L3GD20::measure()
 	} raw_report;
 #pragma pack(pop)
 
-	gyro_report		*report = &_reports[_next_report];
+	gyro_report report;
 
 	/* start the performance counter */
 	perf_begin(_sample_perf);
 
 	/* fetch data from the sensor */
+	memset(&raw_report, 0, sizeof(raw_report));
 	raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
 	transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));
 
+#if L3GD20_USE_DRDY
+        if ((raw_report.status & 0xF) != 0xF) {
+            /*
+              we waited for DRDY, but did not see DRDY on all axes
+              when we captured. That means a transfer error of some sort
+             */
+            perf_count(_errors);            
+            return;
+        }
+#endif
 	/*
 	 * 1) Scale raw value to SI units using scaling from datasheet.
 	 * 2) Subtract static offset (in SI units)
@@ -684,31 +854,61 @@ L3GD20::measure()
 	 *	 	  the offset is 74 from the origin and subtracting
 	 *		  74 from all measurements centers them around zero.
 	 */
-	report->timestamp = hrt_absolute_time();
+	report.timestamp = hrt_absolute_time();
+        report.error_count = 0; // not recorded
 	
-	/* swap x and y and negate y */
-	report->x_raw = raw_report.y;
-	report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
-	report->z_raw = raw_report.z;
+	switch (_orientation) {
+
+		case SENSOR_BOARD_ROTATION_000_DEG:
+			/* keep axes in place */
+			report.x_raw = raw_report.x;
+			report.y_raw = raw_report.y;
+			break;
+
+		case SENSOR_BOARD_ROTATION_090_DEG:
+			/* swap x and y */
+			report.x_raw = raw_report.y;
+			report.y_raw = raw_report.x;
+			break;
+
+		case SENSOR_BOARD_ROTATION_180_DEG:
+			/* swap x and y and negate both */
+			report.x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
+			report.y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y);
+			break;
+
+		case SENSOR_BOARD_ROTATION_270_DEG:
+			/* swap x and y and negate y */
+			report.x_raw = raw_report.y;
+			report.y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
+			break;
+	}
+
+	report.z_raw = raw_report.z;
+
+	report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
+	report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
+	report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
 
-	report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
-	report->y = ((report->y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
-	report->z = ((report->z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
-	report->scaling = _gyro_range_scale;
-	report->range_rad_s = _gyro_range_rad_s;
+	report.x = _gyro_filter_x.apply(report.x);
+	report.y = _gyro_filter_y.apply(report.y);
+	report.z = _gyro_filter_z.apply(report.z);
 
-	/* post a report to the ring - note, not locked */
-	INCREMENT(_next_report, _num_reports);
+	report.scaling = _gyro_range_scale;
+	report.range_rad_s = _gyro_range_rad_s;
 
-	/* if we are running up against the oldest report, fix it */
-	if (_next_report == _oldest_report)
-		INCREMENT(_oldest_report, _num_reports);
+	_reports->force(&report);
 
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
 	/* publish for subscribers */
-	orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report);
+	if (_gyro_topic > 0 && !(_pub_blocked)) {
+		/* publish it */
+		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
+	}
+
+	_read++;
 
 	/* stop the perf counter */
 	perf_end(_sample_perf);
@@ -717,9 +917,33 @@ L3GD20::measure()
 void
 L3GD20::print_info()
 {
+	printf("gyro reads:          %u\n", _read);
 	perf_print_counter(_sample_perf);
-	printf("report queue:   %u (%u/%u @ %p)\n",
-	       _num_reports, _oldest_report, _next_report, _reports);
+	perf_print_counter(_reschedules);
+	perf_print_counter(_errors);
+	_reports->print_info("report queue");
+}
+
+int
+L3GD20::self_test()
+{
+	/* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */
+	if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f)
+		return 1;
+
+	if (fabsf(_gyro_scale.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f)
+		return 1;
+
+	if (fabsf(_gyro_scale.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f)
+		return 1;
+
+	return 0;
 }
 
 /**
@@ -744,10 +968,10 @@ start()
 	int fd;
 
 	if (g_dev != nullptr)
-		errx(1, "already started");
+		errx(0, "already started");
 
 	/* create the driver */
-	g_dev = new L3GD20(1 /* XXX magic number */, GYRO_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
+	g_dev = new L3GD20(1 /* SPI bus 1 */, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
 
 	if (g_dev == nullptr)
 		goto fail;
@@ -756,7 +980,7 @@ start()
 		goto fail;
 
 	/* set the poll rate to default, starts automatic data collection */
-	fd = open(GYRO_DEVICE_PATH, O_RDONLY);
+	fd = open(L3GD20_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0)
 		goto fail;
@@ -764,6 +988,8 @@ start()
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		goto fail;
 
+        close(fd);
+
 	exit(0);
 fail:
 
@@ -788,10 +1014,10 @@ test()
 	ssize_t sz;
 
 	/* get the driver */
-	fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
+	fd_gyro = open(L3GD20_DEVICE_PATH, O_RDONLY);
 
 	if (fd_gyro < 0)
-		err(1, "%s open failed", GYRO_DEVICE_PATH);
+		err(1, "%s open failed", L3GD20_DEVICE_PATH);
 
 	/* reset to manual polling */
 	if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
@@ -812,6 +1038,8 @@ test()
 	warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
 	      (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
 
+        close(fd_gyro);
+
 	/* XXX add poll-rate tests here too */
 
 	reset();
@@ -824,7 +1052,7 @@ test()
 void
 reset()
 {
-	int fd = open(GYRO_DEVICE_PATH, O_RDONLY);
+	int fd = open(L3GD20_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0)
 		err(1, "failed ");
@@ -833,7 +1061,9 @@ reset()
 		err(1, "driver reset failed");
 
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
-		err(1, "driver poll restart failed");
+		err(1, "accel pollrate reset failed");
+
+        close(fd);
 
 	exit(0);
 }
diff --git a/src/drivers/led/led.cpp b/src/drivers/led/led.cpp
index 04b565358..a37eaca53 100644
--- a/src/drivers/led/led.cpp
+++ b/src/drivers/led/led.cpp
@@ -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
@@ -52,6 +52,7 @@ __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
 
 class LED : device::CDev
@@ -98,6 +99,11 @@ LED::ioctl(struct file *filp, int cmd, unsigned long arg)
 		led_off(arg);
 		break;
 
+	case LED_TOGGLE:
+		led_toggle(arg);
+		break;
+
+
 	default:
 		result = CDev::ioctl(filp, cmd, arg);
 	}
@@ -110,11 +116,11 @@ LED	*gLED;
 }
 
 void
-drv_led_start()
+drv_led_start(void)
 {
 	if (gLED == nullptr) {
 		gLED = new LED;
 		if (gLED != nullptr)
 			gLED->init();
 	}
-}
\ No newline at end of file
+}
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
new file mode 100644
index 000000000..4dee7649b
--- /dev/null
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -0,0 +1,2036 @@
+/****************************************************************************
+ *
+ *   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 lsm303d.cpp
+ * Driver for the ST LSM303D MEMS accelerometer / magnetometer connected via SPI.
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdlib.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 <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/clock.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/device/spi.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_mag.h>
+#include <drivers/device/ringbuffer.h>
+#include <drivers/drv_tone_alarm.h>
+
+#include <board_config.h>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+/* 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_MAG		"/dev/lsm303d_mag"
+
+/* register addresses: A: accel, M: mag, T: temp */
+#define ADDR_WHO_AM_I			0x0F
+#define WHO_I_AM			0x49
+
+#define ADDR_OUT_TEMP_L			0x05
+#define ADDR_OUT_TEMP_H			0x06
+#define ADDR_STATUS_M			0x07
+#define ADDR_OUT_X_L_M          	0x08
+#define ADDR_OUT_X_H_M          	0x09
+#define ADDR_OUT_Y_L_M          	0x0A
+#define ADDR_OUT_Y_H_M			0x0B
+#define ADDR_OUT_Z_L_M			0x0C
+#define ADDR_OUT_Z_H_M			0x0D
+
+#define ADDR_INT_CTRL_M			0x12
+#define ADDR_INT_SRC_M			0x13
+#define ADDR_REFERENCE_X		0x1c
+#define ADDR_REFERENCE_Y		0x1d
+#define ADDR_REFERENCE_Z		0x1e
+
+#define ADDR_STATUS_A			0x27
+#define ADDR_OUT_X_L_A			0x28
+#define ADDR_OUT_X_H_A			0x29
+#define ADDR_OUT_Y_L_A			0x2A
+#define ADDR_OUT_Y_H_A			0x2B
+#define ADDR_OUT_Z_L_A			0x2C
+#define ADDR_OUT_Z_H_A			0x2D
+
+#define ADDR_CTRL_REG0			0x1F
+#define ADDR_CTRL_REG1			0x20
+#define ADDR_CTRL_REG2			0x21
+#define ADDR_CTRL_REG3			0x22
+#define ADDR_CTRL_REG4			0x23
+#define ADDR_CTRL_REG5			0x24
+#define ADDR_CTRL_REG6			0x25
+#define ADDR_CTRL_REG7			0x26
+
+#define ADDR_FIFO_CTRL			0x2e
+#define ADDR_FIFO_SRC			0x2f
+
+#define ADDR_IG_CFG1			0x30
+#define ADDR_IG_SRC1			0x31
+#define ADDR_IG_THS1			0x32
+#define ADDR_IG_DUR1			0x33
+#define ADDR_IG_CFG2			0x34
+#define ADDR_IG_SRC2			0x35
+#define ADDR_IG_THS2			0x36
+#define ADDR_IG_DUR2			0x37
+#define ADDR_CLICK_CFG			0x38
+#define ADDR_CLICK_SRC			0x39
+#define ADDR_CLICK_THS			0x3a
+#define ADDR_TIME_LIMIT			0x3b
+#define ADDR_TIME_LATENCY		0x3c
+#define ADDR_TIME_WINDOW		0x3d
+#define ADDR_ACT_THS			0x3e
+#define ADDR_ACT_DUR			0x3f
+
+#define REG1_RATE_BITS_A		((1<<7) | (1<<6) | (1<<5) | (1<<4))
+#define REG1_POWERDOWN_A		((0<<7) | (0<<6) | (0<<5) | (0<<4))
+#define REG1_RATE_3_125HZ_A		((0<<7) | (0<<6) | (0<<5) | (1<<4))
+#define REG1_RATE_6_25HZ_A		((0<<7) | (0<<6) | (1<<5) | (0<<4))
+#define REG1_RATE_12_5HZ_A		((0<<7) | (0<<6) | (1<<5) | (1<<4))
+#define REG1_RATE_25HZ_A		((0<<7) | (1<<6) | (0<<5) | (0<<4))
+#define REG1_RATE_50HZ_A		((0<<7) | (1<<6) | (0<<5) | (1<<4))
+#define REG1_RATE_100HZ_A		((0<<7) | (1<<6) | (1<<5) | (0<<4))
+#define REG1_RATE_200HZ_A		((0<<7) | (1<<6) | (1<<5) | (1<<4))
+#define REG1_RATE_400HZ_A		((1<<7) | (0<<6) | (0<<5) | (0<<4))
+#define REG1_RATE_800HZ_A		((1<<7) | (0<<6) | (0<<5) | (1<<4))
+#define REG1_RATE_1600HZ_A		((1<<7) | (0<<6) | (1<<5) | (0<<4))
+
+#define REG1_BDU_UPDATE			(1<<3)
+#define REG1_Z_ENABLE_A			(1<<2)
+#define REG1_Y_ENABLE_A			(1<<1)
+#define REG1_X_ENABLE_A			(1<<0)
+
+#define REG2_ANTIALIAS_FILTER_BW_BITS_A	((1<<7) | (1<<6))
+#define REG2_AA_FILTER_BW_773HZ_A		((0<<7) | (0<<6))
+#define REG2_AA_FILTER_BW_194HZ_A		((0<<7) | (1<<6))
+#define REG2_AA_FILTER_BW_362HZ_A		((1<<7) | (0<<6))
+#define REG2_AA_FILTER_BW_50HZ_A		((1<<7) | (1<<6))
+
+#define REG2_FULL_SCALE_BITS_A	((1<<5) | (1<<4) | (1<<3))
+#define REG2_FULL_SCALE_2G_A	((0<<5) | (0<<4) | (0<<3))
+#define REG2_FULL_SCALE_4G_A	((0<<5) | (0<<4) | (1<<3))
+#define REG2_FULL_SCALE_6G_A	((0<<5) | (1<<4) | (0<<3))
+#define REG2_FULL_SCALE_8G_A	((0<<5) | (1<<4) | (1<<3))
+#define REG2_FULL_SCALE_16G_A	((1<<5) | (0<<4) | (0<<3))
+
+#define REG5_ENABLE_T			(1<<7)
+
+#define REG5_RES_HIGH_M			((1<<6) | (1<<5))
+#define REG5_RES_LOW_M			((0<<6) | (0<<5))
+
+#define REG5_RATE_BITS_M		((1<<4) | (1<<3) | (1<<2))
+#define REG5_RATE_3_125HZ_M		((0<<4) | (0<<3) | (0<<2))
+#define REG5_RATE_6_25HZ_M		((0<<4) | (0<<3) | (1<<2))
+#define REG5_RATE_12_5HZ_M		((0<<4) | (1<<3) | (0<<2))
+#define REG5_RATE_25HZ_M		((0<<4) | (1<<3) | (1<<2))
+#define REG5_RATE_50HZ_M		((1<<4) | (0<<3) | (0<<2))
+#define REG5_RATE_100HZ_M		((1<<4) | (0<<3) | (1<<2))
+#define REG5_RATE_DO_NOT_USE_M	((1<<4) | (1<<3) | (0<<2))
+
+#define REG6_FULL_SCALE_BITS_M	((1<<6) | (1<<5))
+#define REG6_FULL_SCALE_2GA_M	((0<<6) | (0<<5))
+#define REG6_FULL_SCALE_4GA_M	((0<<6) | (1<<5))
+#define REG6_FULL_SCALE_8GA_M	((1<<6) | (0<<5))
+#define REG6_FULL_SCALE_12GA_M	((1<<6) | (1<<5))
+
+#define REG7_CONT_MODE_M		((0<<1) | (0<<0))
+
+
+#define INT_CTRL_M              0x12
+#define INT_SRC_M               0x13
+
+/* default values for this device */
+#define LSM303D_ACCEL_DEFAULT_RANGE_G			8
+#define LSM303D_ACCEL_DEFAULT_RATE			800
+#define LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ	50
+#define LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ	30
+
+#define LSM303D_MAG_DEFAULT_RANGE_GA			2
+#define LSM303D_MAG_DEFAULT_RATE			100
+
+#define LSM303D_ONE_G					9.80665f
+
+extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
+
+
+class LSM303D_mag;
+
+class LSM303D : public device::SPI
+{
+public:
+	LSM303D(int bus, const char* path, spi_dev_e device);
+	virtual ~LSM303D();
+
+	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();
+
+	/**
+	 * dump register values
+	 */
+	void			print_registers();
+
+	/**
+	 * toggle logging
+	 */
+	void			toggle_logging();
+
+	/**
+	 * check for extreme accel values
+	 */
+	void			check_extremes(const accel_report *arb);
+
+protected:
+	virtual int		probe();
+
+	friend class 		LSM303D_mag;
+
+	virtual ssize_t		mag_read(struct file *filp, char *buffer, size_t buflen);
+	virtual int		mag_ioctl(struct file *filp, int cmd, unsigned long arg);
+
+private:
+
+	LSM303D_mag		*_mag;
+
+	struct hrt_call		_accel_call;
+	struct hrt_call		_mag_call;
+
+	unsigned		_call_accel_interval;
+	unsigned		_call_mag_interval;
+
+	RingBuffer		*_accel_reports;
+	RingBuffer		*_mag_reports;
+
+	struct accel_scale	_accel_scale;
+	unsigned		_accel_range_m_s2;
+	float			_accel_range_scale;
+	unsigned		_accel_samplerate;
+	unsigned		_accel_onchip_filter_bandwith;
+
+	struct mag_scale	_mag_scale;
+	unsigned		_mag_range_ga;
+	float			_mag_range_scale;
+	unsigned		_mag_samplerate;
+
+	orb_advert_t		_accel_topic;
+	int			_accel_class_instance;
+
+	unsigned		_accel_read;
+	unsigned		_mag_read;
+
+	perf_counter_t		_accel_sample_perf;
+	perf_counter_t		_mag_sample_perf;
+	perf_counter_t		_reg1_resets;
+	perf_counter_t		_reg7_resets;
+	perf_counter_t		_extreme_values;
+	perf_counter_t		_accel_reschedules;
+
+	math::LowPassFilter2p	_accel_filter_x;
+	math::LowPassFilter2p	_accel_filter_y;
+	math::LowPassFilter2p	_accel_filter_z;
+
+	// expceted values of reg1 and reg7 to catch in-flight
+	// brownouts of the sensor
+	uint8_t			_reg1_expected;
+	uint8_t			_reg7_expected;
+
+	// accel logging
+	int			_accel_log_fd;
+	bool			_accel_logging_enabled;
+	uint64_t		_last_extreme_us;	
+	uint64_t		_last_log_us;	
+	uint64_t		_last_log_sync_us;	
+	uint64_t		_last_log_reg_us;	
+	uint64_t		_last_log_alarm_us;	
+
+	/**
+	 * Start automatic measurement.
+	 */
+	void			start();
+
+	/**
+	 * Stop automatic measurement.
+	 */
+	void			stop();
+
+	/**
+	 * Reset chip.
+	 *
+	 * Resets the chip and measurements ranges, but not scale and offset.
+	 */
+	void			reset();
+
+	/**
+	 * disable I2C on the chip
+	 */
+	void			disable_i2c();
+
+	/**
+	 * Static trampoline from the hrt_call context; because we don't have a
+	 * generic hrt wrapper yet.
+	 *
+	 * Called by the HRT in interrupt context at the specified rate if
+	 * automatic polling is enabled.
+	 *
+	 * @param arg		Instance pointer for the driver that is polling.
+	 */
+	static void		measure_trampoline(void *arg);
+
+	/**
+	 * Static trampoline for the mag because it runs at a lower rate
+	 *
+	 * @param arg		Instance pointer for the driver that is polling.
+	 */
+	static void		mag_measure_trampoline(void *arg);
+
+	/**
+	 * Fetch accel measurements from the sensor and update the report ring.
+	 */
+	void			measure();
+
+	/**
+	 * Fetch mag measurements from the sensor and update the report ring.
+	 */
+	void			mag_measure();
+
+	/**
+	 * Accel self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	int			accel_self_test();
+
+	/**
+	 * Mag self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	int			mag_self_test();
+
+	/**
+	 * Read a register from the LSM303D
+	 *
+	 * @param		The register to read.
+	 * @return		The value that was read.
+	 */
+	uint8_t			read_reg(unsigned reg);
+
+	/**
+	 * Write a register in the LSM303D
+	 *
+	 * @param reg		The register to write.
+	 * @param value		The new value to write.
+	 */
+	void			write_reg(unsigned reg, uint8_t value);
+
+	/**
+	 * Modify a register in the LSM303D
+	 *
+	 * Bits are cleared before bits are set.
+	 *
+	 * @param reg		The register to modify.
+	 * @param clearbits	Bits in the register to clear.
+	 * @param setbits	Bits in the register to set.
+	 */
+	void			modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
+
+	/**
+	 * Set the LSM303D accel measurement range.
+	 *
+	 * @param max_g	The measurement range of the accel is in g (9.81m/s^2)
+	 *			Zero selects the maximum supported range.
+	 * @return		OK if the value can be supported, -ERANGE otherwise.
+	 */
+	int			accel_set_range(unsigned max_g);
+
+	/**
+	 * Set the LSM303D mag measurement range.
+	 *
+	 * @param max_ga	The measurement range of the mag is in Ga
+	 *			Zero selects the maximum supported range.
+	 * @return		OK if the value can be supported, -ERANGE otherwise.
+	 */
+	int			mag_set_range(unsigned max_g);
+
+	/**
+	 * Set the LSM303D on-chip anti-alias filter bandwith.
+	 *
+	 * @param bandwidth The anti-alias filter bandwidth in Hz
+	 * 			Zero selects the highest bandwidth
+	 * @return		OK if the value can be supported, -ERANGE otherwise.
+	 */
+	int			accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth);
+
+	/**
+	 * Set the driver lowpass filter bandwidth.
+	 *
+	 * @param bandwidth The anti-alias filter bandwidth in Hz
+	 * 			Zero selects the highest bandwidth
+	 * @return		OK if the value can be supported, -ERANGE otherwise.
+	 */
+	int			accel_set_driver_lowpass_filter(float samplerate, float bandwidth);
+
+	/**
+	 * Set the LSM303D internal accel sampling frequency.
+	 *
+	 * @param frequency	The internal accel sampling frequency is set to not less than
+	 *			this value.
+	 *			Zero selects the maximum rate supported.
+	 * @return		OK if the value can be supported.
+	 */
+	int			accel_set_samplerate(unsigned frequency);
+
+	/**
+	 * Set the LSM303D internal mag sampling frequency.
+	 *
+	 * @param frequency	The internal mag sampling frequency is set to not less than
+	 *			this value.
+	 *			Zero selects the maximum rate supported.
+	 * @return		OK if the value can be supported.
+	 */
+	int			mag_set_samplerate(unsigned frequency);
+};
+
+/**
+ * Helper class implementing the mag driver node.
+ */
+class LSM303D_mag : public device::CDev
+{
+public:
+	LSM303D_mag(LSM303D *parent);
+	~LSM303D_mag();
+
+	virtual ssize_t			read(struct file *filp, char *buffer, size_t buflen);
+	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	virtual int		init();
+
+protected:
+	friend class LSM303D;
+
+	void				parent_poll_notify();
+private:
+	LSM303D				*_parent;
+
+	orb_advert_t			_mag_topic;
+	int				_mag_class_instance;
+
+	void				measure();
+
+	void				measure_trampoline(void *arg);
+};
+
+
+LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
+	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),
+	_call_mag_interval(0),
+	_accel_reports(nullptr),
+	_mag_reports(nullptr),
+	_accel_range_m_s2(0.0f),
+	_accel_range_scale(0.0f),
+	_accel_samplerate(0),
+	_accel_onchip_filter_bandwith(0),
+	_mag_range_ga(0.0f),
+	_mag_range_scale(0.0f),
+	_mag_samplerate(0),
+	_accel_topic(-1),
+	_accel_class_instance(-1),
+	_accel_read(0),
+	_mag_read(0),
+	_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
+	_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
+	_reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")),
+	_reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")),
+	_extreme_values(perf_alloc(PC_COUNT, "lsm303d_extremes")),
+	_accel_reschedules(perf_alloc(PC_COUNT, "lsm303d_accel_resched")),
+	_accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_reg1_expected(0),
+	_reg7_expected(0),
+	_accel_log_fd(-1),
+	_accel_logging_enabled(false),
+	_last_log_us(0),
+	_last_log_sync_us(0),
+	_last_log_reg_us(0),
+	_last_log_alarm_us(0)
+{
+	// enable debug() calls
+	_debug_enabled = true;
+
+	// default scale factors
+	_accel_scale.x_offset = 0.0f;
+	_accel_scale.x_scale  = 1.0f;
+	_accel_scale.y_offset = 0.0f;
+	_accel_scale.y_scale  = 1.0f;
+	_accel_scale.z_offset = 0.0f;
+	_accel_scale.z_scale  = 1.0f;
+
+	_mag_scale.x_offset = 0.0f;
+	_mag_scale.x_scale = 1.0f;
+	_mag_scale.y_offset = 0.0f;
+	_mag_scale.y_scale = 1.0f;
+	_mag_scale.z_offset = 0.0f;
+	_mag_scale.z_scale = 1.0f;
+}
+
+LSM303D::~LSM303D()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_accel_reports != nullptr)
+		delete _accel_reports;
+	if (_mag_reports != nullptr)
+		delete _mag_reports;
+
+	if (_accel_class_instance != -1)
+		unregister_class_devname(ACCEL_DEVICE_PATH, _accel_class_instance);
+
+	delete _mag;
+
+	/* delete the perf counter */
+	perf_free(_accel_sample_perf);
+	perf_free(_mag_sample_perf);
+	perf_free(_reg1_resets);
+	perf_free(_reg7_resets);
+	perf_free(_extreme_values);
+	perf_free(_accel_reschedules);
+}
+
+int
+LSM303D::init()
+{
+	int ret = ERROR;
+
+	/* do SPI init (and probe) first */
+	if (SPI::init() != OK) {
+		warnx("SPI init failed");
+		goto out;
+	}
+
+	/* allocate basic report buffers */
+	_accel_reports = new RingBuffer(2, sizeof(accel_report));
+
+	if (_accel_reports == nullptr)
+		goto out;
+
+	/* advertise accel topic */
+	_mag_reports = new RingBuffer(2, sizeof(mag_report));
+
+	if (_mag_reports == nullptr)
+		goto out;
+
+	reset();
+
+	/* do CDev init for the mag device node */
+	ret = _mag->init();
+	if (ret != OK) {
+		warnx("MAG init failed");
+		goto out;
+	}
+
+	/* fill report structures */
+	measure();
+
+	if (_mag->_mag_class_instance == CLASS_DEVICE_PRIMARY) {
+
+		/* advertise sensor topic, measure manually to initialize valid report */
+		struct mag_report mrp;
+		_mag_reports->get(&mrp);
+
+		/* measurement will have generated a report, publish */
+		_mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag), &mrp);
+
+		if (_mag->_mag_topic < 0)
+			debug("failed to create sensor_mag publication");
+
+	}
+
+	_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
+
+	if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {
+
+		/* advertise sensor topic, measure manually to initialize valid report */
+		struct accel_report arp;
+		_accel_reports->get(&arp);
+
+		/* measurement will have generated a report, publish */
+		_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
+
+		if (_accel_topic < 0)
+			debug("failed to create sensor_accel publication");
+
+	}
+
+out:
+	return ret;
+}
+
+void
+LSM303D::disable_i2c(void)
+{
+	uint8_t a = read_reg(0x02);
+	write_reg(0x02, (0x10 | a));
+	a = read_reg(0x02);
+	write_reg(0x02, (0xF7 & a));
+	a = read_reg(0x15);
+	write_reg(0x15, (0x80 | a));
+	a = read_reg(0x02);
+	write_reg(0x02, (0xE7 & a));
+}
+
+void
+LSM303D::reset()
+{
+	// ensure the chip doesn't interpret any other bus traffic as I2C
+	disable_i2c();
+
+	/* enable accel*/
+	_reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A;
+	write_reg(ADDR_CTRL_REG1, _reg1_expected);
+
+	/* enable mag */
+	_reg7_expected = REG7_CONT_MODE_M;
+	write_reg(ADDR_CTRL_REG7, _reg7_expected);
+	write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
+	write_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1
+	write_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2
+
+	accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
+	accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE);
+	accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ);
+
+	// we setup the anti-alias on-chip filter as 50Hz. We believe
+	// this operates in the analog domain, and is critical for
+	// anti-aliasing. The 2 pole software filter is designed to
+	// operate in conjunction with this on-chip filter
+	accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
+
+	mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
+	mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
+
+	_accel_read = 0;
+	_mag_read = 0;
+}
+
+int
+LSM303D::probe()
+{
+	/* read dummy value to void to clear SPI statemachine on sensor */
+	(void)read_reg(ADDR_WHO_AM_I);
+
+	/* verify that the device is attached and functioning */
+	bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM);
+	
+	if (success)
+		return OK;
+
+	return -EIO;
+}
+
+#define ACCEL_LOGFILE "/fs/microsd/lsm303d.log"
+
+/**
+   check for extreme accelerometer values and log to a file on the SD card
+ */
+void
+LSM303D::check_extremes(const accel_report *arb)
+{
+	const float extreme_threshold = 30;
+        static bool boot_ok = false;
+	bool is_extreme = (fabsf(arb->x) > extreme_threshold && 
+			   fabsf(arb->y) > extreme_threshold && 
+			   fabsf(arb->z) > extreme_threshold);
+	if (is_extreme) {
+		perf_count(_extreme_values);
+		// force accel logging on if we see extreme values
+		_accel_logging_enabled = true;
+	} else {
+            boot_ok = true;
+        }
+
+	if (! _accel_logging_enabled) {
+		// logging has been disabled by user, close
+		if (_accel_log_fd != -1) {
+			::close(_accel_log_fd);
+			_accel_log_fd = -1;
+		}
+		return;
+	}
+	if (_accel_log_fd == -1) {
+		// keep last 10 logs
+		::unlink(ACCEL_LOGFILE ".9");
+		for (uint8_t i=8; i>0; i--) {
+			uint8_t len = strlen(ACCEL_LOGFILE)+3;
+			char log1[len], log2[len];
+			snprintf(log1, sizeof(log1), "%s.%u", ACCEL_LOGFILE, (unsigned)i);
+			snprintf(log2, sizeof(log2), "%s.%u", ACCEL_LOGFILE, (unsigned)(i+1));
+			::rename(log1, log2);
+		}
+		::rename(ACCEL_LOGFILE, ACCEL_LOGFILE ".1");
+
+		// open the new logfile
+		_accel_log_fd = ::open(ACCEL_LOGFILE, O_WRONLY|O_CREAT|O_TRUNC, 0666);
+		if (_accel_log_fd == -1) {
+			return;
+		}
+	}
+
+	uint64_t now = hrt_absolute_time();
+	// log accels at 1Hz
+	if (_last_log_us == 0 ||
+	    now - _last_log_us > 1000*1000) {
+		_last_log_us = now;
+		::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d boot_ok=%u\r\n",
+			  (unsigned long long)arb->timestamp, 
+			  (double)arb->x, (double)arb->y, (double)arb->z,
+			  (int)arb->x_raw,
+			  (int)arb->y_raw,
+			  (int)arb->z_raw,
+			  (unsigned)boot_ok);
+	}
+
+        const uint8_t reglist[] = { ADDR_WHO_AM_I, 0x02, 0x15, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, 
+                                    ADDR_CTRL_REG2, ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, 
+                                    ADDR_CTRL_REG7, ADDR_OUT_TEMP_L, ADDR_OUT_TEMP_H, ADDR_INT_CTRL_M, ADDR_INT_SRC_M, 
+                                    ADDR_REFERENCE_X, ADDR_REFERENCE_Y, ADDR_REFERENCE_Z, ADDR_OUT_X_L_A, ADDR_OUT_X_H_A, 
+                                    ADDR_OUT_Y_L_A, ADDR_OUT_Y_H_A, ADDR_OUT_Z_L_A, ADDR_OUT_Z_H_A, ADDR_FIFO_CTRL, 
+                                    ADDR_FIFO_SRC, ADDR_IG_CFG1, ADDR_IG_SRC1, ADDR_IG_THS1, ADDR_IG_DUR1, ADDR_IG_CFG2, 
+                                    ADDR_IG_SRC2, ADDR_IG_THS2, ADDR_IG_DUR2, ADDR_CLICK_CFG, ADDR_CLICK_SRC, 
+                                    ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW, 
+                                    ADDR_ACT_THS, ADDR_ACT_DUR,
+                                    ADDR_OUT_X_L_M, ADDR_OUT_X_H_M, 
+                                    ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M, 0x02, 0x15, ADDR_WHO_AM_I};
+        uint8_t regval[sizeof(reglist)];
+        for (uint8_t i=0; i<sizeof(reglist); i++) {
+            regval[i] = read_reg(reglist[i]);
+        }
+
+	// log registers at 10Hz when we have extreme values, or 0.5 Hz without
+	if (_last_log_reg_us == 0 ||
+	    (is_extreme && (now - _last_log_reg_us > 250*1000)) ||
+	    (now - _last_log_reg_us > 10*1000*1000)) {
+		_last_log_reg_us = now;
+		::dprintf(_accel_log_fd, "XREG %llu", (unsigned long long)hrt_absolute_time());
+		for (uint8_t i=0; i<sizeof(reglist); i++) {
+			::dprintf(_accel_log_fd, " %02x:%02x", (unsigned)reglist[i], (unsigned)regval[i]);
+		}
+		::dprintf(_accel_log_fd, "\n");
+	}
+
+	// fsync at 0.1Hz
+	if (now - _last_log_sync_us > 10*1000*1000) {
+		_last_log_sync_us = now;
+		::fsync(_accel_log_fd);
+	}
+
+	// play alarm every 10s if we have had an extreme value
+	if (perf_event_count(_extreme_values) != 0 && 
+	    (now - _last_log_alarm_us > 10*1000*1000)) {
+		_last_log_alarm_us = now;
+		int tfd = ::open(TONEALARM_DEVICE_PATH, 0);
+		if (tfd != -1) {
+			uint8_t tone = 3;
+			if (!is_extreme) {
+				tone = 3;
+			} else if (boot_ok) {
+				tone = 4;
+			} else {
+				tone = 5;
+			}
+			::ioctl(tfd, TONE_SET_ALARM, tone);
+			::close(tfd);
+		}		
+	}
+}
+
+ssize_t
+LSM303D::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct accel_report);
+	accel_report *arb = reinterpret_cast<accel_report *>(buffer);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1)
+		return -ENOSPC;
+
+	/* if automatic measurement is enabled */
+	if (_call_accel_interval > 0) {
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 */
+		while (count--) {
+			if (_accel_reports->get(arb)) {
+				check_extremes(arb);
+				ret += sizeof(*arb);
+				arb++;
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement */
+	measure();
+
+	/* measurement will have generated a report, copy it out */
+	if (_accel_reports->get(arb))
+		ret = sizeof(*arb);
+
+	return ret;
+}
+
+ssize_t
+LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct mag_report);
+	mag_report *mrb = reinterpret_cast<mag_report *>(buffer);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1)
+		return -ENOSPC;
+
+	/* if automatic measurement is enabled */
+	if (_call_mag_interval > 0) {
+
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 */
+		while (count--) {
+			if (_mag_reports->get(mrb)) {
+				ret += sizeof(*mrb);
+				mrb++;
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement */
+	_mag_reports->flush();
+	measure();
+
+	/* measurement will have generated a report, copy it out */
+	if (_mag_reports->get(mrb))
+		ret = sizeof(*mrb);
+
+	return ret;
+}
+
+int
+LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+		switch (arg) {
+
+			/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_call_accel_interval = 0;
+				return OK;
+
+			/* external signalling not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+			/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+			/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+				return ioctl(filp, SENSORIOCSPOLLRATE, 1600);
+
+			case SENSOR_POLLRATE_DEFAULT:
+				return ioctl(filp, SENSORIOCSPOLLRATE, LSM303D_ACCEL_DEFAULT_RATE);
+
+				/* adjust to a legal polling interval in Hz */
+			default: {
+				/* do we need to start internal polling? */
+				bool want_start = (_call_accel_interval == 0);
+
+				/* convert hz to hrt interval via microseconds */
+				unsigned ticks = 1000000 / arg;
+
+				/* check against maximum sane rate */
+				if (ticks < 500)
+					return -EINVAL;
+
+				/* adjust filters */
+				accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq());
+
+				/* update interval for next measurement */
+				/* XXX this is a bit shady, but no other way to adjust... */
+				_accel_call.period = _call_accel_interval = ticks;
+
+				/* if we need to start the poll state machine, do it */
+				if (want_start)
+					start();
+
+				return OK;
+			}
+		}
+	}
+
+	case SENSORIOCGPOLLRATE:
+		if (_call_accel_interval == 0)
+			return SENSOR_POLLRATE_MANUAL;
+
+		return 1000000 / _call_accel_interval;
+
+	case SENSORIOCSQUEUEDEPTH: {
+		/* lower bound is mandatory, upper bound is a sanity check */
+		if ((arg < 1) || (arg > 100))
+			return -EINVAL;
+
+		irqstate_t flags = irqsave();
+		if (!_accel_reports->resize(arg)) {
+			irqrestore(flags);
+			return -ENOMEM;
+		}
+		irqrestore(flags);
+
+		return OK;
+	}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return _accel_reports->size();
+
+	case SENSORIOCRESET:
+		reset();
+		return OK;
+
+	case ACCELIOCSSAMPLERATE:
+		return accel_set_samplerate(arg);
+
+	case ACCELIOCGSAMPLERATE:
+		return _accel_samplerate;
+
+	case ACCELIOCSLOWPASS: {
+		return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg);
+	}
+
+	case ACCELIOCGLOWPASS:
+		return _accel_filter_x.get_cutoff_freq();
+
+	case ACCELIOCSSCALE: {
+		/* copy scale, but only if off by a few percent */
+		struct accel_scale *s = (struct accel_scale *) arg;
+		float sum = s->x_scale + s->y_scale + s->z_scale;
+		if (sum > 2.0f && sum < 4.0f) {
+			memcpy(&_accel_scale, s, sizeof(_accel_scale));
+			return OK;
+		} else {
+			return -EINVAL;
+		}
+	}
+
+	case ACCELIOCSRANGE:
+		/* arg needs to be in G */
+		return accel_set_range(arg);
+
+	case ACCELIOCGRANGE:
+		/* convert to m/s^2 and return rounded in G */
+		return (unsigned long)((_accel_range_m_s2)/LSM303D_ONE_G + 0.5f);
+
+	case ACCELIOCGSCALE:
+		/* copy scale out */
+		memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
+		return OK;
+
+	case ACCELIOCSELFTEST:
+		return accel_self_test();
+
+	default:
+		/* give it to the superclass */
+		return SPI::ioctl(filp, cmd, arg);
+	}
+}
+
+int
+LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+		switch (arg) {
+
+			/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_call_mag_interval = 0;
+				return OK;
+
+			/* external signalling 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:
+				/* 100 Hz is max for mag */
+				return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100);
+
+			/* adjust to a legal polling interval in Hz */
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_call_mag_interval == 0);
+
+					/* convert hz to hrt interval via microseconds */
+					unsigned ticks = 1000000 / arg;
+
+					/* check against maximum sane rate */
+					if (ticks < 1000)
+						return -EINVAL;
+
+					/* update interval for next measurement */
+					/* XXX this is a bit shady, but no other way to adjust... */
+					_mag_call.period = _call_mag_interval = ticks;
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start();
+
+					return OK;
+				}
+			}
+		}
+
+	case SENSORIOCGPOLLRATE:
+		if (_call_mag_interval == 0)
+			return SENSOR_POLLRATE_MANUAL;
+
+		return 1000000 / _call_mag_interval;
+	
+	case SENSORIOCSQUEUEDEPTH: {
+		/* lower bound is mandatory, upper bound is a sanity check */
+		if ((arg < 1) || (arg > 100))
+			return -EINVAL;
+
+		irqstate_t flags = irqsave();
+		if (!_mag_reports->resize(arg)) {
+			irqrestore(flags);
+			return -ENOMEM;
+		}
+		irqrestore(flags);
+
+		return OK;
+	}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return _mag_reports->size();
+
+	case SENSORIOCRESET:
+		reset();
+		return OK;
+
+	case MAGIOCSSAMPLERATE:
+		return mag_set_samplerate(arg);
+
+	case MAGIOCGSAMPLERATE:
+		return _mag_samplerate;
+
+	case MAGIOCSLOWPASS:
+	case MAGIOCGLOWPASS:
+		/* not supported, no internal filtering */
+		return -EINVAL;
+
+	case MAGIOCSSCALE:
+		/* copy scale in */
+		memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale));
+		return OK;
+
+	case MAGIOCGSCALE:
+		/* copy scale out */
+		memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale));
+		return OK;
+
+	case MAGIOCSRANGE:
+		return mag_set_range(arg);
+
+	case MAGIOCGRANGE:
+		return _mag_range_ga;
+
+	case MAGIOCSELFTEST:
+		return mag_self_test();
+
+	case MAGIOCGEXTERNAL:
+		/* no external mag board yet */
+		return 0;
+
+	default:
+		/* give it to the superclass */
+		return SPI::ioctl(filp, cmd, arg);
+	}
+}
+
+int
+LSM303D::accel_self_test()
+{
+	if (_accel_read == 0)
+		return 1;
+
+	/* inspect accel offsets */
+	if (fabsf(_accel_scale.x_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f)
+		return 1;
+
+	if (fabsf(_accel_scale.y_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f)
+		return 1;
+
+	if (fabsf(_accel_scale.z_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f)
+		return 1;
+
+	return 0;
+}
+
+int
+LSM303D::mag_self_test()
+{
+	if (_mag_read == 0)
+		return 1;
+
+	/**
+	 * inspect mag offsets
+	 * don't check mag scale because it seems this is calibrated on chip
+	 */
+	if (fabsf(_mag_scale.x_offset) < 0.000001f)
+		return 1;
+
+	if (fabsf(_mag_scale.y_offset) < 0.000001f)
+		return 1;
+
+	if (fabsf(_mag_scale.z_offset) < 0.000001f)
+		return 1;
+
+	return 0;
+}
+
+uint8_t
+LSM303D::read_reg(unsigned reg)
+{
+	uint8_t cmd[2];
+
+	cmd[0] = reg | DIR_READ;
+	cmd[1] = 0;
+
+	transfer(cmd, cmd, sizeof(cmd));
+
+	return cmd[1];
+}
+
+void
+LSM303D::write_reg(unsigned reg, uint8_t value)
+{
+	uint8_t	cmd[2];
+
+	cmd[0] = reg | DIR_WRITE;
+	cmd[1] = value;
+
+	transfer(cmd, nullptr, sizeof(cmd));
+}
+
+void
+LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
+{
+	uint8_t	val;
+
+	val = read_reg(reg);
+	val &= ~clearbits;
+	val |= setbits;
+	write_reg(reg, val);
+}
+
+int
+LSM303D::accel_set_range(unsigned max_g)
+{
+	uint8_t setbits = 0;
+	uint8_t clearbits = REG2_FULL_SCALE_BITS_A;
+	float new_scale_g_digit = 0.0f;
+
+	if (max_g == 0)
+		max_g = 16;
+
+	if (max_g <= 2) {
+		_accel_range_m_s2 = 2.0f*LSM303D_ONE_G;
+		setbits |= REG2_FULL_SCALE_2G_A;
+		new_scale_g_digit = 0.061e-3f;
+
+	} else if (max_g <= 4) {
+		_accel_range_m_s2 = 4.0f*LSM303D_ONE_G;
+		setbits |= REG2_FULL_SCALE_4G_A;
+		new_scale_g_digit = 0.122e-3f;
+
+	} else if (max_g <= 6) {
+		_accel_range_m_s2 = 6.0f*LSM303D_ONE_G;
+		setbits |= REG2_FULL_SCALE_6G_A;
+		new_scale_g_digit = 0.183e-3f;
+
+	} else if (max_g <= 8) {
+		_accel_range_m_s2 = 8.0f*LSM303D_ONE_G;
+		setbits |= REG2_FULL_SCALE_8G_A;
+		new_scale_g_digit = 0.244e-3f;
+
+	} else if (max_g <= 16) {
+		_accel_range_m_s2 = 16.0f*LSM303D_ONE_G;
+		setbits |= REG2_FULL_SCALE_16G_A;
+		new_scale_g_digit = 0.732e-3f;
+
+	} else {
+		return -EINVAL;
+	}
+
+	_accel_range_scale = new_scale_g_digit * LSM303D_ONE_G;
+
+
+	modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
+
+	return OK;
+}
+
+int
+LSM303D::mag_set_range(unsigned max_ga)
+{
+	uint8_t setbits = 0;
+	uint8_t clearbits = REG6_FULL_SCALE_BITS_M;
+	float new_scale_ga_digit = 0.0f;
+
+	if (max_ga == 0)
+		max_ga = 12;
+
+	if (max_ga <= 2) {
+		_mag_range_ga = 2;
+		setbits |= REG6_FULL_SCALE_2GA_M;
+		new_scale_ga_digit = 0.080e-3f;
+
+	} else if (max_ga <= 4) {
+		_mag_range_ga = 4;
+		setbits |= REG6_FULL_SCALE_4GA_M;
+		new_scale_ga_digit = 0.160e-3f;
+
+	} else if (max_ga <= 8) {
+		_mag_range_ga = 8;
+		setbits |= REG6_FULL_SCALE_8GA_M;
+		new_scale_ga_digit = 0.320e-3f;
+
+	} else if (max_ga <= 12) {
+		_mag_range_ga = 12;
+		setbits |= REG6_FULL_SCALE_12GA_M;
+		new_scale_ga_digit = 0.479e-3f;
+
+	} else {
+		return -EINVAL;
+	}
+
+	_mag_range_scale = new_scale_ga_digit;
+
+	modify_reg(ADDR_CTRL_REG6, clearbits, setbits);
+
+	return OK;
+}
+
+int
+LSM303D::accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth)
+{
+	uint8_t setbits = 0;
+	uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A;
+
+	if (bandwidth == 0)
+		bandwidth = 773;
+
+	if (bandwidth <= 50) {
+		setbits |= REG2_AA_FILTER_BW_50HZ_A;
+		_accel_onchip_filter_bandwith = 50;
+
+	} else if (bandwidth <= 194) {
+		setbits |= REG2_AA_FILTER_BW_194HZ_A;
+		_accel_onchip_filter_bandwith = 194;
+
+	} else if (bandwidth <= 362) {
+		setbits |= REG2_AA_FILTER_BW_362HZ_A;
+		_accel_onchip_filter_bandwith = 362;
+
+	} else if (bandwidth <= 773) {
+		setbits |= REG2_AA_FILTER_BW_773HZ_A;
+		_accel_onchip_filter_bandwith = 773;
+
+	} else {
+		return -EINVAL;
+	}
+
+	modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
+
+	return OK;
+}
+
+int
+LSM303D::accel_set_driver_lowpass_filter(float samplerate, float bandwidth)
+{
+	_accel_filter_x.set_cutoff_frequency(samplerate, bandwidth);
+	_accel_filter_y.set_cutoff_frequency(samplerate, bandwidth);
+	_accel_filter_z.set_cutoff_frequency(samplerate, bandwidth);
+
+	return OK;
+}
+
+int
+LSM303D::accel_set_samplerate(unsigned frequency)
+{
+	uint8_t setbits = 0;
+	uint8_t clearbits = REG1_RATE_BITS_A;
+
+	if (frequency == 0)
+		frequency = 1600;
+
+	if (frequency <= 100) {
+		setbits |= REG1_RATE_100HZ_A;
+		_accel_samplerate = 100;
+
+	} else if (frequency <= 200) {
+		setbits |= REG1_RATE_200HZ_A;
+		_accel_samplerate = 200;
+
+	} else if (frequency <= 400) {
+		setbits |= REG1_RATE_400HZ_A;
+		_accel_samplerate = 400;
+
+	} else if (frequency <= 800) {
+		setbits |= REG1_RATE_800HZ_A;
+		_accel_samplerate = 800;
+
+	} else if (frequency <= 1600) {
+		setbits |= REG1_RATE_1600HZ_A;
+		_accel_samplerate = 1600;
+
+	} else {
+		return -EINVAL;
+	}
+
+	modify_reg(ADDR_CTRL_REG1, clearbits, setbits);
+	_reg1_expected = (_reg1_expected & ~clearbits) | setbits;
+
+	return OK;
+}
+
+int
+LSM303D::mag_set_samplerate(unsigned frequency)
+{
+	uint8_t setbits = 0;
+	uint8_t clearbits = REG5_RATE_BITS_M;
+
+	if (frequency == 0)
+		frequency = 100;
+
+	if (frequency <= 25) {
+		setbits |= REG5_RATE_25HZ_M;
+		_mag_samplerate = 25;
+
+	} else if (frequency <= 50) {
+		setbits |= REG5_RATE_50HZ_M;
+		_mag_samplerate = 50;
+
+	} else if (frequency <= 100) {
+		setbits |= REG5_RATE_100HZ_M;
+		_mag_samplerate = 100;
+
+	} else {
+		return -EINVAL;
+	}
+
+	modify_reg(ADDR_CTRL_REG5, clearbits, setbits);
+
+	return OK;
+}
+
+void
+LSM303D::start()
+{
+	/* make sure we are stopped first */
+	stop();
+
+	/* reset the report ring */
+	_accel_reports->flush();
+	_mag_reports->flush();
+
+	/* start polling at the specified rate */
+	hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this);
+	hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this);
+}
+
+void
+LSM303D::stop()
+{
+	hrt_cancel(&_accel_call);
+	hrt_cancel(&_mag_call);
+}
+
+void
+LSM303D::measure_trampoline(void *arg)
+{
+	LSM303D *dev = (LSM303D *)arg;
+
+	/* make another measurement */
+	dev->measure();
+}
+
+void
+LSM303D::mag_measure_trampoline(void *arg)
+{
+	LSM303D *dev = (LSM303D *)arg;
+
+	/* make another measurement */
+	dev->mag_measure();
+}
+
+void
+LSM303D::measure()
+{
+	// if the accel 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_ACCEL_DRDY) == 0) {
+		perf_count(_accel_reschedules);
+		hrt_call_delay(&_accel_call, 100);
+		return;
+	}
+	if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) {
+		perf_count(_reg1_resets);
+		reset();
+		return;
+	}
+
+	/* status register and data as read back from the device */
+
+#pragma pack(push, 1)
+	struct {
+		uint8_t		cmd;
+		uint8_t		status;
+		int16_t		x;
+		int16_t		y;
+		int16_t		z;
+	} raw_accel_report;
+#pragma pack(pop)
+
+	accel_report accel_report;
+
+	/* start the performance counter */
+	perf_begin(_accel_sample_perf);
+
+	/* fetch data from the sensor */
+	memset(&raw_accel_report, 0, sizeof(raw_accel_report));
+	raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT;
+	transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report));
+
+	/*
+	 * 1) Scale raw value to SI units using scaling from datasheet.
+	 * 2) Subtract static offset (in SI units)
+	 * 3) Scale the statically calibrated values with a linear
+	 *    dynamically obtained factor
+	 *
+	 * Note: the static sensor offset is the number the sensor outputs
+	 * 	 at a nominally 'zero' input. Therefore the offset has to
+	 * 	 be subtracted.
+	 *
+	 *	 Example: A gyro outputs a value of 74 at zero angular rate
+	 *	 	  the offset is 74 from the origin and subtracting
+	 *		  74 from all measurements centers them around zero.
+	 */
+
+
+	accel_report.timestamp = hrt_absolute_time();
+        accel_report.error_count = 0; // not reported
+
+	accel_report.x_raw = raw_accel_report.x;
+	accel_report.y_raw = raw_accel_report.y;
+	accel_report.z_raw = raw_accel_report.z;
+
+	float x_in_new = ((accel_report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
+	float y_in_new = ((accel_report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
+	float z_in_new = ((accel_report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
+
+	accel_report.x = _accel_filter_x.apply(x_in_new);
+	accel_report.y = _accel_filter_y.apply(y_in_new);
+	accel_report.z = _accel_filter_z.apply(z_in_new);
+
+	accel_report.scaling = _accel_range_scale;
+	accel_report.range_m_s2 = _accel_range_m_s2;
+
+	_accel_reports->force(&accel_report);
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	if (_accel_topic > 0 && !(_pub_blocked)) {
+		/* publish it */
+		orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
+	}
+
+	_accel_read++;
+
+	/* stop the perf counter */
+	perf_end(_accel_sample_perf);
+}
+
+void
+LSM303D::mag_measure()
+{
+	if (read_reg(ADDR_CTRL_REG7) != _reg7_expected) {
+		perf_count(_reg7_resets);
+		reset();
+		return;
+	}
+
+	/* status register and data as read back from the device */
+#pragma pack(push, 1)
+	struct {
+		uint8_t		cmd;
+		uint8_t		status;
+		int16_t		x;
+		int16_t		y;
+		int16_t		z;
+	} raw_mag_report;
+#pragma pack(pop)
+
+	mag_report mag_report;
+
+	/* start the performance counter */
+	perf_begin(_mag_sample_perf);
+
+	/* fetch data from the sensor */
+	memset(&raw_mag_report, 0, sizeof(raw_mag_report));
+	raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT;
+	transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report));
+
+	/*
+	 * 1) Scale raw value to SI units using scaling from datasheet.
+	 * 2) Subtract static offset (in SI units)
+	 * 3) Scale the statically calibrated values with a linear
+	 *    dynamically obtained factor
+	 *
+	 * Note: the static sensor offset is the number the sensor outputs
+	 * 	 at a nominally 'zero' input. Therefore the offset has to
+	 * 	 be subtracted.
+	 *
+	 *	 Example: A gyro outputs a value of 74 at zero angular rate
+	 *	 	  the offset is 74 from the origin and subtracting
+	 *		  74 from all measurements centers them around zero.
+	 */
+
+
+	mag_report.timestamp = hrt_absolute_time();
+
+	mag_report.x_raw = raw_mag_report.x;
+	mag_report.y_raw = raw_mag_report.y;
+	mag_report.z_raw = raw_mag_report.z;
+	mag_report.x = ((mag_report.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
+	mag_report.y = ((mag_report.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
+	mag_report.z = ((mag_report.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
+	mag_report.scaling = _mag_range_scale;
+	mag_report.range_ga = (float)_mag_range_ga;
+
+	_mag_reports->force(&mag_report);
+
+	/* XXX please check this poll_notify, is it the right one? */
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	if (_mag->_mag_topic > 0 && !(_pub_blocked)) {
+		/* publish it */
+		orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
+	}
+
+	_mag_read++;
+
+	/* stop the perf counter */
+	perf_end(_mag_sample_perf);
+}
+
+void
+LSM303D::print_info()
+{
+	printf("accel reads:          %u\n", _accel_read);
+	printf("mag reads:            %u\n", _mag_read);
+	perf_print_counter(_accel_sample_perf);
+	_accel_reports->print_info("accel reports");
+	_mag_reports->print_info("mag reports");
+}
+
+void
+LSM303D::print_registers()
+{
+	const struct {
+		uint8_t reg;
+		const char *name;
+	} regmap[] = {
+		{ ADDR_WHO_AM_I,    "WHO_AM_I" },
+		{ 0x02,             "I2C_CONTROL1" },
+		{ 0x15,             "I2C_CONTROL2" },
+		{ ADDR_STATUS_A,    "STATUS_A" },
+		{ ADDR_STATUS_M,    "STATUS_M" },
+		{ ADDR_CTRL_REG0,   "CTRL_REG0" },
+		{ ADDR_CTRL_REG1,   "CTRL_REG1" },
+		{ ADDR_CTRL_REG2,   "CTRL_REG2" },
+		{ ADDR_CTRL_REG3,   "CTRL_REG3" },
+		{ ADDR_CTRL_REG4,   "CTRL_REG4" },
+		{ ADDR_CTRL_REG5,   "CTRL_REG5" },
+		{ ADDR_CTRL_REG6,   "CTRL_REG6" },
+		{ ADDR_CTRL_REG7,   "CTRL_REG7" },
+		{ ADDR_OUT_TEMP_L,  "TEMP_L" },
+		{ ADDR_OUT_TEMP_H,  "TEMP_H" },
+		{ ADDR_INT_CTRL_M,  "INT_CTRL_M" },
+		{ ADDR_INT_SRC_M,   "INT_SRC_M" },
+		{ ADDR_REFERENCE_X, "REFERENCE_X" },
+		{ ADDR_REFERENCE_Y, "REFERENCE_Y" },
+		{ ADDR_REFERENCE_Z, "REFERENCE_Z" },
+		{ ADDR_OUT_X_L_A,   "ACCEL_XL" },
+		{ ADDR_OUT_X_H_A,   "ACCEL_XH" },
+		{ ADDR_OUT_Y_L_A,   "ACCEL_YL" },
+		{ ADDR_OUT_Y_H_A,   "ACCEL_YH" },
+		{ ADDR_OUT_Z_L_A,   "ACCEL_ZL" },
+		{ ADDR_OUT_Z_H_A,   "ACCEL_ZH" },
+		{ ADDR_FIFO_CTRL,   "FIFO_CTRL" },
+		{ ADDR_FIFO_SRC,    "FIFO_SRC" },
+		{ ADDR_IG_CFG1,     "IG_CFG1" },
+		{ ADDR_IG_SRC1,     "IG_SRC1" },
+		{ ADDR_IG_THS1,     "IG_THS1" },
+		{ ADDR_IG_DUR1,     "IG_DUR1" },
+		{ ADDR_IG_CFG2,     "IG_CFG2" },
+		{ ADDR_IG_SRC2,     "IG_SRC2" },
+		{ ADDR_IG_THS2,     "IG_THS2" },
+		{ ADDR_IG_DUR2,     "IG_DUR2" },
+		{ ADDR_CLICK_CFG,   "CLICK_CFG" },
+		{ ADDR_CLICK_SRC,   "CLICK_SRC" },
+		{ ADDR_CLICK_THS,   "CLICK_THS" },
+		{ ADDR_TIME_LIMIT,  "TIME_LIMIT" },
+		{ ADDR_TIME_LATENCY,"TIME_LATENCY" },
+		{ ADDR_TIME_WINDOW, "TIME_WINDOW" },
+		{ ADDR_ACT_THS,     "ACT_THS" },
+		{ ADDR_ACT_DUR,     "ACT_DUR" }
+	};
+	for (uint8_t i=0; i<sizeof(regmap)/sizeof(regmap[0]); i++) {
+		printf("0x%02x %s\n", read_reg(regmap[i].reg), regmap[i].name);
+	}
+	printf("_reg1_expected=0x%02x\n", _reg1_expected);
+	printf("_reg7_expected=0x%02x\n", _reg7_expected);
+}
+
+void
+LSM303D::toggle_logging()
+{
+	if (! _accel_logging_enabled) {
+		_accel_logging_enabled = true;
+		printf("Started logging to %s\n", ACCEL_LOGFILE);
+	} else {
+		_accel_logging_enabled = false;
+		printf("Stopped logging\n");
+	}
+}
+
+LSM303D_mag::LSM303D_mag(LSM303D *parent) :
+	CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG),
+	_parent(parent),
+	_mag_topic(-1),
+	_mag_class_instance(-1)
+{
+}
+
+LSM303D_mag::~LSM303D_mag()
+{
+	if (_mag_class_instance != -1)
+		unregister_class_devname(MAG_DEVICE_PATH, _mag_class_instance);
+}
+
+int
+LSM303D_mag::init()
+{
+	int ret;
+
+	ret = CDev::init();
+	if (ret != OK)
+		goto out;
+
+	_mag_class_instance = register_class_devname(MAG_DEVICE_PATH);
+
+out:
+	return ret;
+}
+
+void
+LSM303D_mag::parent_poll_notify()
+{
+	poll_notify(POLLIN);
+}
+
+ssize_t
+LSM303D_mag::read(struct file *filp, char *buffer, size_t buflen)
+{
+	return _parent->mag_read(filp, buffer, buflen);
+}
+
+int
+LSM303D_mag::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	return _parent->mag_ioctl(filp, cmd, arg);
+}
+
+void
+LSM303D_mag::measure()
+{
+	_parent->mag_measure();
+}
+
+void
+LSM303D_mag::measure_trampoline(void *arg)
+{
+	_parent->mag_measure_trampoline(arg);
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace lsm303d
+{
+
+LSM303D	*g_dev;
+
+void	start();
+void	test();
+void	reset();
+void	info();
+void	regdump();
+void	logging();
+
+/**
+ * Start the driver.
+ */
+void
+start()
+{
+	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 (g_dev == nullptr) {
+		warnx("failed instantiating LSM303D obj");
+		goto fail;
+	}
+
+	if (OK != g_dev->init())
+		goto fail;
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY);
+
+	if (fd < 0)
+		goto fail;
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		goto fail;
+
+	fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY);
+
+	/* don't fail if open cannot be opened */
+	if (0 <= fd_mag) {
+		if (ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+			goto fail;
+		}
+	}
+
+        close(fd);
+        close(fd_mag);
+
+	exit(0);
+fail:
+
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+	int fd_accel = -1;
+	struct accel_report accel_report;
+	ssize_t sz;
+	int ret;
+
+	/* get the driver */
+	fd_accel = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY);
+
+	if (fd_accel < 0)
+		err(1, "%s open failed", LSM303D_DEVICE_PATH_ACCEL);
+
+	/* do a simple demand read */
+	sz = read(fd_accel, &accel_report, sizeof(accel_report));
+
+	if (sz != sizeof(accel_report))
+		err(1, "immediate read failed");
+
+
+	warnx("accel x: \t% 9.5f\tm/s^2", (double)accel_report.x);
+	warnx("accel y: \t% 9.5f\tm/s^2", (double)accel_report.y);
+	warnx("accel z: \t% 9.5f\tm/s^2", (double)accel_report.z);
+	warnx("accel x: \t%d\traw", (int)accel_report.x_raw);
+	warnx("accel y: \t%d\traw", (int)accel_report.y_raw);
+	warnx("accel z: \t%d\traw", (int)accel_report.z_raw);
+
+	warnx("accel range: %8.4f m/s^2", (double)accel_report.range_m_s2);
+	if (ERROR == (ret = ioctl(fd_accel, ACCELIOCGLOWPASS, 0)))
+		warnx("accel antialias filter bandwidth: fail");
+	else
+		warnx("accel antialias filter bandwidth: %d Hz", ret);
+
+	int fd_mag = -1;
+	struct mag_report m_report;
+
+	/* get the driver */
+	fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY);
+
+	if (fd_mag < 0)
+		err(1, "%s open failed", LSM303D_DEVICE_PATH_MAG);
+
+	/* check if mag is onboard or external */
+	if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0)
+		errx(1, "failed to get if mag is onboard or external");
+	warnx("mag device active: %s", ret ? "external" : "onboard");
+
+	/* do a simple demand read */
+	sz = read(fd_mag, &m_report, sizeof(m_report));
+
+	if (sz != sizeof(m_report))
+		err(1, "immediate read failed");
+
+	warnx("mag x: \t% 9.5f\tga", (double)m_report.x);
+	warnx("mag y: \t% 9.5f\tga", (double)m_report.y);
+	warnx("mag z: \t% 9.5f\tga", (double)m_report.z);
+	warnx("mag x: \t%d\traw", (int)m_report.x_raw);
+	warnx("mag y: \t%d\traw", (int)m_report.y_raw);
+	warnx("mag z: \t%d\traw", (int)m_report.z_raw);
+	warnx("mag range: %8.4f ga", (double)m_report.range_ga);
+
+	/* XXX add poll-rate tests here too */
+
+        close(fd_accel);
+        close(fd_mag);
+
+	reset();
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(LSM303D_DEVICE_PATH_ACCEL, 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, "accel pollrate reset failed");
+
+        close(fd);
+
+	fd = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY);
+
+	if (fd < 0) {
+		warnx("mag could not be opened, external mag might be used");
+	} else {
+		/* no need to reset the mag as well, the reset() is the same */
+		if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+			err(1, "mag pollrate reset failed");
+	}
+
+        close(fd);
+
+	exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running\n");
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	exit(0);
+}
+
+/**
+ * dump registers from device
+ */
+void
+regdump()
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running\n");
+
+	printf("regdump @ %p\n", g_dev);
+	g_dev->print_registers();
+
+	exit(0);
+}
+
+/**
+ * toggle logging
+ */
+void
+logging()
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running\n");
+
+	g_dev->toggle_logging();
+
+	exit(0);
+}
+
+
+} // namespace
+
+int
+lsm303d_main(int argc, char *argv[])
+{
+	/*
+	 * Start/load the driver.
+
+	 */
+	if (!strcmp(argv[1], "start"))
+		lsm303d::start();
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test"))
+		lsm303d::test();
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset"))
+		lsm303d::reset();
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info"))
+		lsm303d::info();
+
+	/*
+	 * dump device registers
+	 */
+	if (!strcmp(argv[1], "regdump"))
+		lsm303d::regdump();
+
+	/*
+	 * dump device registers
+	 */
+	if (!strcmp(argv[1], "logging"))
+		lsm303d::logging();
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'logging' or 'regdump'");
+}
diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk
new file mode 100644
index 000000000..e40f718c5
--- /dev/null
+++ b/src/drivers/lsm303d/module.mk
@@ -0,0 +1,8 @@
+#
+# LSM303D accel/mag driver
+#
+
+MODULE_COMMAND	 = lsm303d
+SRCS		 = lsm303d.cpp
+
+
diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp
index 397686e8b..c910e2890 100644
--- a/src/drivers/mb12xx/mb12xx.cpp
+++ b/src/drivers/mb12xx/mb12xx.cpp
@@ -59,17 +59,18 @@
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/board.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 MB12XX_BUS 			PX4_I2C_BUS_EXPANSION
 #define MB12XX_BASEADDR 	0x70 /* 7-bit address. 8-bit address is 0xE0 */
@@ -119,10 +120,7 @@ private:
 	float				_min_distance;
 	float				_max_distance;
 	work_s				_work;
-	unsigned			_num_reports;
-	volatile unsigned	_next_report;
-	volatile unsigned	_oldest_report;
-	range_finder_report	*_reports;
+	RingBuffer		*_reports;
 	bool				_sensor_ok;
 	int					_measure_ticks;
 	bool				_collect_phase;
@@ -183,9 +181,6 @@ private:
 	
 };
 
-/* helper macro for handling report buffer indices */
-#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
-
 /*
  * Driver 'main' command.
  */
@@ -195,9 +190,6 @@ MB12XX::MB12XX(int bus, int address) :
 	I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000),
 	_min_distance(MB12XX_MIN_DISTANCE),
 	_max_distance(MB12XX_MAX_DISTANCE),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
 	_reports(nullptr),
 	_sensor_ok(false),
 	_measure_ticks(0),
@@ -221,7 +213,7 @@ MB12XX::~MB12XX()
 
 	/* free any existing reports */
 	if (_reports != nullptr)
-		delete[] _reports;
+		delete _reports;
 }
 
 int
@@ -234,17 +226,15 @@ MB12XX::init()
 		goto out;
 
 	/* allocate basic report buffers */
-	_num_reports = 2;
-	_reports = new struct range_finder_report[_num_reports];
+	_reports = new RingBuffer(2, sizeof(range_finder_report));
 
 	if (_reports == nullptr)
 		goto out;
 
-	_oldest_report = _next_report = 0;
-
 	/* get a publish handle on the range finder topic */
-	memset(&_reports[0], 0, sizeof(_reports[0]));
-	_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &_reports[0]);
+	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)
 		debug("failed to create sensor_range_finder object. Did you start uOrb?");
@@ -354,31 +344,22 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return (1000 / _measure_ticks);
 
 	case SENSORIOCSQUEUEDEPTH: {
-			/* add one to account for the sentinel in the ring */
-			arg++;
-
-			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 2) || (arg > 100))
-				return -EINVAL;
-
-			/* allocate new buffer */
-			struct range_finder_report *buf = new struct range_finder_report[arg];
-
-			if (nullptr == buf)
-				return -ENOMEM;
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete[] _reports;
-			_num_reports = arg;
-			_reports = buf;
-			start();
-
-			return OK;
+		/* 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 _num_reports - 1;
+		return _reports->size();
 		
 	case SENSORIOCRESET:
 		/* XXX implement this */
@@ -406,6 +387,7 @@ ssize_t
 MB12XX::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 */
@@ -421,10 +403,9 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
 		 * we are careful to avoid racing with them.
 		 */
 		while (count--) {
-			if (_oldest_report != _next_report) {
-				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
-				ret += sizeof(_reports[0]);
-				INCREMENT(_oldest_report, _num_reports);
+			if (_reports->get(rbuf)) {
+				ret += sizeof(*rbuf);
+				rbuf++;
 			}
 		}
 
@@ -433,9 +414,8 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
 	}
 
 	/* manual measurement - run one conversion */
-	/* XXX really it'd be nice to lock against other readers here */
 	do {
-		_oldest_report = _next_report = 0;
+		_reports->flush();
 
 		/* trigger a measurement */
 		if (OK != measure()) {
@@ -453,8 +433,9 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
 		}
 
 		/* state machine will have generated a report, copy it out */
-		memcpy(buffer, _reports, sizeof(*_reports));
-		ret = sizeof(*_reports);
+		if (_reports->get(rbuf)) {
+			ret = sizeof(*rbuf);
+		}
 
 	} while (0);
 
@@ -498,26 +479,26 @@ MB12XX::collect()
 	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 */
+	struct range_finder_report report;
+
 	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
-	_reports[_next_report].timestamp = hrt_absolute_time();
-	_reports[_next_report].distance = si_units;
-	_reports[_next_report].valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
+	report.timestamp = hrt_absolute_time();
+        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, &_reports[_next_report]);
-
-	/* post a report to the ring - note, not locked */
-	INCREMENT(_next_report, _num_reports);
+	orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
 
-	/* if we are running up against the oldest report, toss it */
-	if (_next_report == _oldest_report) {
+	if (_reports->force(&report)) {
 		perf_count(_buffer_overflows);
-		INCREMENT(_oldest_report, _num_reports);
 	}
 
 	/* notify anyone waiting for data */
@@ -525,11 +506,8 @@ MB12XX::collect()
 
 	ret = OK;
 
-out:
 	perf_end(_sample_perf);
 	return ret;
-	
-	return ret;
 }
 
 void
@@ -537,7 +515,7 @@ MB12XX::start()
 {
 	/* reset the report ring and state machine */
 	_collect_phase = false;
-	_oldest_report = _next_report = 0;
+	_reports->flush();
 
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1);
@@ -626,8 +604,7 @@ MB12XX::print_info()
 	perf_print_counter(_comms_errors);
 	perf_print_counter(_buffer_overflows);
 	printf("poll interval:  %u ticks\n", _measure_ticks);
-	printf("report queue:   %u (%u/%u @ %p)\n",
-	       _num_reports, _oldest_report, _next_report, _reports);
+	_reports->print_info("report queue");
 }
 
 /**
diff --git a/src/drivers/md25/BlockSysIdent.cpp b/src/drivers/md25/BlockSysIdent.cpp
new file mode 100644
index 000000000..23b0724d8
--- /dev/null
+++ b/src/drivers/md25/BlockSysIdent.cpp
@@ -0,0 +1,8 @@
+#include "BlockSysIdent.hpp"
+
+BlockSysIdent::BlockSysIdent() :
+	Block(NULL, "SYSID"),
+	_freq(this, "FREQ"),
+	_ampl(this, "AMPL")
+{
+}
diff --git a/src/drivers/md25/BlockSysIdent.hpp b/src/drivers/md25/BlockSysIdent.hpp
new file mode 100644
index 000000000..270635f40
--- /dev/null
+++ b/src/drivers/md25/BlockSysIdent.hpp
@@ -0,0 +1,10 @@
+#include <controllib/block/Block.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+class BlockSysIdent : public control::Block {
+public:
+	BlockSysIdent();
+private:
+	control::BlockParam<float> _freq;
+	control::BlockParam<float> _ampl;
+};
diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp
index 71932ad65..d43e3aef9 100644
--- a/src/drivers/md25/md25.cpp
+++ b/src/drivers/md25/md25.cpp
@@ -45,9 +45,16 @@
 #include "md25.hpp"
 #include <poll.h>
 #include <stdio.h>
+#include <math.h>
+#include <string.h>
 
 #include <systemlib/err.h>
 #include <arch/board/board.h>
+#include <mavlink/mavlink_log.h>
+
+#include <controllib/uorb/UOrbPublication.hpp>
+#include <uORB/topics/debug_key_value.h>
+#include <drivers/drv_hrt.h>
 
 // registers
 enum {
@@ -72,6 +79,9 @@ enum {
 	REG_COMMAND_RW,
 };
 
+// File descriptors
+static int mavlink_fd;
+
 MD25::MD25(const char *deviceName, int bus,
 	   uint16_t address, uint32_t speed) :
 	I2C("MD25", deviceName, bus, address, speed),
@@ -106,7 +116,8 @@ MD25::MD25(const char *deviceName, int bus,
 	setMotor2Speed(0);
 	resetEncoders();
 	_setMode(MD25::MODE_UNSIGNED_SPEED);
-	setSpeedRegulation(true);
+	setSpeedRegulation(false);
+	setMotorAccel(10);
 	setTimeout(true);
 }
 
@@ -298,6 +309,12 @@ int MD25::setDeviceAddress(uint8_t address)
 	return OK;
 }
 
+int MD25::setMotorAccel(uint8_t accel)
+{
+	return _writeUint8(REG_ACCEL_RATE_RW,
+			   accel);
+}
+
 int MD25::setMotor1Speed(float value)
 {
 	return _writeUint8(REG_SPEED1_RW,
@@ -451,12 +468,12 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address)
 	MD25 md25("/dev/md25", bus, address);
 
 	// print status
-	char buf[200];
+	char buf[400];
 	md25.status(buf, sizeof(buf));
 	printf("%s\n", buf);
 
 	// setup for test
-	md25.setSpeedRegulation(true);
+	md25.setSpeedRegulation(false);
 	md25.setTimeout(true);
 	float dt = 0.1;
 	float speed = 0.2;
@@ -550,4 +567,68 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address)
 	return 0;
 }
 
+int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitude, float frequency)
+{
+	printf("md25 sine: starting\n");
+
+	// setup
+	MD25 md25("/dev/md25", bus, address);
+
+	// print status
+	char buf[400];
+	md25.status(buf, sizeof(buf));
+	printf("%s\n", buf);
+
+	// setup for test
+	md25.setSpeedRegulation(false);
+	md25.setTimeout(true);
+	float dt = 0.01;
+	float t_final = 60.0;
+	float prev_revolution = md25.getRevolutions1();
+
+	// debug publication
+	control::UOrbPublication<debug_key_value_s> debug_msg(NULL,
+			ORB_ID(debug_key_value));
+
+	// sine wave for motor 1
+	md25.resetEncoders();
+	while (true) {
+
+		// input
+		uint64_t timestamp = hrt_absolute_time();
+		float t = timestamp/1000000.0f;
+
+		float input_value = amplitude*sinf(2*M_PI*frequency*t);
+		md25.setMotor1Speed(input_value);
+
+		// output
+		md25.readData();
+		float current_revolution = md25.getRevolutions1();
+
+		// send input message
+		//strncpy(debug_msg.key, "md25 in   ", 10);
+		//debug_msg.timestamp_ms = 1000*timestamp;
+		//debug_msg.value = input_value;
+		//debug_msg.update();
+
+		// send output message
+		strncpy(debug_msg.key, "md25 out  ", 10);
+		debug_msg.timestamp_ms = 1000*timestamp;
+		debug_msg.value = current_revolution;
+		debug_msg.update();
+
+		if (t > t_final) break;
+
+		// update for next step
+		prev_revolution = current_revolution;
+
+		// sleep
+		usleep(1000000 * dt);
+	}
+	md25.setMotor1Speed(0);
+
+	printf("md25 sine complete\n");
+	return 0;
+}
+
 // vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
diff --git a/src/drivers/md25/md25.hpp b/src/drivers/md25/md25.hpp
index e77511b16..1661f67f9 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/block/UOrbSubscription.hpp>
+#include <controllib/uorb/UOrbSubscription.hpp>
 #include <uORB/topics/actuator_controls.h>
 #include <drivers/device/i2c.h>
 
@@ -213,6 +213,19 @@ public:
 	int setDeviceAddress(uint8_t address);
 
 	/**
+	 * set motor acceleration
+	 * @param accel
+	 * controls motor speed change (1-10)
+	 * accel rate  | time for full fwd. to full rev.
+	 *  1          | 6.375 s
+	 *  2          | 1.6 s
+	 *  3          | 0.675 s
+	 *  5(default) | 1.275 s
+	 * 10          | 0.65 s
+	 */
+	int setMotorAccel(uint8_t accel);
+
+	/**
 	 * set motor 1 speed
 	 * @param normSpeed normalize speed between -1 and 1
 	 * @return non-zero -> error
@@ -290,4 +303,7 @@ private:
 // unit testing
 int md25Test(const char *deviceName, uint8_t bus, uint8_t address);
 
+// sine testing
+int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitude, float frequency);
+
 // vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp
index e62c46b0d..7e5904d05 100644
--- a/src/drivers/md25/md25_main.cpp
+++ b/src/drivers/md25/md25_main.cpp
@@ -82,7 +82,7 @@ usage(const char *reason)
 	if (reason)
 		fprintf(stderr, "%s\n", reason);
 
-	fprintf(stderr, "usage: md25 {start|stop|status|search|test|change_address}\n\n");
+	fprintf(stderr, "usage: md25 {start|stop|read|status|search|test|change_address}\n\n");
 	exit(1);
 }
 
@@ -136,6 +136,28 @@ int md25_main(int argc, char *argv[])
 		exit(0);
 	}
 
+	if (!strcmp(argv[1], "sine")) {
+
+		if (argc < 6) {
+			printf("usage: md25 sine bus address amp freq\n");
+			exit(0);
+		}
+
+		const char *deviceName = "/dev/md25";
+
+		uint8_t bus = strtoul(argv[2], nullptr, 0);
+
+		uint8_t address = strtoul(argv[3], nullptr, 0);
+
+		float amplitude = atof(argv[4]);
+
+		float frequency = atof(argv[5]);
+
+		md25Sine(deviceName, bus, address, amplitude, frequency);
+
+		exit(0);
+	}
+
 	if (!strcmp(argv[1], "probe")) {
 		if (argc < 4) {
 			printf("usage: md25 probe bus address\n");
@@ -162,6 +184,29 @@ int md25_main(int argc, char *argv[])
 		exit(0);
 	}
 
+	if (!strcmp(argv[1], "read")) {
+		if (argc < 4) {
+			printf("usage: md25 read bus address\n");
+			exit(0);
+		}
+
+		const char *deviceName = "/dev/md25";
+
+		uint8_t bus = strtoul(argv[2], nullptr, 0);
+
+		uint8_t address = strtoul(argv[3], nullptr, 0);
+
+		MD25 md25(deviceName, bus, address);
+
+		// print status
+		char buf[400];
+		md25.status(buf, sizeof(buf));
+		printf("%s\n", buf);
+
+		exit(0);
+	}
+	
+
 	if (!strcmp(argv[1], "search")) {
 		if (argc < 3) {
 			printf("usage: md25 search bus\n");
@@ -246,7 +291,7 @@ int md25_thread_main(int argc, char *argv[])
 	uint8_t address = strtoul(argv[4], nullptr, 0);
 
 	// start
-	MD25 md25("/dev/md25", bus, address);
+	MD25 md25(deviceName, bus, address);
 
 	thread_running = true;
 
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
new file mode 100644
index 000000000..05ae21c1f
--- /dev/null
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -0,0 +1,520 @@
+/****************************************************************************
+ *
+ *   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 meas_airspeed.cpp
+ * @author Lorenz Meier
+ * @author Sarthak Kaingade
+ * @author Simon Wilks
+ * @author Thomas Gubler
+ *
+ * Driver for the MEAS Spec series connected via I2C.
+ *
+ * Supported sensors:
+ *
+ *    - MS4525DO (http://www.meas-spec.com/downloads/MS4525DO.pdf)
+ *    - untested: MS5525DSO (http://www.meas-spec.com/downloads/MS5525DSO.pdf)
+ *
+ * Interface application notes:
+ *
+ *    - 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>
+
+#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 <board_config.h>
+
+#include <systemlib/airspeed.h>
+#include <systemlib/err.h>
+#include <systemlib/param/param.h>
+#include <systemlib/perf_counter.h>
+
+#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 <drivers/airspeed/airspeed.h>
+
+/* I2C bus address is 1010001x */
+#define I2C_ADDRESS_MS4525DO	0x28	//0x51 /* 7-bit address. */
+/* 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 */
+
+/* Register address */
+#define ADDR_READ_MR			0x00	/* write to this address to start conversion */
+
+/* Measurement rate is 100Hz */
+#define CONVERSION_INTERVAL	(1000000 / 100)	/* microseconds */
+
+class MEASAirspeed : public Airspeed
+{
+public:
+	MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO);
+
+protected:
+
+	/**
+	* Perform a poll cycle; collect from the previous measurement
+	* and start a new one.
+	*/
+	virtual void	cycle();
+	virtual int	measure();
+	virtual int	collect();
+
+};
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]);
+
+MEASAirspeed::MEASAirspeed(int bus, int address) : Airspeed(bus, address,
+			CONVERSION_INTERVAL)
+{
+
+}
+
+int
+MEASAirspeed::measure()
+{
+	int ret;
+
+	/*
+	 * Send the command to begin a measurement.
+	 */
+	uint8_t cmd = 0;
+	ret = transfer(&cmd, 1, nullptr, 0);
+
+	if (OK != ret) {
+		perf_count(_comms_errors);
+	}
+
+	return ret;
+}
+
+int
+MEASAirspeed::collect()
+{
+	int	ret = -EIO;
+
+	/* read from the sensor */
+	uint8_t val[4] = {0, 0, 0, 0};
+
+
+	perf_begin(_sample_perf);
+
+	ret = transfer(nullptr, 0, &val[0], 4);
+
+	if (ret < 0) {
+                perf_count(_comms_errors);
+                perf_end(_sample_perf);
+		return ret;
+	}
+
+	uint8_t status = val[0] & 0xC0;
+
+	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;                
+	}
+
+	int16_t dp_raw = 0, dT_raw = 0;
+	dp_raw = (val[0] << 8) + val[1];
+	/* mask the used bits */
+	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;
+
+	/* calculate differential pressure. As its centered around 8000
+	 * and can go positive or negative, enforce absolute value
+	*/
+	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;
+
+	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;
+	}
+
+	report.timestamp = hrt_absolute_time();
+        report.error_count = perf_event_count(_comms_errors);
+	report.temperature = temperature;
+	report.differential_pressure_pa = diff_press_pa;
+	report.voltage = 0;
+	report.max_differential_pressure_pa = _max_differential_pressure_pa;
+
+	if (_airspeed_pub > 0 && !(_pub_blocked)) {
+		/* publish it */
+		orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
+	}
+
+	new_report(report);
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	ret = OK;
+
+	perf_end(_sample_perf);
+
+	return ret;
+}
+
+void
+MEASAirspeed::cycle()
+{
+	/* collection phase? */
+	if (_collect_phase) {
+
+		/* perform collection */
+		if (OK != collect()) {
+			/* restart the measurement state machine */
+			start();
+			return;
+		}
+
+		/* next phase is measurement */
+		_collect_phase = false;
+
+		/*
+		 * Is there a collect->measure gap?
+		 */
+		if (_measure_ticks > USEC2TICK(CONVERSION_INTERVAL)) {
+
+			/* schedule a fresh cycle call when we are ready to measure again */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&Airspeed::cycle_trampoline,
+				   this,
+				   _measure_ticks - USEC2TICK(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)&Airspeed::cycle_trampoline,
+		   this,
+		   USEC2TICK(CONVERSION_INTERVAL));
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace meas_airspeed
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+const int ERROR = -1;
+
+MEASAirspeed	*g_dev = nullptr;
+
+void	start(int i2c_bus);
+void	stop();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start(int i2c_bus)
+{
+	int fd;
+
+	if (g_dev != nullptr)
+		errx(1, "already started");
+
+	/* create the driver, try the MS4525DO first */
+	g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO);
+
+	/* check if the MS4525DO was instantiated */
+	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);
+
+		/* check if the MS5525DSO was instantiated */
+		if (g_dev == nullptr)
+			goto fail;
+
+		/* both versions failed if the init for the MS5525DSO fails, give up */
+		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)
+		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 differential_pressure_s report;
+	ssize_t sz;
+	int ret;
+
+	int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
+
+	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))
+		err(1, "immediate read failed");
+
+	warnx("single read");
+	warnx("diff pressure: %d pa", (double)report.differential_pressure_pa);
+
+	/* 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("diff pressure: %d pa", 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))
+		errx(1, "failed to set default rate");
+
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(AIRSPEED_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
+
+
+static void
+meas_airspeed_usage()
+{
+	warnx("usage: meas_airspeed command [options]");
+	warnx("options:");
+	warnx("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
+	warnx("command:");
+	warnx("\tstart|stop|reset|test|info");
+}
+
+int
+meas_airspeed_main(int argc, char *argv[])
+{
+	int i2c_bus = PX4_I2C_BUS_DEFAULT;
+
+	int i;
+
+	for (i = 1; i < argc; i++) {
+		if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) {
+			if (argc > i + 1) {
+				i2c_bus = atoi(argv[i + 1]);
+			}
+		}
+	}
+
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[1], "start"))
+		meas_airspeed::start(i2c_bus);
+
+	/*
+	 * Stop the driver
+	 */
+	if (!strcmp(argv[1], "stop"))
+		meas_airspeed::stop();
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test"))
+		meas_airspeed::test();
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset"))
+		meas_airspeed::reset();
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
+		meas_airspeed::info();
+
+	meas_airspeed_usage();
+	exit(0);
+}
diff --git a/src/drivers/meas_airspeed/module.mk b/src/drivers/meas_airspeed/module.mk
new file mode 100644
index 000000000..fed4078b6
--- /dev/null
+++ b/src/drivers/meas_airspeed/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.
+#
+############################################################################
+
+#
+# Makefile to build the MEAS Spec airspeed sensor driver.
+#
+
+MODULE_COMMAND		= meas_airspeed
+MODULE_STACKSIZE	= 2048
+
+SRCS			= meas_airspeed.cpp
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index b54b7aba1..705e98eea 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -1,6 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012-2014 PX4 Development Team. All rights reserved.
+ *   Author: Marco Bauer <marco@wtns.de>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,7 +36,8 @@
  * @file mkblctrl.cpp
  *
  * Driver/configurator for the Mikrokopter BL-Ctrl.
- * Marco Bauer
+ *
+ * @author Marco Bauer <marco@wtns.de>
  *
  */
 
@@ -59,10 +61,10 @@
 #include <nuttx/arch.h>
 #include <nuttx/i2c.h>
 
+#include <board_config.h>
+
 #include <drivers/device/device.h>
 #include <drivers/drv_pwm_output.h>
-#include <drivers/drv_gpio.h>
-#include <drivers/boards/px4fmu/px4fmu_internal.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_rc_input.h>
 
@@ -72,8 +74,9 @@
 #include <drivers/drv_mixer.h>
 
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/esc_status.h>
 
 #include <systemlib/err.h>
 
@@ -86,18 +89,15 @@
 #define BLCTRL_MIN_VALUE				-0.920F
 #define MOTOR_STATE_PRESENT_MASK		0x80
 #define MOTOR_STATE_ERROR_MASK		0x7F
-#define MOTOR_SPINUP_COUNTER			2500
+#define MOTOR_SPINUP_COUNTER			30
+#define ESC_UORB_PUBLISH_DELAY		500000
+
+
 
 
 class MK : public device::I2C
 {
 public:
-	enum Mode {
-		MODE_2PWM,
-		MODE_4PWM,
-		MODE_NONE
-	};
-
 	enum MappingMode {
 		MAPPING_MK = 0,
 		MAPPING_PX4,
@@ -108,39 +108,42 @@ public:
 		FRAME_X,
 	};
 
-	MK(int bus);
+	MK(int bus, const char *_device_path);
 	~MK();
 
 	virtual int	ioctl(file *filp, int cmd, unsigned long arg);
 	virtual int	init(unsigned motors);
 	virtual ssize_t	write(file *filp, const char *buffer, size_t len);
 
-	int		set_mode(Mode mode);
-	int		set_pwm_rate(unsigned rate);
+	int		set_update_rate(unsigned rate);
 	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);
-	unsigned int		mk_check_for_blctrl(unsigned int count, bool showOutput);
+	unsigned int		mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C);
 
 private:
 	static const unsigned _max_actuators = MAX_MOTORS;
 	static const bool	showDebug = false;
 
-	Mode		_mode;
 	int 		_update_rate;
 	int 		_current_update_rate;
 	int		_task;
 	int		_t_actuators;
-	int		_t_armed;
+	int		_t_actuator_armed;
 	unsigned int		_motor;
 	int    _px4mode;
 	int    _frametype;
+	char				_device[20];					///< device
+
 	orb_advert_t	_t_outputs;
-	orb_advert_t	_t_actuators_effective;
+	orb_advert_t	_t_esc_status;
+
 	unsigned int	_num_outputs;
 	bool		_primary_pwm_device;
 	bool     _motortest;
+	bool 		_overrideSecurityChecks;
 
 	volatile bool	_task_should_exit;
 	bool		_armed;
@@ -170,33 +173,15 @@ private:
 	static const GPIOConfig	_gpio_tab[];
 	static const unsigned	_ngpio;
 
-	void		gpio_reset(void);
-	void		gpio_set_function(uint32_t gpios, int function);
-	void		gpio_write(uint32_t gpios, int function);
-	uint32_t	gpio_read(void);
-	int		gpio_ioctl(file *filp, int cmd, unsigned long arg);
 	int			mk_servo_arm(bool status);
-
 	int 		mk_servo_set(unsigned int chan, short val);
 	int 		mk_servo_set_value(unsigned int chan, short val);
 	int 		mk_servo_test(unsigned int chan);
 	short		scaling(float val, float inMin, float inMax, float outMin, float outMax);
-
-
 };
 
-const MK::GPIOConfig MK::_gpio_tab[] = {
-	{GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
-	{GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
-	{GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1},
-	{GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, GPIO_USART2_RTS_1},
-	{GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, GPIO_USART2_TX_1},
-	{GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1},
-	{GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2},
-	{GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2},
-};
 
-const unsigned MK::_ngpio = sizeof(MK::_gpio_tab) / sizeof(MK::_gpio_tab[0]);
+
 
 const int blctrlAddr_quad_plus[] = { 2, 2, -2, -2, 0, 0, 0, 0 };	// Addresstranslator for Quad + configuration
 const int blctrlAddr_hexa_plus[] = { 0, 2, 2, -2, 1, -3, 0, 0 };	// Addresstranslator for Hexa + configuration
@@ -214,6 +199,7 @@ 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
@@ -234,17 +220,17 @@ MK	*g_mk;
 
 } // namespace
 
-MK::MK(int bus) :
+MK::MK(int bus, const char *_device_path) :
 	I2C("mkblctrl", "/dev/mkblctrl", bus, 0, I2C_BUS_SPEED),
-	_mode(MODE_NONE),
-	_update_rate(50),
+	_update_rate(400),
 	_task(-1),
 	_t_actuators(-1),
-	_t_armed(-1),
+	_t_actuator_armed(-1),
 	_t_outputs(0),
-	_t_actuators_effective(0),
+	_t_esc_status(0),
 	_num_outputs(0),
 	_motortest(false),
+	_overrideSecurityChecks(false),
 	_motor(-1),
 	_px4mode(MAPPING_MK),
 	_frametype(FRAME_PLUS),
@@ -253,6 +239,10 @@ MK::MK(int bus) :
 	_armed(false),
 	_mixers(nullptr)
 {
+	strncpy(_device, _device_path, sizeof(_device));
+	/* enforce null termination */
+	_device[sizeof(_device) - 1] = '\0';
+
 	_debug_enabled = true;
 }
 
@@ -279,7 +269,7 @@ MK::~MK()
 
 	/* clean up the alternate device node */
 	if (_primary_pwm_device)
-		unregister_driver(PWM_OUTPUT_DEVICE_PATH);
+		unregister_driver(_device);
 
 	g_mk = nullptr;
 }
@@ -301,24 +291,23 @@ MK::init(unsigned motors)
 
 	usleep(500000);
 
-	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
-	ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
+	if (sizeof(_device) > 0) {
+		ret = register_driver(_device, &fops, 0666, (void *)this);
 
-	if (ret == OK) {
-		log("default PWM output device");
-		_primary_pwm_device = true;
-	}
+		if (ret == OK) {
+			log("creating alternate output device");
+			_primary_pwm_device = true;
+		}
 
-	/* reset GPIOs */
-	gpio_reset();
+	}
 
 	/* start the IO interface task */
 	_task = task_spawn_cmd("mkblctrl",
-			   SCHED_DEFAULT,
-			   SCHED_PRIORITY_MAX - 20,
-			   2048,
-			   (main_t)&MK::task_main_trampoline,
-			   nullptr);
+			       SCHED_DEFAULT,
+			       SCHED_PRIORITY_MAX - 20,
+			       2048,
+			       (main_t)&MK::task_main_trampoline,
+			       nullptr);
 
 
 	if (_task < 0) {
@@ -336,43 +325,7 @@ MK::task_main_trampoline(int argc, char *argv[])
 }
 
 int
-MK::set_mode(Mode mode)
-{
-	/*
-	 * Configure for PWM output.
-	 *
-	 * Note that regardless of the configured mode, the task is always
-	 * listening and mixing; the mode just selects which of the channels
-	 * are presented on the output pins.
-	 */
-	switch (mode) {
-	case MODE_2PWM:
-		up_pwm_servo_deinit();
-		_update_rate = UPDATE_RATE;	/* default output rate */
-		break;
-
-	case MODE_4PWM:
-		up_pwm_servo_deinit();
-		_update_rate = UPDATE_RATE;	/* default output rate */
-		break;
-
-	case MODE_NONE:
-		debug("MODE_NONE");
-		/* disable servo outputs and set a very low update rate */
-		up_pwm_servo_deinit();
-		_update_rate = UPDATE_RATE;
-		break;
-
-	default:
-		return -EINVAL;
-	}
-
-	_mode = mode;
-	return OK;
-}
-
-int
-MK::set_pwm_rate(unsigned rate)
+MK::set_update_rate(unsigned rate)
 {
 	if ((rate > 500) || (rate < 10))
 		return -EINVAL;
@@ -464,6 +417,13 @@ MK::set_motor_test(bool motortest)
 	return OK;
 }
 
+int
+MK::set_overrideSecurityChecks(bool overrideSecurityChecks)
+{
+	_overrideSecurityChecks = overrideSecurityChecks;
+	return OK;
+}
+
 short
 MK::scaling(float val, float inMin, float inMax, float outMin, float outMax)
 {
@@ -496,8 +456,8 @@ MK::task_main()
 	/* force a reset of the update rate */
 	_current_update_rate = 0;
 
-	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
-	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
+	_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
+	orb_set_interval(_t_actuator_armed, 200);		/* 5Hz update rate */
 
 	/* advertise the mixed control outputs */
 	actuator_outputs_s outputs;
@@ -506,20 +466,17 @@ MK::task_main()
 	_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
 				   &outputs);
 
+	/* advertise the blctrl status */
+	esc_status_s esc;
+	memset(&esc, 0, sizeof(esc));
+	_t_esc_status = orb_advertise(ORB_ID(esc_status), &esc);
 
 
-	/* advertise the effective control inputs */
-	actuator_controls_effective_s controls_effective;
-	memset(&controls_effective, 0, sizeof(controls_effective));
-	/* advertise the effective control inputs */
-	_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
-				   &controls_effective);
-
 
 	pollfd fds[2];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
-	fds[1].fd = _t_armed;
+	fds[1].fd = _t_actuator_armed;
 	fds[1].events = POLLIN;
 
 	log("starting");
@@ -572,9 +529,6 @@ MK::task_main()
 				outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs);
 				outputs.timestamp = hrt_absolute_time();
 
-				// XXX output actual limited values
-				memcpy(&controls_effective, &_controls, sizeof(controls_effective));
-
 				/* iterate actuators */
 				for (unsigned int i = 0; i < _num_outputs; i++) {
 
@@ -602,17 +556,21 @@ MK::task_main()
 						}
 					}
 
-					/* don't go under BLCTRL_MIN_VALUE */
-					if (outputs.output[i] < BLCTRL_MIN_VALUE) {
-						outputs.output[i] = BLCTRL_MIN_VALUE;
+					if (!_overrideSecurityChecks) {
+						/* don't go under BLCTRL_MIN_VALUE */
+
+						if (outputs.output[i] < BLCTRL_MIN_VALUE) {
+							outputs.output[i] = BLCTRL_MIN_VALUE;
+						}
+
 					}
 
 					/* output to BLCtrl's */
-					if (_motortest == true) {
-						mk_servo_test(i);
-
-					} else {
-						mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024));	// scale the output to 0 - 1024 and sent to output routine
+					if (_motortest != true) {
+						//mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024));	// scale the output to 0 - 1024 and sent to output routine
+						// 11 Bit
+						Motor[i].SetPoint_PX4 = outputs.output[i];
+						mk_servo_set(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 2047));	// scale the output to 0 - 2047 and sent to output routine
 					}
 
 				}
@@ -628,18 +586,62 @@ MK::task_main()
 			actuator_armed_s aa;
 
 			/* get new value */
-			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
+			orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
 
 			/* update PWM servo armed status if armed and not locked down */
 			mk_servo_arm(aa.armed && !aa.lockdown);
 		}
 
 
+
+		/*
+		 * Only update esc topic every half second.
+		 */
+
+		if (hrt_absolute_time() - esc.timestamp > ESC_UORB_PUBLISH_DELAY) {
+			esc.counter++;
+			esc.timestamp = hrt_absolute_time();
+			esc.esc_count = (uint8_t) _num_outputs;
+			esc.esc_connectiontype = ESC_CONNECTION_TYPE_I2C;
+
+			for (unsigned int i = 0; i < _num_outputs; i++) {
+				esc.esc[i].esc_address = (uint8_t) BLCTRL_BASE_ADDR + i;
+				esc.esc[i].esc_vendor = ESC_VENDOR_MIKROKOPTER;
+				esc.esc[i].esc_version = (uint16_t) Motor[i].Version;
+				esc.esc[i].esc_voltage = (uint16_t) 0;
+				esc.esc[i].esc_current = (uint16_t) Motor[i].Current;
+				esc.esc[i].esc_rpm = (uint16_t) 0;
+				esc.esc[i].esc_setpoint = (float) Motor[i].SetPoint_PX4;
+
+				if (Motor[i].Version == 1) {
+					// BLCtrl 2.0 (11Bit)
+					esc.esc[i].esc_setpoint_raw = (uint16_t)(Motor[i].SetPoint << 3) | Motor[i].SetPointLowerBits;
+
+				} else {
+					// BLCtrl < 2.0 (8Bit)
+					esc.esc[i].esc_setpoint_raw = (uint16_t) Motor[i].SetPoint;
+				}
+
+				esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature;
+				esc.esc[i].esc_state = (uint16_t) Motor[i].State;
+				esc.esc[i].esc_errorcount = (uint16_t) 0;
+
+				// if motortest is requested - do it...
+				if (_motortest == true) {
+					mk_servo_test(i);
+				}
+
+			}
+
+			orb_publish(ORB_ID(esc_status), _t_esc_status, &esc);
+
+		}
+
 	}
 
+	::close(_t_esc_status);
 	::close(_t_actuators);
-	::close(_t_actuators_effective);
-	::close(_t_armed);
+	::close(_t_actuator_armed);
 
 
 	/* make sure servos are off */
@@ -664,8 +666,12 @@ MK::mk_servo_arm(bool status)
 
 
 unsigned int
-MK::mk_check_for_blctrl(unsigned int count, bool showOutput)
+MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C)
 {
+	if (initI2C) {
+		I2C::init();
+	}
+
 	_retries = 50;
 	uint8_t foundMotorCount = 0;
 
@@ -699,7 +705,7 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput)
 			Motor[i].State |= MOTOR_STATE_PRESENT_MASK; // set present bit;
 			foundMotorCount++;
 
-			if (Motor[i].MaxPWM == 250) {
+			if ((Motor[i].MaxPWM & 252) == 248) {
 				Motor[i].Version = BLCTRL_NEW;
 
 			} else {
@@ -715,17 +721,17 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput)
 			fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i, Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature);
 		}
 
-		if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) {
-			_task_should_exit = true;
+
+		if (!_overrideSecurityChecks) {
+			if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) {
+				_task_should_exit = true;
+			}
 		}
 	}
 
 	return foundMotorCount;
 }
 
-
-
-
 int
 MK::mk_servo_set(unsigned int chan, short val)
 {
@@ -738,17 +744,15 @@ MK::mk_servo_set(unsigned int chan, short val)
 
 	tmpVal = val;
 
-	if (tmpVal > 1024) {
-		tmpVal = 1024;
+	if (tmpVal > 2047) {
+		tmpVal = 2047;
 
 	} else if (tmpVal < 0) {
 		tmpVal = 0;
 	}
 
-	Motor[chan].SetPoint = (uint8_t)(tmpVal / 4);
-	//Motor[chan].SetPointLowerBits = (uint8_t) tmpVal % 4;
-
-	Motor[chan].SetPointLowerBits = 0;
+	Motor[chan].SetPoint = (uint8_t)(tmpVal >> 3) & 0xff;
+	Motor[chan].SetPointLowerBits = ((uint8_t)tmpVal % 8) & 0x07;
 
 	if (_armed == false) {
 		Motor[chan].SetPoint = 0;
@@ -891,6 +895,7 @@ MK::mk_servo_test(unsigned int chan)
 		if (_motor >= _num_outputs) {
 			_motor = -1;
 			_motortest = false;
+			fprintf(stderr, "[mkblctrl] Motortest finished...\n");
 		}
 	}
 
@@ -954,25 +959,7 @@ MK::ioctl(file *filp, int cmd, unsigned long arg)
 {
 	int ret;
 
-	// XXX disabled, confusing users
-
-	/* try it as a GPIO ioctl first */
-	ret = gpio_ioctl(filp, cmd, arg);
-
-	if (ret != -ENOTTY)
-		return ret;
-
-	/* if we are in valid PWM mode, try it as a PWM ioctl as well */
-	switch (_mode) {
-	case MODE_2PWM:
-	case MODE_4PWM:
-		ret = pwm_ioctl(filp, cmd, arg);
-		break;
-
-	default:
-		debug("not in a PWM mode");
-		break;
-	}
+	ret = pwm_ioctl(filp, cmd, arg);
 
 	/* if nobody wants it, let CDev have it */
 	if (ret == -ENOTTY)
@@ -1006,7 +993,11 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		ret = OK;
 		break;
 
-	case PWM_SERVO_SELECT_UPDATE_RATE:
+	case PWM_SERVO_GET_UPDATE_RATE:
+		*(uint32_t *)arg = _update_rate;
+		break;
+
+	case PWM_SERVO_SET_SELECT_UPDATE_RATE:
 		ret = OK;
 		break;
 
@@ -1014,7 +1005,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
 		if (arg < 2150) {
 			Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue = (unsigned short)arg;
-			mk_servo_set_value(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 1024));
+			mk_servo_set(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 2047));
 
 		} else {
 			ret = -EINVAL;
@@ -1112,163 +1103,28 @@ ssize_t
 MK::write(file *filp, const char *buffer, size_t len)
 {
 	unsigned count = len / 2;
-	// loeschen uint16_t values[4];
 	uint16_t values[8];
 
-	// loeschen if (count > 4) {
-	// loeschen 	// we only have 4 PWM outputs on the FMU
-	// loeschen	count = 4;
-	// loeschen }
 	if (count > _num_outputs) {
 		// we only have 8 I2C outputs in the driver
 		count = _num_outputs;
 	}
 
-
 	// allow for misaligned values
 	memcpy(values, buffer, count * 2);
 
 	for (uint8_t i = 0; i < count; i++) {
 		Motor[i].RawPwmValue = (unsigned short)values[i];
-		mk_servo_set_value(i, scaling(values[i], 1010, 2100, 0, 1024));
+		mk_servo_set(i, scaling(values[i], 1010, 2100, 0, 2047));
 	}
 
 	return count * 2;
 }
 
-void
-MK::gpio_reset(void)
-{
-	/*
-	 * Setup default GPIO config - all pins as GPIOs, GPIO driver chip
-	 * to input mode.
-	 */
-	for (unsigned i = 0; i < _ngpio; i++)
-		stm32_configgpio(_gpio_tab[i].input);
-
-	stm32_gpiowrite(GPIO_GPIO_DIR, 0);
-	stm32_configgpio(GPIO_GPIO_DIR);
-}
-
-void
-MK::gpio_set_function(uint32_t gpios, int function)
-{
-	/*
-	 * GPIOs 0 and 1 must have the same direction as they are buffered
-	 * by a shared 2-port driver.  Any attempt to set either sets both.
-	 */
-	if (gpios & 3) {
-		gpios |= 3;
-
-		/* flip the buffer to output mode if required */
-		if (GPIO_SET_OUTPUT == function)
-			stm32_gpiowrite(GPIO_GPIO_DIR, 1);
-	}
-
-	/* configure selected GPIOs as required */
-	for (unsigned i = 0; i < _ngpio; i++) {
-		if (gpios & (1 << i)) {
-			switch (function) {
-			case GPIO_SET_INPUT:
-				stm32_configgpio(_gpio_tab[i].input);
-				break;
-
-			case GPIO_SET_OUTPUT:
-				stm32_configgpio(_gpio_tab[i].output);
-				break;
-
-			case GPIO_SET_ALT_1:
-				if (_gpio_tab[i].alt != 0)
-					stm32_configgpio(_gpio_tab[i].alt);
-
-				break;
-			}
-		}
-	}
-
-	/* flip buffer to input mode if required */
-	if ((GPIO_SET_INPUT == function) && (gpios & 3))
-		stm32_gpiowrite(GPIO_GPIO_DIR, 0);
-}
-
-void
-MK::gpio_write(uint32_t gpios, int function)
-{
-	int value = (function == GPIO_SET) ? 1 : 0;
-
-	for (unsigned i = 0; i < _ngpio; i++)
-		if (gpios & (1 << i))
-			stm32_gpiowrite(_gpio_tab[i].output, value);
-}
-
-uint32_t
-MK::gpio_read(void)
-{
-	uint32_t bits = 0;
-
-	for (unsigned i = 0; i < _ngpio; i++)
-		if (stm32_gpioread(_gpio_tab[i].input))
-			bits |= (1 << i);
-
-	return bits;
-}
-
-int
-MK::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
-{
-	int	ret = OK;
-
-	lock();
-
-	switch (cmd) {
-
-	case GPIO_RESET:
-		gpio_reset();
-		break;
-
-	case GPIO_SET_OUTPUT:
-	case GPIO_SET_INPUT:
-	case GPIO_SET_ALT_1:
-		gpio_set_function(arg, cmd);
-		break;
-
-	case GPIO_SET_ALT_2:
-	case GPIO_SET_ALT_3:
-	case GPIO_SET_ALT_4:
-		ret = -EINVAL;
-		break;
-
-	case GPIO_SET:
-	case GPIO_CLEAR:
-		gpio_write(arg, cmd);
-		break;
-
-	case GPIO_GET:
-		*(uint32_t *)arg = gpio_read();
-		break;
-
-	default:
-		ret = -ENOTTY;
-	}
-
-	unlock();
-
-	return ret;
-}
 
 namespace
 {
 
-enum PortMode {
-	PORT_MODE_UNSET = 0,
-	PORT_FULL_GPIO,
-	PORT_FULL_SERIAL,
-	PORT_FULL_PWM,
-	PORT_GPIO_AND_SERIAL,
-	PORT_PWM_AND_SERIAL,
-	PORT_PWM_AND_GPIO,
-};
-
 enum MappingMode {
 	MAPPING_MK = 0,
 	MAPPING_PX4,
@@ -1279,58 +1135,11 @@ enum FrameType {
 	FRAME_X,
 };
 
-PortMode g_port_mode;
 
 int
-mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype)
+mk_new_mode(int update_rate, int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks)
 {
-	uint32_t gpio_bits;
 	int shouldStop = 0;
-	MK::Mode servo_mode;
-
-	/* reset to all-inputs */
-	g_mk->ioctl(0, GPIO_RESET, 0);
-
-	gpio_bits = 0;
-	servo_mode = MK::MODE_NONE;
-
-	switch (new_mode) {
-	case PORT_FULL_GPIO:
-	case PORT_MODE_UNSET:
-		/* nothing more to do here */
-		break;
-
-	case PORT_FULL_SERIAL:
-		/* set all multi-GPIOs to serial mode */
-		gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
-		break;
-
-	case PORT_FULL_PWM:
-		/* select 4-pin PWM mode */
-		servo_mode = MK::MODE_4PWM;
-		break;
-
-	case PORT_GPIO_AND_SERIAL:
-		/* set RX/TX multi-GPIOs to serial mode */
-		gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4;
-		break;
-
-	case PORT_PWM_AND_SERIAL:
-		/* select 2-pin PWM mode */
-		servo_mode = MK::MODE_2PWM;
-		/* set RX/TX multi-GPIOs to serial mode */
-		gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4;
-		break;
-
-	case PORT_PWM_AND_GPIO:
-		/* select 2-pin PWM mode */
-		servo_mode = MK::MODE_2PWM;
-		break;
-	}
-
-	/* adjust GPIO config for serial mode(s) */
-	if (gpio_bits != 0)
-		g_mk->ioctl(0, GPIO_SET_ALT_1, gpio_bits);
 
 	/* native PX4 addressing) */
 	g_mk->set_px4mode(px4mode);
@@ -1341,10 +1150,12 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
 	/* motortest if enabled */
 	g_mk->set_motor_test(motortest);
 
+	/* ovveride security checks if enabled */
+	g_mk->set_overrideSecurityChecks(overrideSecurityChecks);
 
 	/* count used motors */
 	do {
-		if (g_mk->mk_check_for_blctrl(8, false) != 0) {
+		if (g_mk->mk_check_for_blctrl(8, false, false) != 0) {
 			shouldStop = 4;
 
 		} else {
@@ -1354,41 +1165,55 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
 		sleep(1);
 	} while (shouldStop < 3);
 
-	g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true));
-
-	/* (re)set the PWM output mode */
-	g_mk->set_mode(servo_mode);
+	g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true, false));
 
-
-	if ((servo_mode != MK::MODE_NONE) && (update_rate != 0))
-		g_mk->set_pwm_rate(update_rate);
+	g_mk->set_update_rate(update_rate);
 
 	return OK;
 }
 
 int
-mk_start(unsigned bus, unsigned motors)
+mk_start(unsigned motors, char *device_path)
 {
-	int ret = OK;
+	int ret;
 
-	if (g_mk == nullptr) {
+	// try i2c3 first
+	g_mk = new MK(3, device_path);
 
-		g_mk = new MK(bus);
+	if (!g_mk)
+		return -ENOMEM;
 
-		if (g_mk == nullptr) {
-			ret = -ENOMEM;
+	if (OK == g_mk->init(motors)) {
+		warnx("[mkblctrl] scanning i2c3...\n");
+		ret = g_mk->mk_check_for_blctrl(8, false, true);
 
-		} else {
-			ret = g_mk->init(motors);
+		if (ret > 0) {
+			return OK;
+		}
+	}
 
-			if (ret != OK) {
-				delete g_mk;
-				g_mk = nullptr;
-			}
+	delete g_mk;
+	g_mk = nullptr;
+
+	// fallback to bus 1
+	g_mk = new MK(1, device_path);
+
+	if (!g_mk)
+		return -ENOMEM;
+
+	if (OK == g_mk->init(motors)) {
+		warnx("[mkblctrl] scanning i2c1...\n");
+		ret = g_mk->mk_check_for_blctrl(8, false, true);
+
+		if (ret > 0) {
+			return OK;
 		}
 	}
 
-	return ret;
+	delete g_mk;
+	g_mk = nullptr;
+
+	return -ENXIO;
 }
 
 
@@ -1399,33 +1224,21 @@ extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]);
 int
 mkblctrl_main(int argc, char *argv[])
 {
-	PortMode port_mode = PORT_FULL_PWM;
 	int pwm_update_rate_in_hz = UPDATE_RATE;
 	int motorcount = 8;
-	int bus = 1;
 	int px4mode = MAPPING_PX4;
 	int frametype = FRAME_PLUS;	// + plus is default
 	bool motortest = false;
+	bool overrideSecurityChecks = false;
 	bool showHelp = false;
 	bool newMode = false;
+	char *devicepath = "";
 
 	/*
 	 * optional parameters
 	 */
 	for (int i = 1; i < argc; i++) {
 
-		/* look for the optional i2c bus parameter */
-		if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) {
-			if (argc > i + 1) {
-				bus = atoi(argv[i + 1]);
-				newMode = true;
-
-			} else {
-				errx(1, "missing argument for i2c bus (-b)");
-				return 1;
-			}
-		}
-
 		/* look for the optional frame parameter */
 		if (strcmp(argv[i], "-mkmode") == 0 || strcmp(argv[i], "--mkmode") == 0) {
 			if (argc > i + 1) {
@@ -1461,32 +1274,67 @@ mkblctrl_main(int argc, char *argv[])
 			showHelp = true;
 		}
 
+		/* look for the optional --override-security-checks  parameter */
+		if (strcmp(argv[i], "--override-security-checks") == 0) {
+			overrideSecurityChecks = true;
+			newMode = true;
+		}
+
+		/* look for the optional device parameter */
+		if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) {
+			if (argc > i + 1) {
+				devicepath = argv[i + 1];
+				newMode = true;
+
+			} else {
+				errx(1, "missing the devicename (-d)");
+				return 1;
+			}
+		}
+
 	}
 
 	if (showHelp) {
 		fprintf(stderr, "mkblctrl: help:\n");
-		fprintf(stderr, "  [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [-h / --help]\n");
+		fprintf(stderr, "  [-mkmode {+/x}] [-b i2c_bus_number] [-d devicename] [--override-security-checks] [-h / --help]\n\n");
+		fprintf(stderr, "\t -mkmode {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n");
+		fprintf(stderr, "\t -d {devicepath & name}\t\t Create alternate pwm device.\n");
+		fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n");
+		fprintf(stderr, "\n");
+		fprintf(stderr, "Motortest:\n");
+		fprintf(stderr, "First you have to start mkblctrl, the you can enter Motortest Mode with:\n");
+		fprintf(stderr, "mkblctrl -t\n");
+		fprintf(stderr, "This will spin up once every motor in order of motoraddress. (DANGER !!!)\n");
 		exit(1);
 	}
 
 
-	if (g_mk == nullptr) {
-		if (mk_start(bus, motorcount) != OK) {
-			errx(1, "failed to start the MK-BLCtrl driver");
+	if (!motortest) {
+		if (g_mk == nullptr) {
+			if (mk_start(motorcount, devicepath) != OK) {
+				errx(1, "failed to start the MK-BLCtrl driver");
+			}
+
+			/* parameter set ? */
+			if (newMode) {
+				/* switch parameter */
+				return mk_new_mode(pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks);
+			}
+
+			exit(0);
 
 		} else {
-			newMode = true;
+			errx(1, "MK-BLCtrl driver already running");
 		}
-	}
-
 
-	/* parameter set ? */
-	if (newMode) {
-		/* switch parameter */
-		return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype);
-	}
+	} else {
+		if (g_mk == nullptr) {
+			errx(1, "MK-BLCtrl driver not running. You have to start it first.");
 
-	/* test, etc. here g*/
+		} else {
+			g_mk->set_motor_test(motortest);
+			exit(0);
 
-	exit(1);
+		}
+	}
 }
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index df1958186..ac75682c4 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -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
@@ -35,6 +35,9 @@
  * @file mpu6000.cpp
  *
  * Driver for the Invensense MPU6000 connected via SPI.
+ *
+ * @author Andrew Tridgell
+ * @author Pat Hickey
  */
 
 #include <nuttx/config.h>
@@ -60,16 +63,21 @@
 #include <nuttx/arch.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/board.h>
+#include <board_config.h>
 #include <drivers/drv_hrt.h>
 
 #include <drivers/device/spi.h>
+#include <drivers/device/ringbuffer.h>
 #include <drivers/drv_accel.h>
 #include <drivers/drv_gyro.h>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
 
 #define DIR_READ			0x80
 #define DIR_WRITE			0x00
 
+#define MPU_DEVICE_PATH_ACCEL		"/dev/mpu6000_accel"
+#define MPU_DEVICE_PATH_GYRO		"/dev/mpu6000_gyro"
+
 // MPU 6000 registers
 #define MPUREG_WHOAMI			0x75
 #define MPUREG_SMPLRT_DIV		0x19
@@ -144,6 +152,25 @@
 #define MPU6000_REV_D9			0x59
 #define MPU6000_REV_D10			0x5A
 
+#define MPU6000_ACCEL_DEFAULT_RANGE_G			8
+#define MPU6000_ACCEL_DEFAULT_RATE			1000
+#define MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ	30
+
+#define MPU6000_GYRO_DEFAULT_RANGE_G			8
+#define MPU6000_GYRO_DEFAULT_RATE			1000
+#define MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ		30
+
+#define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ		42
+
+#define MPU6000_ONE_G					9.80665f
+
+/*
+  the MPU6000 can only handle high SPI bus speeds on the sensor and
+  interrupt status registers. All other registers have a maximum 1MHz
+  SPI speed
+ */
+#define MPU6000_LOW_BUS_SPEED				1000*1000
+#define MPU6000_HIGH_BUS_SPEED				11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU6K */
 
 class MPU6000_gyro;
 
@@ -178,20 +205,32 @@ private:
 	struct hrt_call		_call;
 	unsigned		_call_interval;
 
-	struct accel_report	_accel_report;
+	RingBuffer		*_accel_reports;
+
 	struct accel_scale	_accel_scale;
 	float			_accel_range_scale;
 	float			_accel_range_m_s2;
 	orb_advert_t		_accel_topic;
+	int			_accel_class_instance;
+
+	RingBuffer		*_gyro_reports;
 
-	struct gyro_report	_gyro_report;
 	struct gyro_scale	_gyro_scale;
 	float			_gyro_range_scale;
 	float			_gyro_range_rad_s;
-	orb_advert_t		_gyro_topic;
 
-	unsigned		_reads;
+	unsigned		_sample_rate;
+	perf_counter_t		_accel_reads;
+	perf_counter_t		_gyro_reads;
 	perf_counter_t		_sample_perf;
+	perf_counter_t		_bad_transfers;
+
+	math::LowPassFilter2p	_accel_filter_x;
+	math::LowPassFilter2p	_accel_filter_y;
+	math::LowPassFilter2p	_accel_filter_z;
+	math::LowPassFilter2p	_gyro_filter_x;
+	math::LowPassFilter2p	_gyro_filter_y;
+	math::LowPassFilter2p	_gyro_filter_z;
 
 	/**
 	 * Start automatic measurement.
@@ -204,6 +243,13 @@ private:
 	void			stop();
 
 	/**
+	 * Reset chip.
+	 *
+	 * Resets the chip and measurements ranges, but not scale and offset.
+	 */
+	void			reset();
+
+	/**
 	 * Static trampoline from the hrt_call context; because we don't have a
 	 * generic hrt wrapper yet.
 	 *
@@ -215,7 +261,7 @@ private:
 	static void		measure_trampoline(void *arg);
 
 	/**
-	 * Fetch measurements from the sensor and update the report ring.
+	 * Fetch measurements from the sensor and update the report buffers.
 	 */
 	void			measure();
 
@@ -261,17 +307,36 @@ private:
 	uint16_t		swap16(uint16_t val) { return (val >> 8) | (val << 8);	}
 
 	/**
-	 * Self test
+	 * Measurement self test
 	 *
 	 * @return 0 on success, 1 on failure
 	 */
 	 int 			self_test();
 
+	/**
+	 * Accel self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	int 			accel_self_test();
+
+	/**
+	 * Gyro self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	 int 			gyro_self_test();
+
 	/*
 	  set low pass filter frequency
 	 */
 	void _set_dlpf_filter(uint16_t frequency_hz);
 
+	/*
+	  set sample rate (approximate) - 1kHz to 5Hz
+	*/
+	void _set_sample_rate(uint16_t desired_sample_rate_hz);
+
 };
 
 /**
@@ -286,30 +351,47 @@ public:
 	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
 	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
 
+	virtual int		init();
+
 protected:
 	friend class MPU6000;
 
 	void			parent_poll_notify();
+
 private:
 	MPU6000			*_parent;
+	orb_advert_t		_gyro_topic;
+	int			_gyro_class_instance;
+
 };
 
 /** driver 'main' command */
 extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); }
 
 MPU6000::MPU6000(int bus, spi_dev_e device) :
-	SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 10000000),
+	SPI("MPU6000", MPU_DEVICE_PATH_ACCEL, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED),
 	_gyro(new MPU6000_gyro(this)),
 	_product(0),
 	_call_interval(0),
+	_accel_reports(nullptr),
 	_accel_range_scale(0.0f),
 	_accel_range_m_s2(0.0f),
 	_accel_topic(-1),
+	_accel_class_instance(-1),
+	_gyro_reports(nullptr),
 	_gyro_range_scale(0.0f),
 	_gyro_range_rad_s(0.0f),
-	_gyro_topic(-1),
-	_reads(0),
-	_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read"))
+	_sample_rate(1000),
+	_accel_reads(perf_alloc(PC_COUNT, "mpu6000_accel_read")),
+	_gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")),
+	_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")),
+	_bad_transfers(perf_alloc(PC_COUNT, "mpu6000_bad_transfers")),
+	_accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_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)
 {
 	// disable debug() calls
 	_debug_enabled = false;
@@ -330,8 +412,6 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
 	_gyro_scale.z_offset = 0;
 	_gyro_scale.z_scale  = 1.0f;
 
-	memset(&_accel_report, 0, sizeof(_accel_report));
-	memset(&_gyro_report, 0, sizeof(_gyro_report));
 	memset(&_call, 0, sizeof(_call));
 }
 
@@ -343,8 +423,20 @@ MPU6000::~MPU6000()
 	/* delete the gyro subdriver */
 	delete _gyro;
 
+	/* free any existing reports */
+	if (_accel_reports != nullptr)
+		delete _accel_reports;
+	if (_gyro_reports != nullptr)
+		delete _gyro_reports;
+
+	if (_accel_class_instance != -1)
+		unregister_class_devname(ACCEL_DEVICE_PATH, _accel_class_instance);
+
 	/* delete the perf counter */
 	perf_free(_sample_perf);
+	perf_free(_accel_reads);
+	perf_free(_gyro_reads);
+	perf_free(_bad_transfers);
 }
 
 int
@@ -361,30 +453,107 @@ MPU6000::init()
 		return ret;
 	}
 
-	/* advertise sensor topics */
-	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_report);
-	_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_gyro_report);
+	/* allocate basic report buffers */
+	_accel_reports = new RingBuffer(2, sizeof(accel_report));
+	if (_accel_reports == nullptr)
+		goto out;
+
+	_gyro_reports = new RingBuffer(2, sizeof(gyro_report));
+	if (_gyro_reports == nullptr)
+		goto out;
+
+	reset();
+
+	/* Initialize offsets and scales */
+	_accel_scale.x_offset = 0;
+	_accel_scale.x_scale  = 1.0f;
+	_accel_scale.y_offset = 0;
+	_accel_scale.y_scale  = 1.0f;
+	_accel_scale.z_offset = 0;
+	_accel_scale.z_scale  = 1.0f;
+
+	_gyro_scale.x_offset = 0;
+	_gyro_scale.x_scale  = 1.0f;
+	_gyro_scale.y_offset = 0;
+	_gyro_scale.y_scale  = 1.0f;
+	_gyro_scale.z_offset = 0;
+	_gyro_scale.z_scale  = 1.0f;
+
+	/* do CDev init for the gyro device node, keep it optional */
+	ret = _gyro->init();
+	/* if probe/setup failed, bail now */
+	if (ret != OK) {
+		debug("gyro init failed");
+		return ret;
+	}
+
+	_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
+
+	measure();
+
+	if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {
+
+		/* advertise sensor topic, measure manually to initialize valid report */
+		struct accel_report arp;
+		_accel_reports->get(&arp);
+
+		/* measurement will have generated a report, publish */
+		_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
+
+		if (_accel_topic < 0)
+			debug("failed to create sensor_accel publication");
+
+	}
+
+	if (_gyro->_gyro_class_instance == CLASS_DEVICE_PRIMARY) {
+
+		/* advertise sensor topic, measure manually to initialize valid report */
+		struct gyro_report grp;
+		_gyro_reports->get(&grp);
+
+		_gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp);
+
+		if (_gyro->_gyro_topic < 0)
+			debug("failed to create sensor_gyro publication");
+
+	}
+
+out:
+	return ret;
+}
+
+void MPU6000::reset()
+{
+	// if the mpu6000 is initialised after the l3gd20 and lsm303d
+	// then if we don't do an irqsave/irqrestore here the mpu6000
+	// frequenctly comes up in a bad state where all transfers
+	// come as zero
+	irqstate_t state;
+	state = irqsave();
 
-	// Chip reset
 	write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
 	up_udelay(10000);
 
-	// Wake up device and select GyroZ clock (better performance)
+	// Wake up device and select GyroZ clock. Note that the
+	// MPU6000 starts up in sleep mode, and it can take some time
+	// for it to come out of sleep
 	write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
 	up_udelay(1000);
 
 	// Disable I2C bus (recommended on datasheet)
 	write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
+        irqrestore(state);
+
 	up_udelay(1000);
 
 	// SAMPLE RATE
-	write_reg(MPUREG_SMPLRT_DIV, 0x04);     // Sample rate = 200Hz    Fsample= 1Khz/(4+1) = 200Hz
+	_set_sample_rate(_sample_rate);
 	usleep(1000);
 
 	// FS & DLPF   FS=2000 deg/s, DLPF = 20Hz (low pass filter)
 	// was 90 Hz, but this ruins quality and does not improve the
 	// system response
-	_set_dlpf_filter(20);
+	_set_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ);
 	usleep(1000);
 	// Gyro scale 2000 deg/s ()
 	write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
@@ -395,12 +564,6 @@ MPU6000::init()
 	// 2000 deg/s = (2000/180)*PI = 34.906585 rad/s
 	// scaling factor:
 	// 1/(2^15)*(2000/180)*PI
-	_gyro_scale.x_offset = 0;
-	_gyro_scale.x_scale  = 1.0f;
-	_gyro_scale.y_offset = 0;
-	_gyro_scale.y_scale  = 1.0f;
-	_gyro_scale.z_offset = 0;
-	_gyro_scale.z_scale  = 1.0f;
 	_gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F);
 	_gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F;
 
@@ -433,14 +596,8 @@ MPU6000::init()
 
 	// Correct accel scale factors of 4096 LSB/g
 	// scale to m/s^2 ( 1g = 9.81 m/s^2)
-	_accel_scale.x_offset = 0;
-	_accel_scale.x_scale  = 1.0f;
-	_accel_scale.y_offset = 0;
-	_accel_scale.y_scale  = 1.0f;
-	_accel_scale.z_offset = 0;
-	_accel_scale.z_scale  = 1.0f;
-	_accel_range_scale = (9.81f / 4096.0f);
-	_accel_range_m_s2 = 8.0f * 9.81f;
+	_accel_range_scale = (MPU6000_ONE_G / 4096.0f);
+	_accel_range_m_s2 = 8.0f * MPU6000_ONE_G;
 
 	usleep(1000);
 
@@ -454,14 +611,6 @@ MPU6000::init()
 	// write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ);
 	usleep(1000);
 
-	/* do CDev init for the gyro device node, keep it optional */
-	int gyro_ret = _gyro->init();
-
-	if (gyro_ret != OK) {
-		_gyro_topic = -1;
-	}
-
-	return ret;
 }
 
 int
@@ -494,6 +643,19 @@ MPU6000::probe()
 }
 
 /*
+  set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro
+*/
+void
+MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz)
+{
+  uint8_t div = 1000 / desired_sample_rate_hz;
+  if(div>200) div=200;
+  if(div<1) div=1;
+  write_reg(MPUREG_SMPLRT_DIV, div-1);
+  _sample_rate = 1000 / div;
+}
+
+/*
   set the DLPF filter frequency. This affects both accel and gyro.
  */
 void
@@ -527,52 +689,132 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz)
 ssize_t
 MPU6000::read(struct file *filp, char *buffer, size_t buflen)
 {
-	int ret = 0;
+	unsigned count = buflen / sizeof(accel_report);
 
 	/* buffer must be large enough */
-	if (buflen < sizeof(_accel_report))
+	if (count < 1)
 		return -ENOSPC;
 
-	/* if automatic measurement is not enabled */
-	if (_call_interval == 0)
+	/* if automatic measurement is not enabled, get a fresh measurement into the buffer */
+	if (_call_interval == 0) {
+		_accel_reports->flush();
 		measure();
+	}
 
-	/* copy out the latest reports */
-	memcpy(buffer, &_accel_report, sizeof(_accel_report));
-	ret = sizeof(_accel_report);
+	/* if no data, error (we could block here) */
+	if (_accel_reports->empty())
+		return -EAGAIN;
 
-	return ret;
+	perf_count(_accel_reads);
+
+	/* copy reports out of our buffer to the caller */
+	accel_report *arp = reinterpret_cast<accel_report *>(buffer);
+	int transferred = 0;
+	while (count--) {
+		if (!_accel_reports->get(arp))
+			break;
+		transferred++;
+		arp++;
+	}
+
+	/* return the number of bytes transferred */
+	return (transferred * sizeof(accel_report));
 }
 
 int
 MPU6000::self_test()
 {
-	if (_reads == 0) {
+	if (perf_event_count(_sample_perf) == 0) {
 		measure();
 	}
 
 	/* return 0 on success, 1 else */
-	return (_reads > 0) ? 0 : 1;
+	return (perf_event_count(_sample_perf) > 0) ? 0 : 1;
+}
+
+int
+MPU6000::accel_self_test()
+{
+	if (self_test())
+		return 1;
+
+	/* inspect accel offsets */
+	if (fabsf(_accel_scale.x_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f)
+		return 1;
+
+	if (fabsf(_accel_scale.y_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f)
+		return 1;
+
+	if (fabsf(_accel_scale.z_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f)
+		return 1;
+
+	return 0;
+}
+
+int
+MPU6000::gyro_self_test()
+{
+	if (self_test())
+		return 1;
+
+	/* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */
+	if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f)
+		return 1;
+
+	if (fabsf(_gyro_scale.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f)
+		return 1;
+
+	if (fabsf(_gyro_scale.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f)
+		return 1;
+
+	return 0;
 }
 
 ssize_t
 MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
 {
-	int ret = 0;
+	unsigned count = buflen / sizeof(gyro_report);
 
 	/* buffer must be large enough */
-	if (buflen < sizeof(_gyro_report))
+	if (count < 1)
 		return -ENOSPC;
 
-	/* if automatic measurement is not enabled */
-	if (_call_interval == 0)
+	/* if automatic measurement is not enabled, get a fresh measurement into the buffer */
+	if (_call_interval == 0) {
+		_gyro_reports->flush();
 		measure();
+	}
 
-	/* copy out the latest report */
-	memcpy(buffer, &_gyro_report, sizeof(_gyro_report));
-	ret = sizeof(_gyro_report);
+	/* if no data, error (we could block here) */
+	if (_gyro_reports->empty())
+		return -EAGAIN;
 
-	return ret;
+	perf_count(_gyro_reads);
+
+	/* copy reports out of our buffer to the caller */
+	gyro_report *grp = reinterpret_cast<gyro_report *>(buffer);
+	int transferred = 0;
+	while (count--) {
+		if (!_gyro_reports->get(grp))
+			break;
+		transferred++;
+		grp++;
+	}
+
+	/* return the number of bytes transferred */
+	return (transferred * sizeof(gyro_report));
 }
 
 int
@@ -580,6 +822,10 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
 	switch (cmd) {
 
+	case SENSORIOCRESET:
+		reset();
+		return OK;
+
 	case SENSORIOCSPOLLRATE: {
 			switch (arg) {
 
@@ -598,9 +844,10 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 				/* set default/max polling rate */
 			case SENSOR_POLLRATE_MAX:
+				return ioctl(filp, SENSORIOCSPOLLRATE, 1000);
+
 			case SENSOR_POLLRATE_DEFAULT:
-				/* XXX 500Hz is just a wild guess */
-				return ioctl(filp, SENSORIOCSPOLLRATE, 500);
+				return ioctl(filp, SENSORIOCSPOLLRATE, MPU6000_ACCEL_DEFAULT_RATE);
 
 				/* adjust to a legal polling interval in Hz */
 			default: {
@@ -614,6 +861,19 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 					if (ticks < 1000)
 						return -EINVAL;
 
+					// adjust filters
+					float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq();
+					float sample_rate = 1.0e6f/ticks;
+					_accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
+					_accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
+					_accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
+
+
+					float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq();
+					_gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
+					_gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
+					_gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
+
 					/* update interval for next measurement */
 					/* XXX this is a bit shady, but no other way to adjust... */
 					_call.period = _call_interval = ticks;
@@ -633,23 +893,42 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 		return 1000000 / _call_interval;
 
-	case SENSORIOCSQUEUEDEPTH:
-		/* XXX not implemented */
-		return -EINVAL;
+	case SENSORIOCSQUEUEDEPTH: {
+		/* lower bound is mandatory, upper bound is a sanity check */
+		if ((arg < 1) || (arg > 100))
+			return -EINVAL;
+		
+		irqstate_t flags = irqsave();
+		if (!_accel_reports->resize(arg)) {
+			irqrestore(flags);
+			return -ENOMEM;
+		}
+		irqrestore(flags);
+		
+		return OK;
+	}
 
 	case SENSORIOCGQUEUEDEPTH:
-		/* XXX not implemented */
-		return -EINVAL;
+		return _accel_reports->size();
 
+	case ACCELIOCGSAMPLERATE:
+		return _sample_rate;
 
 	case ACCELIOCSSAMPLERATE:
-	case ACCELIOCGSAMPLERATE:
-		/* XXX not implemented */
-		return -EINVAL;
+		_set_sample_rate(arg);
+		return OK;
 
-	case ACCELIOCSLOWPASS:
 	case ACCELIOCGLOWPASS:
-		_set_dlpf_filter((uint16_t)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);
+		_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);
 		return OK;
 
 	case ACCELIOCSSCALE:
@@ -671,15 +950,16 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return OK;
 
 	case ACCELIOCSRANGE:
-	case ACCELIOCGRANGE:
 		/* XXX not implemented */
 		// XXX change these two values on set:
 		// _accel_range_scale = (9.81f / 4096.0f);
-		// _accel_range_rad_s = 8.0f * 9.81f;
+		// _accel_range_m_s2 = 8.0f * 9.81f;
 		return -EINVAL;
+	case ACCELIOCGRANGE:
+		return (unsigned long)((_accel_range_m_s2)/MPU6000_ONE_G + 0.5f);
 
 	case ACCELIOCSELFTEST:
-		return self_test();
+		return accel_self_test();
 
 	default:
 		/* give it to the superclass */
@@ -695,19 +975,42 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 		/* these are shared with the accel side */
 	case SENSORIOCSPOLLRATE:
 	case SENSORIOCGPOLLRATE:
-	case SENSORIOCSQUEUEDEPTH:
-	case SENSORIOCGQUEUEDEPTH:
 	case SENSORIOCRESET:
 		return ioctl(filp, cmd, arg);
 
-	case GYROIOCSSAMPLERATE:
+	case SENSORIOCSQUEUEDEPTH: {
+		/* lower bound is mandatory, upper bound is a sanity check */
+		if ((arg < 1) || (arg > 100))
+			return -EINVAL;
+
+		irqstate_t flags = irqsave();
+		if (!_gyro_reports->resize(arg)) {
+			irqrestore(flags);
+			return -ENOMEM;
+		}
+		irqrestore(flags);
+
+		return OK;
+	}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return _gyro_reports->size();
+
 	case GYROIOCGSAMPLERATE:
-		/* XXX not implemented */
-		return -EINVAL;
+		return _sample_rate;
+
+	case GYROIOCSSAMPLERATE:
+		_set_sample_rate(arg);
+		return OK;
 
-	case GYROIOCSLOWPASS:
 	case GYROIOCGLOWPASS:
-		_set_dlpf_filter((uint16_t)arg);
+		return _gyro_filter_x.get_cutoff_freq();
+	case GYROIOCSLOWPASS:
+		_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);
 		return OK;
 
 	case GYROIOCSSCALE:
@@ -721,15 +1024,16 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 		return OK;
 
 	case GYROIOCSRANGE:
-	case GYROIOCGRANGE:
 		/* XXX not implemented */
 		// XXX change these two values on set:
 		// _gyro_range_scale = xx
-		// _gyro_range_m_s2 = xx
+		// _gyro_range_rad_s = xx
 		return -EINVAL;
+	case GYROIOCGRANGE:
+		return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f);
 
 	case GYROIOCSELFTEST:
-		return self_test();
+		return gyro_self_test();
 
 	default:
 		/* give it to the superclass */
@@ -740,9 +1044,10 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 uint8_t
 MPU6000::read_reg(unsigned reg)
 {
-	uint8_t cmd[2];
+	uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0};
 
-	cmd[0] = reg | DIR_READ;
+        // general register transfer at low clock speed
+        set_frequency(MPU6000_LOW_BUS_SPEED);
 
 	transfer(cmd, cmd, sizeof(cmd));
 
@@ -752,9 +1057,10 @@ MPU6000::read_reg(unsigned reg)
 uint16_t
 MPU6000::read_reg16(unsigned reg)
 {
-	uint8_t cmd[3];
+	uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 };
 
-	cmd[0] = reg | DIR_READ;
+        // general register transfer at low clock speed
+        set_frequency(MPU6000_LOW_BUS_SPEED);
 
 	transfer(cmd, cmd, sizeof(cmd));
 
@@ -769,6 +1075,9 @@ MPU6000::write_reg(unsigned reg, uint8_t value)
 	cmd[0] = reg | DIR_WRITE;
 	cmd[1] = value;
 
+        // general register transfer at low clock speed
+        set_frequency(MPU6000_LOW_BUS_SPEED);
+
 	transfer(cmd, nullptr, sizeof(cmd));
 }
 
@@ -831,6 +1140,10 @@ MPU6000::start()
 	/* make sure we are stopped first */
 	stop();
 
+	/* discard any stale data in the buffers */
+	_accel_reports->flush();
+	_gyro_reports->flush();
+
 	/* start polling at the specified rate */
 	hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&MPU6000::measure_trampoline, this);
 }
@@ -844,7 +1157,7 @@ MPU6000::stop()
 void
 MPU6000::measure_trampoline(void *arg)
 {
-	MPU6000 *dev = (MPU6000 *)arg;
+	MPU6000 *dev = reinterpret_cast<MPU6000 *>(arg);
 
 	/* make another measurement */
 	dev->measure();
@@ -888,12 +1201,13 @@ MPU6000::measure()
 	 * Fetch the full set of measurements from the MPU6000 in one pass.
 	 */
 	mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
+
+        // sensor transfer at high clock speed
+        set_frequency(MPU6000_HIGH_BUS_SPEED);
+
 	if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report)))
 		return;
 
-	/* count measurement */
-	_reads++;
-
 	/*
 	 * Convert from big to little endian
 	 */
@@ -908,6 +1222,20 @@ MPU6000::measure()
 	report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y);
 	report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z);
 
+	if (report.accel_x == 0 &&
+	    report.accel_y == 0 &&
+	    report.accel_z == 0 &&
+	    report.temp == 0 &&
+	    report.gyro_x == 0 &&
+	    report.gyro_y == 0 &&
+	    report.gyro_z == 0) {
+		// all zero data - probably a SPI bus error
+		perf_count(_bad_transfers);
+		perf_end(_sample_perf);
+		return;
+	}
+	    
+
 	/*
 	 * Swap axes and negate y
 	 */
@@ -926,10 +1254,16 @@ MPU6000::measure()
 	report.gyro_y = gyro_yt;
 
 	/*
-	 * Adjust and scale results to m/s^2.
+	 * Report buffers.
 	 */
-	_gyro_report.timestamp = _accel_report.timestamp = hrt_absolute_time();
+	accel_report		arb;
+	gyro_report		grb;
 
+	/*
+	 * Adjust and scale results to m/s^2.
+	 */
+	grb.timestamp = arb.timestamp = hrt_absolute_time();
+        grb.error_count = arb.error_count = 0; // not reported
 
 	/*
 	 * 1) Scale raw value to SI units using scaling from datasheet.
@@ -949,40 +1283,57 @@ MPU6000::measure()
 
 	/* NOTE: Axes have been swapped to match the board a few lines above. */
 
-	_accel_report.x_raw = report.accel_x;
-	_accel_report.y_raw = report.accel_y;
-	_accel_report.z_raw = report.accel_z;
+	arb.x_raw = report.accel_x;
+	arb.y_raw = report.accel_y;
+	arb.z_raw = report.accel_z;
 
-	_accel_report.x = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
-	_accel_report.y = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
-	_accel_report.z = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
-	_accel_report.scaling = _accel_range_scale;
-	_accel_report.range_m_s2 = _accel_range_m_s2;
+	float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
+	float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
+	float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
+	
+	arb.x = _accel_filter_x.apply(x_in_new);
+	arb.y = _accel_filter_y.apply(y_in_new);
+	arb.z = _accel_filter_z.apply(z_in_new);
 
-	_accel_report.temperature_raw = report.temp;
-	_accel_report.temperature = (report.temp) / 361.0f + 35.0f;
+	arb.scaling = _accel_range_scale;
+	arb.range_m_s2 = _accel_range_m_s2;
 
-	_gyro_report.x_raw = report.gyro_x;
-	_gyro_report.y_raw = report.gyro_y;
-	_gyro_report.z_raw = report.gyro_z;
+	arb.temperature_raw = report.temp;
+	arb.temperature = (report.temp) / 361.0f + 35.0f;
 
-	_gyro_report.x = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
-	_gyro_report.y = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
-	_gyro_report.z = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
-	_gyro_report.scaling = _gyro_range_scale;
-	_gyro_report.range_rad_s = _gyro_range_rad_s;
+	grb.x_raw = report.gyro_x;
+	grb.y_raw = report.gyro_y;
+	grb.z_raw = report.gyro_z;
 
-	_gyro_report.temperature_raw = report.temp;
-	_gyro_report.temperature = (report.temp) / 361.0f + 35.0f;
+	float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
+	float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
+	float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
+	
+	grb.x = _gyro_filter_x.apply(x_gyro_in_new);
+	grb.y = _gyro_filter_y.apply(y_gyro_in_new);
+	grb.z = _gyro_filter_z.apply(z_gyro_in_new);
+
+	grb.scaling = _gyro_range_scale;
+	grb.range_rad_s = _gyro_range_rad_s;
+
+	grb.temperature_raw = report.temp;
+	grb.temperature = (report.temp) / 361.0f + 35.0f;
+
+	_accel_reports->force(&arb);
+	_gyro_reports->force(&grb);
 
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 	_gyro->parent_poll_notify();
 
-	/* and publish for subscribers */
-	orb_publish(ORB_ID(sensor_accel), _accel_topic, &_accel_report);
-	if (_gyro_topic != -1) {
-		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &_gyro_report);
+	if (_accel_topic > 0 && !(_pub_blocked)) {
+		/* publish it */
+		orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
+	}
+
+	if (_gyro->_gyro_topic > 0 && !(_pub_blocked)) {
+		/* publish it */
+		orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
 	}
 
 	/* stop measuring */
@@ -992,17 +1343,45 @@ MPU6000::measure()
 void
 MPU6000::print_info()
 {
-	printf("reads:          %u\n", _reads);
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_accel_reads);
+	perf_print_counter(_gyro_reads);
+	_accel_reports->print_info("accel queue");
+	_gyro_reports->print_info("gyro queue");
 }
 
 MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) :
-	CDev("MPU6000_gyro", GYRO_DEVICE_PATH),
-	_parent(parent)
+	CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO),
+	_parent(parent),
+	_gyro_topic(-1),
+	_gyro_class_instance(-1)
 {
 }
 
 MPU6000_gyro::~MPU6000_gyro()
 {
+	if (_gyro_class_instance != -1)
+		unregister_class_devname(GYRO_DEVICE_PATH, _gyro_class_instance);
+}
+
+int
+MPU6000_gyro::init()
+{
+	int ret;
+
+	// do base class init
+	ret = CDev::init();
+
+	/* if probe/setup failed, bail now */
+	if (ret != OK) {
+		debug("gyro init failed");
+		return ret;
+	}
+
+	_gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH);
+
+out:
+	return ret;
 }
 
 void
@@ -1045,7 +1424,8 @@ start()
 	int fd;
 
 	if (g_dev != nullptr)
-		errx(1, "already started");
+		/* 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);
@@ -1057,7 +1437,7 @@ start()
 		goto fail;
 
 	/* set the poll rate to default, starts automatic data collection */
-	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+	fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
 
 	if (fd < 0)
 		goto fail;
@@ -1065,6 +1445,8 @@ start()
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		goto fail;
 
+        close(fd);
+
 	exit(0);
 fail:
 
@@ -1084,24 +1466,22 @@ fail:
 void
 test()
 {
-	int fd = -1;
-	int fd_gyro = -1;
-	struct accel_report a_report;
-	struct gyro_report g_report;
+	accel_report a_report;
+	gyro_report g_report;
 	ssize_t sz;
 
 	/* get the driver */
-	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+	int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
 
 	if (fd < 0)
 		err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)",
-		    ACCEL_DEVICE_PATH);
+		    MPU_DEVICE_PATH_ACCEL);
 
 	/* get the driver */
-	fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
+	int fd_gyro = open(MPU_DEVICE_PATH_GYRO, O_RDONLY);
 
 	if (fd_gyro < 0)
-		err(1, "%s open failed", GYRO_DEVICE_PATH);
+		err(1, "%s open failed", MPU_DEVICE_PATH_GYRO);
 
 	/* reset to manual polling */
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
@@ -1110,8 +1490,10 @@ test()
 	/* do a simple demand read */
 	sz = read(fd, &a_report, sizeof(a_report));
 
-	if (sz != sizeof(a_report))
+	if (sz != sizeof(a_report)) {
+		warnx("ret: %d, expected: %d", sz, sizeof(a_report));
 		err(1, "immediate acc read failed");
+	}
 
 	warnx("single read");
 	warnx("time:     %lld", a_report.timestamp);
@@ -1122,13 +1504,15 @@ test()
 	warnx("acc  y:  \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
 	warnx("acc  z:  \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
 	warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
-	      (double)(a_report.range_m_s2 / 9.81f));
+	      (double)(a_report.range_m_s2 / MPU6000_ONE_G));
 
 	/* do a simple demand read */
 	sz = read(fd_gyro, &g_report, sizeof(g_report));
 
-	if (sz != sizeof(g_report))
+	if (sz != sizeof(g_report)) {
+		warnx("ret: %d, expected: %d", sz, sizeof(g_report));
 		err(1, "immediate gyro read failed");
+	}
 
 	warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x);
 	warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y);
@@ -1155,7 +1539,7 @@ test()
 void
 reset()
 {
-	int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+	int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
 
 	if (fd < 0)
 		err(1, "failed ");
@@ -1166,6 +1550,8 @@ reset()
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		err(1, "driver poll restart failed");
 
+        close(fd);
+
 	exit(0);
 }
 
diff --git a/src/drivers/ms5611/module.mk b/src/drivers/ms5611/module.mk
index 3c4b0f093..20f8aa173 100644
--- a/src/drivers/ms5611/module.mk
+++ b/src/drivers/ms5611/module.mk
@@ -37,4 +37,4 @@
 
 MODULE_COMMAND	= ms5611
 
-SRCS		= ms5611.cpp
+SRCS		= ms5611.cpp ms5611_spi.cpp ms5611_i2c.cpp
diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 59ab5936e..0ef056273 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -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
@@ -33,13 +33,11 @@
 
 /**
  * @file ms5611.cpp
- * Driver for the MS5611 barometric pressure sensor connected via I2C.
+ * Driver for the MS5611 barometric pressure sensor connected via I2C or SPI.
  */
 
 #include <nuttx/config.h>
 
-#include <drivers/device/i2c.h>
-
 #include <sys/types.h>
 #include <stdint.h>
 #include <stdbool.h>
@@ -59,12 +57,15 @@
 
 #include <arch/board/board.h>
 
+#include <drivers/device/device.h>
+#include <drivers/drv_baro.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/device/ringbuffer.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
 
-#include <drivers/drv_baro.h>
+#include "ms5611.h"
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -76,35 +77,26 @@ static const int ERROR = -1;
 # error This requires CONFIG_SCHED_WORKQUEUE.
 #endif
 
-/**
- * Calibration PROM as reported by the device.
- */
-#pragma pack(push,1)
-struct ms5611_prom_s {
-	uint16_t factory_setup;
-	uint16_t c1_pressure_sens;
-	uint16_t c2_pressure_offset;
-	uint16_t c3_temp_coeff_pres_sens;
-	uint16_t c4_temp_coeff_pres_offset;
-	uint16_t c5_reference_temp;
-	uint16_t c6_temp_coeff_temp;
-	uint16_t serial_and_crc;
-};
+/* helper macro for handling report buffer indices */
+#define INCREMENT(_x, _lim)	do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
 
-/**
- * Grody hack for crc4()
+/* helper macro for arithmetic - returns the square of the argument */
+#define POW2(_x)		((_x) * (_x))
+
+/*
+ * MS5611 internal constants and data structures.
  */
-union ms5611_prom_u {
-	uint16_t c[8];
-	struct ms5611_prom_s s;
-};
-#pragma pack(pop)
 
-class MS5611 : public device::I2C
+/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */
+#define MS5611_CONVERSION_INTERVAL	10000	/* microseconds */
+#define MS5611_MEASUREMENT_RATIO	3	/* pressure measurements per temperature measurement */
+#define MS5611_BARO_DEVICE_PATH		"/dev/ms5611"
+
+class MS5611 : public device::CDev
 {
 public:
-	MS5611(int bus);
-	virtual ~MS5611();
+	MS5611(device::Device *interface, ms5611::prom_u &prom_buf);
+	~MS5611();
 
 	virtual int		init();
 
@@ -117,18 +109,14 @@ public:
 	void			print_info();
 
 protected:
-	virtual int		probe();
+	Device			*_interface;
 
-private:
-	union ms5611_prom_u	_prom;
+	ms5611::prom_s		_prom;
 
 	struct work_s		_work;
 	unsigned		_measure_ticks;
 
-	unsigned		_num_reports;
-	volatile unsigned	_next_report;
-	volatile unsigned	_oldest_report;
-	struct baro_report	*_reports;
+	RingBuffer		*_reports;
 
 	bool			_collect_phase;
 	unsigned		_measure_phase;
@@ -137,28 +125,23 @@ private:
 	int32_t			_TEMP;
 	int64_t			_OFF;
 	int64_t			_SENS;
+	float			_P;
+	float			_T;
 
 	/* altitude conversion calibration */
 	unsigned		_msl_pressure;	/* in kPa */
 
 	orb_advert_t		_baro_topic;
 
+	int			_class_instance;
+
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_measure_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.
+	 * Initialize 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.
@@ -198,74 +181,24 @@ private:
 	 *
 	 * @return		OK if the measurement command was successful.
 	 */
-	int			measure();
+	virtual int		measure();
 
 	/**
 	 * Collect the result of the most recent measurement.
 	 */
-	int			collect();
-
-	/**
-	 * Send a reset command to the MS5611.
-	 *
-	 * This is required after any bus reset.
-	 */
-	int			cmd_reset();
-
-	/**
-	 * Read the MS5611 PROM
-	 *
-	 * @return		OK if the PROM reads successfully.
-	 */
-	int			read_prom();
-
-	/**
-	 * PROM CRC routine ported from MS5611 application note
-	 *
-	 * @param n_prom	Pointer to words read from PROM.
-	 * @return		True if the CRC matches.
-	 */
-	bool			crc4(uint16_t *n_prom);
-
+	virtual int		collect();
 };
 
-/* helper macro for handling report buffer indices */
-#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
-
-/* helper macro for arithmetic - returns the square of the argument */
-#define POW2(_x)		((_x) * (_x))
-
-/*
- * MS5611 internal constants and data structures.
- */
-
-/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */
-#define MS5611_CONVERSION_INTERVAL	10000	/* microseconds */
-#define MS5611_MEASUREMENT_RATIO	3	/* pressure measurements per temperature measurement */
-
-#define MS5611_BUS			PX4_I2C_BUS_ONBOARD
-#define MS5611_ADDRESS_1		PX4_I2C_OBDEV_MS5611 /* address select pins pulled high (PX4FMU series v1.6+) */
-#define MS5611_ADDRESS_2		0x77    /* address select pins pulled low (PX4FMU prototypes) */
-
-#define ADDR_RESET_CMD			0x1E	/* write to this address to reset chip */
-#define ADDR_CMD_CONVERT_D1		0x48	/* write to this address to start temperature conversion */
-#define ADDR_CMD_CONVERT_D2		0x58	/* write to this address to start pressure conversion */
-#define ADDR_DATA			0x00	/* address of 3 bytes / 32bit pressure data */
-#define ADDR_PROM_SETUP			0xA0	/* address of 8x 2 bytes factory and calibration data */
-#define ADDR_PROM_C1			0xA2	/* address of 6x 2 bytes calibration data */
-
 /*
  * Driver 'main' command.
  */
 extern "C" __EXPORT int ms5611_main(int argc, char *argv[]);
 
-
-MS5611::MS5611(int bus) :
-	I2C("MS5611", BARO_DEVICE_PATH, bus, 0, 400000),
+MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) :
+	CDev("MS5611", MS5611_BARO_DEVICE_PATH),
+	_interface(interface),
+	_prom(prom_buf.s),
 	_measure_ticks(0),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
 	_reports(nullptr),
 	_collect_phase(false),
 	_measure_phase(0),
@@ -274,15 +207,13 @@ MS5611::MS5611(int bus) :
 	_SENS(0),
 	_msl_pressure(101325),
 	_baro_topic(-1),
+	_class_instance(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")),
 	_measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")),
 	_comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows"))
 {
-	// enable debug() calls
-	_debug_enabled = true;
-
-	// work_cancel in the dtor will explode if we don't do this...
+	// work_cancel in stop_cycle called from the dtor will explode if we don't do this...
 	memset(&_work, 0, sizeof(_work));
 }
 
@@ -291,80 +222,102 @@ MS5611::~MS5611()
 	/* make sure we are truly inactive */
 	stop_cycle();
 
+	if (_class_instance != -1)
+		unregister_class_devname(MS5611_BARO_DEVICE_PATH, _class_instance);
+
 	/* free any existing reports */
 	if (_reports != nullptr)
-		delete[] _reports;
+		delete _reports;
+
+	// free perf counters
+	perf_free(_sample_perf);
+	perf_free(_measure_perf);
+	perf_free(_comms_errors);
+	perf_free(_buffer_overflows);
+
+	delete _interface;
 }
 
 int
 MS5611::init()
 {
-	int ret = ERROR;
+	int ret;
 
-	/* do I2C init (and probe) first */
-	if (I2C::init() != OK)
+	ret = CDev::init();
+	if (ret != OK) {
+		debug("CDev init failed");
 		goto out;
+	}
 
 	/* allocate basic report buffers */
-	_num_reports = 2;
-	_reports = new struct baro_report[_num_reports];
+	_reports = new RingBuffer(2, sizeof(baro_report));
 
-	if (_reports == nullptr)
+	if (_reports == nullptr) {
+		debug("can't get memory for reports");
+		ret = -ENOMEM;
 		goto out;
+	}
 
-	_oldest_report = _next_report = 0;
+	/* register alternate interfaces if we have to */
+	_class_instance = register_class_devname(BARO_DEVICE_PATH);
 
-	/* get a publish handle on the baro topic */
-	memset(&_reports[0], 0, sizeof(_reports[0]));
-	_baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]);
+	struct baro_report brp;
+	/* do a first measurement cycle to populate reports with valid data */
+	_measure_phase = 0;
+	_reports->flush();
 
-	if (_baro_topic < 0)
-		debug("failed to create sensor_baro object");
+	/* this do..while is goto without goto */
+	do {
+		/* do temperature first */
+		if (OK != measure()) {
+			ret = -EIO;
+			break;
+		}
 
-	ret = OK;
-out:
-	return ret;
-}
+		usleep(MS5611_CONVERSION_INTERVAL);
 
-int
-MS5611::probe()
-{
-	_retries = 10;
+		if (OK != collect()) {
+			ret = -EIO;
+			break;
+		}
 
-	if ((OK == probe_address(MS5611_ADDRESS_1)) ||
-	    (OK == probe_address(MS5611_ADDRESS_2))) {
-		/*
-	    	 * Disable retries; we may enable them selectively in some cases,
-		 * but the device gets confused if we retry some of the commands.
-	    	 */
-		_retries = 0;
-		return OK;
-	}
+		/* now do a pressure measurement */
+		if (OK != measure()) {
+			ret = -EIO;
+			break;
+		}
 
-	return -EIO;
-}
+		usleep(MS5611_CONVERSION_INTERVAL);
 
-int
-MS5611::probe_address(uint8_t address)
-{
-	/* select the address we are going to try */
-	set_address(address);
+		if (OK != collect()) {
+			ret = -EIO;
+			break;
+		}
 
-	/* send reset command */
-	if (OK != cmd_reset())
-		return -EIO;
+		/* state machine will have generated a report, copy it out */
+		_reports->get(&brp);
 
-	/* read PROM */
-	if (OK != read_prom())
-		return -EIO;
+		ret = OK;
 
-	return OK;
+		if (_class_instance == CLASS_DEVICE_PRIMARY) {
+
+			_baro_topic = orb_advertise(ORB_ID(sensor_baro), &brp);
+
+			if (_baro_topic < 0)
+				debug("failed to create sensor_baro publication");
+		}
+
+	} while (0);
+
+out:
+	return ret;
 }
 
 ssize_t
 MS5611::read(struct file *filp, char *buffer, size_t buflen)
 {
 	unsigned count = buflen / sizeof(struct baro_report);
+	struct baro_report *brp = reinterpret_cast<struct baro_report *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
@@ -380,10 +333,9 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
 		 * we are careful to avoid racing with them.
 		 */
 		while (count--) {
-			if (_oldest_report != _next_report) {
-				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
-				ret += sizeof(_reports[0]);
-				INCREMENT(_oldest_report, _num_reports);
+			if (_reports->get(brp)) {
+				ret += sizeof(*brp);
+				brp++;
 			}
 		}
 
@@ -392,10 +344,9 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
 	}
 
 	/* manual measurement - run one conversion */
-	/* XXX really it'd be nice to lock against other readers here */
 	do {
 		_measure_phase = 0;
-		_oldest_report = _next_report = 0;
+		_reports->flush();
 
 		/* do temperature first */
 		if (OK != measure()) {
@@ -424,8 +375,8 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
 		}
 
 		/* state machine will have generated a report, copy it out */
-		memcpy(buffer, _reports, sizeof(*_reports));
-		ret = sizeof(*_reports);
+		if (_reports->get(brp))
+			ret = sizeof(*brp);
 
 	} while (0);
 
@@ -500,35 +451,28 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return (1000 / _measure_ticks);
 
 	case SENSORIOCSQUEUEDEPTH: {
-			/* add one to account for the sentinel in the ring */
-			arg++;
-
-			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 2) || (arg > 100))
-				return -EINVAL;
-
-			/* allocate new buffer */
-			struct baro_report *buf = new struct baro_report[arg];
-
-			if (nullptr == buf)
-				return -ENOMEM;
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop_cycle();
-			delete[] _reports;
-			_num_reports = arg;
-			_reports = buf;
-			start_cycle();
+		/* lower bound is mandatory, upper bound is a sanity check */
+		if ((arg < 1) || (arg > 100))
+			return -EINVAL;
 
-			return OK;
+		irqstate_t flags = irqsave();
+		if (!_reports->resize(arg)) {
+			irqrestore(flags);
+			return -ENOMEM;
 		}
+		irqrestore(flags);		
+		return OK;
+	}
 
 	case SENSORIOCGQUEUEDEPTH:
-		return _num_reports - 1;
+		return _reports->size();
 
 	case SENSORIOCRESET:
-		/* XXX implement this */
-		return -EINVAL;
+		/*
+		 * Since we are initialized, we do not need to do anything, since the
+		 * PROM is correctly read and the part does not need to be configured.
+		 */
+		return OK;
 
 	case BAROIOCSMSLPRESSURE:
 
@@ -546,8 +490,9 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 		break;
 	}
 
-	/* give it to the superclass */
-	return I2C::ioctl(filp, cmd, arg);
+	/* give it to the bus-specific superclass */
+	// return bus_ioctl(filp, cmd, arg);
+	return CDev::ioctl(filp, cmd, arg);
 }
 
 void
@@ -557,7 +502,7 @@ MS5611::start_cycle()
 	/* reset the report ring and state machine */
 	_collect_phase = false;
 	_measure_phase = 0;
-	_oldest_report = _next_report = 0;
+	_reports->flush();
 
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1);
@@ -572,7 +517,7 @@ MS5611::stop_cycle()
 void
 MS5611::cycle_trampoline(void *arg)
 {
-	MS5611 *dev = (MS5611 *)arg;
+	MS5611 *dev = reinterpret_cast<MS5611 *>(arg);
 
 	dev->cycle();
 }
@@ -592,7 +537,7 @@ MS5611::cycle()
 				/*
 				 * The ms5611 seems to regularly fail to respond to
 				 * its address; this happens often enough that we'd rather not
-				 * spam the console with the message.
+				 * spam the console with a message for this.
 				 */
 			} else {
 				//log("collection error %d", ret);
@@ -654,17 +599,12 @@ MS5611::measure()
 	/*
 	 * In phase zero, request temperature; in other phases, request pressure.
 	 */
-	uint8_t	cmd_data = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1;
+	unsigned addr = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1;
 
 	/*
 	 * Send the command to begin measuring.
-	 *
-	 * Disable retries on this command; we can't know whether failure 
-	 * means the device did or did not see the write.
 	 */
-	_retries = 0;
-	ret = transfer(&cmd_data, 1, nullptr, 0);
-
+	ret = _interface->ioctl(IOCTL_MEASURE, addr);
 	if (OK != ret)
 		perf_count(_comms_errors);
 
@@ -677,47 +617,35 @@ int
 MS5611::collect()
 {
 	int ret;
-	uint8_t cmd;
-	uint8_t data[3];
-	union {
-		uint8_t	b[4];
-		uint32_t w;
-	} cvt;
-
-	/* read the most recent measurement */
-	cmd = 0;
+	uint32_t raw;
 
 	perf_begin(_sample_perf);
 
+	struct baro_report report;
 	/* this should be fairly close to the end of the conversion, so the best approximation of the time */
-	_reports[_next_report].timestamp = hrt_absolute_time();
+	report.timestamp = hrt_absolute_time();
+        report.error_count = perf_event_count(_comms_errors);
 
-	ret = transfer(&cmd, 1, &data[0], 3);
-	if (ret != OK) {
+	/* read the most recent measurement - read offset/size are hardcoded in the interface */
+	ret = _interface->read(0, (void *)&raw, 0);
+	if (ret < 0) {
 		perf_count(_comms_errors);
 		perf_end(_sample_perf);
 		return ret;
 	}
 
-	/* fetch the raw value */
-	cvt.b[0] = data[2];
-	cvt.b[1] = data[1];
-	cvt.b[2] = data[0];
-	cvt.b[3] = 0;
-	uint32_t raw = cvt.w;
-
 	/* handle a measurement */
 	if (_measure_phase == 0) {
 
 		/* temperature offset (in ADC units) */
-		int32_t dT = (int32_t)raw - ((int32_t)_prom.s.c5_reference_temp << 8);
+		int32_t dT = (int32_t)raw - ((int32_t)_prom.c5_reference_temp << 8);
 
 		/* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */
-		_TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.s.c6_temp_coeff_temp) >> 23);
+		_TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.c6_temp_coeff_temp) >> 23);
 
 		/* base sensor scale/offset values */
-		_SENS = ((int64_t)_prom.s.c1_pressure_sens << 15) + (((int64_t)_prom.s.c3_temp_coeff_pres_sens * dT) >> 8);
-		_OFF  = ((int64_t)_prom.s.c2_pressure_offset << 16) + (((int64_t)_prom.s.c4_temp_coeff_pres_offset * dT) >> 7);
+		_SENS = ((int64_t)_prom.c1_pressure_sens << 15) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 8);
+		_OFF  = ((int64_t)_prom.c2_pressure_offset << 16) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 7);
 
 		/* temperature compensation */
 		if (_TEMP < 2000) {
@@ -743,10 +671,12 @@ MS5611::collect()
 
 		/* pressure calculation, result in Pa */
 		int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15;
+		_P = P * 0.01f;
+		_T = _TEMP * 0.01f;
 
 		/* generate a new report */
-		_reports[_next_report].temperature = _TEMP / 100.0f;
-		_reports[_next_report].pressure = P / 100.0f;		/* convert to millibar */
+		report.temperature = _TEMP / 100.0f;
+		report.pressure = P / 100.0f;		/* convert to millibar */
 
 		/* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */
 
@@ -761,30 +691,7 @@ MS5611::collect()
 		 * 	double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us
 		 *	single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us
 		 */
-#if 0/* USE_FLOAT */
-		/* tropospheric properties (0-11km) for standard atmosphere */
-		const float T1 = 15.0f + 273.15f;	/* temperature at base height in Kelvin */
-		const float a  = -6.5f / 1000f;	/* temperature gradient in degrees per metre */
-		const float g  = 9.80665f;	/* gravity constant in m/s/s */
-		const float R  = 287.05f;	/* ideal gas constant in J/kg/K */
-
-		/* current pressure at MSL in kPa */
-		float p1 = _msl_pressure / 1000.0f;
 
-		/* measured pressure in kPa */
-		float p = P / 1000.0f;
-
-		/*
-		 * Solve:
-		 *
-		 *     /        -(aR / g)     \
-		 *    | (p / p1)          . T1 | - T1
-		 *     \                      /
-		 * h = -------------------------------  + h1
-		 *                   a
-		 */
-		_reports[_next_report].altitude = (((powf((p / p1), (-(a * R) / g))) * T1) - T1) / a;
-#else
 		/* tropospheric properties (0-11km) for standard atmosphere */
 		const double T1 = 15.0 + 273.15;	/* temperature at base height in Kelvin */
 		const double a  = -6.5 / 1000;	/* temperature gradient in degrees per metre */
@@ -806,18 +713,16 @@ MS5611::collect()
 		 * h = -------------------------------  + h1
 		 *                   a
 		 */
-		_reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
-#endif
-		/* publish it */
-		orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]);
+		report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
 
-		/* post a report to the ring - note, not locked */
-		INCREMENT(_next_report, _num_reports);
+		/* publish it */
+		if (_baro_topic > 0 && !(_pub_blocked)) {
+			/* publish it */
+			orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
+		}
 
-		/* if we are running up against the oldest report, toss it */
-		if (_next_report == _oldest_report) {
+		if (_reports->force(&report)) {
 			perf_count(_buffer_overflows);
-			INCREMENT(_oldest_report, _num_reports);
 		}
 
 		/* notify anyone waiting for data */
@@ -832,55 +737,50 @@ MS5611::collect()
 	return OK;
 }
 
-int
-MS5611::cmd_reset()
+void
+MS5611::print_info()
 {
-	unsigned	old_retrycount = _retries;
-	uint8_t		cmd = ADDR_RESET_CMD;
-	int		result;
-
-	/* bump the retry count */
-	_retries = 10;
-	result = transfer(&cmd, 1, nullptr, 0);
-	_retries = old_retrycount;
+	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");
+	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("MSL pressure:   %10.4f\n", (double)(_msl_pressure / 100.f));
 
-	return result;
+	printf("factory_setup             %u\n", _prom.factory_setup);
+	printf("c1_pressure_sens          %u\n", _prom.c1_pressure_sens);
+	printf("c2_pressure_offset        %u\n", _prom.c2_pressure_offset);
+	printf("c3_temp_coeff_pres_sens   %u\n", _prom.c3_temp_coeff_pres_sens);
+	printf("c4_temp_coeff_pres_offset %u\n", _prom.c4_temp_coeff_pres_offset);
+	printf("c5_reference_temp         %u\n", _prom.c5_reference_temp);
+	printf("c6_temp_coeff_temp        %u\n", _prom.c6_temp_coeff_temp);
+	printf("serial_and_crc            %u\n", _prom.serial_and_crc);
 }
 
-int
-MS5611::read_prom()
+/**
+ * Local functions in support of the shell command.
+ */
+namespace ms5611
 {
-	uint8_t		prom_buf[2];
-	union {
-		uint8_t		b[2];
-		uint16_t	w;
-	} cvt;
-
-	/*
-	 * Wait for PROM contents to be in the device (2.8 ms) in the case we are
-	 * called immediately after reset.
-	 */
-	usleep(3000);
 
-	/* read and convert PROM words */
-	for (int i = 0; i < 8; i++) {
-		uint8_t cmd = ADDR_PROM_SETUP + (i * 2);
-
-		if (OK != transfer(&cmd, 1, &prom_buf[0], 2))
-			break;
-
-		/* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */
-		cvt.b[0] = prom_buf[1];
-		cvt.b[1] = prom_buf[0];
-		_prom.c[i] = cvt.w;
-	}
+MS5611	*g_dev;
 
-	/* calculate CRC and return success/failure accordingly */
-	return crc4(&_prom.c[0]) ? OK : -EIO;
-}
+void	start();
+void	test();
+void	reset();
+void	info();
+void	calibrate(unsigned altitude);
 
+/**
+ * MS5611 crc4 cribbed from the datasheet
+ */
 bool
-MS5611::crc4(uint16_t *n_prom)
+crc4(uint16_t *n_prom)
 {
 	int16_t cnt;
 	uint16_t n_rem;
@@ -922,43 +822,6 @@ MS5611::crc4(uint16_t *n_prom)
 	return (0x000F & crc_read) == (n_rem ^ 0x00);
 }
 
-void
-MS5611::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);
-	printf("report queue:   %u (%u/%u @ %p)\n",
-	       _num_reports, _oldest_report, _next_report, _reports);
-	printf("TEMP:           %d\n", _TEMP);
-	printf("SENS:           %lld\n", _SENS);
-	printf("OFF:            %lld\n", _OFF);
-	printf("MSL pressure:   %10.4f\n", (double)(_msl_pressure / 100.f));
-
-	printf("factory_setup             %u\n", _prom.s.factory_setup);
-	printf("c1_pressure_sens          %u\n", _prom.s.c1_pressure_sens);
-	printf("c2_pressure_offset        %u\n", _prom.s.c2_pressure_offset);
-	printf("c3_temp_coeff_pres_sens   %u\n", _prom.s.c3_temp_coeff_pres_sens);
-	printf("c4_temp_coeff_pres_offset %u\n", _prom.s.c4_temp_coeff_pres_offset);
-	printf("c5_reference_temp         %u\n", _prom.s.c5_reference_temp);
-	printf("c6_temp_coeff_temp        %u\n", _prom.s.c6_temp_coeff_temp);
-	printf("serial_and_crc            %u\n", _prom.s.serial_and_crc);
-}
-
-/**
- * Local functions in support of the shell command.
- */
-namespace ms5611
-{
-
-MS5611	*g_dev;
-
-void	start();
-void	test();
-void	reset();
-void	info();
-void	calibrate(unsigned altitude);
 
 /**
  * Start the driver.
@@ -967,27 +830,46 @@ void
 start()
 {
 	int fd;
+	prom_u prom_buf;
 
 	if (g_dev != nullptr)
-		errx(1, "already started");
+		/* if already started, the still command succeeded */
+		errx(0, "already started");
 
-	/* create the driver */
-	g_dev = new MS5611(MS5611_BUS);
+	device::Device *interface = nullptr;
 
-	if (g_dev == nullptr)
-		goto fail;
+	/* create the driver, try SPI first, fall back to I2C if unsuccessful */
+	if (MS5611_spi_interface != nullptr)
+		interface = MS5611_spi_interface(prom_buf);
+	if (interface == nullptr && (MS5611_i2c_interface != nullptr))
+		interface = MS5611_i2c_interface(prom_buf);
 
-	if (OK != g_dev->init())
-		goto fail;
+	if (interface == nullptr)
+		errx(1, "failed to allocate an interface");
 
-	/* set the poll rate to default, starts automatic data collection */
-	fd = open(BARO_DEVICE_PATH, O_RDONLY);
+	if (interface->init() != OK) {
+		delete interface;
+		errx(1, "interface init failed");
+	}
 
-	if (fd < 0)
+	g_dev = new MS5611(interface, prom_buf);
+	if (g_dev == nullptr) {
+		delete interface;
+		errx(1, "failed to allocate driver");
+	}
+	if (g_dev->init() != OK)
 		goto fail;
 
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY);
+	if (fd < 0) {
+		warnx("can't open baro device");
 		goto fail;
+	}
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		warnx("failed setting default poll rate");
+		goto fail;
+	}
 
 	exit(0);
 
@@ -1013,10 +895,10 @@ test()
 	ssize_t sz;
 	int ret;
 
-	int fd = open(BARO_DEVICE_PATH, O_RDONLY);
+	int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0)
-		err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH);
+		err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", MS5611_BARO_DEVICE_PATH);
 
 	/* do a simple demand read */
 	sz = read(fd, &report, sizeof(report));
@@ -1072,7 +954,7 @@ test()
 void
 reset()
 {
-	int fd = open(BARO_DEVICE_PATH, O_RDONLY);
+	int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0)
 		err(1, "failed ");
@@ -1111,10 +993,10 @@ calibrate(unsigned altitude)
 	float	pressure;
 	float	p1;
 
-	int fd = open(BARO_DEVICE_PATH, O_RDONLY);
+	int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0)
-		err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH);
+		err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", MS5611_BARO_DEVICE_PATH);
 
 	/* start the sensor polling at max */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX))
@@ -1154,11 +1036,11 @@ calibrate(unsigned altitude)
 	const float g  = 9.80665f;	/* gravity constant in m/s/s */
 	const float R  = 287.05f;	/* ideal gas constant in J/kg/K */
 
-	warnx("averaged pressure %10.4fkPa at %um", pressure, altitude);
+	warnx("averaged pressure %10.4fkPa at %um", (double)pressure, altitude);
 
 	p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R))));
 
-	warnx("calculated MSL pressure %10.4fkPa", p1);
+	warnx("calculated MSL pressure %10.4fkPa", (double)p1);
 
 	/* save as integer Pa */
 	p1 *= 1000.0f;
diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h
new file mode 100644
index 000000000..76fb84de8
--- /dev/null
+++ b/src/drivers/ms5611/ms5611.h
@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   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 ms5611.h
+ *
+ * Shared defines for the ms5611 driver.
+ */
+
+#define ADDR_RESET_CMD			0x1E	/* write to this address to reset chip */
+#define ADDR_CMD_CONVERT_D1		0x48	/* write to this address to start temperature conversion */
+#define ADDR_CMD_CONVERT_D2		0x58	/* write to this address to start pressure conversion */
+#define ADDR_DATA			0x00	/* address of 3 bytes / 32bit pressure data */
+#define ADDR_PROM_SETUP			0xA0	/* address of 8x 2 bytes factory and calibration data */
+#define ADDR_PROM_C1			0xA2	/* address of 6x 2 bytes calibration data */
+
+/* interface ioctls */
+#define IOCTL_RESET			2
+#define IOCTL_MEASURE			3
+
+namespace ms5611
+{
+
+/**
+ * Calibration PROM as reported by the device.
+ */
+#pragma pack(push,1)
+struct prom_s {
+	uint16_t factory_setup;
+	uint16_t c1_pressure_sens;
+	uint16_t c2_pressure_offset;
+	uint16_t c3_temp_coeff_pres_sens;
+	uint16_t c4_temp_coeff_pres_offset;
+	uint16_t c5_reference_temp;
+	uint16_t c6_temp_coeff_temp;
+	uint16_t serial_and_crc;
+};
+
+/**
+ * Grody hack for crc4()
+ */
+union prom_u {
+	uint16_t c[8];
+	prom_s s;
+};
+#pragma pack(pop)
+
+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_i2c_interface(ms5611::prom_u &prom_buf) weak_function;
+
diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp
new file mode 100644
index 000000000..87d9b94a6
--- /dev/null
+++ b/src/drivers/ms5611/ms5611_i2c.cpp
@@ -0,0 +1,278 @@
+/****************************************************************************
+ *
+ *   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 ms5611_i2c.cpp
+  *
+  * I2C interface for MS5611
+  */
+
+/* XXX trim includes */
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <debug.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/device/i2c.h>
+
+#include "ms5611.h"
+
+#include "board_config.h"
+
+#ifdef PX4_I2C_OBDEV_MS5611
+
+#ifndef PX4_I2C_BUS_ONBOARD
+	#define MS5611_BUS		1
+#else
+	#define MS5611_BUS		PX4_I2C_BUS_ONBOARD
+#endif
+
+#define MS5611_ADDRESS_1		0x76	/* address select pins pulled high (PX4FMU series v1.6+) */
+#define MS5611_ADDRESS_2		0x77    /* address select pins pulled low (PX4FMU prototypes) */
+
+
+
+device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf);
+
+class MS5611_I2C : public device::I2C
+{
+public:
+	MS5611_I2C(int bus, ms5611::prom_u &prom_buf);
+	virtual ~MS5611_I2C();
+
+	virtual int	init();
+	virtual int	read(unsigned offset, void *data, unsigned count);
+	virtual int	ioctl(unsigned operation, unsigned &arg);
+
+protected:
+	virtual int	probe();
+
+private:
+	ms5611::prom_u	&_prom;
+
+	int		_probe_address(uint8_t address);
+
+	/**
+	 * Send a reset command to the MS5611.
+	 *
+	 * This is required after any bus reset.
+	 */
+	int		_reset();
+
+	/**
+	 * Send a measure command to the MS5611.
+	 *
+	 * @param addr		Which address to use for the measure operation.
+	 */
+	int		_measure(unsigned addr);
+
+	/**
+	 * Read the MS5611 PROM
+	 *
+	 * @return		OK if the PROM reads successfully.
+	 */
+	int		_read_prom();
+
+};
+
+device::Device *
+MS5611_i2c_interface(ms5611::prom_u &prom_buf)
+{
+	return new MS5611_I2C(MS5611_BUS, prom_buf);
+}
+
+MS5611_I2C::MS5611_I2C(int bus, ms5611::prom_u &prom) :
+	I2C("MS5611_I2C", nullptr, bus, 0, 400000),
+	_prom(prom)
+{
+}
+
+MS5611_I2C::~MS5611_I2C()
+{
+}
+
+int
+MS5611_I2C::init()
+{
+	/* this will call probe(), and thereby _probe_address */
+	return I2C::init();
+}
+
+int
+MS5611_I2C::read(unsigned offset, void *data, unsigned count)
+{
+	union _cvt {
+		uint8_t	b[4];
+		uint32_t w;
+	} *cvt = (_cvt *)data;
+	uint8_t buf[3];
+
+	/* read the most recent measurement */
+	uint8_t cmd = 0;
+	int ret = transfer(&cmd, 1, &buf[0], 3);
+	if (ret == OK) {
+		/* fetch the raw value */
+		cvt->b[0] = buf[2];
+		cvt->b[1] = buf[1];
+		cvt->b[2] = buf[0];
+		cvt->b[3] = 0;
+	}
+
+	return ret;
+}
+
+int
+MS5611_I2C::ioctl(unsigned operation, unsigned &arg)
+{
+	int ret;
+
+	switch (operation) {
+	case IOCTL_RESET:
+		ret = _reset();
+		break;
+
+	case IOCTL_MEASURE:
+		ret = _measure(arg);
+		break;
+
+	default:
+		ret = EINVAL;
+	}
+
+	return ret;
+}
+
+int
+MS5611_I2C::probe()
+{
+	_retries = 10;
+
+	if ((OK == _probe_address(MS5611_ADDRESS_1)) ||
+	    (OK == _probe_address(MS5611_ADDRESS_2))) {
+		/*
+	    	 * Disable retries; we may enable them selectively in some cases,
+		 * but the device gets confused if we retry some of the commands.
+	    	 */
+		_retries = 0;
+		return OK;
+	}
+
+	return -EIO;
+}
+
+int
+MS5611_I2C::_probe_address(uint8_t address)
+{
+	/* select the address we are going to try */
+	set_address(address);
+
+	/* send reset command */
+	if (OK != _reset())
+		return -EIO;
+
+	/* read PROM */
+	if (OK != _read_prom())
+		return -EIO;
+
+	return OK;
+}
+
+
+int
+MS5611_I2C::_reset()
+{
+	unsigned	old_retrycount = _retries;
+	uint8_t		cmd = ADDR_RESET_CMD;
+	int		result;
+
+	/* bump the retry count */
+	_retries = 10;
+	result = transfer(&cmd, 1, nullptr, 0);
+	_retries = old_retrycount;
+
+	return result;
+}
+
+int
+MS5611_I2C::_measure(unsigned addr)
+{
+	/*
+	 * Disable retries on this command; we can't know whether failure 
+	 * means the device did or did not see the command.
+	 */
+	_retries = 0;
+
+	uint8_t cmd = addr;
+	return transfer(&cmd, 1, nullptr, 0);
+}
+
+int
+MS5611_I2C::_read_prom()
+{
+	uint8_t		prom_buf[2];
+	union {
+		uint8_t		b[2];
+		uint16_t	w;
+	} cvt;
+
+	/*
+	 * Wait for PROM contents to be in the device (2.8 ms) in the case we are
+	 * called immediately after reset.
+	 */
+	usleep(3000);
+
+	/* read and convert PROM words */
+	for (int i = 0; i < 8; i++) {
+		uint8_t cmd = ADDR_PROM_SETUP + (i * 2);
+
+		if (OK != transfer(&cmd, 1, &prom_buf[0], 2))
+			break;
+
+		/* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */
+		cvt.b[0] = prom_buf[1];
+		cvt.b[1] = prom_buf[0];
+		_prom.c[i] = cvt.w;
+	}
+
+	/* calculate CRC and return success/failure accordingly */
+	return ms5611::crc4(&_prom.c[0]) ? OK : -EIO;
+}
+
+#endif /* PX4_I2C_OBDEV_MS5611 */
diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp
new file mode 100644
index 000000000..26216e840
--- /dev/null
+++ b/src/drivers/ms5611/ms5611_spi.cpp
@@ -0,0 +1,276 @@
+/****************************************************************************
+ *
+ *   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 ms5611_spi.cpp
+  *
+  * SPI interface for MS5611
+  */
+
+/* XXX trim includes */
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <debug.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/device/spi.h>
+
+#include "ms5611.h"
+#include "board_config.h"
+
+/* SPI protocol address bits */
+#define DIR_READ			(1<<7)
+#define DIR_WRITE			(0<<7)
+#define ADDR_INCREMENT			(1<<6)
+
+#ifdef PX4_SPIDEV_BARO
+
+device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf);
+
+class MS5611_SPI : public device::SPI
+{
+public:
+	MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf);
+	virtual ~MS5611_SPI();
+
+	virtual int	init();
+	virtual int	read(unsigned offset, void *data, unsigned count);
+	virtual int	ioctl(unsigned operation, unsigned &arg);
+
+private:
+	ms5611::prom_u	&_prom;
+
+	/**
+	 * Send a reset command to the MS5611.
+	 *
+	 * This is required after any bus reset.
+	 */
+	int		_reset();
+
+	/**
+	 * Send a measure command to the MS5611.
+	 *
+	 * @param addr		Which address to use for the measure operation.
+	 */
+	int		_measure(unsigned addr);
+
+	/**
+	 * Read the MS5611 PROM
+	 *
+	 * @return		OK if the PROM reads successfully.
+	 */
+	int		_read_prom();
+
+	/**
+	 * Read a 16-bit register value.
+	 *
+	 * @param reg		The register to read.
+	 */
+	uint16_t	_reg16(unsigned reg);
+
+	/**
+	 * Wrapper around transfer() that prevents interrupt-context transfers
+	 * from pre-empting us. The sensor may (does) share a bus with sensors
+	 * that are polled from interrupt context (or we may be pre-empted)
+	 * so we need to guarantee that transfers complete without interruption.
+	 */
+	int		_transfer(uint8_t *send, uint8_t *recv, unsigned len);
+};
+
+device::Device *
+MS5611_spi_interface(ms5611::prom_u &prom_buf)
+{
+	return new MS5611_SPI(1 /* XXX MAGIC NUMBER */, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf);
+}
+
+MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) :
+	SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */),
+	_prom(prom_buf)
+{
+}
+
+MS5611_SPI::~MS5611_SPI()
+{
+}
+
+int
+MS5611_SPI::init()
+{
+	int ret;
+
+	ret = SPI::init();
+	if (ret != OK) {
+		debug("SPI init failed");
+		goto out;
+	}
+
+	/* send reset command */
+	ret = _reset();
+	if (ret != OK) {
+		debug("reset failed");
+		goto out;
+	}
+
+	/* read PROM */
+	ret = _read_prom();
+	if (ret != OK) {
+		debug("prom readout failed");
+		goto out;
+	}
+
+out:
+	return ret;
+}
+
+int
+MS5611_SPI::read(unsigned offset, void *data, unsigned count)
+{
+	union _cvt {
+		uint8_t	b[4];
+		uint32_t w;
+	} *cvt = (_cvt *)data;
+	uint8_t buf[4] = { 0 | DIR_WRITE, 0, 0, 0 };
+
+	/* read the most recent measurement */
+	int ret = _transfer(&buf[0], &buf[0], sizeof(buf));
+
+	if (ret == OK) {
+		/* fetch the raw value */
+		cvt->b[0] = buf[3];
+		cvt->b[1] = buf[2];
+		cvt->b[2] = buf[1];
+		cvt->b[3] = 0;
+
+		ret = count;
+	}
+
+	return ret;
+}
+
+int
+MS5611_SPI::ioctl(unsigned operation, unsigned &arg)
+{
+	int ret;
+
+	switch (operation) {
+	case IOCTL_RESET:
+		ret = _reset();
+		break;
+
+	case IOCTL_MEASURE:
+		ret = _measure(arg);
+		break;
+
+	default:
+		ret = EINVAL;
+	}
+
+	if (ret != OK) {
+		errno = ret;
+		return -1;
+	}
+	return 0;
+}
+
+int
+MS5611_SPI::_reset()
+{
+	uint8_t cmd = ADDR_RESET_CMD | DIR_WRITE;
+
+	return  _transfer(&cmd, nullptr, 1);
+}
+
+int
+MS5611_SPI::_measure(unsigned addr)
+{
+	uint8_t cmd = addr | DIR_WRITE;
+
+	return _transfer(&cmd, nullptr, 1);
+}
+
+
+int
+MS5611_SPI::_read_prom()
+{
+	/*
+	 * Wait for PROM contents to be in the device (2.8 ms) in the case we are
+	 * called immediately after reset.
+	 */
+	usleep(3000);
+
+	/* read and convert PROM words */
+        bool all_zero = true;
+	for (int i = 0; i < 8; i++) {
+		uint8_t cmd = (ADDR_PROM_SETUP + (i * 2));
+		_prom.c[i] = _reg16(cmd);
+                if (_prom.c[i] != 0)
+			all_zero = false;
+                //debug("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]);
+	}
+
+	/* calculate CRC and return success/failure accordingly */
+	int ret = ms5611::crc4(&_prom.c[0]) ? OK : -EIO;
+        if (ret != OK) {
+		debug("crc failed");
+        }
+        if (all_zero) {
+		debug("prom all zero");
+		ret = -EIO;
+        }
+        return ret;
+}
+
+uint16_t
+MS5611_SPI::_reg16(unsigned reg)
+{
+	uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 };
+
+	_transfer(cmd, cmd, sizeof(cmd));
+
+	return (uint16_t)(cmd[1] << 8) | cmd[2];
+}
+
+int
+MS5611_SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len)
+{
+	return transfer(send, recv, len);
+}
+
+#endif /* PX4_SPIDEV_BARO */
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 5147ac500..0fbd84924 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -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
@@ -34,7 +34,7 @@
 /**
  * @file fmu.cpp
  *
- * Driver/configurator for the PX4 FMU multi-purpose port.
+ * Driver/configurator for the PX4 FMU multi-purpose port on v1 and v2 boards.
  */
 
 #include <nuttx/config.h>
@@ -57,29 +57,41 @@
 #include <drivers/device/device.h>
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_gpio.h>
-#include <drivers/boards/px4fmu/px4fmu_internal.h>
 #include <drivers/drv_hrt.h>
 
+#include <board_config.h>
+
 #include <systemlib/systemlib.h>
 #include <systemlib/err.h>
 #include <systemlib/mixer/mixer.h>
+#include <systemlib/pwm_limit/pwm_limit.h>
+#include <systemlib/board_serial.h>
 #include <drivers/drv_mixer.h>
 #include <drivers/drv_rc_input.h>
 
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_armed.h>
 
-#include <systemlib/err.h>
-#include <systemlib/ppm_decode.h>
+
+#ifdef HRT_PPM_CHANNEL
+# include <systemlib/ppm_decode.h>
+#endif
+
+/*
+ * This is the analog to FMU_INPUT_DROP_LIMIT_US on the IO side
+ */
+
+#define CONTROL_INPUT_DROP_LIMIT_MS		20
 
 class PX4FMU : public device::CDev
 {
 public:
 	enum Mode {
+		MODE_NONE,
 		MODE_2PWM,
 		MODE_4PWM,
-		MODE_NONE
+		MODE_6PWM,
 	};
 	PX4FMU();
 	virtual ~PX4FMU();
@@ -95,7 +107,12 @@ public:
 	int		set_pwm_alt_channels(uint32_t channels);
 
 private:
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 	static const unsigned _max_actuators = 4;
+#endif
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+	static const unsigned _max_actuators = 6;
+#endif
 
 	Mode		_mode;
 	unsigned	_pwm_default_rate;
@@ -104,19 +121,27 @@ private:
 	unsigned	_current_update_rate;
 	int		_task;
 	int		_t_actuators;
-	int		_t_armed;
+	int		_t_actuator_armed;
 	orb_advert_t	_t_outputs;
-	orb_advert_t	_t_actuators_effective;
 	unsigned	_num_outputs;
 	bool		_primary_pwm_device;
 
 	volatile bool	_task_should_exit;
 	bool		_armed;
+	bool		_pwm_on;
 
 	MixerGroup	*_mixers;
 
 	actuator_controls_s _controls;
 
+	pwm_limit_t	_pwm_limit;
+	uint16_t	_failsafe_pwm[_max_actuators];
+	uint16_t	_disarmed_pwm[_max_actuators];
+	uint16_t	_min_pwm[_max_actuators];
+	uint16_t	_max_pwm[_max_actuators];
+	unsigned	_num_failsafe_set;
+	unsigned	_num_disarmed_set;
+
 	static void	task_main_trampoline(int argc, char *argv[]);
 	void		task_main() __attribute__((noreturn));
 
@@ -138,6 +163,7 @@ private:
 	static const unsigned	_ngpio;
 
 	void		gpio_reset(void);
+	void		sensor_reset(int ms);
 	void		gpio_set_function(uint32_t gpios, int function);
 	void		gpio_write(uint32_t gpios, int function);
 	uint32_t	gpio_read(void);
@@ -146,6 +172,7 @@ private:
 };
 
 const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 	{GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
 	{GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
 	{GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1},
@@ -154,6 +181,22 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
 	{GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1},
 	{GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2},
 	{GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2},
+#endif
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+	{GPIO_GPIO0_INPUT,       GPIO_GPIO0_OUTPUT,       0},
+	{GPIO_GPIO1_INPUT,       GPIO_GPIO1_OUTPUT,       0},
+	{GPIO_GPIO2_INPUT,       GPIO_GPIO2_OUTPUT,       0},
+	{GPIO_GPIO3_INPUT,       GPIO_GPIO3_OUTPUT,       0},
+	{GPIO_GPIO4_INPUT,       GPIO_GPIO4_OUTPUT,       0},
+	{GPIO_GPIO5_INPUT,       GPIO_GPIO5_OUTPUT,       0},
+
+	{0,                      GPIO_VDD_5V_PERIPH_EN,   0},
+	{0,                      GPIO_VDD_3V3_SENSORS_EN, 0},
+	{GPIO_VDD_BRICK_VALID,   0,                       0},
+	{GPIO_VDD_SERVO_VALID,   0,                       0},
+	{GPIO_VDD_5V_HIPOWER_OC, 0,                       0},
+	{GPIO_VDD_5V_PERIPH_OC,  0,                       0},
+#endif
 };
 
 const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]);
@@ -166,7 +209,7 @@ PX4FMU	*g_fmu;
 } // namespace
 
 PX4FMU::PX4FMU() :
-	CDev("fmuservo", "/dev/px4fmu"),
+	CDev("fmuservo", PX4FMU_DEVICE_PATH),
 	_mode(MODE_NONE),
 	_pwm_default_rate(50),
 	_pwm_alt_rate(50),
@@ -174,15 +217,24 @@ PX4FMU::PX4FMU() :
 	_current_update_rate(0),
 	_task(-1),
 	_t_actuators(-1),
-	_t_armed(-1),
+	_t_actuator_armed(-1),
 	_t_outputs(0),
-	_t_actuators_effective(0),
 	_num_outputs(0),
 	_primary_pwm_device(false),
 	_task_should_exit(false),
 	_armed(false),
-	_mixers(nullptr)
+	_pwm_on(false),
+	_mixers(nullptr),
+	_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;
+	}
+
 	_debug_enabled = true;
 }
 
@@ -240,11 +292,11 @@ PX4FMU::init()
 
 	/* start the IO interface task */
 	_task = task_spawn_cmd("fmuservo",
-			   SCHED_DEFAULT,
-			   SCHED_PRIORITY_DEFAULT,
-			   2048,
-			   (main_t)&PX4FMU::task_main_trampoline,
-			   nullptr);
+			       SCHED_DEFAULT,
+			       SCHED_PRIORITY_DEFAULT,
+			       2048,
+			       (main_t)&PX4FMU::task_main_trampoline,
+			       nullptr);
 
 	if (_task < 0) {
 		debug("task start failed: %d", errno);
@@ -271,17 +323,44 @@ PX4FMU::set_mode(Mode mode)
 	 * are presented on the output pins.
 	 */
 	switch (mode) {
-	case MODE_2PWM:	// multi-port with flow control lines as PWM
-	case MODE_4PWM: // multi-port as 4 PWM outs
-		debug("MODE_%dPWM", (mode == MODE_2PWM) ? 2 : 4);
-		
+	case MODE_2PWM:	// v1 multi-port with flow control lines as PWM
+		debug("MODE_2PWM");
+
+		/* default output rates */
+		_pwm_default_rate = 50;
+		_pwm_alt_rate = 50;
+		_pwm_alt_rate_channels = 0;
+
+		/* XXX magic numbers */
+		up_pwm_servo_init(0x3);
+		set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
+
+		break;
+
+	case MODE_4PWM: // v1 multi-port as 4 PWM outs
+		debug("MODE_4PWM");
+
+		/* default output rates */
+		_pwm_default_rate = 50;
+		_pwm_alt_rate = 50;
+		_pwm_alt_rate_channels = 0;
+
+		/* XXX magic numbers */
+		up_pwm_servo_init(0xf);
+		set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
+
+		break;
+
+	case MODE_6PWM: // v2 PWMs as 6 PWM outs
+		debug("MODE_6PWM");
+
 		/* default output rates */
 		_pwm_default_rate = 50;
 		_pwm_alt_rate = 50;
 		_pwm_alt_rate_channels = 0;
 
 		/* XXX magic numbers */
-		up_pwm_servo_init((mode == MODE_2PWM) ? 0x3 : 0xf);
+		up_pwm_servo_init(0x3f);
 		set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
 
 		break;
@@ -316,6 +395,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
 
 			// get the channel mask for this rate group
 			uint32_t mask = up_pwm_servo_get_rate_group(group);
+
 			if (mask == 0)
 				continue;
 
@@ -329,6 +409,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
 					// not a legal map, bail
 					return -EINVAL;
 				}
+
 			} else {
 				// set it - errors here are unexpected
 				if (alt != 0) {
@@ -336,6 +417,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
 						warn("rate group set alt failed");
 						return -EINVAL;
 					}
+
 				} else {
 					if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) {
 						warn("rate group set default failed");
@@ -345,6 +427,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
 			}
 		}
 	}
+
 	_pwm_alt_rate_channels = rate_map;
 	_pwm_default_rate = default_rate;
 	_pwm_alt_rate = alt_rate;
@@ -376,8 +459,8 @@ PX4FMU::task_main()
 	/* force a reset of the update rate */
 	_current_update_rate = 0;
 
-	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
-	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
+	_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
+	orb_set_interval(_t_actuator_armed, 200);		/* 5Hz update rate */
 
 	/* advertise the mixed control outputs */
 	actuator_outputs_s outputs;
@@ -386,27 +469,23 @@ PX4FMU::task_main()
 	_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
 				   &outputs);
 
-	/* advertise the effective control inputs */
-	actuator_controls_effective_s controls_effective;
-	memset(&controls_effective, 0, sizeof(controls_effective));
-	/* advertise the effective control inputs */
-	_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
-				   &controls_effective);
-
 	pollfd fds[2];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
-	fds[1].fd = _t_armed;
+	fds[1].fd = _t_actuator_armed;
 	fds[1].events = POLLIN;
 
-	unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4;
-
+#ifdef HRT_PPM_CHANNEL
 	// rc input, published to ORB
 	struct rc_input_values rc_in;
 	orb_advert_t to_input_rc = 0;
 
 	memset(&rc_in, 0, sizeof(rc_in));
 	rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;
+#endif
+
+	/* initialize PWM limit lib */
+	pwm_limit_init(&_pwm_limit);
 
 	log("starting");
 
@@ -420,6 +499,7 @@ PX4FMU::task_main()
 		 * We always mix at max rate; some channels may update slower.
 		 */
 		unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate;
+
 		if (_current_update_rate != max_rate) {
 			_current_update_rate = max_rate;
 			int update_rate_in_ms = int(1000 / _current_update_rate);
@@ -428,6 +508,7 @@ PX4FMU::task_main()
 			if (update_rate_in_ms < 2) {
 				update_rate_in_ms = 2;
 			}
+
 			/* reject slower than 10 Hz updates */
 			if (update_rate_in_ms > 100) {
 				update_rate_in_ms = 100;
@@ -442,100 +523,146 @@ PX4FMU::task_main()
 
 		/* sleep waiting for data, stopping to check for PPM
 		 * input at 100Hz */
-		int ret = ::poll(&fds[0], 2, 10);
+		int ret = ::poll(&fds[0], 2, CONTROL_INPUT_DROP_LIMIT_MS);
 
 		/* this would be bad... */
 		if (ret < 0) {
 			log("poll error %d", errno);
 			usleep(1000000);
 			continue;
-		}
 
-		/* do we have a control update? */
-		if (fds[0].revents & POLLIN) {
-
-			/* get controls - must always do this to avoid spinning */
-			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
-
-			/* can we mix? */
-			if (_mixers != nullptr) {
-
-				/* do mixing */
-				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
-				outputs.timestamp = hrt_absolute_time();
-
-				// XXX output actual limited values
-				memcpy(&controls_effective, &_controls, sizeof(controls_effective));
-
-				orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
-
-				/* 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) {
-						/* scale for PWM output 900 - 2100us */
-						outputs.output[i] = 1500 + (600 * outputs.output[i]);
-					} else {
-						/*
-						 * 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] = 900;
+		} else if (ret == 0) {
+			/* timeout: no control data, switch to failsafe values */
+//			warnx("no PWM: failsafe");
+
+		} else {
+
+			/* do we have a control update? */
+			if (fds[0].revents & POLLIN) {
+
+				/* 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) {
+
+					unsigned num_outputs;
+
+					switch (_mode) {
+					case MODE_2PWM:
+						num_outputs = 2;
+						break;
+
+					case MODE_4PWM:
+						num_outputs = 4;
+						break;
+
+					case MODE_6PWM:
+						num_outputs = 6;
+						break;
+
+					default:
+						num_outputs = 0;
+						break;
 					}
 
-					/* output to the servo */
-					up_pwm_servo_set(i, outputs.output[i]);
-				}
+					/* 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;
+						}
+					}
 
-				/* 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);
+					uint16_t pwm_limited[num_outputs];
+
+					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]);
+					}
+
+					/* 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);
+				}
 			}
-		}
 
-		/* how about an arming update? */
-		if (fds[1].revents & POLLIN) {
-			actuator_armed_s aa;
+			/* how about an arming update? */
+			if (fds[1].revents & POLLIN) {
+				actuator_armed_s aa;
+
+				/* get new value */
+				orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
+
+				/* update the armed status and check that we're not locked down */
+				bool set_armed = aa.armed && !aa.lockdown;
 
-			/* get new value */
-			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
+				if (_armed != set_armed)
+					_armed = set_armed;
 
-			/* update PWM servo armed status if armed and not locked down */
-			bool set_armed = aa.armed && !aa.lockdown;
-			if (set_armed != _armed) {
-				_armed = set_armed;
-				up_pwm_servo_arm(set_armed);
+				/* update PWM status if armed or if disarmed PWM values are set */
+				bool pwm_on = (aa.armed || _num_disarmed_set > 0);
+
+				if (_pwm_on != pwm_on) {
+					_pwm_on = pwm_on;
+					up_pwm_servo_arm(pwm_on);
+				}
 			}
 		}
 
+#ifdef HRT_PPM_CHANNEL
+
 		// see if we have new PPM input data
-		if (ppm_last_valid_decode != rc_in.timestamp) {
+		if (ppm_last_valid_decode != rc_in.timestamp_last_signal) {
 			// we have a new PPM frame. Publish it.
 			rc_in.channel_count = ppm_decoded_channels;
+
 			if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) {
 				rc_in.channel_count = RC_INPUT_MAX_CHANNELS;
 			}
-			for (uint8_t i=0; i<rc_in.channel_count; i++) {
+
+			for (uint8_t i = 0; i < rc_in.channel_count; i++) {
 				rc_in.values[i] = ppm_buffer[i];
 			}
-			rc_in.timestamp = ppm_last_valid_decode;
+
+			rc_in.timestamp_publication = ppm_last_valid_decode;
+			rc_in.timestamp_last_signal = ppm_last_valid_decode;
+
+			rc_in.rc_ppm_frame_length = ppm_frame_length;
+			rc_in.rssi = RC_INPUT_RSSI_MAX;
+			rc_in.rc_failsafe = false;
+			rc_in.rc_lost = false;
+			rc_in.rc_lost_frame_count = 0;
+			rc_in.rc_total_frame_count = 0;
 
 			/* lazily advertise on first publication */
 			if (to_input_rc == 0) {
 				to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in);
-			} else { 
+
+			} else {
 				orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
 			}
 		}
+
+#endif
+
 	}
 
 	::close(_t_actuators);
-	::close(_t_actuators_effective);
-	::close(_t_armed);
+	::close(_t_actuator_armed);
 
 	/* make sure servos are off */
 	up_pwm_servo_deinit();
@@ -579,6 +706,7 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
 	switch (_mode) {
 	case MODE_2PWM:
 	case MODE_4PWM:
+	case MODE_6PWM:
 		ret = pwm_ioctl(filp, cmd, arg);
 		break;
 
@@ -615,42 +743,243 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		up_pwm_servo_arm(false);
 		break;
 
+	case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
+		*(uint32_t *)arg = _pwm_default_rate;
+		break;
+
 	case PWM_SERVO_SET_UPDATE_RATE:
 		ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg);
 		break;
 
-	case PWM_SERVO_SELECT_UPDATE_RATE:
+	case PWM_SERVO_GET_UPDATE_RATE:
+		*(uint32_t *)arg = _pwm_alt_rate;
+		break;
+
+	case PWM_SERVO_SET_SELECT_UPDATE_RATE:
 		ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate);
 		break;
 
-	case PWM_SERVO_SET(2):
+	case PWM_SERVO_GET_SELECT_UPDATE_RATE:
+		*(uint32_t *)arg = _pwm_alt_rate_channels;
+		break;
+
+	case PWM_SERVO_SET_FAILSAFE_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			/* discard if too many values are sent */
+			if (pwm->channel_count > _max_actuators) {
+				ret = -EINVAL;
+				break;
+			}
+
+			for (unsigned i = 0; i < pwm->channel_count; i++) {
+				if (pwm->values[i] == 0) {
+					/* ignore 0 */
+				} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
+					_failsafe_pwm[i] = PWM_HIGHEST_MAX;
+
+				} else if (pwm->values[i] < PWM_LOWEST_MIN) {
+					_failsafe_pwm[i] = PWM_LOWEST_MIN;
+
+				} else {
+					_failsafe_pwm[i] = pwm->values[i];
+				}
+			}
+
+			/*
+			 * update the counter
+			 * this is needed to decide if disarmed PWM output should be turned on or not
+			 */
+			_num_failsafe_set = 0;
+
+			for (unsigned i = 0; i < _max_actuators; i++) {
+				if (_failsafe_pwm[i] > 0)
+					_num_failsafe_set++;
+			}
+
+			break;
+		}
+
+	case PWM_SERVO_GET_FAILSAFE_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			for (unsigned i = 0; i < _max_actuators; i++) {
+				pwm->values[i] = _failsafe_pwm[i];
+			}
+
+			pwm->channel_count = _max_actuators;
+			break;
+		}
+
+	case PWM_SERVO_SET_DISARMED_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			/* discard if too many values are sent */
+			if (pwm->channel_count > _max_actuators) {
+				ret = -EINVAL;
+				break;
+			}
+
+			for (unsigned i = 0; i < pwm->channel_count; i++) {
+				if (pwm->values[i] == 0) {
+					/* ignore 0 */
+				} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
+					_disarmed_pwm[i] = PWM_HIGHEST_MAX;
+
+				} else if (pwm->values[i] < PWM_LOWEST_MIN) {
+					_disarmed_pwm[i] = PWM_LOWEST_MIN;
+
+				} else {
+					_disarmed_pwm[i] = pwm->values[i];
+				}
+			}
+
+			/*
+			 * update the counter
+			 * this is needed to decide if disarmed PWM output should be turned on or not
+			 */
+			_num_disarmed_set = 0;
+
+			for (unsigned i = 0; i < _max_actuators; i++) {
+				if (_disarmed_pwm[i] > 0)
+					_num_disarmed_set++;
+			}
+
+			break;
+		}
+
+	case PWM_SERVO_GET_DISARMED_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			for (unsigned i = 0; i < _max_actuators; i++) {
+				pwm->values[i] = _disarmed_pwm[i];
+			}
+
+			pwm->channel_count = _max_actuators;
+			break;
+		}
+
+	case PWM_SERVO_SET_MIN_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			/* discard if too many values are sent */
+			if (pwm->channel_count > _max_actuators) {
+				ret = -EINVAL;
+				break;
+			}
+
+			for (unsigned i = 0; i < pwm->channel_count; i++) {
+				if (pwm->values[i] == 0) {
+					/* ignore 0 */
+				} else if (pwm->values[i] > PWM_HIGHEST_MIN) {
+					_min_pwm[i] = PWM_HIGHEST_MIN;
+
+				} else if (pwm->values[i] < PWM_LOWEST_MIN) {
+					_min_pwm[i] = PWM_LOWEST_MIN;
+
+				} else {
+					_min_pwm[i] = pwm->values[i];
+				}
+			}
+
+			break;
+		}
+
+	case PWM_SERVO_GET_MIN_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			for (unsigned i = 0; i < _max_actuators; i++) {
+				pwm->values[i] = _min_pwm[i];
+			}
+
+			pwm->channel_count = _max_actuators;
+			arg = (unsigned long)&pwm;
+			break;
+		}
+
+	case PWM_SERVO_SET_MAX_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			/* discard if too many values are sent */
+			if (pwm->channel_count > _max_actuators) {
+				ret = -EINVAL;
+				break;
+			}
+
+			for (unsigned i = 0; i < pwm->channel_count; i++) {
+				if (pwm->values[i] == 0) {
+					/* ignore 0 */
+				} else if (pwm->values[i] < PWM_LOWEST_MAX) {
+					_max_pwm[i] = PWM_LOWEST_MAX;
+
+				} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
+					_max_pwm[i] = PWM_HIGHEST_MAX;
+
+				} else {
+					_max_pwm[i] = pwm->values[i];
+				}
+			}
+
+			break;
+		}
+
+	case PWM_SERVO_GET_MAX_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			for (unsigned i = 0; i < _max_actuators; i++) {
+				pwm->values[i] = _max_pwm[i];
+			}
+
+			pwm->channel_count = _max_actuators;
+			arg = (unsigned long)&pwm;
+			break;
+		}
+
+	case PWM_SERVO_SET(5):
+	case PWM_SERVO_SET(4):
+		if (_mode < MODE_6PWM) {
+			ret = -EINVAL;
+			break;
+		}
+
+	/* FALLTHROUGH */
 	case PWM_SERVO_SET(3):
-		if (_mode != MODE_4PWM) {
+	case PWM_SERVO_SET(2):
+		if (_mode < MODE_4PWM) {
 			ret = -EINVAL;
 			break;
 		}
 
-		/* FALLTHROUGH */
-	case PWM_SERVO_SET(0):
+	/* FALLTHROUGH */
 	case PWM_SERVO_SET(1):
-		if (arg < 2100) {
+	case PWM_SERVO_SET(0):
+		if (arg <= 2100) {
 			up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
+
 		} else {
 			ret = -EINVAL;
 		}
 
 		break;
 
-	case PWM_SERVO_GET(2):
+	case PWM_SERVO_GET(5):
+	case PWM_SERVO_GET(4):
+		if (_mode < MODE_6PWM) {
+			ret = -EINVAL;
+			break;
+		}
+
+	/* FALLTHROUGH */
 	case PWM_SERVO_GET(3):
-		if (_mode != MODE_4PWM) {
+	case PWM_SERVO_GET(2):
+		if (_mode < MODE_4PWM) {
 			ret = -EINVAL;
 			break;
 		}
 
-		/* FALLTHROUGH */
-	case PWM_SERVO_GET(0):
+	/* FALLTHROUGH */
 	case PWM_SERVO_GET(1):
+	case PWM_SERVO_GET(0):
 		*(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0));
 		break;
 
@@ -658,20 +987,67 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	case PWM_SERVO_GET_RATEGROUP(1):
 	case PWM_SERVO_GET_RATEGROUP(2):
 	case PWM_SERVO_GET_RATEGROUP(3):
+	case PWM_SERVO_GET_RATEGROUP(4):
+	case PWM_SERVO_GET_RATEGROUP(5):
 		*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
 		break;
 
-	case PWM_SERVO_GET_COUNT:	
+	case PWM_SERVO_GET_COUNT:
 	case MIXERIOCGETOUTPUTCOUNT:
-		if (_mode == MODE_4PWM) {
+		switch (_mode) {
+		case MODE_6PWM:
+			*(unsigned *)arg = 6;
+			break;
+
+		case MODE_4PWM:
 			*(unsigned *)arg = 4;
+			break;
 
-		} else {
+		case MODE_2PWM:
 			*(unsigned *)arg = 2;
+			break;
+
+		default:
+			ret = -EINVAL;
+			break;
 		}
 
 		break;
 
+	case PWM_SERVO_SET_COUNT: {
+		/* change the number of outputs that are enabled for
+		 * PWM. This is used to change the split between GPIO
+		 * and PWM under control of the flight config
+		 * parameters. Note that this does not allow for
+		 * changing a set of pins to be used for serial on
+		 * FMUv1 
+		 */
+		switch (arg) {
+		case 0:
+			set_mode(MODE_NONE);
+			break;
+
+		case 2:
+			set_mode(MODE_2PWM);
+			break;
+
+		case 4:
+			set_mode(MODE_4PWM);
+			break;
+
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+		case 6:
+			set_mode(MODE_6PWM);
+			break;
+#endif
+
+		default:
+			ret = -EINVAL;
+			break;
+		}
+		break;
+	}
+
 	case MIXERIOCRESET:
 		if (_mixers != nullptr) {
 			delete _mixers;
@@ -722,6 +1098,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 					ret = -EINVAL;
 				}
 			}
+
 			break;
 		}
 
@@ -743,39 +1120,132 @@ ssize_t
 PX4FMU::write(file *filp, const char *buffer, size_t len)
 {
 	unsigned count = len / 2;
-	uint16_t values[4];
+	uint16_t values[6];
 
-	if (count > 4) {
-		// we only have 4 PWM outputs on the FMU
-		count = 4;
+	if (count > 6) {
+		// we have at most 6 outputs
+		count = 6;
 	}
 
 	// allow for misaligned values
-	memcpy(values, buffer, count*2);
+	memcpy(values, buffer, count * 2);
 
-	for (uint8_t i=0; i<count; i++) {
+	for (uint8_t i = 0; i < count; i++) {
 		up_pwm_servo_set(i, values[i]);
 	}
+
 	return count * 2;
 }
 
 void
+PX4FMU::sensor_reset(int ms)
+{
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+
+	if (ms < 1) {
+		ms = 1;
+	}
+
+	/* disable SPI bus */
+	stm32_configgpio(GPIO_SPI_CS_GYRO_OFF);
+	stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF);
+	stm32_configgpio(GPIO_SPI_CS_BARO_OFF);
+	stm32_configgpio(GPIO_SPI_CS_MPU_OFF);
+
+	stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0);
+	stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0);
+	stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0);
+	stm32_gpiowrite(GPIO_SPI_CS_MPU_OFF, 0);
+
+	stm32_configgpio(GPIO_SPI1_SCK_OFF);
+	stm32_configgpio(GPIO_SPI1_MISO_OFF);
+	stm32_configgpio(GPIO_SPI1_MOSI_OFF);
+
+	stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0);
+	stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0);
+	stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0);
+
+	stm32_configgpio(GPIO_GYRO_DRDY_OFF);
+	stm32_configgpio(GPIO_MAG_DRDY_OFF);
+	stm32_configgpio(GPIO_ACCEL_DRDY_OFF);
+	stm32_configgpio(GPIO_EXTI_MPU_DRDY_OFF);
+
+	stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0);
+	stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0);
+	stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0);
+	stm32_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 0);
+
+	/* set the sensor rail off */
+	stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
+	stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0);
+
+	/* wait for the sensor rail to reach GND */
+	usleep(ms * 1000);
+	warnx("reset done, %d ms", ms);
+
+	/* re-enable power */
+
+	/* switch the sensor rail back on */
+	stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1);
+
+	/* wait a bit before starting SPI, different times didn't influence results */
+	usleep(100);
+
+	/* reconfigure the SPI pins */
+#ifdef CONFIG_STM32_SPI1
+	stm32_configgpio(GPIO_SPI_CS_GYRO);
+	stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
+	stm32_configgpio(GPIO_SPI_CS_BARO);
+	stm32_configgpio(GPIO_SPI_CS_MPU);
+
+	/* 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_gpiowrite(GPIO_SPI_CS_MPU, 1);
+
+	// // XXX bring up the EXTI pins again
+	// stm32_configgpio(GPIO_GYRO_DRDY);
+	// stm32_configgpio(GPIO_MAG_DRDY);
+	// stm32_configgpio(GPIO_ACCEL_DRDY);
+	// stm32_configgpio(GPIO_EXTI_MPU_DRDY);
+
+#endif
+#endif
+}
+
+
+void
 PX4FMU::gpio_reset(void)
 {
 	/*
-	 * Setup default GPIO config - all pins as GPIOs, GPIO driver chip
-	 * to input mode.
+	 * Setup default GPIO config - all pins as GPIOs, input if
+	 * possible otherwise output if possible.
 	 */
-	for (unsigned i = 0; i < _ngpio; i++)
-		stm32_configgpio(_gpio_tab[i].input);
+	for (unsigned i = 0; i < _ngpio; i++) {
+		if (_gpio_tab[i].input != 0) {
+			stm32_configgpio(_gpio_tab[i].input);
+
+		} else if (_gpio_tab[i].output != 0) {
+			stm32_configgpio(_gpio_tab[i].output);
+		}
+	}
 
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+	/* if we have a GPIO direction control, set it to zero (input) */
 	stm32_gpiowrite(GPIO_GPIO_DIR, 0);
 	stm32_configgpio(GPIO_GPIO_DIR);
+#endif
 }
 
 void
 PX4FMU::gpio_set_function(uint32_t gpios, int function)
 {
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+
 	/*
 	 * GPIOs 0 and 1 must have the same direction as they are buffered
 	 * by a shared 2-port driver.  Any attempt to set either sets both.
@@ -788,6 +1258,8 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
 			stm32_gpiowrite(GPIO_GPIO_DIR, 1);
 	}
 
+#endif
+
 	/* configure selected GPIOs as required */
 	for (unsigned i = 0; i < _ngpio; i++) {
 		if (gpios & (1 << i)) {
@@ -809,9 +1281,13 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
 		}
 	}
 
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+
 	/* flip buffer to input mode if required */
 	if ((GPIO_SET_INPUT == function) && (gpios & 3))
 		stm32_gpiowrite(GPIO_GPIO_DIR, 0);
+
+#endif
 }
 
 void
@@ -849,6 +1325,10 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
 		gpio_reset();
 		break;
 
+	case GPIO_SENSOR_RAIL_RESET:
+		sensor_reset(arg);
+		break;
+
 	case GPIO_SET_OUTPUT:
 	case GPIO_SET_INPUT:
 	case GPIO_SET_ALT_1:
@@ -912,14 +1392,22 @@ fmu_new_mode(PortMode new_mode)
 		/* nothing more to do here */
 		break;
 
-	case PORT_FULL_SERIAL:
-		/* set all multi-GPIOs to serial mode */
-		gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
-		break;
-
 	case PORT_FULL_PWM:
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 		/* select 4-pin PWM mode */
 		servo_mode = PX4FMU::MODE_4PWM;
+#endif
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+		servo_mode = PX4FMU::MODE_6PWM;
+#endif
+		break;
+
+		/* mixed modes supported on v1 board only */
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+
+	case PORT_FULL_SERIAL:
+		/* set all multi-GPIOs to serial mode */
+		gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
 		break;
 
 	case PORT_GPIO_AND_SERIAL:
@@ -938,6 +1426,10 @@ fmu_new_mode(PortMode new_mode)
 		/* select 2-pin PWM mode */
 		servo_mode = PX4FMU::MODE_2PWM;
 		break;
+#endif
+
+	default:
+		return -1;
 	}
 
 	/* adjust GPIO config for serial mode(s) */
@@ -975,25 +1467,128 @@ fmu_start(void)
 	return ret;
 }
 
+int
+fmu_stop(void)
+{
+	int ret = OK;
+
+	if (g_fmu != nullptr) {
+
+		delete g_fmu;
+		g_fmu = nullptr;
+	}
+
+	return ret;
+}
+
+void
+sensor_reset(int ms)
+{
+	int	 fd;
+
+	fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
+
+	if (fd < 0)
+		errx(1, "open fail");
+
+	if (ioctl(fd, GPIO_SENSOR_RAIL_RESET, ms) < 0)
+		err(1, "servo arm failed");
+
+}
+
 void
 test(void)
 {
-	int	fd;
+	int	 fd;
+	unsigned servo_count = 0;
+	unsigned pwm_value = 1000;
+	int	 direction = 1;
+	int	 ret;
 
-	fd = open(PWM_OUTPUT_DEVICE_PATH, 0);
+	fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
 
 	if (fd < 0)
 		errx(1, "open fail");
 
 	if (ioctl(fd, PWM_SERVO_ARM, 0) < 0)       err(1, "servo arm failed");
 
-	if (ioctl(fd, PWM_SERVO_SET(0), 1000) < 0) err(1, "servo 1 set failed");
+	if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) {
+		err(1, "Unable to get servo count\n");
+	}
+
+	warnx("Testing %u servos", (unsigned)servo_count);
+
+	struct pollfd fds;
+	fds.fd = 0; /* stdin */
+	fds.events = POLLIN;
+
+	warnx("Press CTRL-C or 'c' to abort.");
+
+	for (;;) {
+		/* sweep all servos between 1000..2000 */
+		servo_position_t servos[servo_count];
+
+		for (unsigned i = 0; i < servo_count; i++)
+			servos[i] = pwm_value;
+
+		if (direction == 1) {
+			// use ioctl interface for one direction
+			for (unsigned i = 0; i < servo_count;	i++) {
+				if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) {
+					err(1, "servo %u set failed", i);
+				}
+			}
+
+		} else {
+			// and use write interface for the other direction
+			ret = write(fd, servos, sizeof(servos));
+
+			if (ret != (int)sizeof(servos))
+				err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
+		}
+
+		if (direction > 0) {
+			if (pwm_value < 2000) {
+				pwm_value++;
+
+			} else {
+				direction = -1;
+			}
+
+		} else {
+			if (pwm_value > 1000) {
+				pwm_value--;
+
+			} else {
+				direction = 1;
+			}
+		}
+
+		/* readback servo values */
+		for (unsigned i = 0; i < servo_count; i++) {
+			servo_position_t value;
+
+			if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value))
+				err(1, "error reading PWM servo %d", i);
+
+			if (value != servos[i])
+				errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]);
+		}
+
+		/* Check if user wants to quit */
+		char c;
+		ret = poll(&fds, 1, 0);
 
-	if (ioctl(fd, PWM_SERVO_SET(1), 1200) < 0) err(1, "servo 2 set failed");
+		if (ret > 0) {
 
-	if (ioctl(fd, PWM_SERVO_SET(2), 1400) < 0) err(1, "servo 3 set failed");
+			read(0, &c, 1);
 
-	if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed");
+			if (c == 0x03 || c == 0x63 || c == 'q') {
+				warnx("User abort\n");
+				break;
+			}
+		}
+	}
 
 	close(fd);
 
@@ -1044,6 +1639,21 @@ fmu_main(int argc, char *argv[])
 	PortMode new_mode = PORT_MODE_UNSET;
 	const char *verb = argv[1];
 
+	if (!strcmp(verb, "stop")) {
+		fmu_stop();
+		errx(0, "FMU driver stopped");
+	}
+
+	if (!strcmp(verb, "id")) {
+		char id[12];
+		(void)get_board_serial(id);
+
+		errx(0, "Board serial:\n %02X%02X%02X%02X %02X%02X%02X%02X %02X%02X%02X%02X",
+		     (unsigned)id[0], (unsigned)id[1], (unsigned)id[2], (unsigned)id[3], (unsigned)id[4], (unsigned)id[5],
+		     (unsigned)id[6], (unsigned)id[7], (unsigned)id[8], (unsigned)id[9], (unsigned)id[10], (unsigned)id[11]);
+	}
+
+
 	if (fmu_start() != OK)
 		errx(1, "failed to start the FMU driver");
 
@@ -1053,12 +1663,14 @@ fmu_main(int argc, char *argv[])
 	if (!strcmp(verb, "mode_gpio")) {
 		new_mode = PORT_FULL_GPIO;
 
-	} else if (!strcmp(verb, "mode_serial")) {
-		new_mode = PORT_FULL_SERIAL;
-
 	} else if (!strcmp(verb, "mode_pwm")) {
 		new_mode = PORT_FULL_PWM;
 
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+
+	} else if (!strcmp(verb, "mode_serial")) {
+		new_mode = PORT_FULL_SERIAL;
+
 	} else if (!strcmp(verb, "mode_gpio_serial")) {
 		new_mode = PORT_GPIO_AND_SERIAL;
 
@@ -1067,6 +1679,7 @@ fmu_main(int argc, char *argv[])
 
 	} else if (!strcmp(verb, "mode_pwm_gpio")) {
 		new_mode = PORT_PWM_AND_GPIO;
+#endif
 	}
 
 	/* was a new mode set? */
@@ -1087,7 +1700,25 @@ fmu_main(int argc, char *argv[])
 	if (!strcmp(verb, "fake"))
 		fake(argc - 1, argv + 1);
 
+	if (!strcmp(verb, "sensor_reset")) {
+		if (argc > 2) {
+			int reset_time = strtol(argv[2], 0, 0);
+			sensor_reset(reset_time);
+
+		} else {
+			sensor_reset(0);
+			warnx("resettet default time");
+		}
+
+		exit(0);
+	}
+
+
 	fprintf(stderr, "FMU: unrecognised command, try:\n");
-	fprintf(stderr, "  mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
+#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)
+	fprintf(stderr, "  mode_gpio, mode_pwm, test, sensor_reset [milliseconds]\n");
+#endif
 	exit(1);
 }
diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk
index 328e5a684..2054faa12 100644
--- a/src/drivers/px4io/module.mk
+++ b/src/drivers/px4io/module.mk
@@ -38,4 +38,9 @@
 MODULE_COMMAND		= px4io
 
 SRCS			= px4io.cpp \
-			  uploader.cpp
+			  px4io_uploader.cpp \
+			  px4io_serial.cpp \
+			  px4io_i2c.cpp
+
+# XXX prune to just get UART registers
+INCLUDE_DIRS    += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 8b2fae12b..7c7b3dcb7 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -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
@@ -35,7 +35,7 @@
  * @file px4io.cpp
  * Driver for the PX4IO board.
  *
- * PX4IO is connected via I2C.
+ * PX4IO is connected via I2C or DMA enabled high-speed UART.
  */
 
 #include <nuttx/config.h>
@@ -54,13 +54,14 @@
 #include <unistd.h>
 #include <fcntl.h>
 #include <math.h>
+#include <crc32.h>
 
 #include <arch/board/board.h>
 
 #include <drivers/device/device.h>
-#include <drivers/device/i2c.h>
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_pwm_output.h>
+#include <drivers/drv_sbus.h>
 #include <drivers/drv_gpio.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_mixer.h>
@@ -73,96 +74,217 @@
 #include <systemlib/param/param.h>
 
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_outputs.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>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/battery_status.h>
+#include <uORB/topics/servorail_status.h>
 #include <uORB/topics/parameter_update.h>
+
 #include <debug.h>
 
 #include <mavlink/mavlink_log.h>
-#include "uploader.h"
 #include <modules/px4iofirmware/protocol.h>
 
+#include "uploader.h"
+
+extern device::Device *PX4IO_i2c_interface() weak_function;
+extern device::Device *PX4IO_serial_interface() weak_function;
+
 #define PX4IO_SET_DEBUG			_IOC(0xff00, 0)
 #define PX4IO_INAIR_RESTART_ENABLE	_IOC(0xff00, 1)
+#define PX4IO_REBOOT_BOOTLOADER		_IOC(0xff00, 2)
+#define PX4IO_CHECK_CRC			_IOC(0xff00, 3)
+
+#define UPDATE_INTERVAL_MIN		2			// 2 ms	-> 500 Hz
+#define ORB_CHECK_INTERVAL		200000		// 200 ms -> 5 Hz
+#define IO_POLL_INTERVAL		20000		// 20 ms -> 50 Hz
 
-class PX4IO : public device::I2C
+/**
+ * The PX4IO class.
+ *
+ * Encapsulates PX4FMU to PX4IO communications modeled as file operations.
+ */
+class PX4IO : public device::CDev
 {
 public:
-	PX4IO();
+	/**
+	 * Constructor.
+	 *
+	 * Initialize all class variables.
+	 */
+	PX4IO(device::Device *interface);
+
+	/**
+	 * Destructor.
+	 *
+	 * Wait for worker thread to terminate.
+	 */
 	virtual ~PX4IO();
 
+	/**
+	 * Initialize the PX4IO class.
+	 *
+	 * Retrieve relevant initial system parameters. Initialize PX4IO registers.
+	 */
 	virtual int		init();
 
+	/**
+	 * Detect if a PX4IO is connected.
+	 *
+	 * Only validate if there is a PX4IO to talk to.
+	 */
+	virtual int		detect();
+
+	/**
+	 * IO Control handler.
+	 *
+	 * Handle all IOCTL calls to the PX4IO file descriptor.
+	 *
+	 * @param[in] filp file handle (not used). This function is always called directly through object reference
+	 * @param[in] cmd the IOCTL command
+	 * @param[in] the IOCTL command parameter (optional)
+	 */
 	virtual int		ioctl(file *filp, int cmd, unsigned long arg);
+
+	/**
+	 * write handler.
+	 *
+	 * Handle writes to the PX4IO file descriptor.
+	 *
+	 * @param[in] filp file handle (not used). This function is always called directly through object reference
+	 * @param[in] buffer pointer to the data buffer to be written
+	 * @param[in] len size in bytes to be written
+	 * @return number of bytes written
+	 */
 	virtual ssize_t		write(file *filp, const char *buffer, size_t len);
 
 	/**
 	* Set the update rate for actuator outputs from FMU to IO.
 	*
-	* @param rate    The rate in Hz actuator outpus are sent to IO.
-	*      Min 10 Hz, max 400 Hz
+	* @param[in] rate		The rate in Hz actuator outpus are sent to IO.
+	* 			Min 10 Hz, max 400 Hz
 	*/
-	int      set_update_rate(int rate);
+	int      		set_update_rate(int rate);
 
 	/**
 	* Set the battery current scaling and bias
 	*
-	* @param amp_per_volt
-	* @param amp_bias
+	* @param[in] amp_per_volt
+	* @param[in] amp_bias
 	*/
-	void      set_battery_current_scaling(float amp_per_volt, float amp_bias);
+	void      		set_battery_current_scaling(float amp_per_volt, float amp_bias);
 
 	/**
-	* Print the current status of IO
-	*/
+	 * Push failsafe values to IO.
+	 *
+	 * @param[in] vals	Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
+	 * @param[in] len	Number of channels, could up to 8
+	 */
+	int			set_failsafe_values(const uint16_t *vals, unsigned len);
+
+	/**
+	 * Disable RC input handling
+	 */
+	int			disable_rc_handling();
+
+	/**
+	 * Print IO status.
+	 *
+	 * Print all relevant IO status information
+	 */
 	void			print_status();
 
+	/**
+	 * Fetch and print debug console output.
+	 */
+	int			print_debug();
+
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+	/**
+	 * Set the DSM VCC is controlled by relay one flag
+	 *
+	 * @param[in] enable true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled
+	 */
+	inline void		set_dsm_vcc_ctl(bool enable) {
+		_dsm_vcc_ctl = enable;
+	};
+
+	/**
+	 * Get the DSM VCC is controlled by relay one flag
+	 *
+	 * @return true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled
+	 */
+	inline bool		get_dsm_vcc_ctl() {
+		return _dsm_vcc_ctl;
+	};
+#endif
+
+	inline uint16_t		system_status() const {return _status;}
+
 private:
+	device::Device		*_interface;
+
 	// XXX
-	unsigned		_max_actuators;
-	unsigned		_max_controls;
-	unsigned		_max_rc_input;
-	unsigned		_max_relays;
-	unsigned		_max_transfer;
+	unsigned		_hardware;		///< Hardware revision
+	unsigned		_max_actuators;		///< Maximum # of actuators supported by PX4IO
+	unsigned		_max_controls;		///< Maximum # of controls supported by PX4IO
+	unsigned		_max_rc_input;		///< Maximum receiver channels supported by PX4IO
+	unsigned		_max_relays;		///< Maximum relays supported by PX4IO
+	unsigned		_max_transfer;		///< Maximum number of I2C transfers supported by PX4IO
 
-	unsigned 		_update_interval; ///< subscription interval limiting send rate
+	unsigned 		_update_interval;	///< Subscription interval limiting send rate
+	bool			_rc_handling_disabled;	///< If set, IO does not evaluate, but only forward the RC values
+	unsigned		_rc_chan_count;		///< Internal copy of the last seen number of RC channels
+	uint64_t		_rc_last_valid;		///< last valid timestamp
 
-	volatile int		_task;		///< worker task
-	volatile bool		_task_should_exit;
+	volatile int		_task;			///< worker task id
+	volatile bool		_task_should_exit;	///< worker terminate flag
 
-	int			_mavlink_fd;
+	int			_mavlink_fd;		///< mavlink file descriptor.
 
-	perf_counter_t		_perf_update;
+	perf_counter_t		_perf_update;		///<local performance counter for status updates
+	perf_counter_t		_perf_write;		///<local performance counter for PWM control writes
+	perf_counter_t		_perf_chan_count;	///<local performance counter for channel number changes
 
 	/* cached IO state */
-	uint16_t		_status;
-	uint16_t		_alarms;
+	uint16_t		_status;		///< Various IO status flags
+	uint16_t		_alarms;		///< Various IO alarms
 
 	/* subscribed topics */
-	int			_t_actuators;	///< actuator controls topic
-	int			_t_armed;	///< system armed control topic
-	int 			_t_vstatus;	///< system / vehicle status
-	int			_t_param;	///< parameter update topic
+	int			_t_actuator_controls_0;	///< actuator controls group 0 topic
+	int			_t_actuator_controls_1;	///< actuator controls group 1 topic
+	int			_t_actuator_controls_2;	///< actuator controls group 2 topic
+	int			_t_actuator_controls_3;	///< actuator controls group 3 topic
+	int			_t_actuator_armed;	///< system armed control topic
+	int 			_t_vehicle_control_mode;///< vehicle control mode topic
+	int			_t_param;		///< parameter update topic
+	int			_t_vehicle_command;	///< vehicle command topic
 
 	/* advertised topics */
-	orb_advert_t 		_to_input_rc;	///< rc inputs from io
-	orb_advert_t 		_to_actuators_effective; ///< effective actuator controls topic
-	orb_advert_t		_to_outputs;	///< mixed servo outputs topic
-	orb_advert_t		_to_battery;	///< battery status / voltage
+	orb_advert_t 		_to_input_rc;		///< rc inputs from io
+	orb_advert_t		_to_outputs;		///< mixed servo outputs topic
+	orb_advert_t		_to_battery;		///< battery status / voltage
+	orb_advert_t		_to_servorail;		///< servorail status
+	orb_advert_t		_to_safety;		///< status of safety
 
-	actuator_outputs_s	_outputs;	///< mixed outputs
-	actuator_controls_effective_s _controls_effective; ///< effective controls
+	actuator_outputs_s	_outputs;		///< mixed outputs
+	servorail_status_s	_servorail_status;	///< servorail status
 
 	bool			_primary_pwm_device;	///< true if we are the default PWM output
+	bool			_lockdown_override;	///< allow to override the safety lockdown
+
+	float			_battery_amp_per_volt;	///< current sensor amps/volt
+	float			_battery_amp_bias;	///< current sensor bias
+	float			_battery_mamphour_total;///< amp hours consumed so far
+	uint64_t		_battery_last_timestamp;///< last amp hour calculation timestamp
 
-	float			_battery_amp_per_volt;
-	float			_battery_amp_bias;
-	float			_battery_mamphour_total;
-	uint64_t		_battery_last_timestamp;
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+	bool			_dsm_vcc_ctl;		///< true if relay 1 controls DSM satellite RX power
+#endif
 
 	/**
 	 * Trampoline to the worker task
@@ -175,9 +297,14 @@ private:
 	void			task_main();
 
 	/**
-	 * Send controls to IO
+	 * Send controls for one group to IO
+	 */
+	int			io_set_control_state(unsigned group);
+
+	/**
+	 * Send all controls to IO
 	 */
-	int			io_set_control_state();
+	int			io_set_control_groups();
 
 	/**
 	 * Update IO's arming-related state
@@ -197,6 +324,11 @@ private:
 	int			io_get_status();
 
 	/**
+	 * Disable RC input handling
+	 */
+	int			io_disable_rc_handling();
+
+	/**
 	 * Fetch RC inputs from IO.
 	 *
 	 * @param input_rc	Input structure to populate.
@@ -210,11 +342,6 @@ private:
 	int			io_publish_raw_rc();
 
 	/**
-	 * Fetch and publish the mixed control values.
-	 */
-	int			io_publish_mixed_controls();
-
-	/**
 	 * Fetch and publish the PWM servo outputs.
 	 */
 	int			io_publish_pwm_outputs();
@@ -226,7 +353,7 @@ private:
 	 * @param offset	Register offset to start writing at.
 	 * @param values	Pointer to array of values to write.
 	 * @param num_values	The number of values to write.
-	 * @return		Zero if all values were successfully written.
+	 * @return		OK if all values were successfully written.
 	 */
 	int			io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
 
@@ -236,7 +363,7 @@ private:
 	 * @param page		Register page to write to.
 	 * @param offset	Register offset to write to.
 	 * @param value		Value to write.
-	 * @return		Zero if the value was written successfully.
+	 * @return		OK if the value was written successfully.
 	 */
 	int			io_reg_set(uint8_t page, uint8_t offset, const uint16_t value);
 
@@ -247,7 +374,7 @@ private:
 	 * @param offset	Register offset to start reading from.
 	 * @param values	Pointer to array where values should be stored.
 	 * @param num_values	The number of values to read.
-	 * @return		Zero if all values were successfully read.
+	 * @return		OK if all values were successfully read.
 	 */
 	int			io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values);
 
@@ -274,7 +401,7 @@ private:
 	/**
 	 * Send mixer definition text to IO
 	 */
-	int			mixer_send(const char *buf, unsigned buflen);
+	int			mixer_send(const char *buf, unsigned buflen, unsigned retries = 3);
 
 	/**
 	 * Handle a status update from IO.
@@ -294,51 +421,92 @@ private:
 	 */
 	int			io_handle_alarms(uint16_t alarms);
 
-};
+	/**
+	 * Handle issuing dsm bind ioctl to px4io.
+	 *
+	 * @param dsmMode	0:dsm2, 1:dsmx
+	 */
+	void			dsm_bind_ioctl(int dsmMode);
+
+	/**
+	 * Handle a battery update from IO.
+	 *
+	 * Publish IO battery information if necessary.
+	 *
+	 * @param vbatt		vbatt register
+	 * @param ibatt		ibatt register
+	 */
+	void			io_handle_battery(uint16_t vbatt, uint16_t ibatt);
+
+	/**
+	 * Handle a servorail update from IO.
+	 *
+	 * Publish servo rail information if necessary.
+	 *
+	 * @param vservo	vservo register
+	 * @param vrssi 	vrssi register
+	 */
+	void			io_handle_vservo(uint16_t vservo, uint16_t vrssi);
 
+};
 
 namespace
 {
 
-PX4IO	*g_dev;
+PX4IO	*g_dev = nullptr;
 
 }
 
-PX4IO::PX4IO() :
-	I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000),
+PX4IO::PX4IO(device::Device *interface) :
+	CDev("px4io", PX4IO_DEVICE_PATH),
+	_interface(interface),
+	_hardware(0),
 	_max_actuators(0),
 	_max_controls(0),
 	_max_rc_input(0),
 	_max_relays(0),
 	_max_transfer(16),	/* sensible default */
 	_update_interval(0),
+	_rc_handling_disabled(false),
+	_rc_chan_count(0),
+	_rc_last_valid(0),
 	_task(-1),
 	_task_should_exit(false),
 	_mavlink_fd(-1),
-	_perf_update(perf_alloc(PC_ELAPSED, "px4io update")),
+	_perf_update(perf_alloc(PC_ELAPSED, "io update")),
+	_perf_write(perf_alloc(PC_ELAPSED, "io write")),
+	_perf_chan_count(perf_alloc(PC_COUNT, "io rc #")),
 	_status(0),
 	_alarms(0),
-	_t_actuators(-1),
-	_t_armed(-1),
-	_t_vstatus(-1),
+	_t_actuator_controls_0(-1),
+	_t_actuator_controls_1(-1),
+	_t_actuator_controls_2(-1),
+	_t_actuator_controls_3(-1),
+	_t_actuator_armed(-1),
+	_t_vehicle_control_mode(-1),
 	_t_param(-1),
+	_t_vehicle_command(-1),
 	_to_input_rc(0),
-	_to_actuators_effective(0),
 	_to_outputs(0),
 	_to_battery(0),
-	_battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
+	_to_servorail(0),
+	_to_safety(0),
+	_primary_pwm_device(false),
+	_lockdown_override(false),
+	_battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor
 	_battery_amp_bias(0),
 	_battery_mamphour_total(0),
-	_battery_last_timestamp(0),
-	_primary_pwm_device(false)
+	_battery_last_timestamp(0)
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+	, _dsm_vcc_ctl(false)
+#endif
+
 {
 	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
 
-	/* open MAVLink text channel */
-	_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
-
-	_debug_enabled = true;
+	_debug_enabled = false;
+	_servorail_status.rssi_v = 0;
 }
 
 PX4IO::~PX4IO()
@@ -356,10 +524,47 @@ PX4IO::~PX4IO()
 	if (_task != -1)
 		task_delete(_task);
 
+	if (_interface != nullptr)
+		delete _interface;
+
 	g_dev = nullptr;
 }
 
 int
+PX4IO::detect()
+{
+	int ret;
+
+	if (_task == -1) {
+
+		/* do regular cdev init */
+		ret = CDev::init();
+
+		if (ret != OK)
+			return ret;
+
+		/* get some parameters */
+		unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
+
+		if (protocol != PX4IO_PROTOCOL_VERSION) {
+			if (protocol == _io_reg_get_error) {
+				log("IO not installed");
+
+			} else {
+				log("IO version error");
+				mavlink_log_emergency(_mavlink_fd, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!");
+			}
+
+			return -1;
+		}
+	}
+
+	log("IO found");
+
+	return 0;
+}
+
+int
 PX4IO::init()
 {
 	int ret;
@@ -367,33 +572,43 @@ PX4IO::init()
 	ASSERT(_task == -1);
 
 	/* do regular cdev init */
-	ret = I2C::init();
+	ret = CDev::init();
+
 	if (ret != OK)
 		return ret;
 
-	/*
-	 * Enable a couple of retries for operations to IO.
-	 *
-	 * Register read/write operations are intentionally idempotent
-	 * so this is safe as designed.
-	 */
-	_retries = 2;
-
 	/* get some parameters */
+	unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
+
+	if (protocol == _io_reg_get_error) {
+		log("failed to communicate with IO");
+		mavlink_log_emergency(_mavlink_fd, "[IO] failed to communicate with IO, abort.");
+		return -1;
+	}
+
+	if (protocol != PX4IO_PROTOCOL_VERSION) {
+		log("protocol/firmware mismatch");
+		mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort.");
+		return -1;
+	}
+
+	_hardware      = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION);
 	_max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT);
-	_max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT);
+	_max_controls  = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT);
 	_max_relays    = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT);
 	_max_transfer  = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2;
 	_max_rc_input  = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT);
+
 	if ((_max_actuators < 1) || (_max_actuators > 255) ||
-	    (_max_relays < 1)    || (_max_relays > 255)    ||
-	    (_max_transfer < 16)   || (_max_transfer > 255)    ||
+	    (_max_relays > 32)   ||
+	    (_max_transfer < 16) || (_max_transfer > 255)  ||
 	    (_max_rc_input < 1)  || (_max_rc_input > 255)) {
 
-		log("failed getting parameters from PX4IO");
-		mavlink_log_emergency(_mavlink_fd, "[IO] param read fail, abort.");
+		log("config read error");
+		mavlink_log_emergency(_mavlink_fd, "[IO] config read fail, abort.");
 		return -1;
 	}
+
 	if (_max_rc_input > RC_INPUT_MAX_CHANNELS)
 		_max_rc_input = RC_INPUT_MAX_CHANNELS;
 
@@ -408,6 +623,7 @@ PX4IO::init()
 
 	/* get IO's last seen FMU state */
 	ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, &reg, sizeof(reg));
+
 	if (ret != OK)
 		return ret;
 
@@ -418,27 +634,31 @@ PX4IO::init()
 	if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
 	    (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
 
-	    	mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART");
+		/* get a status update from IO */
+		io_get_status();
+
+		mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART");
+		log("INAIR RESTART RECOVERY (needs commander app running)");
 
 		/* WARNING: COMMANDER app/vehicle status must be initialized.
 		 * If this fails (or the app is not started), worst-case IO
 		 * remains untouched (so manual override is still available).
 		 */
 
-		int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+		int safety_sub = orb_subscribe(ORB_ID(actuator_armed));
 		/* fill with initial values, clear updated flag */
-		vehicle_status_s status;
+		struct actuator_armed_s safety;
 		uint64_t try_start_time = hrt_absolute_time();
 		bool updated = false;
-		
-		/* keep checking for an update, ensure we got a recent state,
+
+		/* keep checking for an update, ensure we got a arming information,
 		   not something that was published a long time ago. */
 		do {
-			orb_check(vstatus_sub, &updated);
+			orb_check(safety_sub, &updated);
 
 			if (updated) {
 				/* got data, copy and exit loop */
-				orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status);
+				orb_copy(ORB_ID(actuator_armed), safety_sub, &safety);
 				break;
 			}
 
@@ -446,7 +666,7 @@ PX4IO::init()
 			usleep(10000);
 
 			/* abort after 5s */
-			if ((hrt_absolute_time() - try_start_time)/1000 > 50000) {
+			if ((hrt_absolute_time() - try_start_time) / 1000 > 3000) {
 				log("failed to recover from in-air restart (1), aborting IO driver init.");
 				return 1;
 			}
@@ -464,52 +684,65 @@ 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;
+		// 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;
 
 		/* send command once */
-		(void)orb_advertise(ORB_ID(vehicle_command), &cmd);
+		orb_advert_t pub = orb_advertise(ORB_ID(vehicle_command), &cmd);
 
 		/* spin here until IO's state has propagated into the system */
 		do {
-			orb_check(vstatus_sub, &updated);
+			orb_check(safety_sub, &updated);
 
 			if (updated) {
-				orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status);
+				orb_copy(ORB_ID(actuator_armed), safety_sub, &safety);
 			}
 
-			/* wait 10 ms */
-			usleep(10000);
+			/* wait 50 ms */
+			usleep(50000);
 
 			/* abort after 5s */
-			if ((hrt_absolute_time() - try_start_time)/1000 > 50000) {
+			if ((hrt_absolute_time() - try_start_time) / 1000 > 2000) {
 				log("failed to recover from in-air restart (2), aborting IO driver init.");
 				return 1;
 			}
 
-		/* keep waiting for state change for 10 s */
-		} while (!status.flag_system_armed);
+			/* re-send if necessary */
+			if (!safety.armed) {
+				orb_publish(ORB_ID(vehicle_command), pub, &cmd);
+				log("re-sending arm cmd");
+			}
 
-	/* regular boot, no in-air restart, init IO */
-	} else {
+			/* keep waiting for state change for 2 s */
+		} while (!safety.armed);
 
+		/* regular boot, no in-air restart, init IO */
+
+	} else {
 
 		/* dis-arm IO before touching anything */
-		io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 
-			PX4IO_P_SETUP_ARMING_FMU_ARMED |
-			PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK |
-			PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0);
+		io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING,
+			      PX4IO_P_SETUP_ARMING_FMU_ARMED |
+			      PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK |
+			      PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK |
+			      PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE, 0);
 
-		/* publish RC config to IO */
-		ret = io_set_rc_config();
-		if (ret != OK) {
-			log("failed to update RC input config");
-			mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail");
-			return ret;
+		if (_rc_handling_disabled) {
+			ret = io_disable_rc_handling();
+
+		} else {
+			/* publish RC config to IO */
+			ret = io_set_rc_config();
+
+			if (ret != OK) {
+				log("failed to update RC input config");
+				mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail");
+				return ret;
+			}
 		}
 
 	}
@@ -523,7 +756,7 @@ PX4IO::init()
 	}
 
 	/* start the IO interface task */
-	_task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
+	_task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 2048, (main_t)&PX4IO::task_main_trampoline, nullptr);
 
 	if (_task < 0) {
 		debug("task start failed: %d", errno);
@@ -544,39 +777,41 @@ PX4IO::task_main_trampoline(int argc, char *argv[])
 void
 PX4IO::task_main()
 {
-	hrt_abstime last_poll_time = 0;
+	hrt_abstime poll_last = 0;
+	hrt_abstime orb_check_last = 0;
 
-	log("starting");
+	_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
 
 	/*
 	 * 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));
-	orb_set_interval(_t_actuators, 20);		/* default to 50Hz */
-
-	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
-	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
-
-	_t_vstatus = orb_subscribe(ORB_ID(vehicle_status));
-	orb_set_interval(_t_vstatus, 200);		/* 5Hz update rate max. */
-
+	_t_actuator_controls_0 = orb_subscribe(ORB_ID(actuator_controls_0));
+	orb_set_interval(_t_actuator_controls_0, 20);		/* default to 50Hz */
+	_t_actuator_controls_1 = orb_subscribe(ORB_ID(actuator_controls_1));
+	orb_set_interval(_t_actuator_controls_1, 33);		/* default to 30Hz */
+	_t_actuator_controls_2 = orb_subscribe(ORB_ID(actuator_controls_2));
+	orb_set_interval(_t_actuator_controls_2, 33);		/* default to 30Hz */
+	_t_actuator_controls_3 = orb_subscribe(ORB_ID(actuator_controls_3));
+	orb_set_interval(_t_actuator_controls_3, 33);		/* default to 30Hz */
+	_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
+	_t_vehicle_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_t_param = orb_subscribe(ORB_ID(parameter_update));
-	orb_set_interval(_t_param, 500);		/* 2Hz update rate max. */
+	_t_vehicle_command = orb_subscribe(ORB_ID(vehicle_command));
+
+	if ((_t_actuator_controls_0 < 0) ||
+	    (_t_actuator_armed < 0) ||
+	    (_t_vehicle_control_mode < 0) ||
+	    (_t_param < 0) ||
+	    (_t_vehicle_command < 0)) {
+		log("subscription(s) failed");
+		goto out;
+	}
 
 	/* poll descriptor */
-	pollfd fds[4];
-	fds[0].fd = _t_actuators;
+	pollfd fds[1];
+	fds[0].fd = _t_actuator_controls_0;
 	fds[0].events = POLLIN;
-	fds[1].fd = _t_armed;
-	fds[1].events = POLLIN;
-	fds[2].fd = _t_vstatus;
-	fds[2].events = POLLIN;
-	fds[3].fd = _t_param;
-	fds[3].events = POLLIN;
-
-	debug("ready");
 
 	/* lock against the ioctl handler */
 	lock();
@@ -586,17 +821,23 @@ PX4IO::task_main()
 
 		/* adjust update interval */
 		if (_update_interval != 0) {
-			if (_update_interval < 5)
-				_update_interval = 5;
+			if (_update_interval < UPDATE_INTERVAL_MIN)
+				_update_interval = UPDATE_INTERVAL_MIN;
+
 			if (_update_interval > 100)
 				_update_interval = 100;
-			orb_set_interval(_t_actuators, _update_interval);
+
+			orb_set_interval(_t_actuator_controls_0, _update_interval);
+			/*
+			 * NOT changing the rate of groups 1-3 here, because only attitude
+			 * really needs to run fast.
+			 */
 			_update_interval = 0;
 		}
 
 		/* sleep waiting for topic updates, but no more than 20ms */
 		unlock();
-		int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 20);
+		int ret = ::poll(fds, 1, 20);
 		lock();
 
 		/* this would be bad... */
@@ -609,51 +850,104 @@ PX4IO::task_main()
 		hrt_abstime now = hrt_absolute_time();
 
 		/* if we have new control data from the ORB, handle it */
-		if (fds[0].revents & POLLIN)
-			io_set_control_state();
+		if (fds[0].revents & POLLIN) {
 
-		/* if we have an arming state update, handle it */
-		if ((fds[1].revents & POLLIN) || (fds[2].revents & POLLIN))
-			io_set_arming_state();
+			/* we're not nice to the lower-priority control groups and only check them
+			   when the primary group updated (which is now). */
+			(void)io_set_control_groups();
+		}
 
-		/*
-		 * If it's time for another tick of the polling status machine,
-		 * try it now.
-		 */
-		if ((now - last_poll_time) >= 20000) {
+		if (now >= poll_last + IO_POLL_INTERVAL) {
+			/* run at 50Hz */
+			poll_last = now;
 
-			/*
-			 * Pull status and alarms from IO.
-			 */
+			/* pull status and alarms from IO */
 			io_get_status();
 
-			/*
-			 * Get raw R/C input from IO.
-			 */
+			/* get raw R/C input from IO */
 			io_publish_raw_rc();
 
-			/*
-			 * Fetch mixed servo controls and PWM outputs from IO.
-			 *
-			 * XXX We could do this at a reduced rate in many/most cases.
-			 */
-			io_publish_mixed_controls();
+			/* fetch PWM outputs from IO */
 			io_publish_pwm_outputs();
+		}
+
+		if (now >= orb_check_last + ORB_CHECK_INTERVAL) {
+			/* run at 5Hz */
+			orb_check_last = now;
+
+			/* try to claim the MAVLink log FD */
+			if (_mavlink_fd < 0)
+				_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
+
+			/* check updates on uORB topics and handle it */
+			bool updated = false;
+
+			/* arming state */
+			orb_check(_t_actuator_armed, &updated);
+
+			if (!updated) {
+				orb_check(_t_vehicle_control_mode, &updated);
+			}
+
+			if (updated) {
+				io_set_arming_state();
+			}
+
+			/* vehicle command */
+			orb_check(_t_vehicle_command, &updated);
+
+			if (updated) {
+				struct vehicle_command_s cmd;
+				orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd);
+
+				// Check for a DSM pairing command
+				if (((int)cmd.command == VEHICLE_CMD_START_RX_PAIR) && ((int)cmd.param1 == 0)) {
+					dsm_bind_ioctl((int)cmd.param2);
+				}
+			}
 
 			/*
 			 * If parameters have changed, re-send RC mappings to IO
 			 *
 			 * XXX this may be a bit spammy
 			 */
-			if (fds[3].revents & POLLIN) {
-				parameter_update_s pupdate;
+			orb_check(_t_param, &updated);
 
-				/* copy to reset the notification */
+			if (updated) {
+				parameter_update_s pupdate;
 				orb_copy(ORB_ID(parameter_update), _t_param, &pupdate);
 
+				int32_t dsm_bind_val;
+				param_t dsm_bind_param;
+
+				/* see if bind parameter has been set, and reset it to -1 */
+				param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val);
+
+				if (dsm_bind_val > -1) {
+					dsm_bind_ioctl(dsm_bind_val);
+					dsm_bind_val = -1;
+					param_set(dsm_bind_param, &dsm_bind_val);
+				}
+
 				/* re-upload RC input config as it may have changed */
 				io_set_rc_config();
+
+				/* re-set the battery scaling */
+				int32_t voltage_scaling_val;
+				param_t voltage_scaling_param;
+
+				/* set battery voltage scaling */
+				param_get(voltage_scaling_param = param_find("BAT_V_SCALE_IO"), &voltage_scaling_val);
+
+				/* send scaling voltage to IO */
+				uint16_t scaling = voltage_scaling_val;
+				int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1);
+
+				if (pret != OK) {
+					log("voltage scaling upload failed");
+				}
 			}
+
 		}
 
 		perf_end(_perf_update);
@@ -661,6 +955,7 @@ PX4IO::task_main()
 
 	unlock();
 
+out:
 	debug("exiting");
 
 	/* clean up the alternate device node */
@@ -673,48 +968,112 @@ PX4IO::task_main()
 }
 
 int
-PX4IO::io_set_control_state()
+PX4IO::io_set_control_groups()
+{
+	int ret = io_set_control_state(0);
+
+	/* send auxiliary control groups */
+	(void)io_set_control_state(1);
+	(void)io_set_control_state(2);
+	(void)io_set_control_state(3);
+
+	return ret;
+}
+
+int
+PX4IO::io_set_control_state(unsigned group)
 {
 	actuator_controls_s	controls;	///< actuator outputs
 	uint16_t 		regs[_max_actuators];
 
 	/* get controls */
-	orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
-	     ORB_ID(actuator_controls_1), _t_actuators, &controls);
+	bool changed;
+
+	switch (group) {
+		case 0:
+		{
+			orb_check(_t_actuator_controls_0, &changed);
+
+			if (changed) {
+				orb_copy(ORB_ID(actuator_controls_0), _t_actuator_controls_0, &controls);
+			}
+		}
+		break;
+		case 1:
+		{
+			orb_check(_t_actuator_controls_1, &changed);
+
+			if (changed) {
+				orb_copy(ORB_ID(actuator_controls_1), _t_actuator_controls_1, &controls);
+			}
+		}
+		break;
+		case 2:
+		{
+			orb_check(_t_actuator_controls_2, &changed);
+
+			if (changed) {
+				orb_copy(ORB_ID(actuator_controls_2), _t_actuator_controls_2, &controls);
+			}
+		}
+		break;
+		case 3:
+		{
+			orb_check(_t_actuator_controls_3, &changed);
+
+			if (changed) {
+				orb_copy(ORB_ID(actuator_controls_3), _t_actuator_controls_3, &controls);
+			}
+		}
+		break;
+	}
+
+	if (!changed)
+		return -1;
 
 	for (unsigned i = 0; i < _max_controls; i++)
 		regs[i] = FLOAT_TO_REG(controls.control[i]);
 
 	/* copy values to registers in IO */
-	return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls);
+	return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, _max_controls);
 }
 
+
 int
 PX4IO::io_set_arming_state()
 {
 	actuator_armed_s	armed;		///< system armed state
-	vehicle_status_s	vstatus;	///< overall system state
+	vehicle_control_mode_s	control_mode;	///< vehicle_control_mode
 
-	orb_copy(ORB_ID(actuator_armed), _t_armed, &armed);
-	orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus);
+	orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed);
+	orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode);
 
 	uint16_t set = 0;
 	uint16_t clear = 0;
 
-	if (armed.armed && !armed.lockdown) {
+	if (armed.armed) {
 		set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
+
 	} else {
 		clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
 	}
 
+	if (armed.lockdown && !_lockdown_override) {
+		set |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
+	} else {
+		clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
+	}
+
 	if (armed.ready_to_arm) {
 		set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
+
 	} else {
 		clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
 	}
 
-	if (vstatus.flag_external_manual_override_ok) {
+	if (control_mode.flag_external_manual_override_ok) {
 		set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
+
 	} else {
 		clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
 	}
@@ -723,6 +1082,21 @@ PX4IO::io_set_arming_state()
 }
 
 int
+PX4IO::disable_rc_handling()
+{
+	return io_disable_rc_handling();
+}
+
+int
+PX4IO::io_disable_rc_handling()
+{
+	uint16_t set = PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED;
+	uint16_t clear = 0;
+
+	return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set);
+}
+
+int
 PX4IO::io_set_rc_config()
 {
 	unsigned offset = 0;
@@ -735,8 +1109,10 @@ PX4IO::io_set_rc_config()
 	 * assign RC_MAP_ROLL/PITCH/YAW/THROTTLE to the canonical
 	 * controls.
 	 */
+
+	/* fill the mapping with an error condition triggering value */
 	for (unsigned i = 0; i < _max_rc_input; i++)
-		input_map[i] = -1;
+		input_map[i] = UINT8_MAX;
 
 	/*
 	 * NOTE: The indices for mapped channels are 1-based
@@ -744,25 +1120,29 @@ PX4IO::io_set_rc_config()
 	 *       autopilots / GCS'.
 	 */
 	param_get(param_find("RC_MAP_ROLL"), &ichan);
+
 	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
 		input_map[ichan - 1] = 0;
 
 	param_get(param_find("RC_MAP_PITCH"), &ichan);
+
 	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
 		input_map[ichan - 1] = 1;
 
 	param_get(param_find("RC_MAP_YAW"), &ichan);
+
 	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
 		input_map[ichan - 1] = 2;
 
 	param_get(param_find("RC_MAP_THROTTLE"), &ichan);
+
 	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
 		input_map[ichan - 1] = 3;
 
-	ichan = 4;
-	for (unsigned i = 0; i < _max_rc_input; i++)
-		if (input_map[i] == -1)
-			input_map[i] = ichan++;
+	param_get(param_find("RC_MAP_MODE_SW"), &ichan);
+
+	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+		input_map[ichan - 1] = 4;
 
 	/*
 	 * Iterate all possible RC inputs.
@@ -814,6 +1194,7 @@ PX4IO::io_set_rc_config()
 
 		/* send channel config to IO */
 		ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE);
+
 		if (ret != OK) {
 			log("rc config upload failed");
 			break;
@@ -840,14 +1221,15 @@ PX4IO::io_handle_status(uint16_t status)
 	 */
 
 	/* check for IO reset - force it back to armed if necessary */
-	if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED)
-		&& !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
+	if (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF && !(status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
+	    && !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
 		/* set the arming flag */
-		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED | PX4IO_P_STATUS_FLAGS_ARM_SYNC);
+		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_SAFETY_OFF | PX4IO_P_STATUS_FLAGS_ARM_SYNC);
 
 		/* set new status */
 		_status = status;
-		_status &= PX4IO_P_STATUS_FLAGS_ARMED;
+		_status &= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
+
 	} else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
 
 		/* set the sync flag */
@@ -862,9 +1244,48 @@ PX4IO::io_handle_status(uint16_t status)
 		_status = status;
 	}
 
+	/**
+	 * Get and handle the safety status
+	 */
+	struct safety_s safety;
+	safety.timestamp = hrt_absolute_time();
+
+	if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
+		safety.safety_off = true;
+		safety.safety_switch_available = true;
+
+	} else {
+		safety.safety_off = false;
+		safety.safety_switch_available = true;
+	}
+
+	/* lazily publish the safety status */
+	if (_to_safety > 0) {
+		orb_publish(ORB_ID(safety), _to_safety, &safety);
+
+	} else {
+		_to_safety = orb_advertise(ORB_ID(safety), &safety);
+	}
+
 	return ret;
 }
 
+void
+PX4IO::dsm_bind_ioctl(int dsmMode)
+{
+	if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
+		mavlink_log_info(_mavlink_fd, "[IO] binding DSM%s RX", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "-X" : "-X8"));
+		int ret = ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES));
+
+		if (ret)
+			mavlink_log_critical(_mavlink_fd, "binding failed.");
+
+	} else {
+		mavlink_log_info(_mavlink_fd, "[IO] system armed, bind request rejected");
+	}
+}
+
+
 int
 PX4IO::io_handle_alarms(uint16_t alarms)
 {
@@ -878,53 +1299,92 @@ PX4IO::io_handle_alarms(uint16_t alarms)
 	return 0;
 }
 
+void
+PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
+{
+	/* only publish if battery has a valid minimum voltage */
+	if (vbatt <= 3300) {
+		return;
+	}
+
+	battery_status_s	battery_status;
+	battery_status.timestamp = hrt_absolute_time();
+
+	/* voltage is scaled to mV */
+	battery_status.voltage_v = vbatt / 1000.0f;
+	battery_status.voltage_filtered_v = vbatt / 1000.0f;
+
+	/*
+	  ibatt contains the raw ADC count, as 12 bit ADC
+	  value, with full range being 3.3v
+	*/
+	battery_status.current_a = ibatt * (3.3f / 4096.0f) * _battery_amp_per_volt;
+	battery_status.current_a += _battery_amp_bias;
+
+	/*
+	  integrate battery over time to get total mAh used
+	*/
+	if (_battery_last_timestamp != 0) {
+		_battery_mamphour_total += battery_status.current_a *
+					   (battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600;
+	}
+
+	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);
+
+	} else {
+		_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
+	}
+}
+
+void
+PX4IO::io_handle_vservo(uint16_t vservo, uint16_t vrssi)
+{
+	_servorail_status.timestamp = hrt_absolute_time();
+
+	/* voltage is scaled to mV */
+	_servorail_status.voltage_v = vservo * 0.001f;
+	_servorail_status.rssi_v    = vrssi * 0.001f;
+
+	/* lazily publish the servorail voltages */
+	if (_to_servorail > 0) {
+		orb_publish(ORB_ID(servorail_status), _to_servorail, &_servorail_status);
+
+	} else {
+		_to_servorail = orb_advertise(ORB_ID(servorail_status), &_servorail_status);
+	}
+}
+
 int
 PX4IO::io_get_status()
 {
-	uint16_t	regs[4];
+	uint16_t	regs[6];
 	int		ret;
 
-	/* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */
+	/* get
+	 * STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT,
+	 * STATUS_VSERVO, STATUS_VRSSI, STATUS_PRSSI
+	 * in that order */
 	ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &regs[0], sizeof(regs) / sizeof(regs[0]));
+
 	if (ret != OK)
 		return ret;
 
 	io_handle_status(regs[0]);
 	io_handle_alarms(regs[1]);
 
-	/* only publish if battery has a valid minimum voltage */
-	if (regs[2] > 3300) {
-		battery_status_s	battery_status;
-
-		battery_status.timestamp = hrt_absolute_time();
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+	io_handle_battery(regs[2], regs[3]);
+#endif
 
-		/* voltage is scaled to mV */
-		battery_status.voltage_v = regs[2] / 1000.0f;
-
-		/*
-		  regs[3] contains the raw ADC count, as 12 bit ADC
-		  value, with full range being 3.3v
-		 */
-		battery_status.current_a = regs[3] * (3.3f/4096.0f) * _battery_amp_per_volt;
-		battery_status.current_a += _battery_amp_bias;
-
-		/*
-		  integrate battery over time to get total mAh used
-		 */
-		if (_battery_last_timestamp != 0) {
-			_battery_mamphour_total += battery_status.current_a * 
-				(battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600;
-		}
-		battery_status.discharged_mah = _battery_mamphour_total;
-		_battery_last_timestamp = battery_status.timestamp;
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+	io_handle_vservo(regs[4], regs[5]);
+#endif
 
-		/* 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);
-		}
-	}
 	return ret;
 }
 
@@ -932,114 +1392,102 @@ int
 PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 {
 	uint32_t channel_count;
-	int	ret = OK;
+	int	ret;
 
 	/* 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);
+	uint16_t regs[RC_INPUT_MAX_CHANNELS + prolog];
+
 	/*
-	 * XXX Because the channel count and channel data are fetched
-	 *     separately, there is a risk of a race between the two
-	 *     that could leave us with channel data and a count that 
-	 *     are out of sync.
-	 *     Fixing this would require a guarantee of atomicity from
-	 *     IO, and a single fetch for both count and channels.
-	 *
-	 * XXX Since IO has the input calibration info, we ought to be
-	 *     able to get the pre-fixed-up controls directly.
+	 * Read the channel count and the first 9 channels.
 	 *
-	 * XXX can we do this more cheaply? If we knew we had DMA, it would
-	 *     almost certainly be better to just get all the inputs...
+	 * This should be the common case (9 channel R/C control being a reasonable upper bound).
 	 */
-	channel_count =  io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT);
-	if (channel_count == _io_reg_get_error)
-		return -EIO;
-	if (channel_count > RC_INPUT_MAX_CHANNELS)
-		channel_count = RC_INPUT_MAX_CHANNELS;
-	input_rc.channel_count = channel_count;
+	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)
+		return ret;
+
+	/*
+	 * Get the channel count any any extra channels. This is no more expensive than reading the
+	 * channel count once.
+	 */
+	channel_count = regs[PX4IO_P_RAW_RC_COUNT];
+
+	if (channel_count != _rc_chan_count)
+		perf_count(_perf_chan_count);
+
+	_rc_chan_count = channel_count;
+
+	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_frame_count = regs[PX4IO_P_RAW_LOST_FRAME_COUNT];
+	input_rc.rc_total_frame_count = regs[PX4IO_P_RAW_FRAME_COUNT];
 
-	if (channel_count > 0) {
-		ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, input_rc.values, channel_count);
-		if (ret == OK)
-			input_rc.timestamp = hrt_absolute_time();
+	/* rc_lost has to be set before the call to this function */
+	if (!input_rc.rc_lost && !input_rc.rc_failsafe)
+		_rc_last_valid = input_rc.timestamp_publication;
+
+	input_rc.timestamp_last_signal = _rc_last_valid;
+
+	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);
+
+		if (ret != OK)
+			return ret;
 	}
 
+	input_rc.channel_count = channel_count;
+	memcpy(input_rc.values, &regs[prolog], channel_count * 2);
+
 	return ret;
 }
 
 int
 PX4IO::io_publish_raw_rc()
 {
-	/* if no raw RC, just don't publish */
-	if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK))
-		return OK;
 
 	/* fetch values from IO */
 	rc_input_values	rc_val;
-	rc_val.timestamp = hrt_absolute_time();
+
+	/* set the RC status flag ORDER MATTERS! */
+	rc_val.rc_lost = !(_status & PX4IO_P_STATUS_FLAGS_RC_OK);
 
 	int ret = io_get_raw_rc_input(rc_val);
+
 	if (ret != OK)
 		return ret;
 
 	/* sort out the source of the values */
 	if (_status & PX4IO_P_STATUS_FLAGS_RC_PPM) {
 		rc_val.input_source = RC_INPUT_SOURCE_PX4IO_PPM;
+
 	} else if (_status & PX4IO_P_STATUS_FLAGS_RC_DSM) {
 		rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM;
+
 	} else if (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) {
 		rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SBUS;
+
 	} 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))
+			return OK;
 	}
 
 	/* lazily advertise on first publication */
 	if (_to_input_rc == 0) {
 		_to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_val);
-	} else { 
-		orb_publish(ORB_ID(input_rc), _to_input_rc, &rc_val);
-	}
-
-	return OK;
-}
 
-int
-PX4IO::io_publish_mixed_controls()
-{
-	/* if no FMU comms(!) just don't publish */
-	if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK))
-		return OK;
-
-	/* if not taking raw PPM from us, must be mixing */
-	if (_status & PX4IO_P_STATUS_FLAGS_RAW_PWM)
-		return OK;
-
-	/* data we are going to fetch */
-	actuator_controls_effective_s controls_effective;
-	controls_effective.timestamp = hrt_absolute_time();
-
-	/* get actuator controls from IO */
-	uint16_t act[_max_actuators];
-	int ret = io_reg_get(PX4IO_PAGE_ACTUATORS, 0, act, _max_actuators);
-	if (ret != OK)
-		return ret;
-
-	/* convert from register format to float */
-	for (unsigned i = 0; i < _max_actuators; i++)
-		controls_effective.control_effective[i] = REG_TO_FLOAT(act[i]);
-
-	/* laxily advertise on first publication */
-	if (_to_actuators_effective == 0) {
-		_to_actuators_effective = 
-			orb_advertise((_primary_pwm_device ? 
-				ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : 
-				ORB_ID(actuator_controls_effective_1)),
-					   &controls_effective);
 	} else {
-		orb_publish((_primary_pwm_device ? 
-			ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : 
-			ORB_ID(actuator_controls_effective_1)),
-			_to_actuators_effective, &controls_effective);
+		orb_publish(ORB_ID(input_rc), _to_input_rc, &rc_val);
 	}
 
 	return OK;
@@ -1059,26 +1507,29 @@ PX4IO::io_publish_pwm_outputs()
 	/* get servo values from IO */
 	uint16_t ctl[_max_actuators];
 	int ret = io_reg_get(PX4IO_PAGE_SERVOS, 0, ctl, _max_actuators);
+
 	if (ret != OK)
 		return ret;
 
 	/* convert from register format to float */
 	for (unsigned i = 0; i < _max_actuators; i++)
 		outputs.output[i] = ctl[i];
+
 	outputs.noutputs = _max_actuators;
 
 	/* lazily advertise on first publication */
 	if (_to_outputs == 0) {
 		_to_outputs = orb_advertise((_primary_pwm_device ?
-			ORB_ID_VEHICLE_CONTROLS :
-			ORB_ID(actuator_outputs_1)),
-			&outputs);
+					     ORB_ID_VEHICLE_CONTROLS :
+					     ORB_ID(actuator_outputs_1)),
+					    &outputs);
+
 	} else {
 		orb_publish((_primary_pwm_device ?
-			ORB_ID_VEHICLE_CONTROLS :
-			ORB_ID(actuator_outputs_1)),
-			_to_outputs,
-			&outputs);
+			     ORB_ID_VEHICLE_CONTROLS :
+			     ORB_ID(actuator_outputs_1)),
+			    _to_outputs,
+			    &outputs);
 	}
 
 	return OK;
@@ -1093,25 +1544,14 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned
 		return -EINVAL;
 	}
 
-	/* set up the transfer */
-	uint8_t 	addr[2] = {
-		page,
-		offset
-	};
-	i2c_msg_s	msgv[2];
+	int ret =  _interface->write((page << 8) | offset, (void *)values, num_values);
 
-	msgv[0].flags = 0;
-	msgv[0].buffer = addr;
-	msgv[0].length = 2;
-	msgv[1].flags = I2C_M_NORESTART;
-	msgv[1].buffer = (uint8_t *)values;
-	msgv[1].length = num_values * sizeof(*values);
+	if (ret != (int)num_values) {
+		debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, ret);
+		return -1;
+	}
 
-	/* perform the transfer */
-	int ret = transfer(msgv, 2);
-	if (ret != OK)
-		debug("io_reg_set: error %d", ret);
-	return ret;
+	return OK;
 }
 
 int
@@ -1123,25 +1563,20 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, uint16_t value)
 int
 PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values)
 {
-	/* set up the transfer */
-	uint8_t		addr[2] = {
-		page,
-		offset
-	};
-	i2c_msg_s	msgv[2];
+	/* range check the transfer */
+	if (num_values > ((_max_transfer) / sizeof(*values))) {
+		debug("io_reg_get: too many registers (%u, max %u)", num_values, _max_transfer / 2);
+		return -EINVAL;
+	}
 
-	msgv[0].flags = 0;
-	msgv[0].buffer = addr;
-	msgv[0].length = 2;
-	msgv[1].flags = I2C_M_READ;
-	msgv[1].buffer = (uint8_t *)values;
-	msgv[1].length = num_values * sizeof(*values);
+	int ret = _interface->read((page << 8) | offset, reinterpret_cast<void *>(values), num_values);
 
-	/* perform the transfer */
-	int ret = transfer(msgv, 2);
-	if (ret != OK)
-		debug("io_reg_get: data error %d", ret);
-	return ret;
+	if (ret != (int)num_values) {
+		debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, ret);
+		return -1;
+	}
+
+	return OK;
 }
 
 uint32_t
@@ -1149,7 +1584,7 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset)
 {
 	uint16_t value;
 
-	if (io_reg_get(page, offset, &value, 1))
+	if (io_reg_get(page, offset, &value, 1) != OK)
 		return _io_reg_get_error;
 
 	return value;
@@ -1162,8 +1597,10 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t
 	uint16_t value;
 
 	ret = io_reg_get(page, offset, &value, 1);
-	if (ret)
+
+	if (ret != OK)
 		return ret;
+
 	value &= ~clearbits;
 	value |= setbits;
 
@@ -1171,61 +1608,160 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t
 }
 
 int
-PX4IO::mixer_send(const char *buf, unsigned buflen)
+PX4IO::print_debug()
 {
-	uint8_t	frame[_max_transfer];
-	px4io_mixdata *msg = (px4io_mixdata *)&frame[0];
-	unsigned max_len = _max_transfer - sizeof(px4io_mixdata);
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+	int io_fd = -1;
 
-	msg->f2i_mixer_magic = F2I_MIXER_MAGIC;
-	msg->action = F2I_MIXER_ACTION_RESET;
+	if (io_fd < 0) {
+		io_fd = ::open("/dev/ttyS0", O_RDONLY | O_NONBLOCK | O_NOCTTY);
+	}
 
-	do {
-		unsigned count = buflen;
+	/* read IO's output */
+	if (io_fd > 0) {
+		pollfd fds[1];
+		fds[0].fd = io_fd;
+		fds[0].events = POLLIN;
 
-		if (count > max_len)
-			count = max_len;
+		usleep(500);
+		int pret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), 0);
 
-		if (count > 0) {
-			memcpy(&msg->text[0], buf, count);
-			buf += count;
-			buflen -= count;
-		}
+		if (pret > 0) {
+			int count;
+			char buf[65];
 
-		/*
-		 * We have to send an even number of bytes.  This
-		 * will only happen on the very last transfer of a
-		 * mixer, and we are guaranteed that there will be
-		 * space left to round up as _max_transfer will be
-		 * even.
-		 */
-		unsigned total_len = sizeof(px4io_mixdata) + count;
-		if (total_len % 1) {
-			msg->text[count] = '\0';
-			total_len++;
+			do {
+				count = ::read(io_fd, buf, sizeof(buf) - 1);
+
+				if (count > 0) {
+					/* enforce null termination */
+					buf[count] = '\0';
+					warnx("IO CONSOLE: %s", buf);
+				}
+
+			} while (count > 0);
 		}
 
-		int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2);
+		::close(io_fd);
+		return 0;
+	}
 
-		if (ret) {
-			log("mixer send error %d", ret);
-			return ret;
+#endif
+	return 1;
+
+}
+
+int
+PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
+{
+	/* get debug level */
+	int debuglevel = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG);
+
+	uint8_t	frame[_max_transfer];
+
+	do {
+
+		px4io_mixdata *msg = (px4io_mixdata *)&frame[0];
+		unsigned max_len = _max_transfer - sizeof(px4io_mixdata);
+
+		msg->f2i_mixer_magic = F2I_MIXER_MAGIC;
+		msg->action = F2I_MIXER_ACTION_RESET;
+
+		do {
+			unsigned count = buflen;
+
+			if (count > max_len)
+				count = max_len;
+
+			if (count > 0) {
+				memcpy(&msg->text[0], buf, count);
+				buf += count;
+				buflen -= count;
+
+			} else {
+				continue;
+			}
+
+			/*
+			 * We have to send an even number of bytes.  This
+			 * will only happen on the very last transfer of a
+			 * mixer, and we are guaranteed that there will be
+			 * space left to round up as _max_transfer will be
+			 * even.
+			 */
+			unsigned total_len = sizeof(px4io_mixdata) + count;
+
+			if (total_len % 2) {
+				msg->text[count] = '\0';
+				total_len++;
+			}
+
+			int ret;
+
+			for (int i = 0; i < 30; i++) {
+				/* failed, but give it a 2nd shot */
+				ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2);
+
+				if (ret) {
+					usleep(333);
+				} else {
+					break;
+				}
+			}
+
+			/* print mixer chunk */
+			if (debuglevel > 5 || ret) {
+
+				warnx("fmu sent: \"%s\"", msg->text);
+
+				/* read IO's output */
+				print_debug();
+			}
+
+			if (ret) {
+				log("mixer send error %d", ret);
+				return ret;
+			}
+
+			msg->action = F2I_MIXER_ACTION_APPEND;
+
+		} while (buflen > 0);
+
+		/* ensure a closing newline */
+		msg->text[0] = '\n';
+		msg->text[1] = '\0';
+
+		int ret;
+
+		for (int i = 0; i < 30; i++) {
+			/* failed, but give it a 2nd shot */
+			ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2);
+
+			if (ret) {
+				usleep(333);
+			} else {
+				break;
+			}
 		}
 
-		msg->action = F2I_MIXER_ACTION_APPEND;
+		if (ret)
+			return ret;
+
+		retries--;
+
+		log("mixer sent");
 
-	} while (buflen > 0);
+	} while (retries > 0 && (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK)));
 
 	/* check for the mixer-OK flag */
 	if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) {
-		debug("mixer upload OK");
 		mavlink_log_info(_mavlink_fd, "[IO] mixer upload ok");
 		return 0;
-	} else {
-		debug("mixer rejected by IO");
-		mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail");
 	}
 
+	log("mixer rejected by IO");
+	mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail");
+
 	/* load must have failed for some reason */
 	return -EINVAL;
 }
@@ -1234,123 +1770,195 @@ void
 PX4IO::print_status()
 {
 	/* basic configuration */
-	printf("protocol %u software %u bootloader %u buffer %uB\n",
-		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION),
-		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION),
-		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION),
-		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER));
+	printf("protocol %u hardware %u bootloader %u buffer %uB crc 0x%04x%04x\n",
+	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION),
+	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION),
+	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION),
+	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER),
+	       io_reg_get(PX4IO_PAGE_SETUP,  PX4IO_P_SETUP_CRC),
+	       io_reg_get(PX4IO_PAGE_SETUP,  PX4IO_P_SETUP_CRC+1));
 	printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n",
-		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT),
-		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT),
-		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT),
-		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT),
-		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT));
+	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT),
+	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT),
+	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT),
+	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT),
+	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT));
 
 	/* status */
 	printf("%u bytes free\n",
-		io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
+	       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
 	uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
-	printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s\n",
-		flags,
-		((flags & PX4IO_P_STATUS_FLAGS_ARMED)    ? " ARMED" : ""),
-		((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
-		((flags & PX4IO_P_STATUS_FLAGS_RC_OK)    ? " RC_OK" : " RC_FAIL"),
-		((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)   ? " PPM" : ""),
-		((flags & PX4IO_P_STATUS_FLAGS_RC_DSM)   ? " DSM" : ""),
-		((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)  ? " SBUS" : ""),
-		((flags & PX4IO_P_STATUS_FLAGS_FMU_OK)   ? " FMU_OK" : " FMU_FAIL"),
-		((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)  ? " RAW_PPM" : ""),
-		((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"),
-		((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"),
-		((flags & PX4IO_P_STATUS_FLAGS_INIT_OK)  ? " INIT_OK" : " INIT_FAIL"));
+	uint16_t io_status_flags = flags;
+	printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n",
+	       flags,
+	       ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""),
+	       ((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"),
+	       ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
+	       ((flags & PX4IO_P_STATUS_FLAGS_RC_OK)    ? " RC_OK" : " RC_FAIL"),
+	       ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)   ? " PPM" : ""),
+	       ((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)  ? " SBUS" : ""),
+	       ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK)   ? " FMU_OK" : " FMU_FAIL"),
+	       ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)  ? " RAW_PWM_PASSTHROUGH" : ""),
+	       ((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"),
+	       ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"),
+	       ((flags & PX4IO_P_STATUS_FLAGS_INIT_OK)  ? " INIT_OK" : " INIT_FAIL"),
+	       ((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE)  ? " FAILSAFE" : ""));
 	uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
-	printf("alarms 0x%04x%s%s%s%s%s%s%s\n",
-		alarms,
-		((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW)     ? " VBATT_LOW" : ""),
-		((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE)   ? " TEMPERATURE" : ""),
-		((alarms & PX4IO_P_STATUS_ALARMS_SERVO_CURRENT) ? " SERVO_CURRENT" : ""),
-		((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT)   ? " ACC_CURRENT" : ""),
-		((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST)      ? " FMU_LOST" : ""),
-		((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST)       ? " RC_LOST" : ""),
-		((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR)     ? " PWM_ERROR" : ""));
+	printf("alarms 0x%04x%s%s%s%s%s%s%s%s\n",
+	       alarms,
+	       ((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW)     ? " VBATT_LOW" : ""),
+	       ((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE)   ? " TEMPERATURE" : ""),
+	       ((alarms & PX4IO_P_STATUS_ALARMS_SERVO_CURRENT) ? " SERVO_CURRENT" : ""),
+	       ((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT)   ? " ACC_CURRENT" : ""),
+	       ((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST)      ? " FMU_LOST" : ""),
+	       ((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST)       ? " RC_LOST" : ""),
+	       ((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR)     ? " PWM_ERROR" : ""),
+	       ((alarms & PX4IO_P_STATUS_ALARMS_VSERVO_FAULT)  ? " VSERVO_FAULT" : ""));
 	/* now clear alarms */
 	io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, 0xFFFF);
 
-	printf("vbatt %u ibatt %u vbatt scale %u\n",
-	       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
-	       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT),
-	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE));
-	printf("amp_per_volt %.3f amp_offset %.3f mAhDischarged %.3f\n",
-	       (double)_battery_amp_per_volt,
-	       (double)_battery_amp_bias,
-	       (double)_battery_mamphour_total);
+	if (_hardware == 1) {
+		printf("vbatt mV %u ibatt mV %u vbatt scale %u\n",
+		       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
+		       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT),
+		       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE));
+		printf("amp_per_volt %.3f amp_offset %.3f mAh discharged %.3f\n",
+		       (double)_battery_amp_per_volt,
+		       (double)_battery_amp_bias,
+		       (double)_battery_mamphour_total);
+
+	} else if (_hardware == 2) {
+		printf("vservo %u mV vservo scale %u\n",
+		       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VSERVO),
+		       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VSERVO_SCALE));
+		printf("vrssi %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VRSSI));
+	}
+
 	printf("actuators");
+
 	for (unsigned i = 0; i < _max_actuators; i++)
 		printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i));
+
 	printf("\n");
 	printf("servos");
+
 	for (unsigned i = 0; i < _max_actuators; i++)
 		printf(" %u", io_reg_get(PX4IO_PAGE_SERVOS, i));
+
 	printf("\n");
 	uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT);
 	printf("%d raw R/C inputs", raw_inputs);
+
 	for (unsigned i = 0; i < raw_inputs; i++)
 		printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i));
+
 	printf("\n");
+
+	flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS);
+	printf("R/C flags: 0x%04x%s%s%s%s%s\n", flags,
+		(((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""),
+		(((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : ""),
+		((flags & PX4IO_P_RAW_RC_FLAGS_FRAME_DROP) ? " FRAME_DROP" : ""),
+		((flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) ? " FAILSAFE" : ""),
+		((flags & PX4IO_P_RAW_RC_FLAGS_MAPPING_OK) ? " MAPPING_OK" : "")
+	       );
+
+	if ((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) {
+		int frame_len = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_DATA);
+		printf("RC data (PPM frame len) %u us\n", frame_len);
+
+		if ((frame_len - raw_inputs * 2000 - 3000) < 0) {
+			printf("WARNING  WARNING  WARNING! This RC receiver does not allow safe frame detection.\n");
+		}
+	}
+
 	uint16_t mapped_inputs = io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_VALID);
 	printf("mapped R/C inputs 0x%04x", mapped_inputs);
+
 	for (unsigned i = 0; i < _max_rc_input; i++) {
 		if (mapped_inputs & (1 << i))
 			printf(" %u:%d", i, REG_TO_SIGNED(io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i)));
 	}
+
 	printf("\n");
 	uint16_t adc_inputs = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT);
 	printf("ADC inputs");
+
 	for (unsigned i = 0; i < adc_inputs; i++)
 		printf(" %u", io_reg_get(PX4IO_PAGE_RAW_ADC_INPUT, i));
+
 	printf("\n");
 
 	/* setup and state */
-	printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES));
+	uint16_t features = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES);
+	printf("features 0x%04x%s%s%s%s\n", features,
+		((features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) ? " S.BUS1_OUT" : ""),
+		((features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) ? " S.BUS2_OUT" : ""),
+		((features & PX4IO_P_SETUP_FEATURES_PWM_RSSI) ? " RSSI_PWM" : ""),
+		((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "")
+		);
 	uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
-	printf("arming 0x%04x%s%s%s%s\n",
-		arming,
-		((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)          ? " FMU_ARMED" : ""),
-		((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)	    ? " IO_ARM_OK" : ""),
-		((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
-		((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)   ? " INAIR_RESTART_OK" : ""));
+	printf("arming 0x%04x%s%s%s%s%s%s%s\n",
+	       arming,
+	       ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)		? " FMU_ARMED" : " FMU_DISARMED"),
+	       ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)		? " IO_ARM_OK" : " IO_ARM_DENIED"),
+	       ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK)	? " MANUAL_OVERRIDE_OK" : ""),
+	       ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)		? " FAILSAFE_CUSTOM" : ""),
+	       ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)	? " INAIR_RESTART_OK" : ""),
+	       ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)	? " ALWAYS_PWM_ENABLE" : ""),
+	       ((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)		? " LOCKDOWN" : ""));
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
-		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
-		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
-		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE),
-		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS));
+	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
+	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
+	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE),
+	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS));
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+	printf("rates 0x%04x default %u alt %u\n",
+	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
+	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
+	       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" : ""));
+		       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");
+
 	for (unsigned i = 0; i < _max_actuators; i++)
 		printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i));
+
+	printf("\ndisarmed values");
+
+	for (unsigned i = 0; i < _max_actuators; i++)
+		printf(" %u", io_reg_get(PX4IO_PAGE_DISARMED_PWM, i));
+
 	printf("\n");
 }
 
 int
-PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
+PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
+/* Make it obvious that file * isn't used here */
 {
 	int ret = OK;
 
@@ -1376,92 +1984,269 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0);
 		break;
 
+	case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
+		/* get the default update rate */
+		*(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE);
+		break;
+
 	case PWM_SERVO_SET_UPDATE_RATE:
 		/* set the requested alternate rate */
 		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg);
 		break;
 
-	case PWM_SERVO_SELECT_UPDATE_RATE: {
-
-		/* blindly clear the PWM update alarm - might be set for some other reason */
-		io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR);
+	case PWM_SERVO_GET_UPDATE_RATE:
+		/* get the alternative update rate */
+		*(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE);
+		break;
 
-		/* attempt to set the rate map */
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES, arg);
+	case PWM_SERVO_SET_SELECT_UPDATE_RATE: {
 
-		/* check that the changes took */
-		uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
-		if (alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) {
-			ret = -EINVAL;
+			/* blindly clear the PWM update alarm - might be set for some other reason */
 			io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR);
+
+			/* attempt to set the rate map */
+			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES, arg);
+
+			/* check that the changes took */
+			uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
+
+			if (alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) {
+				ret = -EINVAL;
+				io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR);
+			}
+
+			break;
 		}
+
+	case PWM_SERVO_GET_SELECT_UPDATE_RATE:
+
+		*(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES);
+		break;
+
+	case PWM_SERVO_SET_FAILSAFE_PWM: {
+		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
+		if (pwm->channel_count > _max_actuators)
+			/* fail with error */
+			return E2BIG;
+
+		/* copy values to registers in IO */
+		ret = io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, pwm->channel_count);
+		break;
+	}
+
+	case PWM_SERVO_GET_FAILSAFE_PWM:
+
+		ret = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, 0, (uint16_t*)arg, _max_actuators);
+		if (ret != OK) {
+			ret = -EIO;
+		}
+		break;
+
+	case PWM_SERVO_SET_DISARMED_PWM: {
+		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
+		if (pwm->channel_count > _max_actuators)
+			/* fail with error */
+			return E2BIG;
+
+		/* copy values to registers in IO */
+		ret = io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, pwm->channel_count);
+		break;
+	}
+
+	case PWM_SERVO_GET_DISARMED_PWM:
+
+		ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, (uint16_t*)arg, _max_actuators);
+		if (ret != OK) {
+			ret = -EIO;
+		}
+		break;
+
+	case PWM_SERVO_SET_MIN_PWM: {
+		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
+		if (pwm->channel_count > _max_actuators)
+			/* fail with error */
+			return E2BIG;
+
+		/* copy values to registers in IO */
+		ret = io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, pwm->channel_count);
 		break;
 	}
 
+	case PWM_SERVO_GET_MIN_PWM:
+
+		ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, (uint16_t*)arg, _max_actuators);
+		if (ret != OK) {
+			ret = -EIO;
+		}
+		break;
+
+	case PWM_SERVO_SET_MAX_PWM: {
+		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
+		if (pwm->channel_count > _max_actuators)
+			/* fail with error */
+			return E2BIG;
+
+		/* copy values to registers in IO */
+		ret = io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, pwm->channel_count);
+		break;
+	}
+
+	case PWM_SERVO_GET_MAX_PWM:
+
+		ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, (uint16_t*)arg, _max_actuators);
+		if (ret != OK) {
+			ret = -EIO;
+		}
+		break;
+
 	case PWM_SERVO_GET_COUNT:
 		*(unsigned *)arg = _max_actuators;
 		break;
 
-	case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS - 1): {
+	case PWM_SERVO_SET_DISABLE_LOCKDOWN:
+		_lockdown_override = arg;
+		break;
 
-		/* TODO: we could go lower for e.g. TurboPWM */
-		unsigned channel = cmd - PWM_SERVO_SET(0);
-		if ((channel >= _max_actuators) || (arg < 900) || (arg > 2100)) {
-			ret = -EINVAL;
+	case PWM_SERVO_GET_DISABLE_LOCKDOWN:
+		*(unsigned *)arg = _lockdown_override;
+		break;
+
+	case DSM_BIND_START:
+
+		/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */
+		if (arg == DSM2_BIND_PULSES ||
+		    arg == DSMX_BIND_PULSES ||
+		    arg == DSMX8_BIND_PULSES) {
+			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
+			usleep(500000);
+			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out);
+			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
+			usleep(72000);
+			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4));
+			usleep(50000);
+			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);
+
+			ret = OK;
 		} else {
-			/* send a direct PWM value */
-			ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg);
+			ret = -EINVAL;
 		}
+		break;
 
+	case DSM_BIND_POWER_UP:
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
 		break;
-	}
+
+	case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS - 1): {
+
+			/* TODO: we could go lower for e.g. TurboPWM */
+			unsigned channel = cmd - PWM_SERVO_SET(0);
+
+			if ((channel >= _max_actuators) || (arg < PWM_LOWEST_MIN) || (arg > PWM_HIGHEST_MAX)) {
+				ret = -EINVAL;
+
+			} else {
+				/* send a direct PWM value */
+				ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg);
+			}
+
+			break;
+		}
 
 	case PWM_SERVO_GET(0) ... PWM_SERVO_GET(PWM_OUTPUT_MAX_CHANNELS - 1): {
 
-		unsigned channel = cmd - PWM_SERVO_GET(0);
+			unsigned channel = cmd - PWM_SERVO_GET(0);
+
+			if (channel >= _max_actuators) {
+				ret = -EINVAL;
 
-		if (channel >= _max_actuators) {
-			ret = -EINVAL;
-		} else {
-			/* fetch a current PWM value */
-			uint32_t value = io_reg_get(PX4IO_PAGE_SERVOS, channel);
-			if (value == _io_reg_get_error) {
-				ret = -EIO;
 			} else {
-				*(servo_position_t *)arg = value;
+				/* fetch a current PWM value */
+				uint32_t value = io_reg_get(PX4IO_PAGE_SERVOS, channel);
+
+				if (value == _io_reg_get_error) {
+					ret = -EIO;
+
+				} else {
+					*(servo_position_t *)arg = value;
+				}
 			}
+
+			break;
 		}
-		break;
-	}
 
 	case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): {
 
-		unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0);
+			unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0);
 
-		uint32_t value = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel);
+			*(uint32_t *)arg = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel);
 
-		*(uint32_t *)arg = value;
-		break;
-	}
+			if (*(uint32_t *)arg == _io_reg_get_error)
+				ret = -EIO;
 
-	case GPIO_RESET:
-		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0);
-		break;
+			break;
+		}
+
+	case GPIO_RESET: {
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+			uint32_t bits = (1 << _max_relays) - 1;
+
+			/* don't touch relay1 if it's controlling RX vcc */
+			if (_dsm_vcc_ctl)
+				bits &= ~PX4IO_P_SETUP_RELAYS_POWER1;
+
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0);
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+			ret = -EINVAL;
+#endif
+			break;
+		}
 
 	case GPIO_SET:
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 		arg &= ((1 << _max_relays) - 1);
+
+		/* don't touch relay1 if it's controlling RX vcc */
+		if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) {
+			ret = -EINVAL;
+			break;
+		}
+
 		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg);
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+		ret = -EINVAL;
+#endif
 		break;
 
 	case GPIO_CLEAR:
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 		arg &= ((1 << _max_relays) - 1);
+
+		/* don't touch relay1 if it's controlling RX vcc */
+		if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) {
+			ret = -EINVAL;
+			break;
+		}
+
 		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0);
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+		ret = -EINVAL;
+#endif
 		break;
 
 	case GPIO_GET:
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 		*(uint32_t *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS);
+
 		if (*(uint32_t *)arg == _io_reg_get_error)
 			ret = -EIO;
+
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+		ret = -EINVAL;
+#endif
 		break;
 
 	case MIXERIOCGETOUTPUTCOUNT:
@@ -1473,53 +2258,115 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		break;
 
 	case MIXERIOCLOADBUF: {
-		const char *buf = (const char *)arg;
-		ret = mixer_send(buf, strnlen(buf, 1024));
-		break;
-	}
+			const char *buf = (const char *)arg;
+			ret = mixer_send(buf, strnlen(buf, 2048));
+			break;
+		}
 
 	case RC_INPUT_GET: {
-		uint16_t status;
-		rc_input_values *rc_val = (rc_input_values *)arg;
+			uint16_t status;
+			rc_input_values *rc_val = (rc_input_values *)arg;
 
-		ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &status, 1);
-		if (ret != OK)
-			break;
+			ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &status, 1);
 
-		/* if no R/C input, don't try to fetch anything */
-		if (!(status & PX4IO_P_STATUS_FLAGS_RC_OK)) {
-			ret = -ENOTCONN;
-			break;
-		}
+			if (ret != OK)
+				break;
 
-		/* sort out the source of the values */
-		if (status & PX4IO_P_STATUS_FLAGS_RC_PPM) {
-			rc_val->input_source = RC_INPUT_SOURCE_PX4IO_PPM;
-		} else if (status & PX4IO_P_STATUS_FLAGS_RC_DSM) {
-			rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM;
-		} else if (status & PX4IO_P_STATUS_FLAGS_RC_SBUS) {
-			rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SBUS;
-		} else {
-			rc_val->input_source = RC_INPUT_SOURCE_UNKNOWN;
-		}
+			/* if no R/C input, don't try to fetch anything */
+			if (!(status & PX4IO_P_STATUS_FLAGS_RC_OK)) {
+				ret = -ENOTCONN;
+				break;
+			}
 
-		/* read raw R/C input values */
-		ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, &(rc_val->values[0]), _max_rc_input);
-		break;
-	}
+			/* sort out the source of the values */
+			if (status & PX4IO_P_STATUS_FLAGS_RC_PPM) {
+				rc_val->input_source = RC_INPUT_SOURCE_PX4IO_PPM;
+
+			} else if (status & PX4IO_P_STATUS_FLAGS_RC_DSM) {
+				rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM;
+
+			} else if (status & PX4IO_P_STATUS_FLAGS_RC_SBUS) {
+				rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SBUS;
+
+			} else {
+				rc_val->input_source = RC_INPUT_SOURCE_UNKNOWN;
+			}
+
+			/* read raw R/C input values */
+			ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, &(rc_val->values[0]), _max_rc_input);
+			break;
+		}
 
 	case PX4IO_SET_DEBUG:
 		/* set the debug level */
 		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg);
 		break;
 
+	case PX4IO_REBOOT_BOOTLOADER:
+		if (system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
+			return -EINVAL;
+
+		/* reboot into bootloader - arg must be PX4IO_REBOOT_BL_MAGIC */
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_REBOOT_BL, arg);
+		// we don't expect a reply from this operation
+		ret = OK;
+		break;
+
+	case PX4IO_CHECK_CRC: {
+		/* check IO firmware CRC against passed value */		
+		uint32_t io_crc = 0;
+		ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC, (uint16_t *)&io_crc, 2);
+		if (ret != OK)
+			return ret;
+		if (io_crc != arg) {
+			debug("crc mismatch 0x%08x 0x%08x", (unsigned)io_crc, arg);
+			return -EINVAL;
+		}
+		break;
+	}
+
 	case PX4IO_INAIR_RESTART_ENABLE:
+
 		/* set/clear the 'in-air restart' bit */
 		if (arg) {
 			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK);
+
 		} else {
 			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK, 0);
 		}
+
+		break;
+
+	case RC_INPUT_ENABLE_RSSI_ANALOG:
+
+		if (arg) {
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_ADC_RSSI);
+		} else {
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_ADC_RSSI, 0);
+		}
+
+		break;
+
+	case RC_INPUT_ENABLE_RSSI_PWM:
+
+		if (arg) {
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_PWM_RSSI);
+		} else {
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_PWM_RSSI, 0);
+		}
+
+		break;
+
+	case SBUS_SET_PROTO_VERSION:
+
+		if (arg == 1) {
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS1_OUT);
+		} else if (arg == 2) {
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
+		} else {
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0);
+		}
+
 		break;
 
 	default:
@@ -1531,17 +2378,24 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 }
 
 ssize_t
-PX4IO::write(file *filp, const char *buffer, size_t len)
+PX4IO::write(file * /*filp*/, const char *buffer, size_t len)
+/* Make it obvious that file * isn't used here */
 {
 	unsigned count = len / 2;
 
 	if (count > _max_actuators)
 		count = _max_actuators;
+
 	if (count > 0) {
+
+		perf_begin(_perf_write);
 		int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count);
+		perf_end(_perf_write);
+
 		if (ret != OK)
 			return ret;
 	}
+
 	return count * 2;
 }
 
@@ -1549,8 +2403,9 @@ int
 PX4IO::set_update_rate(int rate)
 {
 	int interval_ms = 1000 / rate;
-	if (interval_ms < 5) {
-		interval_ms = 5;
+
+	if (interval_ms < UPDATE_INTERVAL_MIN) {
+		interval_ms = UPDATE_INTERVAL_MIN;
 		warnx("update rate too high, limiting interval to %d ms (%d Hz).", interval_ms, 1000 / interval_ms);
 	}
 
@@ -1575,27 +2430,224 @@ extern "C" __EXPORT int px4io_main(int argc, char *argv[]);
 namespace
 {
 
+device::Device *
+get_interface()
+{
+	device::Device *interface = nullptr;
+
+#ifndef CONFIG_ARCH_BOARD_PX4FMU_V1
+
+	/* try for a serial interface */
+	if (PX4IO_serial_interface != nullptr)
+		interface = PX4IO_serial_interface();
+
+	if (interface != nullptr)
+		goto got;
+
+#endif
+
+	/* try for an I2C interface if we haven't got a serial one */
+	if (PX4IO_i2c_interface != nullptr)
+		interface = PX4IO_i2c_interface();
+
+	if (interface != nullptr)
+		goto got;
+
+	errx(1, "cannot alloc interface");
+
+got:
+
+	if (interface->init() != OK) {
+		delete interface;
+		errx(1, "interface init failed");
+	}
+
+	return interface;
+}
+
 void
 start(int argc, char *argv[])
 {
 	if (g_dev != nullptr)
-		errx(1, "already loaded");
+		errx(0, "already loaded");
+
+	/* allocate the interface */
+	device::Device *interface = get_interface();
 
 	/* create the driver - it will set g_dev */
-	(void)new PX4IO();
+	(void)new PX4IO(interface);
 
-	if (g_dev == nullptr)
+	if (g_dev == nullptr) {
+		delete interface;
 		errx(1, "driver alloc failed");
+	}
 
 	if (OK != g_dev->init()) {
 		delete g_dev;
+		g_dev = nullptr;
 		errx(1, "driver init failed");
 	}
 
+	/* disable RC handling on request */
+	if (argc > 1) {
+		if (!strcmp(argv[1], "norc")) {
+
+			if (g_dev->disable_rc_handling())
+				warnx("Failed disabling RC handling");
+
+		} else {
+			warnx("unknown argument: %s", argv[1]);
+		}
+	}
+
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+	int dsm_vcc_ctl;
+
+	if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) {
+		if (dsm_vcc_ctl) {
+			g_dev->set_dsm_vcc_ctl(true);
+			g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0);
+		}
+	}
+
+#endif
+	exit(0);
+}
+
+void
+detect(int argc, char *argv[])
+{
+	if (g_dev != nullptr)
+		errx(0, "already loaded");
+
+	/* allocate the interface */
+	device::Device *interface = get_interface();
+
+	/* create the driver - it will set g_dev */
+	(void)new PX4IO(interface);
+
+	if (g_dev == nullptr)
+		errx(1, "driver alloc failed");
+
+	int ret = g_dev->detect();
+
+	delete g_dev;
+	g_dev = nullptr;
+
+	if (ret) {
+		/* nonzero, error */
+		exit(1);
+
+	} else {
+		exit(0);
+	}
+}
+
+void
+checkcrc(int argc, char *argv[])
+{
+	bool keep_running = false;
+
+	if (g_dev == nullptr) {
+		/* allocate the interface */
+		device::Device *interface = get_interface();
+
+		/* create the driver - it will set g_dev */
+		(void)new PX4IO(interface);
+
+		if (g_dev == nullptr)
+			errx(1, "driver alloc failed");
+	} else {
+		/* its already running, don't kill the driver */
+		keep_running = true;
+	}
+
+	/*
+	  check IO CRC against CRC of a file
+	 */
+	if (argc < 2) {
+		printf("usage: px4io checkcrc filename\n");
+		exit(1);
+	}
+	int fd = open(argv[1], O_RDONLY);
+	if (fd == -1) {
+		printf("open of %s failed - %d\n", argv[1], errno);
+		exit(1);			
+	}
+	const uint32_t app_size_max = 0xf000;
+	uint32_t fw_crc = 0;
+	uint32_t nbytes = 0;
+	while (true) {
+		uint8_t buf[16];
+		int n = read(fd, buf, sizeof(buf));
+		if (n <= 0) break;
+		fw_crc = crc32part(buf, n, fw_crc);
+		nbytes += n;
+	}
+	close(fd);
+	while (nbytes < app_size_max) {
+		uint8_t b = 0xff;
+		fw_crc = crc32part(&b, 1, fw_crc);			
+		nbytes++;
+	}
+
+	int ret = g_dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc);
+
+	if (!keep_running) {
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	if (ret != OK) {
+		printf("check CRC failed - %d\n", ret);
+		exit(1);			
+	}
+	printf("CRCs match\n");
 	exit(0);
 }
 
 void
+bind(int argc, char *argv[])
+{
+	int pulses;
+
+	if (g_dev == nullptr)
+		errx(1, "px4io must be started first");
+
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+
+	if (!g_dev->get_dsm_vcc_ctl())
+		errx(1, "DSM bind feature not enabled");
+
+#endif
+
+	if (argc < 3)
+		errx(0, "needs argument, use dsm2, dsmx or dsmx8");
+
+	if (!strcmp(argv[2], "dsm2"))
+		pulses = DSM2_BIND_PULSES;
+	else if (!strcmp(argv[2], "dsmx"))
+		pulses = DSMX_BIND_PULSES;
+	else if (!strcmp(argv[2], "dsmx8"))
+		pulses = DSMX8_BIND_PULSES;
+	else 
+		errx(1, "unknown parameter %s, use dsm2, dsmx or dsmx8", argv[2]);
+	// Test for custom pulse parameter
+	if (argc > 3)
+		pulses = atoi(argv[3]);
+	if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) 
+		errx(1, "system must not be armed");
+
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+	warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1.");
+#endif
+	g_dev->ioctl(nullptr, DSM_BIND_START, pulses);
+
+	exit(0);
+
+}
+
+void
 test(void)
 {
 	int		fd;
@@ -1604,7 +2656,7 @@ test(void)
 	int		direction = 1;
 	int		ret;
 
-	fd = open("/dev/px4io", O_WRONLY);
+	fd = open(PX4IO_DEVICE_PATH, O_WRONLY);
 
 	if (fd < 0)
 		err(1, "failed to open device");
@@ -1612,29 +2664,46 @@ test(void)
 	if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count))
 		err(1, "failed to get servo count");
 
+	/* tell IO that its ok to disable its safety with the switch */
+	ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
+
+	if (ret != OK)
+		err(1, "PWM_SERVO_SET_ARM_OK");
+
 	if (ioctl(fd, PWM_SERVO_ARM, 0))
 		err(1, "failed to arm servos");
 
+	struct pollfd fds;
+	fds.fd = 0; /* stdin */
+	fds.events = POLLIN;
+
+	warnx("Press CTRL-C or 'c' to abort.");
+
 	for (;;) {
 
 		/* sweep all servos between 1000..2000 */
 		servo_position_t servos[servo_count];
+
 		for (unsigned i = 0; i < servo_count; i++)
 			servos[i] = pwm_value;
 
 		ret = write(fd, servos, sizeof(servos));
+
 		if (ret != (int)sizeof(servos))
 			err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
 
 		if (direction > 0) {
 			if (pwm_value < 2000) {
 				pwm_value++;
+
 			} else {
 				direction = -1;
 			}
+
 		} else {
 			if (pwm_value > 1000) {
 				pwm_value--;
+
 			} else {
 				direction = 1;
 			}
@@ -1646,42 +2715,137 @@ test(void)
 
 			if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value))
 				err(1, "error reading PWM servo %d", i);
+
 			if (value != servos[i])
 				errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]);
 		}
+
+		/* Check if user wants to quit */
+		char c;
+		ret = poll(&fds, 1, 0);
+
+		if (ret > 0) {
+
+			read(0, &c, 1);
+
+			if (c == 0x03 || c == 0x63 || c == 'q') {
+				warnx("User abort\n");
+				exit(0);
+			}
+		}
 	}
 }
 
 void
 monitor(void)
 {
-	unsigned cancels = 3;
-	printf("Hit <enter> three times to exit monitor mode\n");
+	/* clear screen */
+	printf("\033[2J");
+
+	unsigned cancels = 2;
 
 	for (;;) {
 		pollfd fds[1];
 
 		fds[0].fd = 0;
 		fds[0].events = POLLIN;
-		poll(fds, 1, 500);
+		poll(fds, 1, 2000);
 
 		if (fds[0].revents == POLLIN) {
 			int c;
 			read(0, &c, 1);
 
-			if (cancels-- == 0)
+			if (cancels-- == 0) {
+				printf("\033[2J\033[H"); /* move cursor home and clear screen */
 				exit(0);
+			}
 		}
 
-#warning implement this
+		if (g_dev != nullptr) {
+
+			printf("\033[2J\033[H"); /* move cursor home and clear screen */
+			(void)g_dev->print_status();
+			(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");
 
-//		if (g_dev != nullptr)
-//			g_dev->dump_one = true;
+		} else {
+			errx(1, "driver not loaded, exiting");
+		}
 	}
 }
 
+void
+if_test(unsigned mode)
+{
+	device::Device *interface = get_interface();
+	int result;
+
+	if (interface) {
+		result = interface->ioctl(1, mode); /* XXX magic numbers */
+		delete interface;
+	} else {
+		errx(1, "interface not loaded, exiting");
+	}
+
+	errx(0, "test returned %d", result);
 }
 
+void
+lockdown(int argc, char *argv[])
+{
+	if (g_dev != nullptr) {
+
+			if (argc > 2 && !strcmp(argv[2], "disable")) {
+
+				warnx("WARNING: ACTUATORS WILL BE LIVE IN HIL! PROCEED?");
+				warnx("Press 'y' to enable, any other key to abort.");
+
+				/* check if user wants to abort */
+				char c;
+
+				struct pollfd fds;
+				int ret;
+				hrt_abstime start = hrt_absolute_time();
+				const unsigned long timeout = 5000000;
+
+				while (hrt_elapsed_time(&start) < timeout) {
+					fds.fd = 0; /* stdin */
+					fds.events = POLLIN;
+					ret = poll(&fds, 1, 0);
+
+					if (ret > 0) {
+
+						read(0, &c, 1);
+
+						if (c != 'y') {
+							exit(0);
+						} else if (c == 'y') {
+							break;
+						}
+					}
+
+					usleep(10000);
+				}
+
+				if (hrt_elapsed_time(&start) > timeout)
+					errx(1, "TIMEOUT! ABORTED WITHOUT CHANGES.");
+
+				(void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 1);
+
+				warnx("WARNING: ACTUATORS ARE NOW LIVE IN HIL!");
+			} else {
+				(void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 0);
+				warnx("ACTUATORS ARE NOW SAFE IN HIL.");
+			}
+			
+		} else {
+			errx(1, "driver not loaded, exiting");
+		}
+	exit(0);
+}
+
+} /* namespace */
+
 int
 px4io_main(int argc, char *argv[])
 {
@@ -1692,67 +2856,181 @@ px4io_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start"))
 		start(argc - 1, argv + 1);
 
-	if (!strcmp(argv[1], "limit")) {
+	if (!strcmp(argv[1], "detect"))
+		detect(argc - 1, argv + 1);
+
+	if (!strcmp(argv[1], "checkcrc"))
+		checkcrc(argc - 1, argv + 1);
+
+	if (!strcmp(argv[1], "update")) {
 
 		if (g_dev != nullptr) {
+			printf("[px4io] loaded, detaching first\n");
+			/* stop the driver */
+			delete g_dev;
+			g_dev = nullptr;
+		}
 
-			if ((argc > 2)) {
-				g_dev->set_update_rate(atoi(argv[2]));
-			} else {
-				errx(1, "missing argument (50 - 200 Hz)");
-				return 1;
-			}
+		PX4IO_Uploader *up;
+		const char *fn[4];
+
+		/* work out what we're uploading... */
+		if (argc > 2) {
+			fn[0] = argv[2];
+			fn[1] = nullptr;
+
+		} else {
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+			fn[0] = "/etc/extras/px4io-v1_default.bin";
+			fn[1] =	"/fs/microsd/px4io1.bin";
+			fn[2] =	"/fs/microsd/px4io.bin";
+			fn[3] =	nullptr;
+#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+			fn[0] = "/etc/extras/px4io-v2_default.bin";
+			fn[1] =	"/fs/microsd/px4io2.bin";
+			fn[2] =	"/fs/microsd/px4io.bin";
+			fn[3] =	nullptr;
+#else
+#error "unknown board"
+#endif
 		}
-		exit(0);
+
+		up = new PX4IO_Uploader;
+		int ret = up->upload(&fn[0]);
+		delete up;
+
+		switch (ret) {
+		case OK:
+			break;
+
+		case -ENOENT:
+			errx(1, "PX4IO firmware file not found");
+
+		case -EEXIST:
+		case -EIO:
+			errx(1, "error updating PX4IO - check that bootloader mode is enabled");
+
+		case -EINVAL:
+			errx(1, "verify failed - retry the update");
+
+		case -ETIMEDOUT:
+			errx(1, "timed out waiting for bootloader - power-cycle and try again");
+
+		default:
+			errx(1, "unexpected error %d", ret);
+		}
+
+		return ret;
 	}
 
-	if (!strcmp(argv[1], "current")) {
-		if (g_dev != nullptr) {
-			if ((argc > 3)) {
-				g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3]));
-			} else {
-				errx(1, "missing argument (apm_per_volt, amp_offset)");
-				return 1;
+	if (!strcmp(argv[1], "iftest")) {
+		if (g_dev != nullptr)
+			errx(1, "can't iftest when started");
+
+		if_test((argc > 2) ? strtol(argv[2], NULL, 0) : 0);
+	}
+
+	if (!strcmp(argv[1], "forceupdate")) {
+		/*
+		  force update of the IO firmware without requiring
+		  the user to hold the safety switch down
+		 */
+		if (argc <= 3) {
+			warnx("usage: px4io forceupdate MAGIC filename");
+			exit(1);
+		}
+		if (g_dev == nullptr) {
+			warnx("px4io is not started, still attempting upgrade");
+
+			/* allocate the interface */
+			device::Device *interface = get_interface();
+
+			/* create the driver - it will set g_dev */
+			(void)new PX4IO(interface);
+
+			if (g_dev == nullptr) {
+				delete interface;
+				errx(1, "driver alloc failed");
 			}
 		}
+
+		uint16_t arg = atol(argv[2]);
+		int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg);
+		if (ret != OK) {
+			printf("reboot failed - %d\n", ret);
+			exit(1);
+		}
+
+		// tear down the px4io instance
+		delete g_dev;
+		g_dev = nullptr;
+
+		// upload the specified firmware
+		const char *fn[2];
+		fn[0] = argv[3];
+		fn[1] = nullptr;
+		PX4IO_Uploader *up = new PX4IO_Uploader;
+		up->upload(&fn[0]);
+		delete up;
 		exit(0);
 	}
 
-	if (!strcmp(argv[1], "recovery")) {
+	/* commands below here require a started driver */
+
+	if (g_dev == nullptr)
+		errx(1, "not started");
+
+	if (!strcmp(argv[1], "limit")) {
+
+		if ((argc > 2)) {
+			g_dev->set_update_rate(atoi(argv[2]));
 
-		if (g_dev != nullptr) {
-			/*
-			 * Enable in-air restart support.
-			 * We can cheat and call the driver directly, as it
-		 	 * doesn't reference filp in ioctl()
-			 */
-			g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1);
 		} else {
-			errx(1, "not loaded");
+			errx(1, "missing argument (50 - 500 Hz)");
+			return 1;
 		}
+
 		exit(0);
 	}
 
-	if (!strcmp(argv[1], "stop")) {
+	if (!strcmp(argv[1], "current")) {
+		if ((argc > 3)) {
+			g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3]));
 
-		if (g_dev != nullptr) {
-			/* stop the driver */
-			delete g_dev;
 		} else {
-			errx(1, "not loaded");
+			errx(1, "missing argument (apm_per_volt, amp_offset)");
+			return 1;
 		}
+
+		exit(0);
+	}
+
+
+
+	if (!strcmp(argv[1], "recovery")) {
+
+		/*
+		 * Enable in-air restart support.
+		 * We can cheat and call the driver directly, as it
+		 * doesn't reference filp in ioctl()
+		 */
+		g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+
+		/* stop the driver */
+		delete g_dev;
+		g_dev = nullptr;
 		exit(0);
 	}
 
 
 	if (!strcmp(argv[1], "status")) {
 
-		if (g_dev != nullptr) {
-			printf("[px4io] loaded\n");
-			g_dev->print_status();
-		} else {
-			printf("[px4io] not loaded\n");
-		}
+		printf("[px4io] loaded\n");
+		g_dev->print_status();
 
 		exit(0);
 	}
@@ -1762,88 +3040,100 @@ px4io_main(int argc, char *argv[])
 			printf("usage: px4io debug LEVEL\n");
 			exit(1);
 		}
+
 		if (g_dev == nullptr) {
 			printf("px4io is not started\n");
 			exit(1);
 		}
+
 		uint8_t level = atoi(argv[2]);
 		/* we can cheat and call the driver directly, as it
 		 * doesn't reference filp in ioctl()
 		 */
-		int ret = g_dev->ioctl(NULL, PX4IO_SET_DEBUG, level);
+		int ret = g_dev->ioctl(nullptr, PX4IO_SET_DEBUG, level);
+
 		if (ret != 0) {
 			printf("SET_DEBUG failed - %d\n", ret);
 			exit(1);
 		}
+
 		printf("SET_DEBUG %u OK\n", (unsigned)level);
 		exit(0);
 	}
 
-	/* note, stop not currently implemented */
+	if (!strcmp(argv[1], "rx_dsm") ||
+	    !strcmp(argv[1], "rx_dsm_10bit") ||
+	    !strcmp(argv[1], "rx_dsm_11bit") ||
+	    !strcmp(argv[1], "rx_sbus") ||
+	    !strcmp(argv[1], "rx_ppm"))
+		errx(0, "receiver type is automatically detected, option '%s' is deprecated", argv[1]);
 
-	if (!strcmp(argv[1], "update")) {
+	if (!strcmp(argv[1], "test"))
+		test();
 
-		if (g_dev != nullptr) {
-			printf("[px4io] loaded, detaching first\n");
-			/* stop the driver */
-			delete g_dev;
-		}
+	if (!strcmp(argv[1], "monitor"))
+		monitor();
 
-		PX4IO_Uploader *up;
-		const char *fn[3];
+	if (!strcmp(argv[1], "bind"))
+		bind(argc, argv);
 
-		/* work out what we're uploading... */
-		if (argc > 2) {
-			fn[0] = argv[2];
-			fn[1] = nullptr;
+	if (!strcmp(argv[1], "lockdown"))
+		lockdown(argc, argv);
 
-		} else {
-			fn[0] = "/fs/microsd/px4io.bin";
-			fn[1] =	"/etc/px4io.bin";
-			fn[2] =	nullptr;
-		}
+	if (!strcmp(argv[1], "sbus1_out")) {
+		/* we can cheat and call the driver directly, as it
+		 * doesn't reference filp in ioctl()
+		 */
+		int ret = g_dev->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 1);
 
-		up = new PX4IO_Uploader;
-		int ret = up->upload(&fn[0]);
-		delete up;
+		if (ret != 0) {
+			errx(ret, "S.BUS v1 failed");
+		}
 
-		switch (ret) {
-		case OK:
-			break;
+		exit(0);
+	}
 
-		case -ENOENT:
-			errx(1, "PX4IO firmware file not found");
+	if (!strcmp(argv[1], "sbus2_out")) {
+		/* we can cheat and call the driver directly, as it
+		 * doesn't reference filp in ioctl()
+		 */
+		int ret = g_dev->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 2);
 
-		case -EEXIST:
-		case -EIO:
-			errx(1, "error updating PX4IO - check that bootloader mode is enabled");
+		if (ret != 0) {
+			errx(ret, "S.BUS v2 failed");
+		}
 
-		case -EINVAL:
-			errx(1, "verify failed - retry the update");
+		exit(0);
+	}
 
-		case -ETIMEDOUT:
-			errx(1, "timed out waiting for bootloader - power-cycle and try again");
+	if (!strcmp(argv[1], "rssi_analog")) {
+		/* we can cheat and call the driver directly, as it
+		 * doesn't reference filp in ioctl()
+		 */
+		int ret = g_dev->ioctl(nullptr, RC_INPUT_ENABLE_RSSI_ANALOG, 1);
 
-		default:
-			errx(1, "unexpected error %d", ret);
+		if (ret != 0) {
+			errx(ret, "RSSI analog failed");
 		}
 
-		return ret;
+		exit(0);
 	}
 
-	if (!strcmp(argv[1], "rx_dsm") ||
-	    !strcmp(argv[1], "rx_dsm_10bit") ||
-	    !strcmp(argv[1], "rx_dsm_11bit") ||
-	    !strcmp(argv[1], "rx_sbus") ||
-	    !strcmp(argv[1], "rx_ppm"))
-		errx(0, "receiver type is automatically detected, option '%s' is deprecated", argv[1]);
+	if (!strcmp(argv[1], "rssi_pwm")) {
+		/* we can cheat and call the driver directly, as it
+		 * doesn't reference filp in ioctl()
+		 */
+		int ret = g_dev->ioctl(nullptr, RC_INPUT_ENABLE_RSSI_PWM, 1);
 
-	if (!strcmp(argv[1], "test"))
-		test();
+		if (ret != 0) {
+			errx(ret, "RSSI PWM failed");
+		}
 
-	if (!strcmp(argv[1], "monitor"))
-		monitor();
+		exit(0);
+	}
 
-	out:
-	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current' or 'update'");
+out:
+	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug <level>',\n"
+	        "'recovery', 'limit <rate>', 'current', 'bind', 'checkcrc',\n"
+	        "'forceupdate', 'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm'");
 }
diff --git a/src/drivers/px4io/px4io_i2c.cpp b/src/drivers/px4io/px4io_i2c.cpp
new file mode 100644
index 000000000..19776c40a
--- /dev/null
+++ b/src/drivers/px4io/px4io_i2c.cpp
@@ -0,0 +1,169 @@
+/****************************************************************************
+ *
+ *   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 px4io_i2c.cpp
+  *
+  * I2C interface for PX4IO
+  */
+
+/* XXX trim includes */
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <debug.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include <arch/board/board.h>
+#include <board_config.h>
+
+#include <drivers/device/i2c.h>
+
+#ifdef PX4_I2C_OBDEV_PX4IO
+
+device::Device	*PX4IO_i2c_interface();
+
+class PX4IO_I2C : public device::I2C
+{
+public:
+	PX4IO_I2C(int bus, uint8_t address);
+	virtual ~PX4IO_I2C();
+
+	virtual int	init();
+	virtual int	read(unsigned offset, void *data, unsigned count = 1);
+	virtual int	write(unsigned address, void *data, unsigned count = 1);
+	virtual int	ioctl(unsigned operation, unsigned &arg);
+
+private:
+
+};
+
+device::Device
+*PX4IO_i2c_interface()
+{
+	return new PX4IO_I2C(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO);
+}
+
+PX4IO_I2C::PX4IO_I2C(int bus, uint8_t address) :
+	I2C("PX4IO_i2c", nullptr, bus, address, 320000)
+{
+	_retries = 3;
+}
+
+PX4IO_I2C::~PX4IO_I2C()
+{
+}
+
+int
+PX4IO_I2C::init()
+{
+	int ret;
+
+	ret = I2C::init();
+	if (ret != OK)
+		goto out;
+
+	/* XXX really should do something more here */
+
+out:
+	return 0;
+}
+
+int
+PX4IO_I2C::ioctl(unsigned operation, unsigned &arg)
+{
+	return 0;
+}
+
+int
+PX4IO_I2C::write(unsigned address, void *data, unsigned count)
+{
+	uint8_t page = address >> 8;
+	uint8_t offset = address & 0xff;
+	const uint16_t *values = reinterpret_cast<const uint16_t *>(data);
+
+	/* set up the transfer */
+	uint8_t 	addr[2] = {
+		page,
+		offset
+	};
+
+	i2c_msg_s	msgv[2];
+
+	msgv[0].flags = 0;
+	msgv[0].buffer = addr;
+	msgv[0].length = 2;
+
+	msgv[1].flags = I2C_M_NORESTART;
+	msgv[1].buffer = (uint8_t *)values;
+	msgv[1].length = 2 * count;
+
+	int ret = transfer(msgv, 2);
+	if (ret == OK)
+		ret = count;
+	return ret;
+}
+
+int
+PX4IO_I2C::read(unsigned address, void *data, unsigned count)
+{
+	uint8_t page = address >> 8;
+	uint8_t offset = address & 0xff;
+	const uint16_t *values = reinterpret_cast<const uint16_t *>(data);
+
+	/* set up the transfer */
+	uint8_t		addr[2] = {
+		page,
+		offset
+	};
+	i2c_msg_s	msgv[2];
+
+	msgv[0].flags = 0;
+	msgv[0].buffer = addr;
+	msgv[0].length = 2;
+
+	msgv[1].flags = I2C_M_READ;
+	msgv[1].buffer = (uint8_t *)values;
+	msgv[1].length = 2 * count;
+
+	int ret = transfer(msgv, 2);
+	if (ret == OK)
+		ret = count;
+	return ret;
+}
+
+#endif /* PX4_I2C_OBDEV_PX4IO */
diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp
new file mode 100644
index 000000000..43318ca84
--- /dev/null
+++ b/src/drivers/px4io/px4io_serial.cpp
@@ -0,0 +1,679 @@
+/****************************************************************************
+ *
+ *   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 px4io_serial.cpp
+  *
+  * Serial interface for PX4IO
+  */
+
+/* XXX trim includes */
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <debug.h>
+#include <time.h>
+#include <errno.h>
+#include <string.h>
+#include <stdio.h>
+
+#include <arch/board/board.h>
+
+/* XXX might be able to prune these */
+#include <nuttx/arch.h>
+#include <chip.h>
+#include <up_internal.h>
+#include <up_arch.h>
+
+#include <debug.h>
+
+#include <drivers/device/device.h>
+#include <drivers/drv_hrt.h>
+#include <board_config.h>
+
+#include <systemlib/perf_counter.h>
+
+#include <modules/px4iofirmware/protocol.h>
+
+#ifdef PX4IO_SERIAL_BASE
+
+device::Device	*PX4IO_serial_interface();
+
+/* serial register accessors */
+#define REG(_x)		(*(volatile uint32_t *)(PX4IO_SERIAL_BASE + _x))
+#define rSR		REG(STM32_USART_SR_OFFSET)
+#define rDR		REG(STM32_USART_DR_OFFSET)
+#define rBRR		REG(STM32_USART_BRR_OFFSET)
+#define rCR1		REG(STM32_USART_CR1_OFFSET)
+#define rCR2		REG(STM32_USART_CR2_OFFSET)
+#define rCR3		REG(STM32_USART_CR3_OFFSET)
+#define rGTPR		REG(STM32_USART_GTPR_OFFSET)
+
+class PX4IO_serial : public device::Device
+{
+public:
+	PX4IO_serial();
+	virtual ~PX4IO_serial();
+
+	virtual int	init();
+	virtual int	read(unsigned offset, void *data, unsigned count = 1);
+	virtual int	write(unsigned address, void *data, unsigned count = 1);
+	virtual int	ioctl(unsigned operation, unsigned &arg);
+
+private:
+	/*
+	 * XXX tune this value
+	 *
+	 * At 1.5Mbps each register takes 13.3µs, and we always transfer a full packet.
+	 * Packet overhead is 26µs for the four-byte header.
+	 *
+	 * 32 registers = 451µs
+	 *
+	 * Maybe we can just send smaller packets (e.g. 8 regs) and loop for larger (less common)
+	 * transfers? Could cause issues with any regs expecting to be written atomically...
+	 */
+	static IOPacket		_dma_buffer;		// XXX static to ensure DMA-able memory
+
+	DMA_HANDLE		_tx_dma;
+	DMA_HANDLE		_rx_dma;
+
+	/** saved DMA status */
+	static const unsigned	_dma_status_inactive = 0x80000000;	// low bits overlap DMA_STATUS_* values
+	static const unsigned	_dma_status_waiting  = 0x00000000;
+	volatile unsigned	_rx_dma_status;
+
+	/** bus-ownership lock */
+	sem_t			_bus_semaphore;
+
+	/** client-waiting lock/signal */
+	sem_t			_completion_semaphore;
+
+	/**
+	 * Start the transaction with IO and wait for it to complete.
+	 */
+	int			_wait_complete();
+
+	/**
+	 * DMA completion handler.
+	 */
+	static void		_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg);
+	void			_do_rx_dma_callback(unsigned status);
+
+	/**
+	 * Serial interrupt handler.
+	 */
+	static int		_interrupt(int vector, void *context);
+	void			_do_interrupt();
+
+	/**
+	 * Cancel any DMA in progress with an error.
+	 */
+	void			_abort_dma();
+
+	/**
+	 * Performance counters.
+	 */
+	perf_counter_t		_pc_txns;
+	perf_counter_t		_pc_dmasetup;
+	perf_counter_t		_pc_retries;
+	perf_counter_t		_pc_timeouts;
+	perf_counter_t		_pc_crcerrs;
+	perf_counter_t		_pc_dmaerrs;
+	perf_counter_t		_pc_protoerrs;
+	perf_counter_t		_pc_uerrs;
+	perf_counter_t		_pc_idle;
+	perf_counter_t		_pc_badidle;
+
+};
+
+IOPacket PX4IO_serial::_dma_buffer;
+static PX4IO_serial *g_interface;
+
+device::Device
+*PX4IO_serial_interface()
+{
+	return new PX4IO_serial();
+}
+
+PX4IO_serial::PX4IO_serial() :
+	Device("PX4IO_serial"),
+	_tx_dma(nullptr),
+	_rx_dma(nullptr),
+	_rx_dma_status(_dma_status_inactive),
+	_pc_txns(perf_alloc(PC_ELAPSED,		"io_txns     ")),
+	_pc_dmasetup(perf_alloc(PC_ELAPSED,	"io_dmasetup ")),
+	_pc_retries(perf_alloc(PC_COUNT,	"io_retries  ")),
+	_pc_timeouts(perf_alloc(PC_COUNT,	"io_timeouts ")),
+	_pc_crcerrs(perf_alloc(PC_COUNT,	"io_crcerrs  ")),
+	_pc_dmaerrs(perf_alloc(PC_COUNT,	"io_dmaerrs  ")),
+	_pc_protoerrs(perf_alloc(PC_COUNT,	"io_protoerrs")),
+	_pc_uerrs(perf_alloc(PC_COUNT,		"io_uarterrs ")),
+	_pc_idle(perf_alloc(PC_COUNT,		"io_idle     ")),
+	_pc_badidle(perf_alloc(PC_COUNT,	"io_badidle  "))
+{
+	g_interface = this;
+}
+
+PX4IO_serial::~PX4IO_serial()
+{
+	if (_tx_dma != nullptr) {
+		stm32_dmastop(_tx_dma);
+		stm32_dmafree(_tx_dma);
+	}
+	if (_rx_dma != nullptr) {
+		stm32_dmastop(_rx_dma);
+		stm32_dmafree(_rx_dma);
+	}
+
+	/* reset the UART */
+	rCR1 = 0;
+	rCR2 = 0;
+	rCR3 = 0;
+
+	/* detach our interrupt handler */
+	up_disable_irq(PX4IO_SERIAL_VECTOR);
+	irq_detach(PX4IO_SERIAL_VECTOR);
+
+	/* restore the GPIOs */
+	stm32_unconfiggpio(PX4IO_SERIAL_TX_GPIO);
+	stm32_unconfiggpio(PX4IO_SERIAL_RX_GPIO);
+
+	/* and kill our semaphores */
+	sem_destroy(&_completion_semaphore);
+	sem_destroy(&_bus_semaphore);
+
+	perf_free(_pc_txns);
+	perf_free(_pc_dmasetup);
+	perf_free(_pc_retries);
+	perf_free(_pc_timeouts);
+	perf_free(_pc_crcerrs);
+	perf_free(_pc_dmaerrs);
+	perf_free(_pc_protoerrs);
+	perf_free(_pc_uerrs);
+	perf_free(_pc_idle);
+	perf_free(_pc_badidle);
+
+	if (g_interface == this)
+		g_interface = nullptr;
+}
+
+int
+PX4IO_serial::init()
+{
+	/* allocate DMA */
+	_tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP);
+	_rx_dma = stm32_dmachannel(PX4IO_SERIAL_RX_DMAMAP);
+	if ((_tx_dma == nullptr) || (_rx_dma == nullptr))
+		return -1;
+
+	/* configure pins for serial use */
+	stm32_configgpio(PX4IO_SERIAL_TX_GPIO);
+	stm32_configgpio(PX4IO_SERIAL_RX_GPIO);
+
+	/* reset & configure the UART */
+	rCR1 = 0;
+	rCR2 = 0;
+	rCR3 = 0;
+
+	/* eat any existing interrupt status */
+	(void)rSR;
+	(void)rDR;
+
+	/* configure line speed */
+	uint32_t usartdiv32 = PX4IO_SERIAL_CLOCK / (PX4IO_SERIAL_BITRATE / 2);
+	uint32_t mantissa = usartdiv32 >> 5;
+	uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1;
+	rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT);
+
+	/* attach serial interrupt handler */
+	irq_attach(PX4IO_SERIAL_VECTOR, _interrupt);
+	up_enable_irq(PX4IO_SERIAL_VECTOR);
+
+	/* enable UART in DMA mode, enable error and line idle interrupts */
+	rCR3 = USART_CR3_EIE;
+
+	rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE;
+
+	/* create semaphores */
+	sem_init(&_completion_semaphore, 0, 0);
+	sem_init(&_bus_semaphore, 0, 1);
+
+
+	/* XXX this could try talking to IO */
+
+	return 0;
+}
+
+int
+PX4IO_serial::ioctl(unsigned operation, unsigned &arg)
+{
+
+	switch (operation) {
+
+	case 1:		/* XXX magic number - test operation */
+		switch (arg) {
+		case 0:
+			lowsyslog("test 0\n");
+
+			/* kill DMA, this is a PIO test */
+			stm32_dmastop(_tx_dma);
+			stm32_dmastop(_rx_dma);
+			rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT);
+
+			for (;;) {
+				while (!(rSR & USART_SR_TXE))
+					;
+				rDR = 0x55;
+			}
+			return 0;
+
+		case 1:
+			{
+				unsigned fails = 0;
+				for (unsigned count = 0;; count++) {
+					uint16_t value = count & 0xffff;
+
+					if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0)
+						fails++;
+						
+					if (count >= 5000) {
+						lowsyslog("==== test 1 : %u failures ====\n", fails);
+						perf_print_counter(_pc_txns);
+						perf_print_counter(_pc_dmasetup);
+						perf_print_counter(_pc_retries);
+						perf_print_counter(_pc_timeouts);
+						perf_print_counter(_pc_crcerrs);
+						perf_print_counter(_pc_dmaerrs);
+						perf_print_counter(_pc_protoerrs);
+						perf_print_counter(_pc_uerrs);
+						perf_print_counter(_pc_idle);
+						perf_print_counter(_pc_badidle);
+						count = 0;
+					}
+				}
+				return 0;
+			}
+		case 2:
+			lowsyslog("test 2\n");
+			return 0;
+		}
+	default:
+		break;
+	}
+
+	return -1;
+}
+
+int
+PX4IO_serial::write(unsigned address, void *data, unsigned count)
+{
+	uint8_t page = address >> 8;
+	uint8_t offset = address & 0xff;
+	const uint16_t *values = reinterpret_cast<const uint16_t *>(data);
+
+	if (count > PKT_MAX_REGS)
+		return -EINVAL;
+
+	sem_wait(&_bus_semaphore);
+
+	int result;
+	for (unsigned retries = 0; retries < 3; retries++) {
+
+		_dma_buffer.count_code = count | PKT_CODE_WRITE;
+		_dma_buffer.page = page;
+		_dma_buffer.offset = offset;
+		memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * count));
+		for (unsigned i = count; i < PKT_MAX_REGS; i++)
+			_dma_buffer.regs[i] = 0x55aa;
+
+		/* XXX implement check byte */
+
+		/* start the transaction and wait for it to complete */
+		result = _wait_complete();
+
+		/* successful transaction? */
+		if (result == OK) {
+
+			/* check result in packet */
+			if (PKT_CODE(_dma_buffer) == PKT_CODE_ERROR) {
+
+				/* IO didn't like it - no point retrying */
+				result = -EINVAL;
+				perf_count(_pc_protoerrs);
+			}
+
+			break;
+		}
+		perf_count(_pc_retries);
+	}
+
+	sem_post(&_bus_semaphore);
+
+	if (result == OK)
+		result = count;
+	return result;
+}
+
+int
+PX4IO_serial::read(unsigned address, void *data, unsigned count)
+{
+	uint8_t page = address >> 8;
+	uint8_t offset = address & 0xff;
+	uint16_t *values = reinterpret_cast<uint16_t *>(data);
+
+	if (count > PKT_MAX_REGS)
+		return -EINVAL;
+
+	sem_wait(&_bus_semaphore);
+
+	int result;
+	for (unsigned retries = 0; retries < 3; retries++) {
+
+		_dma_buffer.count_code = count | PKT_CODE_READ;
+		_dma_buffer.page = page;
+		_dma_buffer.offset = offset;
+
+		/* start the transaction and wait for it to complete */
+		result = _wait_complete();
+
+		/* successful transaction? */
+		if (result == OK) {
+
+			/* check result in packet */
+			if (PKT_CODE(_dma_buffer) == PKT_CODE_ERROR) {
+
+				/* IO didn't like it - no point retrying */
+				result = -EINVAL;
+				perf_count(_pc_protoerrs);
+
+			/* compare the received count with the expected count */
+			} else if (PKT_COUNT(_dma_buffer) != count) {
+
+				/* IO returned the wrong number of registers - no point retrying */
+				result = -EIO;
+				perf_count(_pc_protoerrs);
+
+			/* successful read */				
+			} else {
+
+				/* copy back the result */
+				memcpy(values, &_dma_buffer.regs[0], (2 * count));
+			}
+
+			break;
+		}
+		perf_count(_pc_retries);
+	}
+
+	sem_post(&_bus_semaphore);
+
+	if (result == OK)
+		result = count;
+	return result;
+}
+
+int
+PX4IO_serial::_wait_complete()
+{
+	/* clear any lingering error status */
+	(void)rSR;
+	(void)rDR;
+
+	/* start RX DMA */
+	perf_begin(_pc_txns);
+	perf_begin(_pc_dmasetup);
+
+	/* DMA setup time ~3µs */
+	_rx_dma_status = _dma_status_waiting;
+
+	/*
+	 * Note that we enable circular buffer mode as a workaround for
+	 * there being no API to disable the DMA FIFO. We need direct mode
+	 * because otherwise when the line idle interrupt fires there
+	 * will be packet bytes still in the DMA FIFO, and we will assume
+	 * that the idle was spurious.
+	 *
+	 * XXX this should be fixed with a NuttX change.
+	 */
+	stm32_dmasetup(
+		_rx_dma,
+		PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET,
+		reinterpret_cast<uint32_t>(&_dma_buffer),
+		sizeof(_dma_buffer),
+		DMA_SCR_CIRC		|	/* XXX see note above */
+		DMA_SCR_DIR_P2M		|
+		DMA_SCR_MINC		|
+		DMA_SCR_PSIZE_8BITS	|
+		DMA_SCR_MSIZE_8BITS	|
+		DMA_SCR_PBURST_SINGLE	|
+		DMA_SCR_MBURST_SINGLE);
+	stm32_dmastart(_rx_dma, _dma_callback, this, false);
+	rCR3 |= USART_CR3_DMAR;
+
+	/* start TX DMA - no callback if we also expect a reply */
+	/* DMA setup time ~3µs */
+	_dma_buffer.crc = 0;
+	_dma_buffer.crc = crc_packet(&_dma_buffer);
+	stm32_dmasetup(
+		_tx_dma,
+		PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET,
+		reinterpret_cast<uint32_t>(&_dma_buffer),
+		PKT_SIZE(_dma_buffer),
+		DMA_SCR_DIR_M2P		|
+		DMA_SCR_MINC		|
+		DMA_SCR_PSIZE_8BITS	|
+		DMA_SCR_MSIZE_8BITS	|
+		DMA_SCR_PBURST_SINGLE	|
+		DMA_SCR_MBURST_SINGLE);
+	stm32_dmastart(_tx_dma, nullptr, nullptr, false);
+	//rCR1 &= ~USART_CR1_TE;
+	//rCR1 |= USART_CR1_TE;
+	rCR3 |= USART_CR3_DMAT;
+
+	perf_end(_pc_dmasetup);
+
+	/* compute the deadline for a 10ms timeout */
+	struct timespec abstime;
+	clock_gettime(CLOCK_REALTIME, &abstime);
+	abstime.tv_nsec += 10*1000*1000;
+	if (abstime.tv_nsec >= 1000*1000*1000) {
+		abstime.tv_sec++;
+		abstime.tv_nsec -= 1000*1000*1000;
+	}
+
+	/* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */
+	int ret;
+	for (;;) {
+		ret = sem_timedwait(&_completion_semaphore, &abstime);
+
+		if (ret == OK) {
+			/* check for DMA errors */
+			if (_rx_dma_status & DMA_STATUS_TEIF) {
+				perf_count(_pc_dmaerrs);
+				ret = -EIO;
+				break;
+			}
+
+			/* check packet CRC - corrupt packet errors mean IO receive CRC error */
+			uint8_t crc = _dma_buffer.crc;
+			_dma_buffer.crc = 0;
+			if ((crc != crc_packet(&_dma_buffer)) | (PKT_CODE(_dma_buffer) == PKT_CODE_CORRUPT)) {
+				perf_count(_pc_crcerrs);
+				ret = -EIO;
+				break;
+			}
+
+			/* successful txn (may still be reporting an error) */
+			break;
+		}
+
+		if (errno == ETIMEDOUT) {
+			/* something has broken - clear out any partial DMA state and reconfigure */
+			_abort_dma();
+			perf_count(_pc_timeouts);
+			perf_cancel(_pc_txns);		/* don't count this as a transaction */
+			break;
+		}
+
+		/* we might? see this for EINTR */
+		lowsyslog("unexpected ret %d/%d\n", ret, errno);
+	}
+
+	/* reset DMA status */
+	_rx_dma_status = _dma_status_inactive;
+
+	/* update counters */
+	perf_end(_pc_txns);
+
+	return ret;
+}
+
+void
+PX4IO_serial::_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg)
+{
+	if (arg != nullptr) {
+		PX4IO_serial *ps = reinterpret_cast<PX4IO_serial *>(arg);
+
+		ps->_do_rx_dma_callback(status);
+	}
+}
+
+void
+PX4IO_serial::_do_rx_dma_callback(unsigned status)
+{
+	/* on completion of a reply, wake the waiter */
+	if (_rx_dma_status == _dma_status_waiting) {
+
+		/* check for packet overrun - this will occur after DMA completes */
+		uint32_t sr = rSR;
+		if (sr & (USART_SR_ORE | USART_SR_RXNE)) {
+			(void)rDR;
+			status = DMA_STATUS_TEIF;
+		}
+
+		/* save RX status */
+		_rx_dma_status = status;
+
+		/* disable UART DMA */
+		rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
+
+		/* complete now */
+		sem_post(&_completion_semaphore);
+	}
+}
+
+int
+PX4IO_serial::_interrupt(int irq, void *context)
+{
+	if (g_interface != nullptr)
+		g_interface->_do_interrupt();
+	return 0;
+}
+
+void
+PX4IO_serial::_do_interrupt()
+{
+	uint32_t sr = rSR;	/* get UART status register */
+	(void)rDR;		/* read DR to clear status */
+
+	if (sr & (USART_SR_ORE |	/* overrun error - packet was too big for DMA or DMA was too slow */
+		USART_SR_NE |		/* noise error - we have lost a byte due to noise */
+		USART_SR_FE)) {		/* framing error - start/stop bit lost or line break */
+		
+		/* 
+		 * If we are in the process of listening for something, these are all fatal;
+		 * abort the DMA with an error.
+		 */
+		if (_rx_dma_status == _dma_status_waiting) {
+			_abort_dma();
+
+			perf_count(_pc_uerrs);
+			/* complete DMA as though in error */
+			_do_rx_dma_callback(DMA_STATUS_TEIF);
+
+			return;
+		}
+
+		/* XXX we might want to use FE / line break as an out-of-band handshake ... handle it here */
+
+		/* don't attempt to handle IDLE if it's set - things went bad */
+		return;
+	}
+
+	if (sr & USART_SR_IDLE) {
+
+		/* if there is DMA reception going on, this is a short packet */
+		if (_rx_dma_status == _dma_status_waiting) {
+
+			/* verify that the received packet is complete */
+			unsigned length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma);
+			if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) {
+				perf_count(_pc_badidle);
+
+				/* stop the receive DMA */
+				stm32_dmastop(_rx_dma);
+
+				/* complete the short reception */
+				_do_rx_dma_callback(DMA_STATUS_TEIF);
+				return;
+			}
+
+			perf_count(_pc_idle);
+
+			/* stop the receive DMA */
+			stm32_dmastop(_rx_dma);
+
+			/* complete the short reception */
+			_do_rx_dma_callback(DMA_STATUS_TCIF);
+		}
+	}
+}
+
+void
+PX4IO_serial::_abort_dma()
+{
+	/* disable UART DMA */
+	rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
+	(void)rSR;
+	(void)rDR;
+	(void)rDR;
+
+	/* stop DMA */
+	stm32_dmastop(_tx_dma);
+	stm32_dmastop(_rx_dma);
+}
+
+#endif /* PX4IO_SERIAL_BASE */
diff --git a/src/drivers/px4io/uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp
index 15524c3ae..dd8abbac5 100644
--- a/src/drivers/px4io/uploader.cpp
+++ b/src/drivers/px4io/px4io_uploader.cpp
@@ -49,53 +49,18 @@
 #include <unistd.h>
 #include <fcntl.h>
 #include <poll.h>
+#include <termios.h>
 #include <sys/stat.h>
+#include <nuttx/arch.h>
+
+#include <crc32.h>
 
 #include "uploader.h"
 
-static const uint32_t crctab[] =
-{
-	0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
-	0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
-	0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
-	0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
-	0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
-	0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
-	0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
-	0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
-	0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
-	0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
-	0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
-	0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
-	0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
-	0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
-	0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
-	0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
-	0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
-	0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
-	0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
-	0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
-	0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
-	0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
-	0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
-	0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
-	0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
-	0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
-	0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
-	0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
-	0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
-	0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
-	0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
-	0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
-};
-
-static uint32_t
-crc32(const uint8_t *src, unsigned len, unsigned state)
-{
-	for (unsigned i = 0; i < len; i++)
-		state = crctab[(state ^ src[i]) & 0xff] ^ (state >> 8);
-	return state;
-}
+#include <board_config.h>
+
+// define for comms logging
+//#define UDEBUG
 
 PX4IO_Uploader::PX4IO_Uploader() :
 	_io_fd(-1),
@@ -114,24 +79,11 @@ PX4IO_Uploader::upload(const char *filenames[])
 	const char *filename = NULL;
 	size_t fw_size;
 
-	_io_fd = open("/dev/ttyS2", O_RDWR);
-
-	if (_io_fd < 0) {
-		log("could not open interface");
-		return -errno;
-	}
-
-	/* look for the bootloader */
-	ret = sync();
-
-	if (ret != OK) {
-		/* this is immediately fatal */
-		log("bootloader not responding");
-		close(_io_fd);
-		_io_fd = -1;
-		return -EIO;
-	}
+#ifndef PX4IO_SERIAL_DEVICE
+#error Must define PX4IO_SERIAL_DEVICE in board configuration to support firmware upload
+#endif
 
+	/* allow an early abort and look for file first */
 	for (unsigned i = 0; filenames[i] != nullptr; i++) {
 		_fw_fd = open(filenames[i], O_RDONLY);
 
@@ -152,9 +104,46 @@ PX4IO_Uploader::upload(const char *filenames[])
 		return -ENOENT;
 	}
 
+	_io_fd = open(PX4IO_SERIAL_DEVICE, O_RDWR);
+
+	if (_io_fd < 0) {
+		log("could not open interface");
+		return -errno;
+	}
+
+	/* save initial uart configuration to reset after the update */
+	struct termios t_original;
+	tcgetattr(_io_fd, &t_original);
+
+	/* adjust line speed to match bootloader */
+	struct termios t;
+	tcgetattr(_io_fd, &t);
+	cfsetspeed(&t, 115200);
+	tcsetattr(_io_fd, TCSANOW, &t);
+
+	/* look for the bootloader for 150 ms */
+	for (int i = 0; i < 15; i++) {
+		ret = sync();
+		if (ret == OK) {
+			break;
+		} else {
+			usleep(10000);
+		}
+	}
+
+	if (ret != OK) {
+		/* this is immediately fatal */
+		log("bootloader not responding");
+		tcsetattr(_io_fd, TCSANOW, &t_original);
+		close(_io_fd);
+		_io_fd = -1;
+		return -EIO;
+	}
+
 	struct stat st;
 	if (stat(filename, &st) != 0) {
 		log("Failed to stat %s - %d\n", filename, (int)errno);
+		tcsetattr(_io_fd, TCSANOW, &t_original);
 		close(_io_fd);
 		_io_fd = -1;
 		return -errno;
@@ -162,6 +151,7 @@ PX4IO_Uploader::upload(const char *filenames[])
 	fw_size = st.st_size;
 
 	if (_fw_fd == -1) {
+		tcsetattr(_io_fd, TCSANOW, &t_original);
 		close(_io_fd);
 		_io_fd = -1;
 		return -ENOENT;
@@ -176,6 +166,7 @@ PX4IO_Uploader::upload(const char *filenames[])
 			if (ret != OK) {
 				/* this is immediately fatal */
 				log("bootloader not responding");
+				tcsetattr(_io_fd, TCSANOW, &t_original);
 				close(_io_fd);
 				_io_fd = -1;
 				return -EIO;
@@ -189,6 +180,7 @@ PX4IO_Uploader::upload(const char *filenames[])
 				log("found bootloader revision: %d", bl_rev);
 			} else {
 				log("found unsupported bootloader revision %d, exiting", bl_rev);
+				tcsetattr(_io_fd, TCSANOW, &t_original);
 				close(_io_fd);
 				_io_fd = -1;
 				return OK;
@@ -224,6 +216,9 @@ PX4IO_Uploader::upload(const char *filenames[])
 
 		if (ret != OK) {
 			log("reboot failed");
+			tcsetattr(_io_fd, TCSANOW, &t_original);
+			close(_io_fd);
+			_io_fd = -1;
 			return ret;
 		}
 
@@ -233,9 +228,17 @@ PX4IO_Uploader::upload(const char *filenames[])
 		break;
 	}
 
+	/* reset uart to previous/default baudrate */
+	tcsetattr(_io_fd, TCSANOW, &t_original);
+
 	close(_fw_fd);
 	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);
+
 	return ret;
 }
 
@@ -247,16 +250,20 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
 	fds[0].fd = _io_fd;
 	fds[0].events = POLLIN;
 
-	/* wait 100 ms for a character */
+	/* wait <timout> ms for a character */
 	int ret = ::poll(&fds[0], 1, timeout);
 
 	if (ret < 1) {
+#ifdef UDEBUG
 		log("poll timeout %d", ret);
+#endif
 		return -ETIMEDOUT;
 	}
 
 	read(_io_fd, &c, 1);
-	//log("recv 0x%02x", c);
+#ifdef UDEBUG
+	log("recv 0x%02x", c);
+#endif
 	return OK;
 }
 
@@ -280,18 +287,25 @@ PX4IO_Uploader::drain()
 	int ret;
 
 	do {
-		ret = recv(c, 1000);
+		// the small recv timeout here is to allow for fast
+		// drain when rebooting the io board for a forced
+		// update of the fw without using the safety switch
+		ret = recv(c, 40);
 
+#ifdef UDEBUG
 		if (ret == OK) {
-			//log("discard 0x%02x", c);
+			log("discard 0x%02x", c);
 		}
+#endif
 	} while (ret == OK);
 }
 
 int
 PX4IO_Uploader::send(uint8_t c)
 {
-	//log("send 0x%02x", c);
+#ifdef UDEBUG
+	log("send 0x%02x", c);
+#endif
 	if (write(_io_fd, &c, 1) != 1)
 		return -errno;
 
@@ -565,14 +579,14 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local)
 			return -errno;
 
 		/* calculate crc32 sum */
-		sum = crc32((uint8_t *)&file_buf, sizeof(file_buf), sum);
+		sum = crc32part((uint8_t *)&file_buf, sizeof(file_buf), sum);
 
 		bytes_read += count;
 	}
 
 	/* fill the rest with 0xff */
 	while (bytes_read < fw_size_remote) {
-		sum = crc32(&fill_blank, sizeof(fill_blank), sum);
+		sum = crc32part(&fill_blank, sizeof(fill_blank), sum);
 		bytes_read += sizeof(fill_blank);
 	}
 
diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h
index 135202dd1..55f63eef9 100644
--- a/src/drivers/px4io/uploader.h
+++ b/src/drivers/px4io/uploader.h
@@ -69,7 +69,7 @@ private:
 		PROTO_REBOOT		= 0x30,
 
 		INFO_BL_REV		= 1,		/**< bootloader protocol revision */
-		BL_REV			= 3,		/**< supported bootloader protocol  */
+		BL_REV			= 4,		/**< supported bootloader protocol  */
 		INFO_BOARD_ID		= 2,		/**< board type */
 		INFO_BOARD_REV		= 3,		/**< board revision */
 		INFO_FLASH_SIZE		= 4,		/**< max firmware size in bytes */
@@ -91,7 +91,7 @@ private:
 	void			drain();
 	int			send(uint8_t c);
 	int			send(uint8_t *p, unsigned count);
-	int			get_sync(unsigned timeout = 1000);
+	int			get_sync(unsigned timeout = 40);
 	int			sync();
 	int			get_info(int param, uint32_t &val);
 	int			erase();
diff --git a/src/drivers/rgbled/module.mk b/src/drivers/rgbled/module.mk
new file mode 100644
index 000000000..39b53ec9e
--- /dev/null
+++ b/src/drivers/rgbled/module.mk
@@ -0,0 +1,6 @@
+#
+# TCA62724FMG driver for RGB LED
+#
+
+MODULE_COMMAND	 = rgbled
+SRCS		 = rgbled.cpp
diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp
new file mode 100644
index 000000000..4f58891ed
--- /dev/null
+++ b/src/drivers/rgbled/rgbled.cpp
@@ -0,0 +1,713 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Julian Oes <joes@student.ethz.ch>
+ *           Anton Babushkin <anton.babushkin@me.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * 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 rgbled.cpp
+ *
+ * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C.
+ *
+ */
+
+#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_rgbled.h>
+
+#define RGBLED_ONTIME 120
+#define RGBLED_OFFTIME 120
+
+#define ADDR			PX4_I2C_OBDEV_LED	/**< I2C adress of TCA62724FMG */
+#define SUB_ADDR_START		0x01	/**< write everything (with auto-increment) */
+#define SUB_ADDR_PWM0		0x81	/**< blue     (without auto-increment) */
+#define SUB_ADDR_PWM1		0x82	/**< green    (without auto-increment) */
+#define SUB_ADDR_PWM2		0x83	/**< red      (without auto-increment) */
+#define SUB_ADDR_SETTINGS	0x84	/**< settings (without auto-increment)*/
+
+#define SETTING_NOT_POWERSAVE	0x01	/**< power-save mode not off */
+#define SETTING_ENABLE   	0x02	/**< on */
+
+
+class RGBLED : public device::I2C
+{
+public:
+	RGBLED(int bus, int rgbled);
+	virtual ~RGBLED();
+
+
+	virtual int		init();
+	virtual int		probe();
+	virtual int		info();
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+private:
+	work_s			_work;
+
+	rgbled_mode_t		_mode;
+	rgbled_pattern_t	_pattern;
+
+	uint8_t			_r;
+	uint8_t			_g;
+	uint8_t			_b;
+	float			_brightness;
+
+	bool			_running;
+	int			_led_interval;
+	bool			_should_run;
+	int			_counter;
+
+	void 			set_color(rgbled_color_t ledcolor);
+	void			set_mode(rgbled_mode_t mode);
+	void			set_pattern(rgbled_pattern_t *pattern);
+
+	static void		led_trampoline(void *arg);
+	void			led();
+
+	int			send_led_enable(bool enable);
+	int			send_led_rgb();
+	int			get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b);
+};
+
+/* for now, we only support one RGBLED */
+namespace
+{
+RGBLED *g_rgbled;
+}
+
+void rgbled_usage();
+
+extern "C" __EXPORT int rgbled_main(int argc, char *argv[]);
+
+RGBLED::RGBLED(int bus, int rgbled) :
+	I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000),
+	_mode(RGBLED_MODE_OFF),
+	_r(0),
+	_g(0),
+	_b(0),
+	_brightness(1.0f),
+	_running(false),
+	_led_interval(0),
+	_should_run(false),
+	_counter(0)
+{
+	memset(&_work, 0, sizeof(_work));
+	memset(&_pattern, 0, sizeof(_pattern));
+}
+
+RGBLED::~RGBLED()
+{
+}
+
+int
+RGBLED::init()
+{
+	int ret;
+	ret = I2C::init();
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	/* switch off LED on start */
+	send_led_enable(false);
+	send_led_rgb();
+
+	return OK;
+}
+
+int
+RGBLED::probe()
+{
+	int ret;
+	bool on, powersave;
+	uint8_t r, g, b;
+
+	/**
+	   this may look strange, but is needed. There is a serial
+	   EEPROM (Microchip-24aa01) on the PX4FMU-v1 that responds to
+	   a bunch of I2C addresses, including the 0x55 used by this
+	   LED device. So we need to do enough operations to be sure
+	   we are talking to the right device. These 3 operations seem
+	   to be enough, as the 3rd one consistently fails if no
+	   RGBLED is on the bus.
+	 */
+	if ((ret=get(on, powersave, r, g, b)) != OK ||
+	    (ret=send_led_enable(false) != OK) ||
+	    (ret=send_led_enable(false) != OK)) {
+		return ret;
+	}
+
+	return ret;
+}
+
+int
+RGBLED::info()
+{
+	int ret;
+	bool on, powersave;
+	uint8_t r, g, b;
+
+	ret = get(on, powersave, r, g, b);
+
+	if (ret == OK) {
+		/* we don't care about power-save mode */
+		log("state: %s", on ? "ON" : "OFF");
+		log("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b);
+
+	} else {
+		warnx("failed to read led");
+	}
+
+	return ret;
+}
+
+int
+RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	int ret = ENOTTY;
+
+	switch (cmd) {
+	case RGBLED_SET_RGB:
+		/* set the specified color */
+		_r = ((rgbled_rgbset_t *) arg)->red;
+		_g = ((rgbled_rgbset_t *) arg)->green;
+		_b = ((rgbled_rgbset_t *) arg)->blue;
+		send_led_rgb();
+		return OK;
+
+	case RGBLED_SET_COLOR:
+		/* set the specified color name */
+		set_color((rgbled_color_t)arg);
+		send_led_rgb();
+		return OK;
+
+	case RGBLED_SET_MODE:
+		/* set the specified mode */
+		set_mode((rgbled_mode_t)arg);
+		return OK;
+
+	case RGBLED_SET_PATTERN:
+		/* set a special pattern */
+		set_pattern((rgbled_pattern_t *)arg);
+		return OK;
+
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+
+void
+RGBLED::led_trampoline(void *arg)
+{
+	RGBLED *rgbl = reinterpret_cast<RGBLED *>(arg);
+
+	rgbl->led();
+}
+
+/**
+ * Main loop function
+ */
+void
+RGBLED::led()
+{
+	if (!_should_run) {
+		_running = false;
+		return;
+	}
+
+	switch (_mode) {
+	case RGBLED_MODE_BLINK_SLOW:
+	case RGBLED_MODE_BLINK_NORMAL:
+	case RGBLED_MODE_BLINK_FAST:
+		if (_counter >= 2)
+			_counter = 0;
+
+		send_led_enable(_counter == 0);
+
+		break;
+
+	case RGBLED_MODE_BREATHE:
+
+		if (_counter >= 62)
+			_counter = 0;
+
+		int n;
+
+		if (_counter < 32) {
+			n = _counter;
+
+		} else {
+			n = 62 - _counter;
+		}
+
+		_brightness = n * n / (31.0f * 31.0f);
+		send_led_rgb();
+		break;
+
+	case RGBLED_MODE_PATTERN:
+
+		/* don't run out of the pattern array and stop if the next frame is 0 */
+		if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0)
+			_counter = 0;
+
+		set_color(_pattern.color[_counter]);
+		send_led_rgb();
+		_led_interval = _pattern.duration[_counter];
+		break;
+
+	default:
+		break;
+	}
+
+	_counter++;
+
+	/* re-queue ourselves to run again later */
+	work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, _led_interval);
+}
+
+/**
+ * Parse color constant and set _r _g _b values
+ */
+void
+RGBLED::set_color(rgbled_color_t color)
+{
+	switch (color) {
+	case RGBLED_COLOR_OFF:
+		_r = 0;
+		_g = 0;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_RED:
+		_r = 255;
+		_g = 0;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_YELLOW:
+		_r = 255;
+		_g = 200;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_PURPLE:
+		_r = 255;
+		_g = 0;
+		_b = 255;
+		break;
+
+	case RGBLED_COLOR_GREEN:
+		_r = 0;
+		_g = 255;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_BLUE:
+		_r = 0;
+		_g = 0;
+		_b = 255;
+		break;
+
+	case RGBLED_COLOR_WHITE:
+		_r = 255;
+		_g = 255;
+		_b = 255;
+		break;
+
+	case RGBLED_COLOR_AMBER:
+		_r = 255;
+		_g = 80;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_DIM_RED:
+		_r = 90;
+		_g = 0;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_DIM_YELLOW:
+		_r = 80;
+		_g = 30;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_DIM_PURPLE:
+		_r = 45;
+		_g = 0;
+		_b = 45;
+		break;
+
+	case RGBLED_COLOR_DIM_GREEN:
+		_r = 0;
+		_g = 90;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_DIM_BLUE:
+		_r = 0;
+		_g = 0;
+		_b = 90;
+		break;
+
+	case RGBLED_COLOR_DIM_WHITE:
+		_r = 30;
+		_g = 30;
+		_b = 30;
+		break;
+
+	case RGBLED_COLOR_DIM_AMBER:
+		_r = 80;
+		_g = 20;
+		_b = 0;
+		break;
+
+	default:
+		warnx("color unknown");
+		break;
+	}
+}
+
+/**
+ * Set mode, if mode not changed has no any effect (doesn't reset blinks phase)
+ */
+void
+RGBLED::set_mode(rgbled_mode_t mode)
+{
+	if (mode != _mode) {
+		_mode = mode;
+
+		switch (mode) {
+		case RGBLED_MODE_OFF:
+			_should_run = false;
+			send_led_enable(false);
+			break;
+
+		case RGBLED_MODE_ON:
+			_brightness = 1.0f;
+			send_led_rgb();
+			send_led_enable(true);
+			break;
+
+		case RGBLED_MODE_BLINK_SLOW:
+			_should_run = true;
+			_counter = 0;
+			_led_interval = 2000;
+			_brightness = 1.0f;
+			send_led_rgb();
+			break;
+
+		case RGBLED_MODE_BLINK_NORMAL:
+			_should_run = true;
+			_counter = 0;
+			_led_interval = 500;
+			_brightness = 1.0f;
+			send_led_rgb();
+			break;
+
+		case RGBLED_MODE_BLINK_FAST:
+			_should_run = true;
+			_counter = 0;
+			_led_interval = 100;
+			_brightness = 1.0f;
+			send_led_rgb();
+			break;
+
+		case RGBLED_MODE_BREATHE:
+			_should_run = true;
+			_counter = 0;
+			_led_interval = 25;
+			send_led_enable(true);
+			break;
+
+		case RGBLED_MODE_PATTERN:
+			_should_run = true;
+			_counter = 0;
+			_brightness = 1.0f;
+			send_led_enable(true);
+			break;
+
+		default:
+			warnx("mode unknown");
+			break;
+		}
+
+		/* if it should run now, start the workq */
+		if (_should_run && !_running) {
+			_running = true;
+			work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1);
+		}
+
+	}
+}
+
+/**
+ * Set pattern for PATTERN mode, but don't change current mode
+ */
+void
+RGBLED::set_pattern(rgbled_pattern_t *pattern)
+{
+	memcpy(&_pattern, pattern, sizeof(rgbled_pattern_t));
+}
+
+/**
+ * Sent ENABLE flag to LED driver
+ */
+int
+RGBLED::send_led_enable(bool enable)
+{
+	uint8_t settings_byte = 0;
+
+	if (enable)
+		settings_byte |= SETTING_ENABLE;
+
+	settings_byte |= SETTING_NOT_POWERSAVE;
+
+	const uint8_t msg[2] = { SUB_ADDR_SETTINGS, settings_byte};
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+/**
+ * Send RGB PWM settings to LED driver according to current color and brightness
+ */
+int
+RGBLED::send_led_rgb()
+{
+	/* To scale from 0..255 -> 0..15 shift right by 4 bits */
+	const uint8_t msg[6] = {
+		SUB_ADDR_PWM0, (uint8_t)((int)(_b * _brightness) >> 4),
+		SUB_ADDR_PWM1, (uint8_t)((int)(_g * _brightness) >> 4),
+		SUB_ADDR_PWM2, (uint8_t)((int)(_r * _brightness) >> 4)
+	};
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b)
+{
+	uint8_t result[2];
+	int ret;
+
+	ret = transfer(nullptr, 0, &result[0], 2);
+
+	if (ret == OK) {
+		on = result[0] & SETTING_ENABLE;
+		powersave = !(result[0] & SETTING_NOT_POWERSAVE);
+		/* XXX check, looks wrong */
+		r = (result[0] & 0x0f) << 4;
+		g = (result[1] & 0xf0);
+		b = (result[1] & 0x0f) << 4;
+	}
+
+	return ret;
+}
+
+void
+rgbled_usage()
+{
+	warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50'");
+	warnx("options:");
+	warnx("    -b i2cbus (%d)", PX4_I2C_BUS_LED);
+	warnx("    -a addr (0x%x)", ADDR);
+}
+
+int
+rgbled_main(int argc, char *argv[])
+{
+	int i2cdevice = -1;
+	int rgbledadr = 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':
+			rgbledadr = strtol(optarg, NULL, 0);
+			break;
+
+		case 'b':
+			i2cdevice = strtol(optarg, NULL, 0);
+			break;
+
+		default:
+			rgbled_usage();
+			exit(0);
+		}
+	}
+
+        if (optind >= argc) {
+            rgbled_usage();
+            exit(1);
+        }
+
+	const char *verb = argv[optind];
+
+	int fd;
+	int ret;
+
+	if (!strcmp(verb, "start")) {
+		if (g_rgbled != nullptr)
+			errx(1, "already started");
+
+		if (i2cdevice == -1) {
+			// try the external bus first
+			i2cdevice = PX4_I2C_BUS_EXPANSION;
+			g_rgbled = new RGBLED(PX4_I2C_BUS_EXPANSION, rgbledadr);
+
+			if (g_rgbled != nullptr && OK != g_rgbled->init()) {
+				delete g_rgbled;
+				g_rgbled = nullptr;
+			}
+
+			if (g_rgbled == 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_rgbled == nullptr) {
+			g_rgbled = new RGBLED(i2cdevice, rgbledadr);
+
+			if (g_rgbled == nullptr)
+				errx(1, "new failed");
+
+			if (OK != g_rgbled->init()) {
+				delete g_rgbled;
+				g_rgbled = nullptr;
+				errx(1, "init failed");
+			}
+		}
+
+		exit(0);
+	}
+
+	/* need the driver past this point */
+	if (g_rgbled == nullptr) {
+		warnx("not started");
+		rgbled_usage();
+		exit(1);
+	}
+
+	if (!strcmp(verb, "test")) {
+		fd = open(RGBLED_DEVICE_PATH, 0);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " RGBLED_DEVICE_PATH);
+		}
+
+		rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_WHITE, RGBLED_COLOR_OFF, RGBLED_COLOR_OFF},
+			{500, 500, 500, 500, 1000, 0 }	// "0" indicates end of pattern
+		};
+
+		ret = ioctl(fd, RGBLED_SET_PATTERN, (unsigned long)&pattern);
+		ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_PATTERN);
+
+		close(fd);
+		exit(ret);
+	}
+
+	if (!strcmp(verb, "info")) {
+		g_rgbled->info();
+		exit(0);
+	}
+
+	if (!strcmp(verb, "off") || !strcmp(verb, "stop")) {
+		fd = open(RGBLED_DEVICE_PATH, 0);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " RGBLED_DEVICE_PATH);
+		}
+
+		ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF);
+		close(fd);
+		exit(ret);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		delete g_rgbled;
+		g_rgbled = nullptr;
+		exit(0);
+	}
+
+	if (!strcmp(verb, "rgb")) {
+		if (argc < 5) {
+			errx(1, "Usage: rgbled rgb <red> <green> <blue>");
+		}
+
+		fd = open(RGBLED_DEVICE_PATH, 0);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " RGBLED_DEVICE_PATH);
+		}
+
+		rgbled_rgbset_t v;
+		v.red   = strtol(argv[2], NULL, 0);
+		v.green = strtol(argv[3], NULL, 0);
+		v.blue  = strtol(argv[4], NULL, 0);
+		ret = ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v);
+		ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON);
+		close(fd);
+		exit(ret);
+	}
+
+	rgbled_usage();
+	exit(0);
+}
diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp
new file mode 100644
index 000000000..d65a9be36
--- /dev/null
+++ b/src/drivers/roboclaw/RoboClaw.cpp
@@ -0,0 +1,328 @@
+/****************************************************************************
+ *
+ *   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 RoboClaw.cpp
+ *
+ * RoboClaw Motor Driver
+ *
+ * references:
+ * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf
+ *
+ */
+
+#include "RoboClaw.hpp"
+#include <poll.h>
+#include <stdio.h>
+#include <math.h>
+#include <string.h>
+#include <fcntl.h>
+#include <termios.h>
+
+#include <systemlib/err.h>
+#include <arch/board/board.h>
+#include <mavlink/mavlink_log.h>
+
+#include <controllib/uorb/UOrbPublication.hpp>
+#include <uORB/topics/debug_key_value.h>
+#include <drivers/drv_hrt.h>
+
+uint8_t RoboClaw::checksum_mask = 0x7f;
+
+RoboClaw::RoboClaw(const char *deviceName, uint16_t address,
+		uint16_t pulsesPerRev):
+	_address(address),
+	_pulsesPerRev(pulsesPerRev),
+	_uart(0),
+	_controlPoll(),
+	_actuators(NULL, ORB_ID(actuator_controls_0), 20),
+	_motor1Position(0),
+	_motor1Speed(0),
+	_motor1Overflow(0),
+	_motor2Position(0),
+	_motor2Speed(0),
+	_motor2Overflow(0)
+{
+	// setup control polling
+	_controlPoll.fd = _actuators.getHandle();
+	_controlPoll.events = POLLIN;
+
+	// start serial port
+	_uart = open(deviceName, O_RDWR | O_NOCTTY);
+	if (_uart < 0) err(1, "could not open %s", deviceName);
+	int ret = 0;
+	struct termios uart_config;
+	ret = tcgetattr(_uart, &uart_config);
+	if (ret < 0) err (1, "failed to get attr");
+	uart_config.c_oflag &= ~ONLCR; // no CR for every LF
+	ret = cfsetispeed(&uart_config, B38400);
+	if (ret < 0) err (1, "failed to set input speed");
+	ret = cfsetospeed(&uart_config, B38400);
+	if (ret < 0) err (1, "failed to set output speed");
+	ret = tcsetattr(_uart, TCSANOW, &uart_config);
+	if (ret < 0) err (1, "failed to set attr");
+
+	// setup default settings, reset encoders
+	resetEncoders();
+}
+
+RoboClaw::~RoboClaw()
+{
+	setMotorDutyCycle(MOTOR_1, 0.0);
+	setMotorDutyCycle(MOTOR_2, 0.0);
+	close(_uart);
+}
+
+int RoboClaw::readEncoder(e_motor motor)
+{
+	uint16_t sum = 0;
+	if (motor == MOTOR_1) {
+		_sendCommand(CMD_READ_ENCODER_1, nullptr, 0, sum);
+	} else if (motor == MOTOR_2) {
+		_sendCommand(CMD_READ_ENCODER_2, nullptr, 0, sum);
+	}
+	uint8_t rbuf[50];
+	usleep(5000);
+	int nread = read(_uart, rbuf, 50);
+	if (nread < 6) { 
+		printf("failed to read\n");
+		return -1;
+	}
+	//printf("received: \n");
+	//for (int i=0;i<nread;i++) {
+		//printf("%d\t", rbuf[i]);
+	//}
+	//printf("\n");
+	uint32_t count = 0;
+	uint8_t * countBytes = (uint8_t *)(&count);
+	countBytes[3] = rbuf[0];
+	countBytes[2] = rbuf[1];
+	countBytes[1] = rbuf[2];
+	countBytes[0] = rbuf[3];
+	uint8_t status = rbuf[4];
+	uint8_t checksum = rbuf[5];
+	uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) & 
+		checksum_mask;
+	// check if checksum is valid
+	if (checksum != checksum_computed) {
+		printf("checksum failed: expected %d got %d\n",
+				checksum, checksum_computed);
+		return -1;
+	}
+	int overFlow = 0;
+
+	if (status & STATUS_REVERSE) {
+		//printf("roboclaw: reverse\n");
+	}
+
+	if (status & STATUS_UNDERFLOW) {
+		//printf("roboclaw: underflow\n");
+		overFlow = -1;
+	} else if (status & STATUS_OVERFLOW) {
+		//printf("roboclaw: overflow\n");
+		overFlow = +1;
+	}
+
+	static int64_t overflowAmount = 0x100000000LL;
+	if (motor == MOTOR_1) {
+		_motor1Overflow += overFlow;
+		_motor1Position = float(int64_t(count) + 
+			_motor1Overflow*overflowAmount)/_pulsesPerRev;
+	} else if (motor == MOTOR_2) {
+		_motor2Overflow += overFlow;
+		_motor2Position = float(int64_t(count) + 
+			_motor2Overflow*overflowAmount)/_pulsesPerRev;
+	}
+	return 0;
+}
+
+void RoboClaw::printStatus(char *string, size_t n)
+{
+	snprintf(string, n,
+		 "pos1,spd1,pos2,spd2: %10.2f %10.2f %10.2f %10.2f\n",
+		 double(getMotorPosition(MOTOR_1)),
+		 double(getMotorSpeed(MOTOR_1)),
+		 double(getMotorPosition(MOTOR_2)),
+		 double(getMotorSpeed(MOTOR_2)));
+}
+
+float RoboClaw::getMotorPosition(e_motor motor)
+{
+	if (motor == MOTOR_1) {
+		return _motor1Position;
+	} else if (motor == MOTOR_2) {
+		return _motor2Position;
+	}
+}
+
+float RoboClaw::getMotorSpeed(e_motor motor)
+{
+	if (motor == MOTOR_1) {
+		return _motor1Speed;
+	} else if (motor == MOTOR_2) {
+		return _motor2Speed;
+	}
+}
+
+int RoboClaw::setMotorSpeed(e_motor motor, float value)
+{
+	uint16_t sum = 0;
+	// bound
+	if (value > 1) value = 1;
+	if (value < -1) value = -1;
+	uint8_t speed = fabs(value)*127;
+	// send command
+	if (motor == MOTOR_1) {
+		if (value > 0) {
+			return _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum);
+		} else {
+			return _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum);
+		}
+	} else if (motor == MOTOR_2) {
+		if (value > 0) {
+			return _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum);
+		} else {
+			return _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum);
+		}
+	}
+	return -1;
+}
+
+int RoboClaw::setMotorDutyCycle(e_motor motor, float value)
+{
+	uint16_t sum = 0;
+	// bound
+	if (value > 1) value = 1;
+	if (value < -1) value = -1;
+	int16_t duty = 1500*value;
+	// send command
+	if (motor == MOTOR_1) {
+		return _sendCommand(CMD_SIGNED_DUTYCYCLE_1,
+				(uint8_t *)(&duty), 2, sum);
+	} else if (motor == MOTOR_2) {
+		return _sendCommand(CMD_SIGNED_DUTYCYCLE_2,
+				(uint8_t *)(&duty), 2, sum);
+	}
+	return -1;
+}
+
+int RoboClaw::resetEncoders() 
+{
+	uint16_t sum = 0;
+	return _sendCommand(CMD_RESET_ENCODERS,
+			nullptr, 0, sum);
+}
+
+int RoboClaw::update()
+{
+	// wait for an actuator publication,
+	// check for exit condition every second
+	// note "::poll" is required to distinguish global
+	// poll from member function for driver
+	if (::poll(&_controlPoll, 1, 1000) < 0) return -1; // poll error
+
+	// if new data, send to motors
+	if (_actuators.updated()) {
+		_actuators.update();
+		setMotorDutyCycle(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]);
+		setMotorDutyCycle(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]);
+	}
+	return 0;
+}
+
+uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n)
+{
+	uint16_t sum = 0;
+	//printf("sum\n");
+	for (size_t i=0;i<n;i++) {
+		sum += buf[i];
+		//printf("%d\t", buf[i]);
+	}
+	//printf("total sum %d\n", sum);
+	return sum;
+}
+
+int RoboClaw::_sendCommand(e_command cmd, uint8_t * data, 
+		size_t n_data, uint16_t & prev_sum)
+{
+	tcflush(_uart, TCIOFLUSH); // flush  buffers
+	uint8_t buf[n_data + 3];
+	buf[0] = _address;
+	buf[1] = cmd;
+	for (size_t i=0;i<n_data;i++) {
+		buf[i+2] = data[n_data - i - 1]; // MSB
+	}
+	uint16_t sum = _sumBytes(buf, n_data + 2);
+	prev_sum += sum;
+	buf[n_data + 2] = sum & checksum_mask;
+	//printf("\nmessage:\n");
+	//for (size_t i=0;i<n_data+3;i++) {
+		//printf("%d\t", buf[i]);
+	//}
+	//printf("\n");
+	return write(_uart, buf, n_data + 3);
+}
+
+int roboclawTest(const char *deviceName, uint8_t address, 
+		uint16_t pulsesPerRev)
+{
+	printf("roboclaw test: starting\n");
+
+	// setup
+	RoboClaw roboclaw(deviceName, address, pulsesPerRev);
+	roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, 0.3);
+	roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_2, 0.3);
+	char buf[200];
+	for (int i=0; i<10; i++) {
+		usleep(100000);
+		roboclaw.readEncoder(RoboClaw::MOTOR_1);
+		roboclaw.readEncoder(RoboClaw::MOTOR_2);
+		roboclaw.printStatus(buf,200);
+		printf("%s", buf);
+	}
+
+	roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, -0.3);
+	roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_2, -0.3);
+	for (int i=0; i<10; i++) {
+		usleep(100000);
+		roboclaw.readEncoder(RoboClaw::MOTOR_1);
+		roboclaw.readEncoder(RoboClaw::MOTOR_2);
+		roboclaw.printStatus(buf,200);
+		printf("%s", buf);
+	}
+
+	printf("Test complete\n");
+	return 0;
+}
+
+// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp
new file mode 100644
index 000000000..e9f35cf95
--- /dev/null
+++ b/src/drivers/roboclaw/RoboClaw.hpp
@@ -0,0 +1,192 @@
+/****************************************************************************
+ *
+ *   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 RoboClas.hpp
+ *
+ * RoboClaw Motor Driver
+ *
+ * references:
+ * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf
+ *
+ */
+
+#pragma once
+
+#include <poll.h>
+#include <stdio.h>
+#include <controllib/uorb/UOrbSubscription.hpp>
+#include <uORB/topics/actuator_controls.h>
+#include <drivers/device/i2c.h>
+
+/**
+ * This is a driver for the RoboClaw motor controller
+ */
+class RoboClaw
+{
+public:
+
+	/** control channels */
+	enum e_channel {
+		CH_VOLTAGE_LEFT = 0,
+		CH_VOLTAGE_RIGHT
+	};
+	
+	/**  motors */
+	enum e_motor {
+		MOTOR_1 = 0,
+		MOTOR_2
+	};
+
+	/**
+	 * constructor
+	 * @param deviceName the name of the 
+	 * 	serial port e.g. "/dev/ttyS2"
+	 * @param address the adddress  of the motor
+	 * 	(selectable on roboclaw)
+	 * @param pulsesPerRev # of encoder
+	 *  pulses per revolution of wheel
+	 */
+	RoboClaw(const char *deviceName, uint16_t address, 
+			uint16_t pulsesPerRev);
+
+	/**
+	 * deconstructor
+	 */
+	virtual ~RoboClaw();
+
+	/**
+	 * @return position of a motor, rev
+	 */
+	float getMotorPosition(e_motor motor);
+
+	/**
+	 * @return speed of a motor, rev/sec
+	 */
+	float getMotorSpeed(e_motor motor);
+
+	/**
+	 * set the speed of a motor, rev/sec
+	 */
+	int setMotorSpeed(e_motor motor, float value);
+
+	/**
+	 * set the duty cycle of a motor, rev/sec
+	 */
+	int setMotorDutyCycle(e_motor motor, float value);
+
+	/**
+	 * reset the encoders
+	 * @return status
+	 */
+	int resetEncoders();
+
+	/**
+	 * main update loop that updates RoboClaw motor
+	 * dutycycle based on actuator publication
+	 */
+	int update();
+
+	/**
+	 * read data from serial
+	 */
+	int readEncoder(e_motor motor);
+
+	/**
+	 * print status
+	 */
+	void printStatus(char *string, size_t n);
+
+private:
+
+	// Quadrature status flags
+	enum e_quadrature_status_flags {
+		STATUS_UNDERFLOW = 1 << 0, /**< encoder went below 0 **/
+		STATUS_REVERSE = 1 << 1, /**< motor doing in reverse dir **/
+		STATUS_OVERFLOW = 1 << 2, /**< encoder went above 2^32 **/
+	};
+
+	// commands
+	// We just list the commands we want from the manual here.
+	enum e_command {
+
+		// simple
+		CMD_DRIVE_FWD_1 = 0,
+		CMD_DRIVE_REV_1 = 1,
+		CMD_DRIVE_FWD_2 = 4,
+		CMD_DRIVE_REV_2 = 5,
+
+		// encoder commands
+		CMD_READ_ENCODER_1 = 16,
+		CMD_READ_ENCODER_2 = 17,
+		CMD_RESET_ENCODERS = 20,
+
+		// advanced motor control
+		CMD_READ_SPEED_HIRES_1 = 30,
+		CMD_READ_SPEED_HIRES_2 = 31, 
+		CMD_SIGNED_DUTYCYCLE_1 = 32,
+		CMD_SIGNED_DUTYCYCLE_2 = 33,
+	};
+
+	static uint8_t checksum_mask;
+
+	uint16_t _address;
+	uint16_t _pulsesPerRev;
+
+	int _uart;
+
+	/** poll structure for control packets */
+	struct pollfd _controlPoll;
+
+	/** actuator controls subscription */
+	control::UOrbSubscription<actuator_controls_s> _actuators;
+
+	// private data
+	float _motor1Position;
+	float _motor1Speed;
+	int16_t _motor1Overflow;
+
+	float _motor2Position;
+	float _motor2Speed;
+	int16_t _motor2Overflow;
+
+	// private methods
+	uint16_t _sumBytes(uint8_t * buf, size_t n);
+	int _sendCommand(e_command cmd, uint8_t * data, size_t n_data, uint16_t & prev_sum);
+};
+
+// unit testing
+int roboclawTest(const char *deviceName, uint8_t address,
+		uint16_t pulsesPerRev);
+
+// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
diff --git a/src/drivers/roboclaw/module.mk b/src/drivers/roboclaw/module.mk
new file mode 100644
index 000000000..1abecf198
--- /dev/null
+++ b/src/drivers/roboclaw/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.
+#
+############################################################################
+
+#
+# RoboClaw Motor Controller
+#
+
+MODULE_COMMAND		= roboclaw
+
+SRCS			= roboclaw_main.cpp \
+			  RoboClaw.cpp
diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp
new file mode 100644
index 000000000..44ed03254
--- /dev/null
+++ b/src/drivers/roboclaw/roboclaw_main.cpp
@@ -0,0 +1,195 @@
+/****************************************************************************
+ *
+ *   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 roboclaw_main.cpp
+ *
+ * RoboClaw Motor Driver
+ *
+ * references:
+ * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf
+ *
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
+
+#include <arch/board/board.h>
+#include "RoboClaw.hpp"
+
+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 */
+
+/**
+ * Deamon management function.
+ */
+extern "C" __EXPORT int roboclaw_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of deamon.
+ */
+int roboclaw_thread_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+static void usage();
+
+static void usage()
+{
+	fprintf(stderr, "usage: roboclaw "
+			"{start|stop|status|test}\n\n");
+}
+
+/**
+ * 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 roboclaw_main(int argc, char *argv[])
+{
+
+	if (argc < 1)
+		usage();
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			printf("roboclaw already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		deamon_task = task_spawn_cmd("roboclaw",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 10,
+					 2048,
+					 roboclaw_thread_main,
+					 (const char **)argv);
+		exit(0);
+
+	} else if (!strcmp(argv[1], "test")) {
+
+		const char * deviceName = "/dev/ttyS2";
+		uint8_t address = 128;
+		uint16_t pulsesPerRev = 1200;
+
+		if (argc == 2) {
+			printf("testing with default settings\n");
+		} else if (argc != 4) {
+			printf("usage: roboclaw test device address pulses_per_rev\n");
+			exit(-1);
+		} else {
+			deviceName = argv[2];
+			address = strtoul(argv[3], nullptr, 0);
+			pulsesPerRev = strtoul(argv[4], nullptr, 0);
+		}
+
+		printf("device:\t%s\taddress:\t%d\tpulses per rev:\t%ld\n",
+			deviceName, address, pulsesPerRev);
+
+		roboclawTest(deviceName, address, pulsesPerRev);
+		thread_should_exit = true;
+		exit(0);
+
+	} else if (!strcmp(argv[1], "stop")) {
+
+		thread_should_exit = true;
+		exit(0);
+
+	} else if (!strcmp(argv[1], "status")) {
+
+		if (thread_running) {
+			printf("\troboclaw app is running\n");
+
+		} else {
+			printf("\troboclaw app not started\n");
+		}
+		exit(0);
+	}
+
+	usage();
+	exit(1);
+}
+
+int roboclaw_thread_main(int argc, char *argv[])
+{
+	printf("[roboclaw] starting\n");
+
+	// skip parent process args
+	argc -=2;
+	argv +=2;
+
+	if (argc < 3) {
+		printf("usage: roboclaw start device address\n");
+		return -1;
+	}
+
+	const char *deviceName = argv[1];
+	uint8_t address = strtoul(argv[2], nullptr, 0);
+	uint16_t pulsesPerRev = strtoul(argv[3], nullptr, 0);
+
+	printf("device:\t%s\taddress:\t%d\tpulses per rev:\t%ld\n",
+			deviceName, address, pulsesPerRev);
+
+	// start
+	RoboClaw roboclaw(deviceName, address, pulsesPerRev);
+
+	thread_running = true;
+
+	// loop
+	while (!thread_should_exit) {
+		roboclaw.update();
+	}
+
+	// exit
+	printf("[roboclaw] exiting.\n");
+	thread_running = false;
+	return 0;
+}
+
+// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp
index 1020eb946..00e46d6b8 100644
--- a/src/drivers/stm32/adc/adc.cpp
+++ b/src/drivers/stm32/adc/adc.cpp
@@ -227,7 +227,6 @@ ADC::init()
 		if ((hrt_absolute_time() - now) > 500) {
 			log("sample timeout");
 			return -1;
-			return 0xffff;
 		}
 	}
 
@@ -282,7 +281,7 @@ ADC::close_last(struct file *filp)
 void
 ADC::_tick_trampoline(void *arg)
 {
-	((ADC *)arg)->_tick();
+	(reinterpret_cast<ADC *>(arg))->_tick();
 }
 
 void
@@ -342,7 +341,7 @@ test(void)
 		err(1, "can't open ADC device");
 
 	for (unsigned i = 0; i < 50; i++) {
-		adc_msg_s data[8];
+		adc_msg_s data[10];
 		ssize_t count = read(fd, data, sizeof(data));
 
 		if (count < 0)
@@ -366,8 +365,15 @@ int
 adc_main(int argc, char *argv[])
 {
 	if (g_adc == nullptr) {
-		/* XXX this hardcodes the default channel set for PX4FMU - should be configurable */
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+		/* XXX this hardcodes the default channel set for PX4FMUv1 - should be configurable */
 		g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13));
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+		/* XXX this hardcodes the default channel set for PX4FMUv2 - should be configurable */
+		g_adc = new ADC((1 << 2) | (1 << 3) | (1 << 4) | 
+			(1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15));
+#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 05e1cd8ec..b7c9b89a4 100644
--- a/src/drivers/stm32/drv_hrt.c
+++ b/src/drivers/stm32/drv_hrt.c
@@ -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
@@ -59,7 +59,7 @@
 #include <errno.h>
 #include <string.h>
 
-#include <arch/board/board.h>
+#include <board_config.h>
 #include <drivers/drv_hrt.h>
 
 #include "chip.h"
@@ -70,7 +70,7 @@
 #include "stm32_gpio.h"
 #include "stm32_tim.h"
 
-#ifdef CONFIG_HRT_TIMER
+#ifdef HRT_TIMER
 
 /* HRT configuration */
 #if   HRT_TIMER == 1
@@ -155,7 +155,7 @@
 #  error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=11
 # endif
 #else
-# error HRT_TIMER must be set in board.h if CONFIG_HRT_TIMER=y
+# error HRT_TIMER must be a value between 1 and 11
 #endif
 
 /*
@@ -168,7 +168,7 @@
 # error HRT_TIMER_CLOCK must be greater than 1MHz
 #endif
 
-/*
+/**
  * Minimum/maximum deadlines.
  *
  * These are suitable for use with a 16-bit timer/counter clocked
@@ -275,11 +275,17 @@ static void		hrt_call_invoke(void);
 /*
  * Specific registers and bits used by PPM sub-functions
  */
-#ifdef CONFIG_HRT_PPM
-/* 
+#ifdef HRT_PPM_CHANNEL
+/*
  * If the timer hardware doesn't support GTIM_CCER_CCxNP, then we will work around it.
+ *
+ * Note that we assume that M3 means STM32F1 (since we don't really care about the F2).
  */
-# ifndef GTIM_CCER_CC1NP
+# ifdef CONFIG_ARCH_CORTEXM3
+#  undef GTIM_CCER_CC1NP
+#  undef GTIM_CCER_CC2NP
+#  undef GTIM_CCER_CC3NP
+#  undef GTIM_CCER_CC4NP
 #  define GTIM_CCER_CC1NP 0
 #  define GTIM_CCER_CC2NP 0
 #  define GTIM_CCER_CC3NP 0
@@ -324,20 +330,27 @@ static void		hrt_call_invoke(void);
 #  define CCER_PPM	(GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP) /* CC4, both edges */
 #  define CCER_PPM_FLIP	GTIM_CCER_CC4P
 # else
-#  error HRT_PPM_CHANNEL must be a value between 1 and 4 if CONFIG_HRT_PPM is set
+#  error HRT_PPM_CHANNEL must be a value between 1 and 4
 # endif
 
 /*
  * PPM decoder tuning parameters
  */
-# define PPM_MAX_PULSE_WIDTH	550		/* maximum width of a valid pulse */
-# define PPM_MIN_CHANNEL_VALUE	800		/* shortest valid channel signal */
-# define PPM_MAX_CHANNEL_VALUE	2200		/* longest valid channel signal */
-# define PPM_MIN_START		2500		/* shortest valid start gap */
+# define PPM_MIN_PULSE_WIDTH	200		/**< minimum width of a valid first pulse */
+# define PPM_MAX_PULSE_WIDTH	600		/**< maximum width of a valid first pulse */
+# define PPM_MIN_CHANNEL_VALUE	800		/**< shortest valid channel signal */
+# define PPM_MAX_CHANNEL_VALUE	2200		/**< longest valid channel signal */
+# define PPM_MIN_START		2300		/**< shortest valid start gap (only 2nd part of pulse) */
 
 /* decoded PPM buffer */
-#define PPM_MAX_CHANNELS	12
+#define PPM_MIN_CHANNELS	5
+#define PPM_MAX_CHANNELS	20
+
+/** Number of same-sized frames required to 'lock' */
+#define PPM_CHANNEL_LOCK	4		/**< should be less than the input timeout */
+
 __EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS];
+__EXPORT uint16_t ppm_frame_length = 0;
 __EXPORT unsigned ppm_decoded_channels = 0;
 __EXPORT uint64_t ppm_last_valid_decode = 0;
 
@@ -351,11 +364,12 @@ unsigned ppm_pulse_next;
 
 static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS];
 
-/* PPM decoder state machine */
+/** PPM decoder state machine */
 struct {
-	uint16_t	last_edge;	/* last capture time */
-	uint16_t	last_mark;	/* last significant edge */
-	unsigned	next_channel;
+	uint16_t	last_edge;	/**< last capture time */
+	uint16_t	last_mark;	/**< last significant edge */
+	uint16_t	frame_start;	/**< the frame width */
+	unsigned	next_channel;	/**< next channel index */
 	enum {
 		UNSYNCH = 0,
 		ARM,
@@ -375,9 +389,9 @@ static void	hrt_ppm_decode(uint32_t status);
 # define CCMR1_PPM	0
 # define CCMR2_PPM	0
 # define CCER_PPM	0
-#endif /* CONFIG_HRT_PPM */
+#endif /* HRT_PPM_CHANNEL */
 
-/*
+/**
  * Initialise the timer we are going to use.
  *
  * We expect that we'll own one of the reduced-function STM32 general
@@ -422,8 +436,8 @@ hrt_tim_init(void)
 	up_enable_irq(HRT_TIMER_VECTOR);
 }
 
-#ifdef CONFIG_HRT_PPM
-/*
+#ifdef HRT_PPM_CHANNEL
+/**
  * Handle the PPM decoder state machine.
  */
 static void
@@ -438,9 +452,8 @@ hrt_ppm_decode(uint32_t status)
 	if (status & SR_OVF_PPM)
 		goto error;
 
-	/* how long since the last edge? */
+	/* how long since the last edge? - this handles counter wrapping implicitely. */
 	width = count - ppm.last_edge;
-	ppm.last_edge = count;
 
 	ppm_edge_history[ppm_edge_next++] = width;
 
@@ -453,14 +466,39 @@ hrt_ppm_decode(uint32_t status)
 	 */
 	if (width >= PPM_MIN_START) {
 
-		/* export the last set of samples if we got something sensible */
-		if (ppm.next_channel > 4) {
-			for (i = 0; i < ppm.next_channel && i < PPM_MAX_CHANNELS; i++)
-				ppm_buffer[i] = ppm_temp_buffer[i];
+		/*
+		 * If the number of channels changes unexpectedly, we don't want
+		 * to just immediately jump on the new count as it may be a result
+		 * of noise or dropped edges.  Instead, take a few frames to settle.
+		 */
+		if (ppm.next_channel != ppm_decoded_channels) {
+			static unsigned new_channel_count;
+			static unsigned new_channel_holdoff;
+
+			if (new_channel_count != ppm.next_channel) {
+				/* start the lock counter for the new channel count */
+				new_channel_count = ppm.next_channel;
+				new_channel_holdoff = PPM_CHANNEL_LOCK;
+
+			} else if (new_channel_holdoff > 0) {
+				/* this frame matched the last one, decrement the lock counter */
+				new_channel_holdoff--;
+
+			} else {
+				/* we have seen PPM_CHANNEL_LOCK frames with the new count, accept it */
+				ppm_decoded_channels = new_channel_count;
+				new_channel_count = 0;
+			}
+
+		} else {
+			/* frame channel count matches expected, let's use it */
+			if (ppm.next_channel > PPM_MIN_CHANNELS) {
+				for (i = 0; i < ppm.next_channel; i++)
+					ppm_buffer[i] = ppm_temp_buffer[i];
 
-			ppm_decoded_channels = i;
-			ppm_last_valid_decode = hrt_absolute_time();
+				ppm_last_valid_decode = hrt_absolute_time();
 
+			}
 		}
 
 		/* reset for the next frame */
@@ -469,29 +507,39 @@ hrt_ppm_decode(uint32_t status)
 		/* next edge is the reference for the first channel */
 		ppm.phase = ARM;
 
+		ppm.last_edge = count;
 		return;
 	}
 
 	switch (ppm.phase) {
 	case UNSYNCH:
 		/* we are waiting for a start pulse - nothing useful to do here */
-		return;
+		break;
 
 	case ARM:
 
 		/* we expect a pulse giving us the first mark */
-		if (width > PPM_MAX_PULSE_WIDTH)
-			goto error;		/* pulse was too long */
+		if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH)
+			goto error;		/* pulse was too short or too long */
 
 		/* record the mark timing, expect an inactive edge */
-		ppm.last_mark = count;
-		ppm.phase = INACTIVE;
-		return;
+		ppm.last_mark = ppm.last_edge;
+
+		/* frame length is everything including the start gap */
+		ppm_frame_length = (uint16_t)(ppm.last_edge - ppm.frame_start);
+		ppm.frame_start = ppm.last_edge;
+		ppm.phase = ACTIVE;
+		break;
 
 	case INACTIVE:
+
+		/* we expect a short pulse */
+		if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH)
+			goto error;		/* pulse was too short or too long */
+
 		/* this edge is not interesting, but now we are ready for the next mark */
 		ppm.phase = ACTIVE;
-		return;
+		break;
 
 	case ACTIVE:
 		/* determine the interval from the last mark */
@@ -512,10 +560,13 @@ hrt_ppm_decode(uint32_t status)
 			ppm_temp_buffer[ppm.next_channel++] = interval;
 
 		ppm.phase = INACTIVE;
-		return;
+		break;
 
 	}
 
+	ppm.last_edge = count;
+	return;
+
 	/* the state machine is corrupted; reset it */
 
 error:
@@ -524,9 +575,9 @@ error:
 	ppm_decoded_channels = 0;
 
 }
-#endif /* CONFIG_HRT_PPM */
+#endif /* HRT_PPM_CHANNEL */
 
-/*
+/**
  * Handle the compare interupt by calling the callout dispatcher
  * and then re-scheduling the next deadline.
  */
@@ -544,7 +595,7 @@ hrt_tim_isr(int irq, void *context)
 	/* ack the interrupts we just read */
 	rSR = ~status;
 
-#ifdef CONFIG_HRT_PPM
+#ifdef HRT_PPM_CHANNEL
 
 	/* was this a PPM edge? */
 	if (status & (SR_INT_PPM | SR_OVF_PPM)) {
@@ -555,6 +606,7 @@ hrt_tim_isr(int irq, void *context)
 
 		hrt_ppm_decode(status);
 	}
+
 #endif
 
 	/* was this a timer tick? */
@@ -573,7 +625,7 @@ hrt_tim_isr(int irq, void *context)
 	return OK;
 }
 
-/*
+/**
  * Fetch a never-wrapping absolute time value in microseconds from
  * some arbitrary epoch shortly after system start.
  */
@@ -620,7 +672,7 @@ hrt_absolute_time(void)
 	return abstime;
 }
 
-/*
+/**
  * Convert a timespec to absolute time
  */
 hrt_abstime
@@ -634,7 +686,7 @@ ts_to_abstime(struct timespec *ts)
 	return result;
 }
 
-/*
+/**
  * Convert absolute time to a timespec.
  */
 void
@@ -645,7 +697,7 @@ abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
 	ts->tv_nsec = abstime * 1000;
 }
 
-/*
+/**
  * Compare a time value with the current time.
  */
 hrt_abstime
@@ -660,7 +712,7 @@ hrt_elapsed_time(const volatile hrt_abstime *then)
 	return delta;
 }
 
-/*
+/**
  * Store the absolute time in an interrupt-safe fashion
  */
 hrt_abstime
@@ -675,7 +727,7 @@ hrt_store_absolute_time(volatile hrt_abstime *now)
 	return ts;
 }
 
-/*
+/**
  * Initalise the high-resolution timing module.
  */
 void
@@ -684,13 +736,13 @@ hrt_init(void)
 	sq_init(&callout_queue);
 	hrt_tim_init();
 
-#ifdef CONFIG_HRT_PPM
+#ifdef HRT_PPM_CHANNEL
 	/* configure the PPM input pin */
 	stm32_configgpio(GPIO_PPM_IN);
 #endif
 }
 
-/*
+/**
  * Call callout(arg) after interval has elapsed.
  */
 void
@@ -703,7 +755,7 @@ hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, v
 			  arg);
 }
 
-/*
+/**
  * Call callout(arg) at calltime.
  */
 void
@@ -712,7 +764,7 @@ hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, v
 	hrt_call_internal(entry, calltime, 0, callout, arg);
 }
 
-/*
+/**
  * Call callout(arg) every period.
  */
 void
@@ -731,6 +783,13 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
 	irqstate_t flags = irqsave();
 
 	/* if the entry is currently queued, remove it */
+	/* note that we are using a potentially uninitialised
+	   entry->link here, but it is safe as sq_rem() doesn't
+	   dereference the passed node unless it is found in the
+	   list. So we potentially waste a bit of time searching the
+	   queue for the uninitialised entry->link but we don't do
+	   anything actually unsafe.
+	*/
 	if (entry->deadline != 0)
 		sq_rem(&entry->link, &callout_queue);
 
@@ -744,7 +803,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
 	irqrestore(flags);
 }
 
-/*
+/**
  * If this returns true, the call has been invoked and removed from the callout list.
  *
  * Always returns false for repeating callouts.
@@ -755,7 +814,7 @@ hrt_called(struct hrt_call *entry)
 	return (entry->deadline == 0);
 }
 
-/*
+/**
  * Remove the entry from the callout list.
  */
 void
@@ -837,13 +896,19 @@ hrt_call_invoke(void)
 
 		/* if the callout has a non-zero period, it has to be re-entered */
 		if (call->period != 0) {
-			call->deadline = deadline + call->period;
+			// re-check call->deadline to allow for
+			// callouts to re-schedule themselves
+			// using hrt_call_delay()
+			if (call->deadline <= now) {
+				call->deadline = deadline + call->period;
+			}
+
 			hrt_call_enter(call);
 		}
 	}
 }
 
-/*
+/**
  * Reschedule the next timer interrupt.
  *
  * This routine must be called with interrupts disabled.
@@ -904,5 +969,16 @@ hrt_latency_update(void)
 	latency_counters[index]++;
 }
 
+void
+hrt_call_init(struct hrt_call *entry)
+{
+	memset(entry, 0, sizeof(*entry));
+}
+
+void
+hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
+{
+	entry->deadline = hrt_absolute_time() + delay;
+}
 
-#endif /* CONFIG_HRT_TIMER */
+#endif /* HRT_TIMER */
diff --git a/src/drivers/stm32/drv_pwm_servo.c b/src/drivers/stm32/drv_pwm_servo.c
index 7b060412c..dbb45a138 100644
--- a/src/drivers/stm32/drv_pwm_servo.c
+++ b/src/drivers/stm32/drv_pwm_servo.c
@@ -88,6 +88,7 @@
 #define rCCR4(_tmr)   	REG(_tmr, STM32_GTIM_CCR4_OFFSET)
 #define rDCR(_tmr)    	REG(_tmr, STM32_GTIM_DCR_OFFSET)
 #define rDMAR(_tmr)   	REG(_tmr, STM32_GTIM_DMAR_OFFSET)
+#define rBDTR(_tmr)	REG(_tmr, STM32_ATIM_BDTR_OFFSET)
 
 static void		pwm_timer_init(unsigned timer);
 static void		pwm_timer_set_rate(unsigned timer, unsigned rate);
@@ -110,6 +111,11 @@ pwm_timer_init(unsigned timer)
 	rCCER(timer) = 0;
 	rDCR(timer) = 0;
 
+	if ((pwm_timers[timer].base == STM32_TIM1_BASE) || (pwm_timers[timer].base == STM32_TIM8_BASE)) {
+		/* master output enable = on */
+		rBDTR(timer) = ATIM_BDTR_MOE;
+	}
+
 	/* configure the timer to free-run at 1MHz */
 	rPSC(timer) = (pwm_timers[timer].clock_freq / 1000000) - 1;
 
diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index 167ef30a8..f36f2091e 100644
--- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -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
@@ -104,7 +104,7 @@
 #include <math.h>
 #include <ctype.h>
 
-#include <arch/board/board.h>
+#include <board_config.h>
 #include <drivers/drv_hrt.h>
 
 #include <arch/stm32/chip.h>
@@ -117,10 +117,6 @@
 
 #include <systemlib/err.h>
 
-#ifndef CONFIG_HRT_TIMER
-# error This driver requires CONFIG_HRT_TIMER
-#endif
-
 /* Tone alarm configuration */
 #if   TONE_ALARM_TIMER == 2
 # define TONE_ALARM_BASE		STM32_TIM2_BASE
@@ -234,13 +230,18 @@ public:
 
 	virtual int		ioctl(file *filp, int cmd, unsigned long arg);
 	virtual ssize_t		write(file *filp, const char *buffer, size_t len);
+	inline const char	*name(int tune) {
+		return _tune_names[tune];
+	}
 
 private:
-	static const unsigned	_tune_max = 1024; // be reasonable about user tunes
-	static const char	* const _default_tunes[];
-	static const unsigned	_default_ntunes;
+	static const unsigned	_tune_max = 1024 * 8; // be reasonable about user tunes
+	const char		* _default_tunes[TONE_NUMBER_OF_TUNES];
+	const char		* _tune_names[TONE_NUMBER_OF_TUNES];
 	static const uint8_t	_note_tab[];
 
+	unsigned		_default_tune_number; // number of currently playing default tune (0 for none)
+
 	const char		*_user_tune;
 
 	const char		*_tune;		// current tune string
@@ -306,160 +307,6 @@ private:
 
 };
 
-// predefined tune array
-const char * const ToneAlarm::_default_tunes[] = {
-	"MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc",		// startup tune
-	"MBT200a8a8a8PaaaP",					// ERROR tone
-	"MFT200e8a8a",						// NotifyPositive tone
-	"MFT200e8e",						// NotifyNeutral tone
-	"MFT200e8c8e8c8e8c8",					// NotifyNegative tone
-	"MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4", // charge!
-	"MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16",	// dixie
-	"MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8",		// cucuracha
-	"MNT150L8O2GGABGBADGGABL4GL8F+",			// yankee
-	"MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8", // daisy
-	"T200O2B4P8B16B16B4P8B16B16B8G+8E8G+8B8G+8B8O3E8"	// william tell
-	"O2B8G+8E8G+8B8G+8B8O3E8O2B4P8B16B16B4P8B16"
-	"O2B16B4P8B16B16B4P8B16B16B8B16B16B8B8B8B16"
-	"O2B16B8B8B8B16B16B8B8B8B16B16B8B8B2B2B8P8"
-	"P4P4P8O1B16B16B8B16B16B8B16B16O2E8F+8G+8"
-	"O1B16B16B8B16B16O2E8G+16G+16F+8D+8O1B8B16"
-	"O1B16B8B16B16B8B16B16O2E8F+8G+8E16G+16B4"
-	"O2B16A16G+16F+16E8G+8E8O3B16B16B8B16B16B8"
-	"O3B16B16O4E8F+8G+8O3B16B16B8B16B16O4E8G+16"
-	"O4G+16F+8D+8O3B8B16B16B8B16B16B8B16B16O4E8"
-	"O4F+8G+8E16G+16B4B16A16G+16F+16E8G+8E8O3G+16"
-	"O3G+16G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8"
-	"O4C+8O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8"
-	"O3G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8"
-	"O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8"
-	"O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8"
-	"O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16"
-	"O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8O2B16"
-	"O2B16B8F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8"
-	"O2E4G+8F+8F+8F+8O3F+16F+16F+8F+16F+16F+8"
-	"O3G+8A8F+4A8G+8E4G+8F+8O2B16B16B8O1B16B16"
-	"O1B8B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16"
-	"O1B16O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16"
-	"O1B8B16B16O2E8F+8G+8E16G+16B4B16A16G+16F+16"
-	"O2E8G+8E8O3B16B16B8B16B16B8B16B16O4E8F+8"
-	"O4G+8O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8"
-	"O3B16B16B8B16B16B8B16B16O4E8F+8G+8E16G+16"
-	"O4B4B16A16G+16F+16E8G+8E8O3E64F64G64A64B64"
-	"O4C64D64E8E16E16E8E8G+4.F+8E8D+8E8C+8O3B16"
-	"O4C+16O3B16O4C+16O3B16O4C+16D+16E16O3A16"
-	"O3B16A16B16A16B16O4C+16D+16O3G+16A16G+16"
-	"O3A16G+16A16B16O4C+16O3F+16G+16F+16G+16F+16"
-	"O3G+16F+16G+16F+16G+16F+16D+16O2B16O3B16"
-	"O4C+16D+16E8D+8E8C+8O3B16O4C+16O3B16O4C+16"
-	"O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16"
-	"O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16"
-	"O3G+16F+16G+16F+16A16F+16E16E8P8C+4C+16O2C16"
-	"O3C+16O2C16O3D+16C+16O2B16A16A16G+16E16C+16"
-	"O2C+16C+16C+16C+16E16D+16O1C16G+16G+16G+16"
-	"O1G+16G+16G+16O2C+16E16G+16O3C+16C+16C+16"
-	"O3C+16C+16O2C16O3C+16O2C16O3D+16C+16O2B16"
-	"O2A16A16G+16E16C+16C+16C+16C+16C+16E16D+16"
-	"O1C16G+16G+16G+16G+16G+16G+16O2C+16E16G+16"
-	"O3C+16E16D+16C+16D+16O2C16G+16G+16G+16O3G+16"
-	"O3E16C+16D+16O2C16G+16G+16G+16O3G+16E16C+16"
-	"O3D+16O2B16G+16G+16A+16G16D+16D+16G+16G16"
-	"O2G+16G16G+16A16G+16F+16E16O1B16A+16B16O2E16"
-	"O1B16O2F+16O1B16O2G+16E16D+16E16G+16E16A16"
-	"O2F+16B16O3G+16F+16E16D+16F+16E16C+16O2B16"
-	"O3C+16O2B16O3C+16D+16E16F+16G+16O2A16B16"
-	"O2A16B16O3C+16D+16E16F+16O2G+16A16G+16A16"
-	"O2C16O3C+16D+16E16O2F+16G+16F+16G+16F+16"
-	"O2G+16F+16G+16F+16G+16F+16D+16O1B16C16O2C+16"
-	"O2D+16E16O1B16A+16B16O2E16O1B16O2F+16O1B16"
-	"O2G+16E16D+16E16G+16E16A16F+16B16O3G+16F+16"
-	"O3E16D+16F+16E16C+16O2B16O3C+16O2B16O3C+16"
-	"O3D+16E16F+16G+16O2A16B16A16B16O3C+16D+16"
-	"O3E16F+16O2G+16A16G+16A16B16O3C+16D+16E16"
-	"O2F+16O3C+16O2C16O3C+16D+16C+16O2A16F+16"
-	"O2E16O3E16F+16G+16A16B16O4C+16D+16E8E16E16"
-	"O4E8E8G+4.F8E8D+8E8C+8O3B16O4C+16O3B16O4C+16"
-	"O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16"
-	"O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16"
-	"O3G+16F+16G+16F+16G+16F+16G+16F+16G+16F+16"
-	"O3D+16O2B16O3B16O4C+16D+16E8E16E16E8E8G+4."
-	"O4F+8E8D+8E8C+8O3B16O4C+16O3B16O4C+16O3B16"
-	"O4C+16D+16E16O3A16B16A16B16A16B16O4C+16D+16"
-	"O3G+16A16G+16A16G+16A16B16O4C+16O3F+16G+16"
-	"O3F+16G+16F+16A16G+16F+16E8O2B8O3E8G+16G+16"
-	"O3G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8"
-	"O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8G+16"
-	"O3G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8"
-	"O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8"
-	"O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8"
-	"O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16"
-	"O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8A+8"
-	"O3B8O2F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8"
-	"O2E4G+8F+8B8O1B8O2F+16F+16F+8F+16F+16F+8"
-	"O2G+8A8F+4A8G+8E4G+8F+8B16B16B8O1B16B16B8"
-	"O1B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16B16"
-	"O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16B8B16"
-	"O1B16O2E8F+8G+8E16G+16B4B16A16G+16F+16E8"
-	"O1B8O2E8O3B16B16B8B16B16B8B16B16O4E8F+8G+8"
-	"O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8B16"
-	"O3B16B8B16B16B8B16B16O4E8F+8G+8O3E16G+16"
-	"O3B4B16A16G+16F+16E16F+16G+16A16G+16A16B16"
-	"O4C+16O3B16O4C+16D+16E16D+16E16F+16G+16A16"
-	"O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16"
-	"O3B16O4A16O3B16O4A16O3B16O4A16O3B16E16F+16"
-	"O3G+16A16G+16A16B16O4C+16O3B16O4C+16D+16"
-	"O4E16D+16E16F+16G+16A16O3B16O4A16O3B16O4A16"
-	"O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16"
-	"O3B16O4A16O3B16P16G+16O4G+16O3G+16P16D+16"
-	"O4D+16O3D+16P16E16O4E16O3E16P16A16O4A16O3A16"
-	"P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16"
-	"P16O3E16O4E16O3E16P16A16O4A16O3A16O4G16O3G16"
-	"O4G16O3G16O4G16O3G16O4G16O3G16O4G8E8C8E8"
-	"O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+16"
-	"O3G+16O4G+8E8O3B8O4E8G+16O3G+16O4G+16O3G+16"
-	"O4G+16O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16"
-	"O3A+16O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16"
-	"O4A+8G8E8G8B8P16A+16P16A16P16G+16P16F+16"
-	"P16O4E16P16D+16P16C+16P16O3B16P16A+16P16"
-	"O3A16P16G+16P16F+16P16E16P16D+16P16F+16E16"
-	"O3F+16G+16A16G+16A16B16O4C+16O3B16O4C+16"
-	"O4D+16E16D+16E16F+16G+16A16O3B16O4A16O3B16"
-	"O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16"
-	"O4A16O3B16O4A16O3B16E16F+16G+16A16G+16A16"
-	"O3B16O4C+16O3B16O4C+16D+16E16D+16E16F+16"
-	"O4G+16A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16"
-	"O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16"
-	"P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16"
-	"P16O3E16O4E16O3E16P16A16O4A16O3A16P16G+16"
-	"O4G+16O3G+16P16D+16O4D+16O3D+16P16E16O4E16"
-	"O3E16P16A16O4A16O3A16O4G16O3G16O4G16O3G16"
-	"O4G16O3G16O4G16O3G16O4G8E8C8E8G+16O3G+16"
-	"O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+8"
-	"O4E8O3B8O4E8G+16O3G+16O4G+16O3G+16O4G+16"
-	"O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16O3A+16"
-	"O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16O4A+8"
-	"O4G8E8G8B8P16A+16P16A16P16G+16P16F+16P16"
-	"O4E16P16D+16P16C+16P16O3B16P16A+16P16A16"
-	"P16O3G+16P16F+16P16E16P16D+16P16F16E16D+16"
-	"O3E16D+16E8B16B16B8B16B16B8B16B16O4E8F+8"
-	"O4G+8O3B16B16B8B16B16B8B16B16O4G+8A8B8P8"
-	"O4E8F+8G+8P8O3G+8A8B8P8P2O2B16C16O3C+16D16"
-	"O3D+16E16F16F+16G16G+16A16A+16B16C16O4C+16"
-	"O4D+16E16D+16F+16D+16E16D+16F+16D+16E16D+16"
-	"O4F+16D+16E16D+16F+16D+16E16D+16F+16D+16"
-	"O4E16D+16F+16D+16E16D+16F+16D+16E16D+16F+16"
-	"O4D+16E8E16O3E16O4E16O3E16O4E16O3E16O4E8"
-	"O3B16O2B16O3B16O2B16O3B16O2B16O3B8G+16O2G+16"
-	"O3G+16O2G+16O3G+16O2G+16O3G8E16O2E16O3E16"
-	"O2E16O3E16O2E16O3E8E16E16E8E8E8O2B16B16B8"
-	"O2B8B8G+16G+16G+8G+8G+8E16E16E8E8E8O1B8O2E8"
-	"O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8"
-	"O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16"
-	"O2E2P64",
-};
-
-const unsigned ToneAlarm::_default_ntunes = sizeof(_default_tunes) / sizeof(_default_tunes[0]);
-
 // semitone offsets from C for the characters 'A'-'G'
 const uint8_t ToneAlarm::_note_tab[] = {9, 11, 0, 2, 4, 5, 7};
 
@@ -470,13 +317,33 @@ extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]);
 
 
 ToneAlarm::ToneAlarm() :
-	CDev("tone_alarm", "/dev/tone_alarm"),
+	CDev("tone_alarm", TONEALARM_DEVICE_PATH),
+	_default_tune_number(0),
 	_user_tune(nullptr),
 	_tune(nullptr),
 	_next(nullptr)
 {
 	// enable debug() calls
 	//_debug_enabled = true;
+	_default_tunes[TONE_STARTUP_TUNE] = "MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc";		// startup tune
+	_default_tunes[TONE_ERROR_TUNE] = "MBT200a8a8a8PaaaP";						// ERROR tone
+	_default_tunes[TONE_NOTIFY_POSITIVE_TUNE] = "MFT200e8a8a";					// Notify Positive tone
+	_default_tunes[TONE_NOTIFY_NEUTRAL_TUNE] = "MFT200e8e";						// Notify Neutral tone
+	_default_tunes[TONE_NOTIFY_NEGATIVE_TUNE] = "MFT200e8c8e8c8e8c8";				// Notify Negative tone
+	_default_tunes[TONE_ARMING_WARNING_TUNE] = "MNT75L1O2G";					//arming warning
+	_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
+
+	_tune_names[TONE_STARTUP_TUNE] = "startup";			// startup tune
+	_tune_names[TONE_ERROR_TUNE] = "error";				// ERROR tone
+	_tune_names[TONE_NOTIFY_POSITIVE_TUNE] = "positive";		// Notify Positive tone
+	_tune_names[TONE_NOTIFY_NEUTRAL_TUNE] = "neutral";		// Notify Neutral tone
+	_tune_names[TONE_NOTIFY_NEGATIVE_TUNE] = "negative";		// Notify Negative tone
+	_tune_names[TONE_ARMING_WARNING_TUNE] = "arming";		// arming warning
+	_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
 }
 
 ToneAlarm::~ToneAlarm()
@@ -803,8 +670,12 @@ tune_error:
 	// stop (and potentially restart) the tune
 tune_end:
 	stop_note();
-	if (_repeat)
+	if (_repeat) {
 		start_tune(_tune);
+	} else {
+		_tune = nullptr;
+		_default_tune_number = 0;
+	}
 	return;
 }
 
@@ -867,14 +738,20 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg)
 	case TONE_SET_ALARM:
 		debug("TONE_SET_ALARM %u", arg);
 
-		if (arg <= _default_ntunes) {
-			if (arg == 0) {
+		if (arg < TONE_NUMBER_OF_TUNES) {
+			if (arg == TONE_STOP_TUNE) {
 				// stop the tune
 				_tune = nullptr;
 				_next = nullptr;
+				_repeat = false;
+				_default_tune_number = 0;
 			} else {
-				// play the selected tune
-				start_tune(_default_tunes[arg - 1]);
+				/* always interrupt alarms, unless they are repeating and already playing */
+				if (!(_repeat && _default_tune_number == arg)) {
+					/* play the selected tune */
+					_default_tune_number = arg;
+					start_tune(_default_tunes[arg]);
+				}
 			}
 		} else {
 			result = -EINVAL;
@@ -945,10 +822,10 @@ play_tune(unsigned tune)
 {
 	int	fd, ret;
 
-	fd = open("/dev/tone_alarm", 0);
+	fd = open(TONEALARM_DEVICE_PATH, 0);
 
 	if (fd < 0)
-		err(1, "/dev/tone_alarm");
+		err(1, TONEALARM_DEVICE_PATH);
 
 	ret = ioctl(fd, TONE_SET_ALARM, tune);
 	close(fd);
@@ -960,18 +837,21 @@ play_tune(unsigned tune)
 }
 
 int
-play_string(const char *str)
+play_string(const char *str, bool free_buffer)
 {
 	int	fd, ret;
 
-	fd = open("/dev/tone_alarm", O_WRONLY);
+	fd = open(TONEALARM_DEVICE_PATH, O_WRONLY);
 
 	if (fd < 0)
-		err(1, "/dev/tone_alarm");
+		err(1, TONEALARM_DEVICE_PATH);
 
 	ret = write(fd, str, strlen(str) + 1);
 	close(fd);
 
+	if (free_buffer)
+		free((void *)str);
+
 	if (ret < 0)
 		err(1, "play tune");
 	exit(0);
@@ -997,22 +877,50 @@ tone_alarm_main(int argc, char *argv[])
 		}
 	}
 
-	if ((argc > 1) && !strcmp(argv[1], "start"))
-		play_tune(1);
-
-	if ((argc > 1) && !strcmp(argv[1], "stop"))
-		play_tune(0);
-
-	if ((tune = strtol(argv[1], nullptr, 10)) != 0)
-		play_tune(tune);
+	if (argc > 1) {
+		const char *argv1 = argv[1];
+
+		if (!strcmp(argv1, "start"))
+			play_tune(TONE_STARTUP_TUNE);
+
+		if (!strcmp(argv1, "stop"))
+			play_tune(TONE_STOP_TUNE);
+
+		if ((tune = strtol(argv1, nullptr, 10)) != 0)
+			play_tune(tune);
+
+		/* It might be a tune name */
+		for (tune = 1; tune < TONE_NUMBER_OF_TUNES; tune++)
+			if (!strcmp(g_dev->name(tune), argv1))
+				play_tune(tune);
+
+		/* If it is a file name then load and play it as a string */
+		if (*argv1 == '/') {
+			FILE *fd = fopen(argv1, "r");
+			int sz;
+			char *buffer;
+			if (fd == nullptr)
+				errx(1, "couldn't open '%s'", argv1);
+			fseek(fd, 0, SEEK_END);
+			sz = ftell(fd);
+			fseek(fd, 0, SEEK_SET);
+			buffer = (char *)malloc(sz + 1);
+			if (buffer == nullptr)
+				errx(1, "not enough memory memory");
+			fread(buffer, sz, 1, fd);
+			/* terminate the string */
+			buffer[sz] = 0;
+			play_string(buffer, true);
+		}
 
-	/* if it looks like a PLAY string... */
-	if (strlen(argv[1]) > 2) {
-		const char *str = argv[1];
-		if (str[0] == 'M') {
-			play_string(str);
+		/* if it looks like a PLAY string... */
+		if (strlen(argv1) > 2) {
+			if (*argv1 == 'M') {
+				play_string(argv1, false);
+			}
 		}
+
 	}
 
-	errx(1, "unrecognised command, try 'start', 'stop' or an alarm number");
+	errx(1, "unrecognized command, try 'start', 'stop', an alarm number or name, or a file name starting with a '/'");
 }
diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c
index 27888523b..b286e0007 100644
--- a/src/examples/fixedwing_control/main.c
+++ b/src/examples/fixedwing_control/main.c
@@ -66,7 +66,7 @@
 #include <uORB/topics/parameter_update.h>
 #include <systemlib/param/param.h>
 #include <systemlib/pid/pid.h>
-#include <systemlib/geo/geo.h>
+#include <geo/geo.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/err.h>
@@ -380,9 +380,10 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 
 				/* control */
 
-				/* if in auto mode, fly global position setpoint */
-				if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
-				    vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
+#warning fix this
+#if 0
+				if (vstatus.navigation_state == NAVIGATION_STATE_AUTO_ ||
+				    vstatus.navigation_state == NAVIGATION_STATE_STABILIZED) {
 
 				    	/* simple heading control */
 				    	control_heading(&global_pos, &global_sp, &att, &att_sp);
@@ -400,6 +401,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 					/* set flaps to zero */
 					actuators.control[4] = 0.0f;
 
+				} else if (vstatus.navigation_state == NAVIGATION_STATE_MANUAL) {
 				/* if in manual mode, decide between attitude stabilization (SAS) and full manual pass-through */
 				} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
 					if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
@@ -465,6 +467,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 						}
 					}
 				}
+#endif
 
 				/* publish rates */
 				orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
@@ -512,7 +515,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 ex_fixedwing_control_main(int argc, char *argv[])
 {
diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c
new file mode 100644
index 000000000..3125ce246
--- /dev/null
+++ b/src/examples/flow_position_control/flow_position_control_main.c
@@ -0,0 +1,614 @@
+/****************************************************************************
+ *
+ *   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
new file mode 100644
index 000000000..eb1473647
--- /dev/null
+++ b/src/examples/flow_position_control/flow_position_control_params.c
@@ -0,0 +1,124 @@
+/****************************************************************************
+ *
+ *   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
new file mode 100644
index 000000000..d0c8fc722
--- /dev/null
+++ b/src/examples/flow_position_control/flow_position_control_params.h
@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   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_control/module.mk b/src/examples/flow_position_control/module.mk
new file mode 100644
index 000000000..b10dc490a
--- /dev/null
+++ b/src/examples/flow_position_control/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   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 position control
+#
+
+MODULE_COMMAND	= flow_position_control
+
+SRCS		= flow_position_control_main.c \
+		  flow_position_control_params.c
diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c
new file mode 100644
index 000000000..495c415f2
--- /dev/null
+++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c
@@ -0,0 +1,475 @@
+/****************************************************************************
+ *
+ *   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_estimator_main.c
+ *
+ * Optical flow position estimator
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <float.h>
+#include <nuttx/sched.h>
+#include <sys/prctl.h>
+#include <drivers/drv_hrt.h>
+#include <termios.h>
+#include <errno.h>
+#include <limits.h>
+#include <math.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/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/filtered_bottom_flow.h>
+#include <systemlib/perf_counter.h>
+#include <poll.h>
+
+#include "flow_position_estimator_params.h"
+
+__EXPORT int flow_position_estimator_main(int argc, char *argv[]);
+static bool thread_should_exit = false;		/**< Daemon exit flag */
+static bool thread_running = false;		/**< Daemon status flag */
+static int daemon_task;				/**< Handle of daemon task / thread */
+
+int flow_position_estimator_thread_main(int argc, char *argv[]);
+static void usage(const char *reason);
+
+static void usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+	fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
+	exit(1);
+}
+
+/**
+ * The daemon 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_estimator_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start"))
+	{
+		if (thread_running)
+		{
+			printf("flow position estimator already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		daemon_task = task_spawn_cmd("flow_position_estimator",
+					 SCHED_RR,
+					 SCHED_PRIORITY_MAX - 5,
+					 4096,
+					 flow_position_estimator_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 estimator is running\n");
+		else
+			printf("\tflow position estimator not started\n");
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+int flow_position_estimator_thread_main(int argc, char *argv[])
+{
+	/* welcome user */
+	thread_running = true;
+	printf("[flow position estimator] starting\n");
+
+	/* rotation matrix for transformation of optical flow speed vectors */
+	static const int8_t rotM_flow_sensor[3][3] =   {{  0, -1, 0 },
+													{ 1, 0, 0 },
+													{  0, 0, 1 }}; // 90deg rotated
+	const float time_scale = powf(10.0f,-6.0f);
+	static float speed[3] = {0.0f, 0.0f, 0.0f};
+	static float flow_speed[3] = {0.0f, 0.0f, 0.0f};
+	static float global_speed[3] = {0.0f, 0.0f, 0.0f};
+	static uint32_t counter = 0;
+	static uint64_t time_last_flow = 0; // in ms
+	static float dt = 0.0f; // seconds
+	static float sonar_last = 0.0f;
+	static bool sonar_valid = false;
+	static float sonar_lp = 0.0f;
+
+	/* subscribe to vehicle status, attitude, sensors and flow*/
+	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 vehicle_attitude_setpoint_s att_sp;
+	memset(&att_sp, 0, sizeof(att_sp));
+	struct optical_flow_s flow;
+	memset(&flow, 0, sizeof(flow));
+
+	/* subscribe to parameter changes */
+	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
+
+	/* subscribe to armed topic */
+	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+
+	/* subscribe to safety topic */
+	int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+
+	/* subscribe to attitude */
+	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+
+	/* subscribe to attitude setpoint */
+	int vehicle_attitude_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+
+	/* subscribe to optical flow*/
+	int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
+
+	/* init local position and filtered flow struct */
+	struct vehicle_local_position_s local_pos = {
+			.x = 0.0f,
+			.y = 0.0f,
+			.z = 0.0f,
+			.vx = 0.0f,
+			.vy = 0.0f,
+			.vz = 0.0f
+	};
+	struct filtered_bottom_flow_s filtered_flow = {
+			.sumx = 0.0f,
+			.sumy = 0.0f,
+			.vx = 0.0f,
+			.vy = 0.0f
+	};
+
+	/* advert pub messages */
+	orb_advert_t local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
+	orb_advert_t filtered_flow_pub = orb_advertise(ORB_ID(filtered_bottom_flow), &filtered_flow);
+
+	/* vehicle flying status parameters */
+	bool vehicle_liftoff = false;
+	bool sensors_ready = false;
+
+	/* parameters init*/
+	struct flow_position_estimator_params params;
+	struct flow_position_estimator_param_handles param_handles;
+	parameters_init(&param_handles);
+	parameters_update(&param_handles, &params);
+
+	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_position_estimator_runtime");
+	perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_position_estimator_interval");
+	perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_estimator_err");
+
+	while (!thread_should_exit)
+	{
+
+		if (sensors_ready)
+		{
+			/*This runs at the rate of the sensors */
+			struct pollfd fds[2] = {
+					{ .fd = optical_flow_sub, .events = POLLIN },
+					{ .fd = parameter_update_sub,   .events = POLLIN }
+			};
+
+			/* 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(mc_err_perf);
+
+			}
+			else if (ret == 0)
+			{
+				/* no return value, ignore */
+//				printf("[flow position estimator] no bottom flow.\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);
+					printf("[flow position estimator] parameters updated.\n");
+				}
+
+				/* only if flow data changed */
+				if (fds[0].revents & POLLIN)
+				{
+					perf_begin(mc_loop_perf);
+
+					orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
+					/* got flow, updating attitude and status as well */
+					orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
+					orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp);
+					orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
+					orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
+
+					/* vehicle state estimation */
+					float sonar_new = flow.ground_distance_m;
+
+					/* set liftoff boolean
+					 * -> at bottom sonar sometimes does not work correctly, and has to be calibrated (distance higher than 0.3m)
+					 * -> accept sonar measurements after reaching calibration distance (values between 0.3m and 1.0m for some time)
+					 * -> minimum sonar value 0.3m
+					 */
+
+					if (!vehicle_liftoff)
+					{
+						if (armed.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f)
+							vehicle_liftoff = true;
+					}
+					else
+					{
+						if (!armed.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f))
+							vehicle_liftoff = false;
+					}
+
+					/* calc dt between flow timestamps */
+					/* ignore first flow msg */
+					if(time_last_flow == 0)
+					{
+						time_last_flow = flow.timestamp;
+						continue;
+					}
+					dt = (float)(flow.timestamp - time_last_flow) * time_scale ;
+					time_last_flow = flow.timestamp;
+
+					/* only make position update if vehicle is lift off or DEBUG is activated*/
+					if (vehicle_liftoff || params.debug)
+					{
+						/* copy flow */
+						flow_speed[0] = flow.flow_comp_x_m;
+						flow_speed[1] = flow.flow_comp_y_m;
+						flow_speed[2] = 0.0f;
+
+						/* convert to bodyframe velocity */
+						for(uint8_t i = 0; i < 3; i++)
+						{
+							float sum = 0.0f;
+							for(uint8_t j = 0; j < 3; j++)
+								sum = sum + flow_speed[j] * rotM_flow_sensor[j][i];
+
+							speed[i] = sum;
+						}
+
+						/* update filtered flow */
+						filtered_flow.sumx = filtered_flow.sumx + speed[0] * dt;
+						filtered_flow.sumy = filtered_flow.sumy + speed[1] * dt;
+						filtered_flow.vx = speed[0];
+						filtered_flow.vy = speed[1];
+
+						// TODO add yaw rotation correction (with distance to vehicle zero)
+
+						/* convert to globalframe velocity
+						 * -> local position is currently not used for position control
+						 */
+						for(uint8_t i = 0; i < 3; i++)
+						{
+							float sum = 0.0f;
+							for(uint8_t j = 0; j < 3; j++)
+								sum = sum + speed[j] * att.R[i][j];
+
+							global_speed[i] = sum;
+						}
+
+						local_pos.x = local_pos.x + global_speed[0] * dt;
+						local_pos.y = local_pos.y + global_speed[1] * dt;
+						local_pos.vx = global_speed[0];
+						local_pos.vy = global_speed[1];
+						local_pos.xy_valid = true;
+						local_pos.v_xy_valid = true;
+					}
+					else
+					{
+						/* set speed to zero and let position as it is */
+						filtered_flow.vx = 0;
+						filtered_flow.vy = 0;
+						local_pos.vx = 0;
+						local_pos.vy = 0;
+						local_pos.xy_valid = false;
+						local_pos.v_xy_valid = false;
+					}
+
+					/* filtering ground distance */
+					if (!vehicle_liftoff || !armed.armed)
+					{
+						/* not possible to fly */
+						sonar_valid = false;
+						local_pos.z = 0.0f;
+						local_pos.z_valid = false;
+					}
+					else
+					{
+						sonar_valid = true;
+					}
+
+					if (sonar_valid || params.debug)
+					{
+						/* simple lowpass sonar filtering */
+						/* if new value or with sonar update frequency */
+						if (sonar_new != sonar_last || counter % 10 == 0)
+						{
+							sonar_lp = 0.05f * sonar_new + 0.95f * sonar_lp;
+							sonar_last = sonar_new;
+						}
+
+						float height_diff = sonar_new - sonar_lp;
+
+						/* if over 1/2m spike follow lowpass */
+						if (height_diff < -params.sonar_lower_lp_threshold || height_diff > params.sonar_upper_lp_threshold)
+						{
+							local_pos.z = -sonar_lp;
+						}
+						else
+						{
+							local_pos.z = -sonar_new;
+						}
+
+						local_pos.z_valid = true;
+					}
+
+					filtered_flow.timestamp = hrt_absolute_time();
+					local_pos.timestamp = hrt_absolute_time();
+
+					/* publish local position */
+					if(isfinite(local_pos.x) && isfinite(local_pos.y) && isfinite(local_pos.z)
+							&& isfinite(local_pos.vx) && isfinite(local_pos.vy))
+					{
+						orb_publish(ORB_ID(vehicle_local_position), local_pos_pub, &local_pos);
+					}
+
+					/* publish filtered flow */
+					if(isfinite(filtered_flow.sumx) && isfinite(filtered_flow.sumy) && isfinite(filtered_flow.vx) && isfinite(filtered_flow.vy))
+					{
+						orb_publish(ORB_ID(filtered_bottom_flow), filtered_flow_pub, &filtered_flow);
+					}
+
+					/* measure in what intervals the position estimator runs */
+					perf_count(mc_interval_perf);
+					perf_end(mc_loop_perf);
+
+				}
+			}
+
+		}
+		else
+		{
+			/* sensors not ready waiting for first attitude msg */
+
+			/* polling */
+			struct pollfd fds[1] = {
+				{ .fd = vehicle_attitude_sub, .events = POLLIN },
+			};
+
+			/* wait for a attitude message, 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 */
+				printf("[flow position estimator] no attitude received.\n");
+			}
+			else
+			{
+				if (fds[0].revents & POLLIN){
+					sensors_ready = true;
+					printf("[flow position estimator] initialized.\n");
+				}
+			}
+		}
+
+		counter++;
+	}
+
+	printf("[flow position estimator] exiting.\n");
+	thread_running = false;
+
+	close(vehicle_attitude_setpoint_sub);
+	close(vehicle_attitude_sub);
+	close(armed_sub);
+	close(control_mode_sub);
+	close(parameter_update_sub);
+	close(optical_flow_sub);
+
+	perf_print_counter(mc_loop_perf);
+	perf_free(mc_loop_perf);
+
+	fflush(stdout);
+	return 0;
+}
+
+
diff --git a/src/examples/flow_position_estimator/flow_position_estimator_params.c b/src/examples/flow_position_estimator/flow_position_estimator_params.c
new file mode 100644
index 000000000..ec3c3352d
--- /dev/null
+++ b/src/examples/flow_position_estimator/flow_position_estimator_params.c
@@ -0,0 +1,72 @@
+/****************************************************************************
+ *
+ *   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_estimator_params.c
+ * 
+ * Parameters for position estimator
+ */
+
+#include "flow_position_estimator_params.h"
+
+/* Extended Kalman Filter covariances */
+
+/* controller parameters */
+PARAM_DEFINE_FLOAT(FPE_LO_THRUST, 0.4f);
+PARAM_DEFINE_FLOAT(FPE_SONAR_LP_U, 0.5f);
+PARAM_DEFINE_FLOAT(FPE_SONAR_LP_L, 0.2f);
+PARAM_DEFINE_INT32(FPE_DEBUG, 0);
+
+
+int parameters_init(struct flow_position_estimator_param_handles *h)
+{
+	/* PID parameters */
+	h->minimum_liftoff_thrust	=	param_find("FPE_LO_THRUST");
+	h->sonar_upper_lp_threshold	=	param_find("FPE_SONAR_LP_U");
+	h->sonar_lower_lp_threshold	=	param_find("FPE_SONAR_LP_L");
+	h->debug					=	param_find("FPE_DEBUG");
+
+	return OK;
+}
+
+int parameters_update(const struct flow_position_estimator_param_handles *h, struct flow_position_estimator_params *p)
+{
+	param_get(h->minimum_liftoff_thrust, &(p->minimum_liftoff_thrust));
+	param_get(h->sonar_upper_lp_threshold, &(p->sonar_upper_lp_threshold));
+	param_get(h->sonar_lower_lp_threshold, &(p->sonar_lower_lp_threshold));
+	param_get(h->debug, &(p->debug));
+
+	return OK;
+}
diff --git a/src/examples/flow_position_estimator/flow_position_estimator_params.h b/src/examples/flow_position_estimator/flow_position_estimator_params.h
new file mode 100644
index 000000000..f9a9bb303
--- /dev/null
+++ b/src/examples/flow_position_estimator/flow_position_estimator_params.h
@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   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_estimator_params.h
+ * 
+ * Parameters for position estimator
+ */
+
+#include <systemlib/param/param.h>
+
+struct flow_position_estimator_params {
+	float minimum_liftoff_thrust;
+	float sonar_upper_lp_threshold;
+	float sonar_lower_lp_threshold;
+	int debug;
+};
+
+struct flow_position_estimator_param_handles {
+	param_t minimum_liftoff_thrust;
+	param_t sonar_upper_lp_threshold;
+	param_t sonar_lower_lp_threshold;
+	param_t debug;
+};
+
+/**
+ * Initialize all parameter handles and values
+ *
+ */
+int parameters_init(struct flow_position_estimator_param_handles *h);
+
+/**
+ * Update all parameters
+ *
+ */
+int parameters_update(const struct flow_position_estimator_param_handles *h, struct flow_position_estimator_params *p);
diff --git a/src/examples/flow_position_estimator/module.mk b/src/examples/flow_position_estimator/module.mk
new file mode 100644
index 000000000..88c9ceb93
--- /dev/null
+++ b/src/examples/flow_position_estimator/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   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 position estimator
+#
+
+MODULE_COMMAND		= flow_position_estimator
+
+SRCS		= flow_position_estimator_main.c \
+		  flow_position_estimator_params.c
diff --git a/src/examples/flow_speed_control/flow_speed_control_main.c b/src/examples/flow_speed_control/flow_speed_control_main.c
new file mode 100644
index 000000000..feed0d23c
--- /dev/null
+++ b/src/examples/flow_speed_control/flow_speed_control_main.c
@@ -0,0 +1,387 @@
+/****************************************************************************
+ *
+ *   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_speed_control.c
+ *
+ * Optical flow speed 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/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_bodyframe_speed_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_speed_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_speed_control_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of position controller.
+ */
+static int flow_speed_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_speed_control_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start"))
+	{
+		if (thread_running)
+		{
+			printf("flow speed control already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		deamon_task = task_spawn_cmd("flow_speed_control",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 6,
+					 4096,
+					 flow_speed_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 speed control app is running\n");
+		else
+			printf("\tflow speed control app not started\n");
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+static int
+flow_speed_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,"[fsc] started");
+
+	uint32_t counter = 0;
+
+	/* 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 filtered_bottom_flow_s filtered_flow;
+	memset(&filtered_flow, 0, sizeof(filtered_flow));
+	struct vehicle_bodyframe_speed_setpoint_s speed_sp;
+	memset(&speed_sp, 0, sizeof(speed_sp));
+	struct vehicle_attitude_setpoint_s att_sp;
+	memset(&att_sp, 0, sizeof(att_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 filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow));
+	int vehicle_bodyframe_speed_setpoint_sub = orb_subscribe(ORB_ID(vehicle_bodyframe_speed_setpoint));
+
+	orb_advert_t att_sp_pub;
+	bool attitude_setpoint_adverted = false;
+
+	/* parameters init*/
+	struct flow_speed_control_params params;
+	struct flow_speed_control_param_handles param_handles;
+	parameters_init(&param_handles);
+	parameters_update(&param_handles, &params);
+
+	/* register the perf counter */
+	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_speed_control_runtime");
+	perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_speed_control_interval");
+	perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_speed_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 = vehicle_bodyframe_speed_setpoint_sub, .events = POLLIN }, // speed setpoint from pos controller
+				{ .fd = parameter_update_sub,   .events = POLLIN }
+			};
+
+			/* wait for a position update, check for exit condition every 5000 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 speed control] no bodyframe speed setpoints 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,"[fsp] 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 armed topic */
+					orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
+					/* get a local copy of the control mode */
+					orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
+					/* 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 bodyframe speed setpoint */
+					orb_copy(ORB_ID(vehicle_bodyframe_speed_setpoint), vehicle_bodyframe_speed_setpoint_sub, &speed_sp);
+					/* 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)
+					{
+						/* calc new roll/pitch */
+						float pitch_body = -(speed_sp.vx - filtered_flow.vx) * params.speed_p;
+						float roll_body  =  (speed_sp.vy - filtered_flow.vy) * params.speed_p;
+
+						if(status_changed == false)
+							mavlink_log_info(mavlink_fd,"[fsc] flow SPEED control engaged");
+
+						status_changed = true;
+
+						/* limit roll and pitch corrections */
+						if((pitch_body <= params.limit_pitch) && (pitch_body >= -params.limit_pitch))
+						{
+							att_sp.pitch_body = pitch_body;
+						}
+						else
+						{
+							if(pitch_body > params.limit_pitch)
+								att_sp.pitch_body = params.limit_pitch;
+							if(pitch_body < -params.limit_pitch)
+								att_sp.pitch_body = -params.limit_pitch;
+						}
+
+						if((roll_body <= params.limit_roll) && (roll_body >= -params.limit_roll))
+						{
+							att_sp.roll_body = roll_body;
+						}
+						else
+						{
+							if(roll_body > params.limit_roll)
+								att_sp.roll_body = params.limit_roll;
+							if(roll_body < -params.limit_roll)
+								att_sp.roll_body = -params.limit_roll;
+						}
+
+						/* set yaw setpoint forward*/
+						att_sp.yaw_body = speed_sp.yaw_sp;
+
+						/* add trim from parameters */
+						att_sp.roll_body = att_sp.roll_body + params.trim_roll;
+						att_sp.pitch_body = att_sp.pitch_body + params.trim_pitch;
+
+						att_sp.thrust = speed_sp.thrust_sp;
+						att_sp.timestamp = hrt_absolute_time();
+
+						/* publish new attitude setpoint */
+						if(isfinite(att_sp.pitch_body) && isfinite(att_sp.roll_body) && isfinite(att_sp.yaw_body) && isfinite(att_sp.thrust))
+						{
+							if (attitude_setpoint_adverted)
+							{
+								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);
+								attitude_setpoint_adverted = true;
+							}
+						}
+						else
+						{
+							warnx("NaN in flow speed controller!");
+						}
+					}
+					else
+					{
+						if(status_changed == true)
+							mavlink_log_info(mavlink_fd,"[fsc] flow SPEED controller disengaged.");
+
+						status_changed = false;
+
+						/* reset attitude setpoint */
+						att_sp.roll_body = 0.0f;
+						att_sp.pitch_body = 0.0f;
+						att_sp.thrust = 0.0f;
+						att_sp.yaw_body = 0.0f;
+					}
+
+					/* 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,"[fsc] no attitude received.");
+			}
+			else
+			{
+				if (fds[0].revents & POLLIN)
+				{
+					sensors_ready = true;
+					mavlink_log_info(mavlink_fd,"[fsp] initialized.");
+				}
+			}
+		}
+	}
+
+	mavlink_log_info(mavlink_fd,"[fsc] ending now...");
+
+	thread_running = false;
+
+	close(parameter_update_sub);
+	close(vehicle_attitude_sub);
+	close(vehicle_bodyframe_speed_setpoint_sub);
+	close(filtered_bottom_flow_sub);
+	close(armed_sub);
+	close(control_mode_sub);
+	close(att_sp_pub);
+
+	perf_print_counter(mc_loop_perf);
+	perf_free(mc_loop_perf);
+
+	fflush(stdout);
+	return 0;
+}
diff --git a/src/examples/flow_speed_control/flow_speed_control_params.c b/src/examples/flow_speed_control/flow_speed_control_params.c
new file mode 100644
index 000000000..8dfe54173
--- /dev/null
+++ b/src/examples/flow_speed_control/flow_speed_control_params.c
@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   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_speed_control_params.c
+ * 
+ */
+
+#include "flow_speed_control_params.h"
+
+/* controller parameters */
+PARAM_DEFINE_FLOAT(FSC_S_P, 0.1f);
+PARAM_DEFINE_FLOAT(FSC_L_PITCH, 0.4f);
+PARAM_DEFINE_FLOAT(FSC_L_ROLL, 0.4f);
+
+int parameters_init(struct flow_speed_control_param_handles *h)
+{
+	/* PID parameters */
+	h->speed_p	 			=	param_find("FSC_S_P");
+	h->limit_pitch 			=	param_find("FSC_L_PITCH");
+	h->limit_roll 			=	param_find("FSC_L_ROLL");
+	h->trim_roll 			=	param_find("TRIM_ROLL");
+	h->trim_pitch 			=	param_find("TRIM_PITCH");
+
+
+	return OK;
+}
+
+int parameters_update(const struct flow_speed_control_param_handles *h, struct flow_speed_control_params *p)
+{
+	param_get(h->speed_p, &(p->speed_p));
+	param_get(h->limit_pitch, &(p->limit_pitch));
+	param_get(h->limit_roll, &(p->limit_roll));
+	param_get(h->trim_roll, &(p->trim_roll));
+	param_get(h->trim_pitch, &(p->trim_pitch));
+
+	return OK;
+}
diff --git a/src/modules/multirotor_pos_control/position_control.h b/src/examples/flow_speed_control/flow_speed_control_params.h
index 2144ebc34..eec27a2bf 100644
--- a/src/modules/multirotor_pos_control/position_control.h
+++ b/src/examples/flow_speed_control/flow_speed_control_params.h
@@ -1,10 +1,8 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Laurens Mackay <mackayl@student.ethz.ch>
- *           @author Tobias Naegeli <naegelit@student.ethz.ch>
- *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ *   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
@@ -35,16 +33,38 @@
  *
  ****************************************************************************/
 
-/**
- * @file multirotor_position_control.h
- * Definition of the position control for a multirotor VTOL
+/*
+ * @file flow_speed_control_params.h
+ * 
+ * Parameters for speed controller
  */
 
-// #ifndef POSITION_CONTROL_H_
-// #define POSITION_CONTROL_H_
+#include <systemlib/param/param.h>
+
+struct flow_speed_control_params {
+	float speed_p;
+	float limit_pitch;
+	float limit_roll;
+	float trim_roll;
+	float trim_pitch;
+};
 
-// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
-//  const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
-//  const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp);
+struct flow_speed_control_param_handles {
+	param_t speed_p;
+	param_t limit_pitch;
+	param_t limit_roll;
+	param_t trim_roll;
+	param_t trim_pitch;
+};
 
-// #endif /* POSITION_CONTROL_H_ */
+/**
+ * Initialize all parameter handles and values
+ *
+ */
+int parameters_init(struct flow_speed_control_param_handles *h);
+
+/**
+ * Update all parameters
+ *
+ */
+int parameters_update(const struct flow_speed_control_param_handles *h, struct flow_speed_control_params *p);
diff --git a/src/examples/flow_speed_control/module.mk b/src/examples/flow_speed_control/module.mk
new file mode 100644
index 000000000..5a4182146
--- /dev/null
+++ b/src/examples/flow_speed_control/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   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 flow speed control
+#
+
+MODULE_COMMAND	= flow_speed_control
+
+SRCS		= flow_speed_control_main.c \
+		  flow_speed_control_params.c
diff --git a/src/examples/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c
new file mode 100644
index 000000000..06337be32
--- /dev/null
+++ b/src/examples/hwtest/hwtest.c
@@ -0,0 +1,74 @@
+/****************************************************************************
+ *
+ *   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 hwtest.c
+ *
+ * Simple functional hardware test.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/actuator_controls.h>
+
+__EXPORT int ex_hwtest_main(int argc, char *argv[]);
+
+int ex_hwtest_main(int argc, char *argv[])
+{
+    struct actuator_controls_s actuators;
+    memset(&actuators, 0, sizeof(actuators));
+    orb_advert_t actuator_pub_fd = orb_advertise(ORB_ID(actuator_controls_0), &actuators);
+
+    int i;
+    float rcvalue = -1.0f;
+    hrt_abstime stime;
+
+    while (true) {
+        stime = hrt_absolute_time();
+        while (hrt_absolute_time() - stime < 1000000) {
+            for (i=0; i<8; i++)
+                actuators.control[i] = rcvalue;
+            actuators.timestamp = hrt_absolute_time(); 
+            orb_publish(ORB_ID(actuator_controls_0), actuator_pub_fd, &actuators);
+        }
+        warnx("servos set to %.1f", rcvalue);
+        rcvalue *= -1.0f;
+    }
+
+    return OK;
+}
diff --git a/src/systemcmds/eeprom/module.mk b/src/examples/hwtest/module.mk
index 07f3945be..6e70863ed 100644
--- a/src/systemcmds/eeprom/module.mk
+++ b/src/examples/hwtest/module.mk
@@ -32,8 +32,9 @@
 ############################################################################
 
 #
-# EEPROM file system driver
+# Hardware test example application
 #
 
-MODULE_COMMAND	 = eeprom
-SRCS		 = 24xxxx_mtd.c eeprom.c
+MODULE_COMMAND	= ex_hwtest
+
+SRCS		= hwtest.c
diff --git a/src/examples/px4_daemon_app/px4_daemon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c
index fbb38e4b9..53f1b4a9a 100644
--- a/src/examples/px4_daemon_app/px4_daemon_app.c
+++ b/src/examples/px4_daemon_app/px4_daemon_app.c
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Example User <mail@example.com>
+ *   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
@@ -33,27 +32,33 @@
  ****************************************************************************/
 
 /**
- * @file px4_deamon_app.c
- * Deamon application example for PX4 autopilot
+ * @file px4_daemon_app.c
+ * daemon application example for PX4 autopilot
+ * 
+ * @author Example User <mail@example.com>
  */
 
 #include <nuttx/config.h>
+#include <nuttx/sched.h>
 #include <unistd.h>
 #include <stdio.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 */
+#include <systemlib/systemlib.h>
+#include <systemlib/err.h>
+
+static bool thread_should_exit = false;		/**< daemon exit flag */
+static bool thread_running = false;		/**< daemon status flag */
+static int daemon_task;				/**< Handle of daemon task / thread */
 
 /**
- * Deamon management function.
+ * daemon management function.
  */
-__EXPORT int px4_deamon_app_main(int argc, char *argv[]);
+__EXPORT int px4_daemon_app_main(int argc, char *argv[]);
 
 /**
- * Mainloop of deamon.
+ * Mainloop of daemon.
  */
-int px4_deamon_thread_main(int argc, char *argv[]);
+int px4_daemon_thread_main(int argc, char *argv[]);
 
 /**
  * Print the correct usage.
@@ -64,20 +69,19 @@ 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);
+		warnx("%s\n", reason);
+	errx(1, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
 }
 
 /**
- * The deamon app only briefly exists to start
+ * The daemon 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 px4_deamon_app_main(int argc, char *argv[])
+int px4_daemon_app_main(int argc, char *argv[])
 {
 	if (argc < 1)
 		usage("missing command");
@@ -85,17 +89,17 @@ int px4_deamon_app_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			printf("deamon already running\n");
+			warnx("daemon already running\n");
 			/* this is not an error */
 			exit(0);
 		}
 
 		thread_should_exit = false;
-		deamon_task = task_spawn_cmd("deamon",
+		daemon_task = task_spawn_cmd("daemon",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_DEFAULT,
 					 4096,
-					 px4_deamon_thread_main,
+					 px4_daemon_thread_main,
 					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
 	}
@@ -107,9 +111,9 @@ int px4_deamon_app_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			printf("\tdeamon app is running\n");
+			warnx("\trunning\n");
 		} else {
-			printf("\tdeamon app not started\n");
+			warnx("\tnot started\n");
 		}
 		exit(0);
 	}
@@ -118,18 +122,18 @@ int px4_deamon_app_main(int argc, char *argv[])
 	exit(1);
 }
 
-int px4_deamon_thread_main(int argc, char *argv[]) {
+int px4_daemon_thread_main(int argc, char *argv[]) {
 
-	printf("[deamon] starting\n");
+	warnx("[daemon] starting\n");
 
 	thread_running = true;
 
 	while (!thread_should_exit) {
-		printf("Hello Deamon!\n");
+		warnx("Hello daemon!\n");
 		sleep(10);
 	}
 
-	printf("[deamon] exiting.\n");
+	warnx("[daemon] exiting.\n");
 
 	thread_running = false;
 
diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c
index 7f655964d..44e6dc7f3 100644
--- a/src/examples/px4_simple_app/px4_simple_app.c
+++ b/src/examples/px4_simple_app/px4_simple_app.c
@@ -71,7 +71,7 @@ int px4_simple_app_main(int argc, char *argv[])
 
 	int error_counter = 0;
 
-	while (true) {
+	for (int i = 0; i < 5; i++) {
 		/* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
 		int poll_ret = poll(fds, 1, 1000);
 	 
diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h
index a28ff3a68..5054937e0 100644
--- a/src/include/mavlink/mavlink_log.h
+++ b/src/include/mavlink/mavlink_log.h
@@ -107,7 +107,6 @@ struct mavlink_logmessage {
 
 struct mavlink_logbuffer {
 	unsigned int start;
-	// unsigned int end;
 	unsigned int size;
 	int count;
 	struct mavlink_logmessage *elems;
@@ -115,6 +114,8 @@ struct mavlink_logbuffer {
 
 void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size);
 
+void mavlink_logbuffer_destroy(struct mavlink_logbuffer *lb);
+
 int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb);
 
 int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb);
diff --git a/src/lib/conversion/module.mk b/src/lib/conversion/module.mk
new file mode 100644
index 000000000..f5f59a2dc
--- /dev/null
+++ b/src/lib/conversion/module.mk
@@ -0,0 +1,38 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Conversion library
+#
+
+SRCS		 = rotation.cpp
diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp
new file mode 100644
index 000000000..b078562c2
--- /dev/null
+++ b/src/lib/conversion/rotation.cpp
@@ -0,0 +1,62 @@
+/****************************************************************************
+ *
+ *   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 rotation.cpp
+ *
+ * Vector rotation library
+ */
+
+#include "math.h"
+#include "rotation.h"
+
+__EXPORT void
+get_rot_matrix(enum Rotation rot, math::Matrix *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);
+
+	math::Dcm R(euler);
+
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++) {
+			(*rot_matrix)(i, j) = R(i, j);
+		}
+	}
+}
diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h
new file mode 100644
index 000000000..85c63c0fc
--- /dev/null
+++ b/src/lib/conversion/rotation.h
@@ -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 rotation.h
+ *
+ * Vector rotation library
+ */
+
+#ifndef ROTATION_H_
+#define ROTATION_H_
+
+#include <unistd.h>
+#include <mathlib/mathlib.h>
+
+/**
+ * Enum for board and external compass rotations.
+ * This enum maps from board attitude to airframe attitude.
+ */
+enum Rotation {
+	ROTATION_NONE                = 0,
+	ROTATION_YAW_45              = 1,
+	ROTATION_YAW_90              = 2,
+	ROTATION_YAW_135             = 3,
+	ROTATION_YAW_180             = 4,
+	ROTATION_YAW_225             = 5,
+	ROTATION_YAW_270             = 6,
+	ROTATION_YAW_315             = 7,
+	ROTATION_ROLL_180            = 8,
+	ROTATION_ROLL_180_YAW_45     = 9,
+	ROTATION_ROLL_180_YAW_90     = 10,
+	ROTATION_ROLL_180_YAW_135    = 11,
+	ROTATION_PITCH_180           = 12,
+	ROTATION_ROLL_180_YAW_225    = 13,
+	ROTATION_ROLL_180_YAW_270    = 14,
+	ROTATION_ROLL_180_YAW_315    = 15,
+	ROTATION_ROLL_90             = 16,
+	ROTATION_ROLL_90_YAW_45      = 17,
+	ROTATION_ROLL_90_YAW_90      = 18,
+	ROTATION_ROLL_90_YAW_135     = 19,
+	ROTATION_ROLL_270            = 20,
+	ROTATION_ROLL_270_YAW_45     = 21,
+	ROTATION_ROLL_270_YAW_90     = 22,
+	ROTATION_ROLL_270_YAW_135    = 23,
+	ROTATION_PITCH_90            = 24,
+	ROTATION_PITCH_270           = 25,
+	ROTATION_MAX
+};
+
+typedef struct {
+	uint16_t roll;
+	uint16_t pitch;
+	uint16_t yaw;
+} rot_lookup_t;
+
+const rot_lookup_t rot_lookup[] = {
+	{  0,   0,   0 },
+	{  0,   0,  45 },
+	{  0,   0,  90 },
+	{  0,   0, 135 },
+	{  0,   0, 180 },
+	{  0,   0, 225 },
+	{  0,   0, 270 },
+	{  0,   0, 315 },
+	{180,   0,   0 },
+	{180,   0,  45 },
+	{180,   0,  90 },
+	{180,   0, 135 },
+	{  0, 180,   0 },
+	{180,   0, 225 },
+	{180,   0, 270 },
+	{180,   0, 315 },
+	{ 90,   0,   0 },
+	{ 90,   0,  45 },
+	{ 90,   0,  90 },
+	{ 90,   0, 135 },
+	{270,   0,   0 },
+	{270,   0,  45 },
+	{270,   0,  90 },
+	{270,   0, 135 },
+	{  0,  90,   0 },
+	{  0, 270,   0 }
+};
+
+/**
+ * Get the rotation matrix
+ */
+__EXPORT void
+get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix);
+
+#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
new file mode 100644
index 000000000..2eb58abd6
--- /dev/null
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -0,0 +1,149 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_pitch_controller.cpp
+ * Implementation of a simple orthogonal pitch PID controller.
+ *
+ * Authors and acknowledgements in header.
+ */
+
+#include "ecl_pitch_controller.h"
+#include <math.h>
+#include <stdint.h>
+#include <float.h>
+#include <geo/geo.h>
+#include <ecl/ecl.h>
+#include <mathlib/mathlib.h>
+
+ECL_PitchController::ECL_PitchController() :
+	_last_run(0),
+	_last_output(0.0f),
+	_integrator(0.0f),
+	_rate_error(0.0f),
+	_rate_setpoint(0.0f),
+	_max_deflection_rad(math::radians(45.0f))
+{
+}
+
+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)
+{
+	/* 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;
+
+	/* 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;
+	}
+
+	/* flying inverted (wings upside down) ? */
+	bool inverted = false;
+
+	/* roll is used as feedforward term and inverted flight needs to be considered */
+	if (fabsf(roll) < math::radians(90.0f)) {
+		/* not inverted, but numerically still potentially close to infinity */
+		roll = math::constrain(roll, math::radians(-80.0f), math::radians(80.0f));
+	} else {
+		/* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */
+
+		/* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */
+		if (roll > 0.0f) {
+			/* right hemisphere */
+			roll = math::constrain(roll, math::radians(100.0f), math::radians(180.0f));
+		} else {
+			/* left hemisphere */
+			roll = math::constrain(roll, math::radians(-100.0f), math::radians(-180.0f));
+		}
+	}
+
+	/* calculate the offset in the rate resulting from rolling  */
+	float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) *
+				tanf(roll) * sinf(roll)) * _roll_ff;
+	if (inverted)
+		turn_offset = -turn_offset;
+
+	float pitch_error = pitch_setpoint - pitch;
+	/* 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;
+
+	float ilimit_scaled = _integrator_max * scaler;
+
+	if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
+
+		float id = _rate_error * k_i_rate * dt * scaler;
+
+		/*
+		 * anti-windup: do not allow integrator to increase into the
+		 * wrong direction if actuator is at limit
+		 */
+		if (_last_output < -_max_deflection_rad) {
+			/* only allow motion to center: increase value */
+			id = math::max(id, 0.0f);
+		} else if (_last_output > _max_deflection_rad) {
+			/* only allow motion to center: decrease value */
+			id = math::min(id, 0.0f);
+		}
+
+		_integrator += id;
+	}
+
+	/* 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);
+}
+
+void ECL_PitchController::reset_integrator()
+{
+	_integrator = 0.0f;
+}
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
new file mode 100644
index 000000000..1e6cec6a1
--- /dev/null
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
@@ -0,0 +1,115 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_pitch_controller.h
+ * Definition of a simple orthogonal pitch PID controller.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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_PITCH_CONTROLLER_H
+#define ECL_PITCH_CONTROLLER_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+class __EXPORT ECL_PitchController
+{
+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));
+
+	void reset_integrator();
+
+	void set_time_constant(float time_constant) {
+		_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_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;
+	}
+
+	float get_rate_error() {
+		return _rate_error;
+	}
+
+	float get_desired_rate() {
+		return _rate_setpoint;
+	}
+
+private:
+
+	uint64_t _last_run;
+	float _tc;
+	float _k_p;
+	float _k_i;
+	float _k_d;
+	float _integrator_max;
+	float _max_rate_pos;
+	float _max_rate_neg;
+	float _roll_ff;
+	float _last_output;
+	float _integrator;
+	float _rate_error;
+	float _rate_setpoint;
+	float _max_deflection_rad;
+};
+
+#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
new file mode 100644
index 000000000..0b1ffa7a4
--- /dev/null
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -0,0 +1,129 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_roll_controller.cpp
+ * Implementation of a simple orthogonal roll PID controller.
+ *
+ * Authors and acknowledgements in header.
+ */
+
+#include "../ecl.h"
+#include "ecl_roll_controller.h"
+#include <stdint.h>
+#include <float.h>
+#include <geo/geo.h>
+#include <ecl/ecl.h>
+#include <mathlib/mathlib.h>
+
+ECL_RollController::ECL_RollController() :
+	_last_run(0),
+	_tc(0.1f),
+	_last_output(0.0f),
+	_integrator(0.0f),
+	_rate_error(0.0f),
+	_rate_setpoint(0.0f),
+	_max_deflection_rad(math::radians(45.0f))
+{
+
+}
+
+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)
+{
+	/* 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_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
+	float k_i_rate = _k_i * _tc;
+
+	/* 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 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;
+
+
+	float ilimit_scaled = _integrator_max * scaler;
+
+	if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
+
+		float id = _rate_error * k_i_rate * dt * scaler;
+
+		/*
+		 * anti-windup: do not allow integrator to increase into the
+		 * wrong direction if actuator is at limit
+		 */
+		if (_last_output < -_max_deflection_rad) {
+			/* only allow motion to center: increase value */
+			id = math::max(id, 0.0f);
+		} else if (_last_output > _max_deflection_rad) {
+			/* only allow motion to center: decrease value */
+			id = math::min(id, 0.0f);
+		}
+
+		_integrator += id;
+	}
+
+	/* 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;
+
+	return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
+}
+
+void ECL_RollController::reset_integrator()
+{
+	_integrator = 0.0f;
+}
+
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
new file mode 100644
index 000000000..0d4ea9333
--- /dev/null
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_roll_controller.h
+ * Definition of a simple orthogonal roll PID controller.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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_ROLL_CONTROLLER_H
+#define ECL_ROLL_CONTROLLER_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+class __EXPORT ECL_RollController
+{
+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));
+
+	void reset_integrator();
+
+	void set_time_constant(float time_constant) {
+		if (time_constant > 0.1f && time_constant < 3.0f) {
+			_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_integrator_max(float max) {
+		_integrator_max = max;
+	}
+	void set_max_rate(float max_rate) {
+		_max_rate = max_rate;
+	}
+
+	float get_rate_error() {
+		return _rate_error;
+	}
+
+	float get_desired_rate() {
+		return _rate_setpoint;
+	}
+
+private:
+	uint64_t _last_run;
+	float _tc;
+	float _k_p;
+	float _k_i;
+	float _k_d;
+	float _integrator_max;
+	float _max_rate;
+	float _last_output;
+	float _integrator;
+	float _rate_error;
+	float _rate_setpoint;
+	float _max_deflection_rad;
+};
+
+#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
new file mode 100644
index 000000000..b786acf24
--- /dev/null
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_yaw_controller.cpp
+ * Implementation of a simple orthogonal coordinated turn yaw PID controller.
+ *
+ * Authors and acknowledgements in header.
+ */
+
+#include "ecl_yaw_controller.h"
+#include <stdint.h>
+#include <float.h>
+#include <geo/geo.h>
+#include <ecl/ecl.h>
+#include <mathlib/mathlib.h>
+
+ECL_YawController::ECL_YawController() :
+	_last_run(0),
+	_last_error(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)
+{
+
+}
+
+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)
+{
+	/* get the usual dt estimate */
+	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
+	_last_run = ecl_absolute_time();
+
+	float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
+
+	return 0.0f;
+}
+
+void ECL_YawController::reset_integrator()
+{
+	_integrator = 0.0f;
+}
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
new file mode 100644
index 000000000..66b227918
--- /dev/null
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_yaw_controller.h
+ * Definition of a simple orthogonal coordinated turn yaw PID controller.
+ *
+ */
+#ifndef ECL_YAW_CONTROLLER_H
+#define ECL_YAW_CONTROLLER_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+class __EXPORT ECL_YawController
+{
+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));
+
+	void reset_integrator();
+
+	void set_k_side(float k_a) {
+		_k_side = k_a;
+	}
+	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_integrator_max(float max) {
+		_integrator_max = max;
+	}
+
+private:
+	uint64_t _last_run;
+
+	float _k_side;
+	float _k_i;
+	float _k_d;
+	float _k_roll_ff;
+	float _integrator_max;
+
+	float _last_error;
+	float _last_output;
+	float _last_rate_hp_out;
+	float _last_rate_hp_in;
+	float _k_d_last;
+	float _integrator;
+
+};
+
+#endif // ECL_YAW_CONTROLLER_H
diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h
new file mode 100644
index 000000000..e0f207696
--- /dev/null
+++ b/src/lib/ecl/ecl.h
@@ -0,0 +1,44 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 APL 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 ecl.h
+ * Adapter / shim layer for system calls needed by ECL
+ *
+ */
+
+#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
diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
new file mode 100644
index 000000000..27d76f959
--- /dev/null
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
@@ -0,0 +1,366 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_l1_pos_controller.h
+ * Implementation of L1 position control.
+ * Authors and acknowledgements in header.
+ *
+ */
+
+#include "ecl_l1_pos_controller.h"
+
+float ECL_L1_Pos_Controller::nav_roll()
+{
+	float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G);
+	ret = math::constrain(ret, -_roll_lim_rad, _roll_lim_rad);
+	return ret;
+}
+
+float ECL_L1_Pos_Controller::nav_lateral_acceleration_demand()
+{
+	return _lateral_accel;
+}
+
+float ECL_L1_Pos_Controller::nav_bearing()
+{
+	return _wrap_pi(_nav_bearing);
+}
+
+float ECL_L1_Pos_Controller::bearing_error()
+{
+	return _bearing_error;
+}
+
+float ECL_L1_Pos_Controller::target_bearing()
+{
+	return _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);
+}
+
+bool ECL_L1_Pos_Controller::reached_loiter_target(void)
+{
+	return _circle_mode;
+}
+
+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)
+{
+
+	/* this follows the logic presented in [1] */
+
+	float eta;
+	float xtrack_vel;
+	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());
+
+	/* enforce a minimum ground speed of 0.1 m/s to avoid singularities */
+	float ground_speed = math::max(ground_speed_vector.length(), 0.1f);
+
+	/* calculate the L1 length required for the desired period */
+	_L1_distance = _L1_ratio * ground_speed;
+
+	/* calculate vector from A to B */
+	math::Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B);
+
+	/*
+	 * check if waypoints are on top of each other. If yes,
+	 * skip A and directly continue to B
+	 */
+	if (vector_AB.length() < 1.0e-6f) {
+		vector_AB = get_local_planar_vector(vector_curr_position, vector_B);
+	}
+
+	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);
+
+	/* calculate crosstrack error (output only) */
+	_crosstrack_error = vector_AB % vector_A_to_airplane;
+
+	/*
+	 * If the current position is in a +-135 degree angle behind waypoint A
+	 * and further away from A than the L1 distance, then A becomes the L1 point.
+	 * If the aircraft is already between A and B normal L1 logic is applied.
+	 */
+	float distance_A_to_airplane = vector_A_to_airplane.length();
+	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();
+	
+	/* calculate angle of airplane position vector relative to line) */
+
+	// XXX this could probably also be based solely on the dot product
+	float AB_to_BP_bearing = atan2f(vector_B_to_P_unit % vector_AB, vector_B_to_P_unit * vector_AB);
+
+	/* extension from [2], fly directly to A */
+	if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) {
+
+		/* 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();
+		/* 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());
+
+	/*
+	 * If the AB vector and the vector from B to airplane point in the same
+	 * direction, we have missed the waypoint. At +- 90 degrees we are just passing it.
+	 */
+	} else if (fabsf(AB_to_BP_bearing) < math::radians(100.0f)) {
+		/*
+		 * Extension, fly back to waypoint.
+		 * 
+		 * This corner case is possible if the system was following
+		 * the AB line from waypoint A to waypoint B, then is
+		 * switched to manual mode (or otherwise misses the waypoint)
+		 * and behind the waypoint continues to follow the AB line.
+		 */
+
+		/* calculate eta to fly to waypoint B */
+		
+		/* velocity across / orthogonal to line */
+		xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit);
+		/* velocity along line */
+		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());
+
+	} else {
+
+		/* calculate eta to fly along the line between A and B */
+
+		/* velocity across / orthogonal to line */
+		xtrack_vel = ground_speed_vector % vector_AB;
+		/* velocity along line */
+		ltrack_vel = ground_speed_vector * vector_AB;
+		/* calculate eta2 (angle of velocity vector relative to line) */
+		float eta2 = atan2f(xtrack_vel, ltrack_vel);
+		/* calculate eta1 (angle to L1 point) */
+		float xtrackErr = vector_A_to_airplane % vector_AB;
+		float sine_eta1 = xtrackErr / math::max(_L1_distance , 0.1f);
+		/* limit output to 45 degrees */
+		sine_eta1 = math::constrain(sine_eta1, -0.7071f, 0.7071f); //sin(pi/4) = 0.7071
+		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;
+
+	}
+
+	/* limit angle to +-90 degrees */
+	eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f);
+	_lateral_accel = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta);
+
+	/* flying to waypoints, not circling them */
+	_circle_mode = false;
+
+	/* the bearing angle, in NED frame */
+	_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)
+{
+	/* the complete guidance logic in this section was proposed by [2] */
+
+	/* calculate the gains for the PD loop (circle tracking) */
+	float omega = (2.0f * M_PI_F / _L1_period);
+	float K_crosstrack = omega * omega;
+	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());
+
+	/* ground speed, enforce minimum of 0.1 m/s to avoid singularities */
+	float ground_speed = math::max(ground_speed_vector.length() , 0.1f);
+
+	/* calculate the L1 length required for the desired period */
+	_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);
+
+	/* store the normalized vector from waypoint A to current position */
+	math::Vector2f vector_A_to_airplane_unit = (vector_A_to_airplane).normalized();
+
+	/* calculate eta angle towards the loiter center */
+
+	/* velocity across / orthogonal to line from waypoint to current position */
+	float xtrack_vel_center = vector_A_to_airplane_unit % ground_speed_vector;
+	/* velocity along line from waypoint to current position */
+	float ltrack_vel_center = - (ground_speed_vector * vector_A_to_airplane_unit);
+	float eta = atan2f(xtrack_vel_center, ltrack_vel_center);
+	/* limit eta to 90 degrees */
+	eta = math::constrain(eta, -M_PI_F / 2.0f, +M_PI_F / 2.0f);
+
+	/* calculate the lateral acceleration to capture the center point */
+	float lateral_accel_sp_center = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta);
+
+	/* for PD control: Calculate radial position and velocity errors */
+
+	/* radial velocity error */
+	float xtrack_vel_circle = -ltrack_vel_center;
+	/* radial distance from the loiter circle (not center) */
+	float xtrack_err_circle = vector_A_to_airplane.length() - radius;
+
+	/* cross track error for feedback */
+	_crosstrack_error = xtrack_err_circle;
+
+	/* calculate PD update to circle waypoint */
+	float lateral_accel_sp_circle_pd = (xtrack_err_circle * K_crosstrack + xtrack_vel_circle * K_velocity);
+
+	/* calculate velocity on circle / along tangent */
+	float tangent_vel = xtrack_vel_center * loiter_direction;
+
+	/* prevent PD output from turning the wrong way */
+	if (tangent_vel < 0.0f) {
+		lateral_accel_sp_circle_pd = math::max(lateral_accel_sp_circle_pd , 0.0f);
+	}
+
+	/* calculate centripetal acceleration setpoint */
+	float lateral_accel_sp_circle_centripetal = tangent_vel * tangent_vel / math::max((0.5f * radius) , (radius + xtrack_err_circle));
+
+	/* add PD control on circle and centripetal acceleration for total circle command */
+	float lateral_accel_sp_circle = loiter_direction * (lateral_accel_sp_circle_pd + lateral_accel_sp_circle_centripetal);
+
+	/*
+	 * Switch between circle (loiter) and capture (towards waypoint center) mode when
+	 * the commands switch over. Only fly towards waypoint if outside the circle.
+	 */
+
+	// XXX check switch over
+	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());
+
+	} 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());
+	}
+}
+
+
+void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed_vector)
+{
+	/* the complete guidance logic in this section was proposed by [2] */
+
+	float eta;
+
+	/* 
+	 * As the commanded heading is the only reference
+	 * (and no crosstrack correction occurs),
+	 * target and navigation bearing become the same
+	 */
+	_target_bearing = _nav_bearing = _wrap_pi(navigation_heading);
+	eta = _target_bearing - _wrap_pi(current_heading);
+	eta = _wrap_pi(eta);
+
+	/* consequently the bearing error is exactly eta: */
+	_bearing_error = eta; 
+
+	/* ground speed is the length of the ground speed vector */
+	float ground_speed = ground_speed_vector.length();
+
+	/* adjust L1 distance to keep constant frequency */
+	_L1_distance = ground_speed / _heading_omega;
+	float omega_vel = ground_speed * _heading_omega;
+
+	/* not circling a waypoint */
+	_circle_mode = false;
+
+	/* navigating heading means by definition no crosstrack error */
+	_crosstrack_error = 0;
+
+	/* limit eta to 90 degrees */
+	eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f);
+	_lateral_accel = 2.0f * sinf(eta) * omega_vel;
+}
+
+void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading)
+{
+	/* the logic in this section is trivial, but originally proposed by [2] */
+
+	/* reset all heading / error measures resulting in zero roll */
+	_target_bearing = current_heading;
+	_nav_bearing = current_heading;
+	_bearing_error = 0;
+	_crosstrack_error = 0;
+	_lateral_accel = 0;
+
+	/* not circling a waypoint when flying level */
+	_circle_mode = false;
+
+}
+
+
+math::Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &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()))));
+
+	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
new file mode 100644
index 000000000..7a3c42a92
--- /dev/null
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.h
@@ -0,0 +1,268 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_l1_pos_control.h
+ * Implementation of L1 position control.
+ *
+ *
+ * Acknowledgements and References:
+ *
+ *    This implementation has been built for PX4 based on the original
+ *    publication from [1] and does include a lot of the ideas (not code)
+ *    from [2].
+ *
+ *
+ *    [1] S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking,"
+ *    Proceedings of the AIAA Guidance, Navigation and Control
+ *    Conference, Aug 2004. AIAA-2004-4900.
+ *
+ *    [2] Paul Riseborough, Brandon Jones and Andrew Tridgell, L1 control for APM. Aug 2013.
+ *     - Explicit control over frequency and damping
+ *     - Explicit control over track capture angle
+ *     - Ability to use loiter radius smaller than L1 length
+ *     - Modified to use PD control for circle tracking to enable loiter radius less than L1 length
+ *     - Modified to enable period and damping of guidance loop to be set explicitly
+ *     - Modified to provide explicit control over capture angle
+ *
+ */
+
+#ifndef ECL_L1_POS_CONTROLLER_H
+#define ECL_L1_POS_CONTROLLER_H
+
+#include <mathlib/mathlib.h>
+#include <geo/geo.h>
+#include <ecl/ecl.h>
+
+/**
+ * L1 Nonlinear Guidance Logic
+ */
+class __EXPORT ECL_L1_Pos_Controller
+{
+public:
+	ECL_L1_Pos_Controller() {
+		_L1_period = 25;
+		_L1_damping = 0.75f;
+	}
+
+	/**
+	 * The current target bearing
+	 *
+	 * @return bearing angle (-pi..pi, in NED frame)
+	 */
+	float nav_bearing();
+
+
+	/**
+	 * Get lateral acceleration demand.
+	 *
+	 * @return Lateral acceleration in m/s^2
+	 */
+	float nav_lateral_acceleration_demand();
+
+
+	/**
+	 * Heading error.
+	 *
+	 * The heading error is either compared to the current track
+	 * or to the tangent of the current loiter radius.
+	 */
+	float bearing_error();
+
+
+	/**
+	 * Bearing from aircraft to current target.
+	 *
+	 * @return bearing angle (-pi..pi, in NED frame)
+	 */
+	float target_bearing();
+
+
+	/**
+	 * Get roll angle setpoint for fixed wing.
+	 *
+	 * @return Roll angle (in NED frame)
+	 */
+	float nav_roll();
+
+
+	/**
+	 * Get the current crosstrack error.
+	 *
+	 * @return Crosstrack error in meters.
+	 */
+	float crosstrack_error();
+
+
+	/**
+	 * Returns true if the loiter waypoint has been reached
+	 */
+	bool reached_loiter_target();
+
+
+	/**
+	 * Returns true if following a circle (loiter)
+	 */
+	bool circle_mode() {
+		return _circle_mode;
+	}
+
+
+	/**
+	 * Get the switch distance
+	 * 
+	 * This is the distance at which the system will
+	 * switch to the next waypoint. This depends on the
+	 * period and damping
+	 *
+	 * @param waypoint_switch_radius The switching radius the waypoint has set.
+	 */
+	float switch_distance(float waypoint_switch_radius);
+
+
+	/**
+	 * Navigate between two waypoints
+	 *
+	 * Calling this function with two waypoints results in the
+	 * control outputs to fly to the line segment defined by
+	 * the points and once captured following the line segment.
+	 * This follows the logic in [1].
+	 *
+	 * @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);
+
+
+	/**
+	 * Navigate on an orbit around a loiter waypoint.
+	 *
+	 * This allow orbits smaller than the L1 length,
+	 * this modification was introduced in [2].
+	 *
+	 * @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);
+
+
+	/**
+	 * Navigate on a fixed bearing.
+	 *
+	 * This only holds a certain direction and does not perform cross
+	 * track correction. Helpful for semi-autonomous modes. Introduced
+	 * by [2].
+	 *
+	 * @return sets _lateral_accel setpoint
+	 */
+	void navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed);
+
+
+	/**
+	 * Keep the wings level.
+	 *
+	 * This is typically needed for maximum-lift-demand situations,
+	 * such as takeoff or near stall. Introduced in [2].
+	 */
+	void navigate_level_flight(float current_heading);
+
+
+	/**
+	 * Set the L1 period.
+	 */
+	void set_l1_period(float period) {
+		_L1_period = period;
+		/* calculate the ratio introduced in [2] */
+		_L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period;
+		/* calculate normalized frequency for heading tracking */
+		_heading_omega = sqrtf(2.0f) * M_PI_F / _L1_period;
+	}
+
+
+	/**
+	 * Set the L1 damping factor.
+	 *
+	 * The original publication recommends a default of sqrt(2) / 2 = 0.707
+	 */
+	void set_l1_damping(float damping) {
+		_L1_damping = damping;
+		/* calculate the ratio introduced in [2] */
+		_L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period;
+		/* calculate the L1 gain (following [2]) */
+		_K_L1 = 4.0f * _L1_damping * _L1_damping;
+	}
+
+
+	/**
+	 * Set the maximum roll angle output in radians
+	 *
+	 */
+	void set_l1_roll_limit(float roll_lim_rad) {
+		_roll_lim_rad = roll_lim_rad;
+	}
+
+private:
+
+	float _lateral_accel;		///< Lateral acceleration setpoint in m/s^2
+	float _L1_distance;		///< L1 lead distance, defined by period and damping
+	bool _circle_mode;		///< flag for loiter mode
+	float _nav_bearing;		///< bearing to L1 reference point
+	float _bearing_error;		///< bearing error
+	float _crosstrack_error;	///< crosstrack error in meters
+	float _target_bearing;		///< the heading setpoint
+
+	float _L1_period;		///< L1 tracking period in seconds
+	float _L1_damping;		///< L1 damping ratio
+	float _L1_ratio;		///< L1 ratio for navigation
+	float _K_L1;			///< L1 control gain for _L1_damping
+	float _heading_omega;		///< Normalized frequency
+
+	float _roll_lim_rad;  ///<maximum roll angle
+
+	/**
+	 * Convert a 2D vector from WGS84 to planar coordinates.
+	 *
+	 * This converts from latitude and longitude to planar
+	 * coordinates with (0,0) being at the position of ref and
+	 * returns a vector in meters towards wp.
+	 *
+	 * @param ref The reference position in WGS84 coordinates
+	 * @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;
+
+};
+
+
+#endif /* ECL_L1_POS_CONTROLLER_H */
diff --git a/src/lib/ecl/module.mk b/src/lib/ecl/module.mk
new file mode 100644
index 000000000..f2aa3db6a
--- /dev/null
+++ b/src/lib/ecl/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2013 Estimation and Control Library (ECL). 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.
+#
+############################################################################
+
+#
+# Estimation and Control Library
+#
+
+SRCS		 = 	attitude_fw/ecl_pitch_controller.cpp \
+			attitude_fw/ecl_roll_controller.cpp \
+			attitude_fw/ecl_yaw_controller.cpp \
+			l1/ecl_l1_pos_controller.cpp
diff --git a/src/lib/external_lgpl/module.mk b/src/lib/external_lgpl/module.mk
new file mode 100644
index 000000000..53f1629e3
--- /dev/null
+++ b/src/lib/external_lgpl/module.mk
@@ -0,0 +1,48 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+#  W A R N I N G: The contents of this directory are license-incompatible
+# 			with the rest of the codebase. Do NOT copy any parts of it
+#			into other folders.
+#
+#  Acknowledgements:
+#
+#			The algorithms in this folder have been developed by Paul Riseborough
+#			with support from Andrew Tridgell.
+#			Originally licensed as LGPL for APM. As this is built as library and
+#			linked, use of this code does not change the usability of PX4 in general
+#			or any of the license implications.
+#
+
+SRCS		 = 	tecs/tecs.cpp
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
new file mode 100644
index 000000000..1d5c85699
--- /dev/null
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -0,0 +1,542 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+#include "tecs.h"
+#include <ecl/ecl.h>
+
+using namespace math;
+
+#ifndef CONSTANTS_ONE_G
+#define CONSTANTS_ONE_G GRAVITY
+#endif
+
+/**
+ * @file tecs.cpp
+ *
+ * @author Paul Riseborough
+ *
+ *  Written by Paul Riseborough 2013 to provide:
+ *  - Combined control of speed and height using throttle to control
+ *    total energy and pitch angle to control exchange of energy between
+ *    potential and kinetic.
+ *    Selectable speed or height priority modes when calculating pitch angle
+ *  - Fallback mode when no airspeed measurement is available that
+ *    sets throttle based on height rate demand and switches pitch angle control to
+ *    height priority
+ *  - Underspeed protection that demands maximum throttle and switches pitch angle control
+ *    to speed priority mode
+ *  - Relative ease of tuning through use of intuitive time constant, integrator and damping gains and the use
+ *    of easy to measure aircraft performance data
+ *
+ */
+
+void TECS::update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth)
+{
+	// Implement third order complementary filter for height and height rate
+	// estimted height rate = _integ2_state
+	// estimated height     = _integ3_state
+	// Reference Paper :
+	// Optimising the Gains of the Baro-Inertial Vertical Channel
+	// Widnall W.S, Sinha P.K,
+	// AIAA Journal of Guidance and Control, 78-1307R
+
+	// Calculate time in seconds since last update
+	uint64_t now = ecl_absolute_time();
+	float DT = max((now - _update_50hz_last_usec), 0ULL) * 1.0e-6f;
+
+	// printf("dt: %10.6f baro alt: %6.2f eas: %6.2f R(0,0): %6.2f, R(1,1): %6.2f\naccel body: %6.2f %6.2f %6.2f\naccel earth: %6.2f %6.2f %6.2f\n",
+	// 	DT, baro_altitude, airspeed, rotMat(0, 0), rotMat(1, 1), accel_body(0), accel_body(1), accel_body(2),
+	// 	accel_earth(0), accel_earth(1), accel_earth(2));
+
+	if (DT > 1.0f) {
+		_integ3_state = baro_altitude;
+		_integ2_state = 0.0f;
+		_integ1_state = 0.0f;
+		DT            = 0.02f; // when first starting TECS, use a
+		// small time constant
+	}
+
+	_update_50hz_last_usec = now;
+	_EAS = airspeed;
+
+	// Get height acceleration
+	float hgt_ddot_mea = -(accel_earth(2) + CONSTANTS_ONE_G);
+	// Perform filter calculation using backwards Euler integration
+	// Coefficients selected to place all three filter poles at omega
+	float omega2 = _hgtCompFiltOmega * _hgtCompFiltOmega;
+	float hgt_err = baro_altitude - _integ3_state;
+	float integ1_input = hgt_err * omega2 * _hgtCompFiltOmega;
+	_integ1_state = _integ1_state + integ1_input * DT;
+	float integ2_input = _integ1_state + hgt_ddot_mea + hgt_err * omega2 * 3.0f;
+	_integ2_state = _integ2_state + integ2_input * DT;
+	float integ3_input = _integ2_state + hgt_err * _hgtCompFiltOmega * 3.0f;
+
+	// If more than 1 second has elapsed since last update then reset the integrator state
+	// to the measured height
+	if (DT > 1.0f) {
+		_integ3_state = baro_altitude;
+
+	} else {
+		_integ3_state = _integ3_state + integ3_input * DT;
+	}
+
+	// Update and average speed rate of change
+	// Only required if airspeed is being measured and controlled
+	float temp = 0;
+
+	if (isfinite(airspeed) && airspeed_sensor_enabled()) {
+		// Get DCM
+		// Calculate speed rate of change
+		// XXX check
+		temp = rotMat(2, 0) * CONSTANTS_ONE_G + accel_body(0);
+		// take 5 point moving average
+		//_vel_dot = _vdot_filter.apply(temp);
+		// XXX resolve this properly
+		_vel_dot = 0.9f * _vel_dot + 0.1f * temp;
+
+	} else {
+		_vel_dot = 0.0f;
+	}
+
+}
+
+void TECS::_update_speed(float airspeed_demand, float indicated_airspeed,
+			 float indicated_airspeed_min, float indicated_airspeed_max, float EAS2TAS)
+{
+	// Calculate time in seconds since last update
+	uint64_t now = ecl_absolute_time();
+	float DT = max((now - _update_speed_last_usec), 0ULL) * 1.0e-6f;
+	_update_speed_last_usec = now;
+
+	// Convert equivalent airspeeds to true airspeeds
+
+	_EAS_dem = airspeed_demand;
+	_TAS_dem  = _EAS_dem * EAS2TAS;
+	_TASmax   = indicated_airspeed_max * EAS2TAS;
+	_TASmin   = indicated_airspeed_min * EAS2TAS;
+
+	// Reset states of time since last update is too large
+	if (DT > 1.0f) {
+		_integ5_state = (_EAS * EAS2TAS);
+		_integ4_state = 0.0f;
+		DT            = 0.1f; // when first starting TECS, use a
+		// small time constant
+	}
+
+	// Get airspeed or default to halfway between min and max if
+	// airspeed is not being used and set speed rate to zero
+	if (!isfinite(indicated_airspeed) || !airspeed_sensor_enabled()) {
+		// If no airspeed available use average of min and max
+		_EAS = 0.5f * (indicated_airspeed_min + indicated_airspeed_max);
+
+	} else {
+		_EAS = indicated_airspeed;
+	}
+
+	// Implement a second order complementary filter to obtain a
+	// smoothed airspeed estimate
+	// airspeed estimate is held in _integ5_state
+	float aspdErr = (_EAS * EAS2TAS) - _integ5_state;
+	float integ4_input = aspdErr * _spdCompFiltOmega * _spdCompFiltOmega;
+
+	// Prevent state from winding up
+	if (_integ5_state < 3.1f) {
+		integ4_input = max(integ4_input , 0.0f);
+	}
+
+	_integ4_state = _integ4_state + integ4_input * DT;
+	float integ5_input = _integ4_state + _vel_dot + aspdErr * _spdCompFiltOmega * 1.4142f;
+	_integ5_state = _integ5_state + integ5_input * DT;
+	// limit the airspeed to a minimum of 3 m/s
+	_integ5_state = max(_integ5_state, 3.0f);
+
+}
+
+void TECS::_update_speed_demand(void)
+{
+	// Set the airspeed demand to the minimum value if an underspeed condition exists
+	// or a bad descent condition exists
+	// This will minimise the rate of descent resulting from an engine failure,
+	// enable the maximum climb rate to be achieved and prevent continued full power descent
+	// into the ground due to an unachievable airspeed value
+	if ((_badDescent) || (_underspeed)) {
+		_TAS_dem     = _TASmin;
+	}
+
+	// Constrain speed demand
+	_TAS_dem = constrain(_TAS_dem, _TASmin, _TASmax);
+
+	// 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;
+	}
+
+	// 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;
+}
+
+void TECS::_update_height_demand(float demand)
+{
+	// 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);
+}
+
+void TECS::_detect_underspeed(void)
+{
+	if (((_integ5_state < _TASmin * 0.9f) && (_throttle_dem >= _THRmaxf * 0.95f)) || ((_integ3_state < _hgt_dem_adj) && _underspeed)) {
+		_underspeed = true;
+
+	} else {
+		_underspeed = false;
+	}
+}
+
+void TECS::_update_energies(void)
+{
+	// Calculate specific energy demands
+	_SPE_dem = _hgt_dem_adj * CONSTANTS_ONE_G;
+	_SKE_dem = 0.5f * _TAS_dem_adj * _TAS_dem_adj;
+
+	// Calculate specific energy rate demands
+	_SPEdot_dem = _hgt_rate_dem * CONSTANTS_ONE_G;
+	_SKEdot_dem = _integ5_state * _TAS_rate_dem;
+
+	// Calculate specific energy
+	_SPE_est = _integ3_state * CONSTANTS_ONE_G;
+	_SKE_est = 0.5f * _integ5_state * _integ5_state;
+
+	// Calculate specific energy rate
+	_SPEdot = _integ2_state * CONSTANTS_ONE_G;
+	_SKEdot = _integ5_state * _vel_dot;
+}
+
+void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat)
+{
+	// Calculate total energy values
+	_STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est;
+	float STEdot_dem = constrain((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max);
+	float STEdot_error = STEdot_dem - _SPEdot - _SKEdot;
+
+	// Apply 0.5 second first order filter to STEdot_error
+	// This is required to remove accelerometer noise from the  measurement
+	STEdot_error = 0.2f * STEdot_error + 0.8f * _STEdotErrLast;
+	_STEdotErrLast = STEdot_error;
+
+	// Calculate throttle demand
+	// If underspeed condition is set, then demand full throttle
+	if (_underspeed) {
+		_throttle_dem_unc = 1.0f;
+
+	} else {
+		// Calculate gain scaler from specific energy error to throttle
+		float K_STE2Thr = 1 / (_timeConst * (_STEdot_max - _STEdot_min));
+
+		// Calculate feed-forward throttle
+		float ff_throttle = 0;
+		float nomThr = throttle_cruise;
+		// Use the demanded rate of change of total energy as the feed-forward demand, but add
+		// 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);
+
+		if (STEdot_dem >= 0) {
+			ff_throttle = nomThr + STEdot_dem / _STEdot_max * (1.0f - nomThr);
+
+		} else {
+			ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr;
+		}
+
+		// Calculate PD + FF throttle
+		_throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
+
+		// Rate limit PD + FF throttle
+		// Calculate the throttle increment from the specified slew time
+		if (fabsf(_throttle_slewrate) > 0.01f) {
+			float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate;
+
+			_throttle_dem = constrain(_throttle_dem,
+						  _last_throttle_dem - thrRateIncr,
+						  _last_throttle_dem + thrRateIncr);
+			_last_throttle_dem = _throttle_dem;
+		}
+
+
+		// Calculate integrator state upper and lower limits
+		// Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand
+		float integ_max = (_THRmaxf - _throttle_dem + 0.1f);
+		float integ_min = (_THRminf - _throttle_dem - 0.1f);
+
+		// Calculate integrator state, constraining state
+		// Set integrator to a max throttle value dduring climbout
+		_integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr;
+
+		if (_climbOutDem) {
+			_integ6_state = integ_max;
+
+		} else {
+			_integ6_state = constrain(_integ6_state, integ_min, integ_max);
+		}
+
+		// Sum the components.
+		// Only use feed-forward component if airspeed is not being used
+		if (airspeed_sensor_enabled()) {
+			_throttle_dem = _throttle_dem + _integ6_state;
+
+		} else {
+			_throttle_dem = ff_throttle;
+		}
+	}
+
+	// Constrain throttle demand
+	_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
+}
+
+void TECS::_detect_bad_descent(void)
+{
+	// Detect a demanded airspeed too high for the aircraft to achieve. This will be
+	// evident by the the following conditions:
+	// 1) Underspeed protection not active
+	// 2) Specific total energy error > 200 (greater than ~20m height error)
+	// 3) Specific total energy reducing
+	// 4) throttle demand > 90%
+	// If these four conditions exist simultaneously, then the protection
+	// mode will be activated.
+	// Once active, the following condition are required to stay in the mode
+	// 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;
+	}
+}
+
+void TECS::_update_pitch(void)
+{
+	// Calculate Speed/Height Control Weighting
+	// This is used to determine how the pitch control prioritises speed and height control
+	// A weighting of 1 provides equal priority (this is the normal mode of operation)
+	// A SKE_weighting of 0 provides 100% priority to height control. This is used when no airspeed measurement is available
+	// A SKE_weighting of 2 provides 100% priority to speed control. This is used when an underspeed condition is detected
+	// or during takeoff/climbout where a minimum pitch angle is set to ensure height is gained. In this instance, if airspeed
+	// rises above the demanded value, the pitch angle will be increased by the TECS controller.
+	float SKE_weighting = constrain(_spdWeight, 0.0f, 2.0f);
+
+	if ((_underspeed || _climbOutDem) && airspeed_sensor_enabled()) {
+		SKE_weighting = 2.0f;
+
+	} else if (!airspeed_sensor_enabled()) {
+		SKE_weighting = 0.0f;
+	}
+
+	float SPE_weighting = 2.0f - SKE_weighting;
+
+	// Calculate Specific Energy Balance demand, and error
+	float SEB_dem      = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting;
+	float SEBdot_dem   = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting;
+	float SEB_error    = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting);
+	float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting);
+
+	// Calculate integrator state, constraining input if pitch limits are exceeded
+	float integ7_input = SEB_error * _integGain;
+
+	if (_pitch_dem_unc > _PITCHmaxf) {
+		integ7_input = min(integ7_input, _PITCHmaxf - _pitch_dem_unc);
+
+	} else if (_pitch_dem_unc < _PITCHminf) {
+		integ7_input = max(integ7_input, _PITCHminf - _pitch_dem_unc);
+	}
+
+	_integ7_state = _integ7_state + integ7_input * _DT;
+
+	// Apply max and min values for integrator state that will allow for no more than
+	// 5deg of saturation. This allows for some pitch variation due to gusts before the
+	// integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence
+	// During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle
+	// demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
+	// integrator has to catch up before the nose can be raised to reduce speed during climbout.
+	float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G);
+	float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
+	if (_climbOutDem)
+	{
+		temp += _PITCHminf * gainInv;
+	}
+	_integ7_state = constrain(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp);
+
+
+	// Calculate pitch demand from specific energy balance signals
+	_pitch_dem_unc = (temp + _integ7_state) / gainInv;
+
+	// Constrain pitch demand
+	_pitch_dem = constrain(_pitch_dem_unc, _PITCHminf, _PITCHmaxf);
+
+	// Rate limit the pitch demand to comply with specified vertical
+	// acceleration limit
+	float ptchRateIncr = _DT * _vertAccLim / _integ5_state;
+
+	if ((_pitch_dem - _last_pitch_dem) > ptchRateIncr) {
+		_pitch_dem = _last_pitch_dem + ptchRateIncr;
+
+	} else if ((_pitch_dem - _last_pitch_dem) < -ptchRateIncr) {
+		_pitch_dem = _last_pitch_dem - ptchRateIncr;
+	}
+
+	_last_pitch_dem = _pitch_dem;
+}
+
+void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad)
+{
+	// Initialise states and variables if DT > 1 second or in climbout
+	if (_DT > 1.0f) {
+		_integ6_state      = 0.0f;
+		_integ7_state      = 0.0f;
+		_last_throttle_dem = throttle_cruise;
+		_last_pitch_dem    = pitch;
+		_hgt_dem_adj_last  = baro_altitude;
+		_hgt_dem_adj       = _hgt_dem_adj_last;
+		_hgt_dem_prev      = _hgt_dem_adj_last;
+		_hgt_dem_in_old    = _hgt_dem_adj_last;
+		_TAS_dem_last      = _TAS_dem;
+		_TAS_dem_adj       = _TAS_dem;
+		_underspeed        = false;
+		_badDescent        = false;
+		_DT                = 0.1f; // when first starting TECS, use a
+		// small time constant
+
+	} else if (_climbOutDem) {
+		_PITCHminf          = ptchMinCO_rad;
+		_THRminf            = _THRmaxf - 0.01f;
+		_hgt_dem_adj_last  = baro_altitude;
+		_hgt_dem_adj       = _hgt_dem_adj_last;
+		_hgt_dem_prev      = _hgt_dem_adj_last;
+		_TAS_dem_last      = _TAS_dem;
+		_TAS_dem_adj       = _TAS_dem;
+		_underspeed        = false;
+		_badDescent 	   = false;
+	}
+}
+
+void TECS::_update_STE_rate_lim(void)
+{
+	// Calculate Specific Total Energy Rate Limits
+	// This is a tivial calculation at the moment but will get bigger once we start adding altitude effects
+	_STEdot_max = _maxClimbRate * CONSTANTS_ONE_G;
+	_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,
+				 float throttle_min, float throttle_max, float throttle_cruise,
+				 float pitch_limit_min, float pitch_limit_max)
+{
+	// Calculate time in seconds since last update
+	uint64_t now = ecl_absolute_time();
+	_DT = max((now - _update_pitch_throttle_last_usec), 0ULL) * 1.0e-6f;
+	_update_pitch_throttle_last_usec = now;
+
+	// printf("tecs in: dt:%10.6f pitch: %6.2f baro_alt: %6.2f alt sp: %6.2f\neas sp: %6.2f eas: %6.2f, eas2tas: %6.2f\n %s pitch min C0: %6.2f thr min: %6.2f, thr max: %6.2f thr cruis: %6.2f pt min: %6.2f, pt max: %6.2f\n",
+	// 	_DT, pitch, baro_altitude, hgt_dem, EAS_dem, indicated_airspeed, EAS2TAS, (climbOutDem) ? "climb" : "level", ptchMinCO, throttle_min, throttle_max, throttle_cruise, pitch_limit_min, pitch_limit_max);
+
+	// Update the speed estimate using a 2nd order complementary filter
+	_update_speed(EAS_dem, indicated_airspeed, _indicated_airspeed_min, _indicated_airspeed_max, EAS2TAS);
+
+	// Convert inputs
+	_THRmaxf  = throttle_max;
+	_THRminf  = throttle_min;
+	_PITCHmaxf = pitch_limit_max;
+	_PITCHminf = pitch_limit_min;
+	_climbOutDem = climbOutDem;
+
+	// initialise selected states and variables if DT > 1 second or in climbout
+	_initialise_states(pitch, throttle_cruise, baro_altitude, ptchMinCO);
+
+	// Calculate Specific Total Energy Rate Limits
+	_update_STE_rate_lim();
+
+	// Calculate the speed demand
+	_update_speed_demand();
+
+	// Calculate the height demand
+	_update_height_demand(hgt_dem);
+
+	// Detect underspeed condition
+	_detect_underspeed();
+
+	// Calculate specific energy quantitiues
+	_update_energies();
+
+	// Calculate throttle demand
+	_update_throttle(throttle_cruise, rotMat);
+
+	// Detect bad descent due to demanded airspeed being too high
+	_detect_bad_descent();
+
+	// Calculate pitch demand
+	_update_pitch();
+
+//    // Write internal variables to the log_tuning structure. This
+//    // structure will be logged in dataflash at 10Hz
+	// log_tuning.hgt_dem  = _hgt_dem_adj;
+	// log_tuning.hgt      = _integ3_state;
+	// log_tuning.dhgt_dem = _hgt_rate_dem;
+	// log_tuning.dhgt     = _integ2_state;
+	// log_tuning.spd_dem  = _TAS_dem_adj;
+	// log_tuning.spd      = _integ5_state;
+	// log_tuning.dspd     = _vel_dot;
+	// log_tuning.ithr     = _integ6_state;
+	// log_tuning.iptch    = _integ7_state;
+	// log_tuning.thr      = _throttle_dem;
+	// log_tuning.ptch     = _pitch_dem;
+	// log_tuning.dspd_dem = _TAS_rate_dem;
+}
diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h
new file mode 100644
index 000000000..f8f832ed7
--- /dev/null
+++ b/src/lib/external_lgpl/tecs/tecs.h
@@ -0,0 +1,357 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+/// @file    tecs.h
+/// @brief   Combined Total Energy Speed & Height Control.
+
+/*
+ *  Written by Paul Riseborough 2013 to provide:
+ *  - Combined control of speed and height using throttle to control
+ *    total energy and pitch angle to control exchange of energy between
+ *    potential and kinetic.
+ *    Selectable speed or height priority modes when calculating pitch angle
+ *  - Fallback mode when no airspeed measurement is available that
+ *    sets throttle based on height rate demand and switches pitch angle control to
+ *    height priority
+ *  - Underspeed protection that demands maximum throttle switches pitch angle control
+ *    to speed priority mode
+ *  - Relative ease of tuning through use of intuitive time constant, trim rate and damping parameters and the use
+ *    of easy to measure aircraft performance data
+ */
+
+#ifndef TECS_H
+#define TECS_H
+
+#include <mathlib/mathlib.h>
+#include <stdint.h>
+
+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),
+		_integ1_state(0.0f),
+		_integ2_state(0.0f),
+		_integ3_state(0.0f),
+		_integ4_state(0.0f),
+		_integ5_state(0.0f),
+		_integ6_state(0.0f),
+		_integ7_state(0.0f),
+		_pitch_dem(0.0f),
+		_last_pitch_dem(0.0f),
+		_SPE_dem(0.0f),
+		_SKE_dem(0.0f),
+		_SPEdot_dem(0.0f),
+		_SKEdot_dem(0.0f),
+		_SPE_est(0.0f),
+		_SKE_est(0.0f),
+		_SPEdot(0.0f),
+		_SKEdot(0.0f),
+		_vel_dot(0.0f),
+		_STEdotErrLast(0.0f) {
+
+	}
+
+	bool airspeed_sensor_enabled() {
+		return _airspeed_enabled;
+	}
+
+	void enable_airspeed(bool enabled) {
+		_airspeed_enabled = enabled;
+	}
+
+	// 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);
+
+	// 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,
+				   float throttle_min, float throttle_max, float throttle_cruise,
+				   float pitch_limit_min, float pitch_limit_max);
+	// demanded throttle in percentage
+	// should return 0 to 100
+	float get_throttle_demand(void) {
+		return _throttle_dem;
+	}
+	int32_t get_throttle_demand_percent(void) {
+		return get_throttle_demand();
+	}
+
+
+	float get_pitch_demand() { return _pitch_dem; }
+
+	// demanded pitch angle in centi-degrees
+	// should return between -9000 to +9000
+	int32_t get_pitch_demand_cd() { return int32_t(get_pitch_demand() * 5729.5781f);}
+
+	// Rate of change of velocity along X body axis in m/s^2
+	float get_VXdot(void) { return _vel_dot; }
+
+
+	float get_speed_weight() {
+		return _spdWeight;
+	}
+
+	// log data on internal state of the controller. Called at 10Hz
+	// void log_data(DataFlash_Class &dataflash, uint8_t msgid);
+
+	// struct PACKED log_TECS_Tuning {
+	// 	LOG_PACKET_HEADER;
+	// 	float hgt;
+	// 	float dhgt;
+	// 	float hgt_dem;
+	// 	float dhgt_dem;
+	// 	float spd_dem;
+	// 	float spd;
+	// 	float dspd;
+	// 	float ithr;
+	// 	float iptch;
+	// 	float thr;
+	// 	float ptch;
+	// 	float dspd_dem;
+	// } log_tuning;
+
+	void set_time_const(float time_const) {
+		_timeConst = time_const;
+	}
+
+	void set_min_sink_rate(float rate) {
+		_minSinkRate = rate;
+	}
+
+	void set_max_sink_rate(float sink_rate) {
+		_maxSinkRate = sink_rate;
+	}
+
+	void set_max_climb_rate(float climb_rate) {
+		_maxClimbRate = climb_rate;
+	}
+
+	void set_throttle_damp(float throttle_damp) {
+		_thrDamp = throttle_damp;
+	}
+
+	void set_integrator_gain(float gain) {
+		_integGain = gain;
+	}
+
+	void set_vertical_accel_limit(float limit) {
+		_vertAccLim = limit;
+	}
+
+	void set_height_comp_filter_omega(float omega) {
+		_hgtCompFiltOmega = omega;
+	}
+
+	void set_speed_comp_filter_omega(float omega) {
+		_spdCompFiltOmega = omega;
+	}
+
+	void set_roll_throttle_compensation(float compensation) {
+		_rollComp = compensation;
+	}
+
+	void set_speed_weight(float weight) {
+		_spdWeight = weight;
+	}
+
+	void set_pitch_damping(float damping) {
+		_ptchDamp = damping;
+	}
+
+	void set_throttle_slewrate(float slewrate) {
+		_throttle_slewrate = slewrate;
+	}
+
+	void set_indicated_airspeed_min(float airspeed) {
+		_indicated_airspeed_min = airspeed;
+	}
+
+	void set_indicated_airspeed_max(float airspeed) {
+		_indicated_airspeed_max = airspeed;
+	}
+
+private:
+	// Last time update_50Hz was called
+	uint64_t _update_50hz_last_usec;
+
+	// Last time update_speed was called
+	uint64_t _update_speed_last_usec;
+
+	// Last time update_pitch_throttle was called
+	uint64_t _update_pitch_throttle_last_usec;
+
+	// TECS tuning parameters
+	float _hgtCompFiltOmega;
+	float _spdCompFiltOmega;
+	float _maxClimbRate;
+	float _minSinkRate;
+	float _maxSinkRate;
+	float _timeConst;
+	float _ptchDamp;
+	float _thrDamp;
+	float _integGain;
+	float _vertAccLim;
+	float _rollComp;
+	float _spdWeight;
+
+	// throttle demand in the range from 0.0 to 1.0
+	float _throttle_dem;
+
+	// pitch angle demand in radians
+	float _pitch_dem;
+
+	// Integrator state 1 - height filter second derivative
+	float _integ1_state;
+
+	// Integrator state 2 - height rate
+	float _integ2_state;
+
+	// Integrator state 3 - height
+	float _integ3_state;
+
+	// Integrator state 4 - airspeed filter first derivative
+	float _integ4_state;
+
+	// Integrator state 5 - true airspeed
+	float _integ5_state;
+
+	// Integrator state 6 - throttle integrator
+	float _integ6_state;
+
+	// Integrator state 6 - pitch integrator
+	float _integ7_state;
+
+	// throttle demand rate limiter state
+	float _last_throttle_dem;
+
+	// pitch demand rate limiter state
+	float _last_pitch_dem;
+
+	// Rate of change of speed along X axis
+	float _vel_dot;
+
+	// Equivalent airspeed
+	float _EAS;
+
+	// True airspeed limits
+	float _TASmax;
+	float _TASmin;
+
+	// Current and last true airspeed demand
+	float _TAS_dem;
+	float _TAS_dem_last;
+
+	// Equivalent airspeed demand
+	float _EAS_dem;
+
+	// height demands
+	float _hgt_dem;
+	float _hgt_dem_in_old;
+	float _hgt_dem_adj;
+	float _hgt_dem_adj_last;
+	float _hgt_rate_dem;
+	float _hgt_dem_prev;
+
+	// Speed demand after application of rate limiting
+	// This is the demand tracked by the TECS control loops
+	float _TAS_dem_adj;
+
+	// Speed rate demand after application of rate limiting
+	// This is the demand tracked by the TECS control loops
+	float _TAS_rate_dem;
+
+	// Total energy rate filter state
+	float _STEdotErrLast;
+
+	// Underspeed condition
+	bool _underspeed;
+
+	// Bad descent condition caused by unachievable airspeed demand
+	bool _badDescent;
+
+	// climbout mode
+	bool _climbOutDem;
+
+	// throttle demand before limiting
+	float _throttle_dem_unc;
+
+	// pitch demand before limiting
+	float _pitch_dem_unc;
+
+	// Maximum and minimum specific total energy rate limits
+	float _STEdot_max;
+	float _STEdot_min;
+
+	// Maximum and minimum floating point throttle limits
+	float _THRmaxf;
+	float _THRminf;
+
+	// Maximum and minimum floating point pitch limits
+	float _PITCHmaxf;
+	float _PITCHminf;
+
+	// Specific energy quantities
+	float _SPE_dem;
+	float _SKE_dem;
+	float _SPEdot_dem;
+	float _SKEdot_dem;
+	float _SPE_est;
+	float _SKE_est;
+	float _SPEdot;
+	float _SKEdot;
+
+	// Specific energy error quantities
+	float _STE_error;
+
+	// Time since last update of main TECS loop (seconds)
+	float _DT;
+
+	bool _airspeed_enabled;
+	float _throttle_slewrate;
+	float _indicated_airspeed_min;
+	float _indicated_airspeed_max;
+
+	// Update the airspeed internal state using a second order complementary filter
+	void _update_speed(float airspeed_demand, float indicated_airspeed,
+			   float indicated_airspeed_min, float indicated_airspeed_max, float EAS2TAS);
+
+	// Update the demanded airspeed
+	void _update_speed_demand(void);
+
+	// Update the demanded height
+	void _update_height_demand(float demand);
+
+	// Detect an underspeed condition
+	void _detect_underspeed(void);
+
+	// Update Specific Energy Quantities
+	void _update_energies(void);
+
+	// Update Demanded Throttle
+	void _update_throttle(float throttle_cruise, const math::Dcm &rotMat);
+
+	// Detect Bad Descent
+	void _detect_bad_descent(void);
+
+	// Update Demanded Pitch Angle
+	void _update_pitch(void);
+
+	// Initialise states and variables
+	void _initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad);
+
+	// Calculate specific total energy rate limits
+	void _update_STE_rate_lim(void);
+
+};
+
+#endif //TECS_H
diff --git a/src/modules/systemlib/geo/geo.c b/src/lib/geo/geo.c
index 6746e8ff3..43105fdba 100644
--- a/src/modules/systemlib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -44,7 +44,7 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
-#include <systemlib/geo/geo.h>
+#include <geo/geo.h>
 #include <nuttx/config.h>
 #include <unistd.h>
 #include <pthread.h>
@@ -185,12 +185,11 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
 	double d_lat = lat_next_rad - lat_now_rad;
 	double d_lon = lon_next_rad - lon_now_rad;
 
-	double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad);
+	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 radius_earth * c; 
 }
 
 __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
@@ -211,6 +210,36 @@ __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)
+{
+	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);
+}
+
+__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy)
+{
+	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 * d_lon;
+	*vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad);
+}
+
 // 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)
diff --git a/src/modules/systemlib/geo/geo.h b/src/lib/geo/geo.h
index 0c0b5c533..123ff80f1 100644
--- a/src/modules/systemlib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -45,9 +45,19 @@
  * Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
  */
 
+#pragma once
+
+__BEGIN_DECLS
 
 #include <stdbool.h>
 
+#define CONSTANTS_ONE_G					9.80665f		/* m/s^2		*/
+#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C		1.225f			/* kg/m^3		*/
+#define CONSTANTS_AIR_GAS_CONST				287.1f 			/* J/(kg * K)		*/
+#define CONSTANTS_ABSOLUTE_NULL_CELSIUS			-273.15f		/* °C			*/
+#define CONSTANTS_RADIUS_OF_EARTH			6371000			/* meters (m)		*/
+
+// XXX remove
 struct crosstrack_error_s {
 	bool past_end;		// Flag indicating we are past the end of the line/arc segment
 	float distance;		// Distance in meters to closest point on line/arc
@@ -82,10 +92,30 @@ __EXPORT void map_projection_project(double lat, double lon, float *x, float *y)
  */
 __EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon);
 
+/**
+ * Returns the distance to the next waypoint in meters.
+ *
+ * @param lat_now current position in degrees (47.1234567°, not 471234567°)
+ * @param lon_now current position in degrees (8.1234567°, not 81234567°)
+ * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°)
+ * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°)
+ */
 __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
 
+/**
+ * Returns the bearing to the next waypoint in radians.
+ *
+ * @param lat_now current position in degrees (47.1234567°, not 471234567°)
+ * @param lon_now current position in degrees (8.1234567°, not 81234567°)
+ * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°)
+ * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°)
+ */
 __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_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy);
+
 __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,
@@ -95,3 +125,5 @@ __EXPORT float _wrap_180(float bearing);
 __EXPORT float _wrap_360(float bearing);
 __EXPORT float _wrap_pi(float bearing);
 __EXPORT float _wrap_2pi(float bearing);
+
+__END_DECLS
diff --git a/src/lib/geo/module.mk b/src/lib/geo/module.mk
new file mode 100644
index 000000000..30a2dc99f
--- /dev/null
+++ b/src/lib/geo/module.mk
@@ -0,0 +1,38 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Geo library
+#
+
+SRCS		 = geo.c
diff --git a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h
index 8f39acd9d..8f39acd9d 100644
--- a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h
+++ b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h
diff --git a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h
index 181b7e433..181b7e433 100644
--- a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h
+++ b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h
diff --git a/src/modules/mathlib/CMSIS/Include/arm_common_tables.h b/src/lib/mathlib/CMSIS/Include/arm_common_tables.h
index 9c37ab4e5..9c37ab4e5 100644
--- a/src/modules/mathlib/CMSIS/Include/arm_common_tables.h
+++ b/src/lib/mathlib/CMSIS/Include/arm_common_tables.h
diff --git a/src/modules/mathlib/CMSIS/Include/arm_const_structs.h b/src/lib/mathlib/CMSIS/Include/arm_const_structs.h
index 406f737dc..406f737dc 100644
--- a/src/modules/mathlib/CMSIS/Include/arm_const_structs.h
+++ b/src/lib/mathlib/CMSIS/Include/arm_const_structs.h
diff --git a/src/modules/mathlib/CMSIS/Include/arm_math.h b/src/lib/mathlib/CMSIS/Include/arm_math.h
index 6f66f9ee3..6f66f9ee3 100644
--- a/src/modules/mathlib/CMSIS/Include/arm_math.h
+++ b/src/lib/mathlib/CMSIS/Include/arm_math.h
diff --git a/src/modules/mathlib/CMSIS/Include/core_cm3.h b/src/lib/mathlib/CMSIS/Include/core_cm3.h
index 8ac6dc078..8ac6dc078 100644
--- a/src/modules/mathlib/CMSIS/Include/core_cm3.h
+++ b/src/lib/mathlib/CMSIS/Include/core_cm3.h
diff --git a/src/modules/mathlib/CMSIS/Include/core_cm4.h b/src/lib/mathlib/CMSIS/Include/core_cm4.h
index 93efd3a7a..93efd3a7a 100644
--- a/src/modules/mathlib/CMSIS/Include/core_cm4.h
+++ b/src/lib/mathlib/CMSIS/Include/core_cm4.h
diff --git a/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h b/src/lib/mathlib/CMSIS/Include/core_cm4_simd.h
index af1831ee1..af1831ee1 100644
--- a/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h
+++ b/src/lib/mathlib/CMSIS/Include/core_cm4_simd.h
diff --git a/src/modules/mathlib/CMSIS/Include/core_cmFunc.h b/src/lib/mathlib/CMSIS/Include/core_cmFunc.h
index 139bc3c5e..139bc3c5e 100644
--- a/src/modules/mathlib/CMSIS/Include/core_cmFunc.h
+++ b/src/lib/mathlib/CMSIS/Include/core_cmFunc.h
diff --git a/src/modules/mathlib/CMSIS/Include/core_cmInstr.h b/src/lib/mathlib/CMSIS/Include/core_cmInstr.h
index 8946c2c49..8946c2c49 100644
--- a/src/modules/mathlib/CMSIS/Include/core_cmInstr.h
+++ b/src/lib/mathlib/CMSIS/Include/core_cmInstr.h
diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a b/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a
index 6898bc27d..6898bc27d 100644
--- a/src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a
+++ b/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a
diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a b/src/lib/mathlib/CMSIS/libarm_cortexM4l_math.a
index a0185eaa9..a0185eaa9 100755
--- a/src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a
+++ b/src/lib/mathlib/CMSIS/libarm_cortexM4l_math.a
diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a b/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a
index 94525528e..94525528e 100755
--- a/src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a
+++ b/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a
diff --git a/src/modules/mathlib/CMSIS/library.mk b/src/lib/mathlib/CMSIS/library.mk
index 0cc1b559d..0cc1b559d 100644
--- a/src/modules/mathlib/CMSIS/library.mk
+++ b/src/lib/mathlib/CMSIS/library.mk
diff --git a/src/modules/mathlib/CMSIS/license.txt b/src/lib/mathlib/CMSIS/license.txt
index 31afac1ec..31afac1ec 100644
--- a/src/modules/mathlib/CMSIS/license.txt
+++ b/src/lib/mathlib/CMSIS/license.txt
diff --git a/src/modules/mathlib/math/Dcm.cpp b/src/lib/mathlib/math/Dcm.cpp
index c3742e288..f509f7081 100644
--- a/src/modules/mathlib/math/Dcm.cpp
+++ b/src/lib/mathlib/math/Dcm.cpp
@@ -69,6 +69,15 @@ Dcm::Dcm(float c00, float c01, float c02,
 	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)
 {
diff --git a/src/modules/mathlib/math/Dcm.hpp b/src/lib/mathlib/math/Dcm.hpp
index 28d840b10..df8970d3a 100644
--- a/src/modules/mathlib/math/Dcm.hpp
+++ b/src/lib/mathlib/math/Dcm.hpp
@@ -77,6 +77,11 @@ public:
 	Dcm(const float *data);
 
 	/**
+	 * array ctor
+	 */
+	Dcm(const float data[3][3]);
+
+	/**
 	 * quaternion ctor
 	 */
 	Dcm(const Quaternion &q);
diff --git a/src/modules/mathlib/math/EulerAngles.cpp b/src/lib/mathlib/math/EulerAngles.cpp
index e733d23bb..e733d23bb 100644
--- a/src/modules/mathlib/math/EulerAngles.cpp
+++ b/src/lib/mathlib/math/EulerAngles.cpp
diff --git a/src/modules/mathlib/math/EulerAngles.hpp b/src/lib/mathlib/math/EulerAngles.hpp
index 399eecfa7..399eecfa7 100644
--- a/src/modules/mathlib/math/EulerAngles.hpp
+++ b/src/lib/mathlib/math/EulerAngles.hpp
diff --git a/src/lib/mathlib/math/Limits.cpp b/src/lib/mathlib/math/Limits.cpp
new file mode 100644
index 000000000..d4c892d8a
--- /dev/null
+++ b/src/lib/mathlib/math/Limits.cpp
@@ -0,0 +1,146 @@
+/****************************************************************************
+ *
+ *   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 Limits.cpp
+ *
+ * Limiting / constrain helper functions
+ */
+
+
+#include <math.h>
+#include <stdint.h>
+
+#include "Limits.hpp"
+
+
+namespace math {
+
+
+float __EXPORT min(float val1, float val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+int __EXPORT min(int val1, int val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+unsigned __EXPORT min(unsigned val1, unsigned val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+uint64_t __EXPORT min(uint64_t val1, uint64_t val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+double __EXPORT min(double val1, double val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+float __EXPORT max(float val1, float val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
+int __EXPORT max(int val1, int val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
+unsigned __EXPORT max(unsigned val1, unsigned val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
+uint64_t __EXPORT max(uint64_t val1, uint64_t val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
+double __EXPORT max(double val1, double val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
+
+float __EXPORT constrain(float val, float min, float max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
+int __EXPORT constrain(int val, int min, int max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
+unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
+uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
+double __EXPORT constrain(double val, double min, double max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
+float __EXPORT radians(float degrees)
+{
+	return (degrees / 180.0f) * M_PI_F;
+}
+
+double __EXPORT radians(double degrees)
+{
+	return (degrees / 180.0) * M_PI;
+}
+
+float __EXPORT degrees(float radians)
+{
+	return (radians / M_PI_F) * 180.0f;
+}
+
+double __EXPORT degrees(double radians)
+{
+	return (radians / M_PI) * 180.0;
+}
+
+}
\ No newline at end of file
diff --git a/src/lib/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp
new file mode 100644
index 000000000..fb778dd66
--- /dev/null
+++ b/src/lib/mathlib/math/Limits.hpp
@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   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 Limits.hpp
+ *
+ * Limiting / constrain helper functions
+ */
+
+#pragma once
+
+#include <nuttx/config.h>
+#include <stdint.h>
+
+namespace math {
+
+
+float __EXPORT min(float val1, float val2);
+
+int __EXPORT min(int val1, int val2);
+
+unsigned __EXPORT min(unsigned val1, unsigned val2);
+
+uint64_t __EXPORT min(uint64_t val1, uint64_t val2);
+
+double __EXPORT min(double val1, double val2);
+
+float __EXPORT max(float val1, float val2);
+
+int __EXPORT max(int val1, int val2);
+
+unsigned __EXPORT max(unsigned val1, unsigned val2);
+
+uint64_t __EXPORT max(uint64_t val1, uint64_t val2);
+
+double __EXPORT max(double val1, double val2);
+
+
+float __EXPORT constrain(float val, float min, float max);
+
+int __EXPORT constrain(int val, int min, int max);
+
+unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max);
+
+uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max);
+
+double __EXPORT constrain(double val, double min, double max);
+
+float __EXPORT radians(float degrees);
+
+double __EXPORT radians(double degrees);
+
+float __EXPORT degrees(float radians);
+
+double __EXPORT degrees(double radians);
+
+}
\ No newline at end of file
diff --git a/src/modules/mathlib/math/Matrix.cpp b/src/lib/mathlib/math/Matrix.cpp
index ebd1aeda3..ebd1aeda3 100644
--- a/src/modules/mathlib/math/Matrix.cpp
+++ b/src/lib/mathlib/math/Matrix.cpp
diff --git a/src/modules/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index f19db15ec..f19db15ec 100644
--- a/src/modules/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
diff --git a/src/modules/mathlib/math/Quaternion.cpp b/src/lib/mathlib/math/Quaternion.cpp
index 02fec4ca6..02fec4ca6 100644
--- a/src/modules/mathlib/math/Quaternion.cpp
+++ b/src/lib/mathlib/math/Quaternion.cpp
diff --git a/src/modules/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp
index 4b4e959d8..048a55d33 100644
--- a/src/modules/mathlib/math/Quaternion.hpp
+++ b/src/lib/mathlib/math/Quaternion.hpp
@@ -37,7 +37,7 @@
  * math quaternion lib
  */
 
-//#pragma once
+#pragma once
 
 #include "Vector.hpp"
 #include "Matrix.hpp"
diff --git a/src/modules/mathlib/math/Vector.cpp b/src/lib/mathlib/math/Vector.cpp
index 35158a396..35158a396 100644
--- a/src/modules/mathlib/math/Vector.cpp
+++ b/src/lib/mathlib/math/Vector.cpp
diff --git a/src/modules/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp
index 73de793d5..73de793d5 100644
--- a/src/modules/mathlib/math/Vector.hpp
+++ b/src/lib/mathlib/math/Vector.hpp
diff --git a/src/lib/mathlib/math/Vector2f.cpp b/src/lib/mathlib/math/Vector2f.cpp
new file mode 100644
index 000000000..68e741817
--- /dev/null
+++ b/src/lib/mathlib/math/Vector2f.cpp
@@ -0,0 +1,103 @@
+/****************************************************************************
+ *
+ *   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 Vector2f.cpp
+ *
+ * math vector
+ */
+
+#include "test/test.hpp"
+
+#include "Vector2f.hpp"
+
+namespace math
+{
+
+Vector2f::Vector2f() :
+	Vector(2)
+{
+}
+
+Vector2f::Vector2f(const Vector &right) :
+	Vector(right)
+{
+#ifdef VECTOR_ASSERT
+	ASSERT(right.getRows() == 2);
+#endif
+}
+
+Vector2f::Vector2f(float x, float y) :
+	Vector(2)
+{
+	setX(x);
+	setY(y);
+}
+
+Vector2f::Vector2f(const float *data) :
+	Vector(2, data)
+{
+}
+
+Vector2f::~Vector2f()
+{
+}
+
+float Vector2f::cross(const Vector2f &b) const
+{
+	const Vector2f &a = *this;
+	return a(0)*b(1) - a(1)*b(0);
+}
+
+float Vector2f::operator %(const Vector2f &v) const
+{
+	return cross(v);
+}
+    
+float Vector2f::operator *(const Vector2f &v) const
+{
+    return dot(v);
+}
+
+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;
+}
+
+} // namespace math
diff --git a/src/lib/mathlib/math/Vector2f.hpp b/src/lib/mathlib/math/Vector2f.hpp
new file mode 100644
index 000000000..ecd62e81c
--- /dev/null
+++ b/src/lib/mathlib/math/Vector2f.hpp
@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   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 Vector2f.hpp
+ *
+ * math 3 vector
+ */
+
+#pragma once
+
+#include "Vector.hpp"
+
+namespace math
+{
+
+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);
+	}
+
+	/**
+	 * 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
+
diff --git a/src/modules/mathlib/math/Vector3.cpp b/src/lib/mathlib/math/Vector3.cpp
index 61fcc442f..dcb85600e 100644
--- a/src/modules/mathlib/math/Vector3.cpp
+++ b/src/lib/mathlib/math/Vector3.cpp
@@ -74,9 +74,9 @@ Vector3::~Vector3()
 {
 }
 
-Vector3 Vector3::cross(const Vector3 &b)
+Vector3 Vector3::cross(const Vector3 &b) const
 {
-	Vector3 &a = *this;
+	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);
diff --git a/src/modules/mathlib/math/Vector3.hpp b/src/lib/mathlib/math/Vector3.hpp
index 8c36ac134..568d9669a 100644
--- a/src/modules/mathlib/math/Vector3.hpp
+++ b/src/lib/mathlib/math/Vector3.hpp
@@ -53,7 +53,7 @@ public:
 	Vector3(float x, float y, float z);
 	Vector3(const float *data);
 	virtual ~Vector3();
-	Vector3 cross(const Vector3 &b);
+	Vector3 cross(const Vector3 &b) const;
 
 	/**
 	 * accessors
@@ -65,6 +65,11 @@ public:
 	const float &getY() const { return (*this)(1); }
 	const float &getZ() const { return (*this)(2); }
 };
+    
+class __EXPORT Vector3f :
+	public Vector3
+{
+};
 
 int __EXPORT vector3Test();
 } // math
diff --git a/src/modules/mathlib/math/arm/Matrix.cpp b/src/lib/mathlib/math/arm/Matrix.cpp
index 21661622a..21661622a 100644
--- a/src/modules/mathlib/math/arm/Matrix.cpp
+++ b/src/lib/mathlib/math/arm/Matrix.cpp
diff --git a/src/modules/mathlib/math/arm/Matrix.hpp b/src/lib/mathlib/math/arm/Matrix.hpp
index 715fd3a5e..1945bb02d 100644
--- a/src/modules/mathlib/math/arm/Matrix.hpp
+++ b/src/lib/mathlib/math/arm/Matrix.hpp
@@ -121,10 +121,10 @@ public:
 		for (size_t i = 0; i < getRows(); i++) {
 			for (size_t j = 0; j < getCols(); j++) {
 				float sig;
-				int exp;
+				int exponent;
 				float num = (*this)(i, j);
-				float2SigExp(num, sig, exp);
-				printf("%6.3fe%03.3d,", (double)sig, exp);
+				float2SigExp(num, sig, exponent);
+				printf("%6.3fe%03d ", (double)sig, exponent);
 			}
 
 			printf("\n");
diff --git a/src/modules/mathlib/math/arm/Vector.cpp b/src/lib/mathlib/math/arm/Vector.cpp
index 7ea6496bb..7ea6496bb 100644
--- a/src/modules/mathlib/math/arm/Vector.cpp
+++ b/src/lib/mathlib/math/arm/Vector.cpp
diff --git a/src/modules/mathlib/math/arm/Vector.hpp b/src/lib/mathlib/math/arm/Vector.hpp
index 58d51107d..52220fc15 100644
--- a/src/modules/mathlib/math/arm/Vector.hpp
+++ b/src/lib/mathlib/math/arm/Vector.hpp
@@ -109,10 +109,10 @@ public:
 	inline void print() const {
 		for (size_t i = 0; i < getRows(); i++) {
 			float sig;
-			int exp;
+			int exponent;
 			float num = (*this)(i);
-			float2SigExp(num, sig, exp);
-			printf("%6.3fe%03.3d,", (double)sig, exp);
+			float2SigExp(num, sig, exponent);
+			printf("%6.3fe%03d ", (double)sig, exponent);
 		}
 
 		printf("\n");
@@ -178,8 +178,15 @@ public:
 			    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) {
+	inline float dot(const Vector &right) const {
 		float result = 0;
 		arm_dot_prod_f32((float *)getData(),
 				 (float *)right.getData(),
@@ -187,12 +194,21 @@ public:
 				 &result);
 		return result;
 	}
-	inline float norm() {
+	inline float norm() const {
 		return sqrtf(dot(*this));
 	}
-	inline Vector unit() {
+	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
diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
new file mode 100644
index 000000000..3699d9bce
--- /dev/null
+++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
@@ -0,0 +1,85 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/****************************************************************************
+ *
+ *   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	LowPassFilter.cpp
+/// @brief	A class to implement a second order low pass filter 
+/// Author: Leonard Hall <LeonardTHall@gmail.com>
+
+#include "LowPassFilter2p.hpp"
+#include "math.h"
+
+namespace math
+{
+
+void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
+{
+    _cutoff_freq = cutoff_freq;
+    if (_cutoff_freq <= 0.0f) {
+        // no filtering
+        return;
+    }
+    float fr = sample_freq/_cutoff_freq;
+    float ohm = tanf(M_PI_F/fr);
+    float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm;
+    _b0 = ohm*ohm/c;
+    _b1 = 2.0f*_b0;
+    _b2 = _b0;
+    _a1 = 2.0f*(ohm*ohm-1.0f)/c;
+    _a2 = (1.0f-2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm)/c;
+}
+
+float LowPassFilter2p::apply(float sample)
+{
+    if (_cutoff_freq <= 0.0f) {
+        // no filtering
+        return 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
+        delay_element_0 = sample;
+    }
+    float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
+    
+    _delay_element_2 = _delay_element_1;
+    _delay_element_1 = delay_element_0;
+
+    // return the value.  Should be no need to check limits
+    return output;
+}
+
+} // namespace math
+
diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
new file mode 100644
index 000000000..208ec98d4
--- /dev/null
+++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
@@ -0,0 +1,78 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/****************************************************************************
+ *
+ *   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	LowPassFilter.h
+/// @brief	A class to implement a second order low pass filter 
+/// Author: Leonard Hall <LeonardTHall@gmail.com>
+/// Adapted for PX4 by Andrew Tridgell
+
+#pragma once
+
+namespace math
+{
+class __EXPORT LowPassFilter2p
+{
+public:
+    // constructor
+    LowPassFilter2p(float sample_freq, float cutoff_freq) {
+        // set initial parameters
+        set_cutoff_frequency(sample_freq, cutoff_freq);
+        _delay_element_1 = _delay_element_2 = 0;
+    }
+
+    // change 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
+    float apply(float sample);
+
+    // return the cutoff frequency
+    float get_cutoff_freq(void) const {
+        return _cutoff_freq;
+    }
+
+private:
+    float           _cutoff_freq; 
+    float           _a1;
+    float           _a2;
+    float           _b0;
+    float           _b1;
+    float           _b2;
+    float           _delay_element_1;        // buffered sample -1
+    float           _delay_element_2;        // buffered sample -2
+};
+
+} // namespace math
diff --git a/src/lib/mathlib/math/filter/module.mk b/src/lib/mathlib/math/filter/module.mk
new file mode 100644
index 000000000..f5c0647c8
--- /dev/null
+++ b/src/lib/mathlib/math/filter/module.mk
@@ -0,0 +1,43 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# filter library
+#
+SRCS		 = LowPassFilter2p.cpp
+
+#
+# In order to include .config we first have to save off the
+# current makefile name, since app.mk needs it.
+#
+APP_MAKEFILE	:= $(lastword $(MAKEFILE_LIST))
diff --git a/src/modules/mathlib/math/generic/Matrix.cpp b/src/lib/mathlib/math/generic/Matrix.cpp
index 21661622a..21661622a 100644
--- a/src/modules/mathlib/math/generic/Matrix.cpp
+++ b/src/lib/mathlib/math/generic/Matrix.cpp
diff --git a/src/modules/mathlib/math/generic/Matrix.hpp b/src/lib/mathlib/math/generic/Matrix.hpp
index 5601a3447..5601a3447 100644
--- a/src/modules/mathlib/math/generic/Matrix.hpp
+++ b/src/lib/mathlib/math/generic/Matrix.hpp
diff --git a/src/modules/mathlib/math/generic/Vector.cpp b/src/lib/mathlib/math/generic/Vector.cpp
index 7ea6496bb..7ea6496bb 100644
--- a/src/modules/mathlib/math/generic/Vector.cpp
+++ b/src/lib/mathlib/math/generic/Vector.cpp
diff --git a/src/modules/mathlib/math/generic/Vector.hpp b/src/lib/mathlib/math/generic/Vector.hpp
index 1a7363779..8cfdc676d 100644
--- a/src/modules/mathlib/math/generic/Vector.hpp
+++ b/src/lib/mathlib/math/generic/Vector.hpp
@@ -184,8 +184,17 @@ public:
 
 		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) {
+	inline float dot(const Vector &right) const {
 		float result = 0;
 
 		for (size_t i = 0; i < getRows(); i++) {
@@ -194,12 +203,21 @@ public:
 
 		return result;
 	}
-	inline float norm() {
+	inline float norm() const {
 		return sqrtf(dot(*this));
 	}
-	inline Vector unit() {
+	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
diff --git a/src/modules/mathlib/math/nasa_rotation_def.pdf b/src/lib/mathlib/math/nasa_rotation_def.pdf
index eb67a4bfc..eb67a4bfc 100644
--- a/src/modules/mathlib/math/nasa_rotation_def.pdf
+++ b/src/lib/mathlib/math/nasa_rotation_def.pdf
diff --git a/src/modules/mathlib/math/test/test.cpp b/src/lib/mathlib/math/test/test.cpp
index 2fa2f7e7c..2fa2f7e7c 100644
--- a/src/modules/mathlib/math/test/test.cpp
+++ b/src/lib/mathlib/math/test/test.cpp
diff --git a/src/modules/mathlib/math/test/test.hpp b/src/lib/mathlib/math/test/test.hpp
index 2027bb827..2027bb827 100644
--- a/src/modules/mathlib/math/test/test.hpp
+++ b/src/lib/mathlib/math/test/test.hpp
diff --git a/src/modules/mathlib/math/test_math.sce b/src/lib/mathlib/math/test_math.sce
index c3fba4729..c3fba4729 100644
--- a/src/modules/mathlib/math/test_math.sce
+++ b/src/lib/mathlib/math/test_math.sce
diff --git a/src/modules/mathlib/mathlib.h b/src/lib/mathlib/mathlib.h
index b919d53db..40ffb22bc 100644
--- a/src/modules/mathlib/mathlib.h
+++ b/src/lib/mathlib/mathlib.h
@@ -39,12 +39,16 @@
 
 #ifdef __cplusplus
 
+#pragma once
+
 #include "math/Dcm.hpp"
 #include "math/EulerAngles.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
 
diff --git a/src/modules/mathlib/module.mk b/src/lib/mathlib/module.mk
index bcdb2afe5..72bc7db8a 100644
--- a/src/modules/mathlib/module.mk
+++ b/src/lib/mathlib/module.mk
@@ -36,18 +36,19 @@
 #
 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/Matrix.cpp \
+		   math/Limits.cpp
 
 #
 # In order to include .config we first have to save off the
 # current makefile name, since app.mk needs it.
 #
 APP_MAKEFILE	:= $(lastword $(MAKEFILE_LIST))
--include $(TOPDIR)/.config
 
 ifeq ($(CONFIG_ARCH_CORTEXM4)$(CONFIG_ARCH_FPU),yy)
 INCLUDE_DIRS	+= math/arm
diff --git a/src/lib/version/version.h b/src/lib/version/version.h
new file mode 100644
index 000000000..af733aaf0
--- /dev/null
+++ b/src/lib/version/version.h
@@ -0,0 +1,62 @@
+/****************************************************************************
+ *
+ *   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 version.h
+ *
+ * Tools for system version detection.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef VERSION_H_
+#define VERSION_H_
+
+/*
+ GIT_VERSION is defined at build time via a Makefile call to the
+ git command line.
+ */
+#define FREEZE_STR(s) #s
+#define STRINGIFY(s) FREEZE_STR(s)
+#define FW_GIT STRINGIFY(GIT_VERSION)
+
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+#define	HW_ARCH "PX4FMU_V1"
+#endif
+
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+#define	HW_ARCH "PX4FMU_V2"
+#endif
+
+#endif /* VERSION_H_ */
diff --git a/src/mainpage.dox b/src/mainpage.dox
new file mode 100644
index 000000000..7ca410341
--- /dev/null
+++ b/src/mainpage.dox
@@ -0,0 +1,9 @@
+/**
+\mainpage PX4 Autopilot Flight Control Stack and Middleware
+
+This software repository offers a middleware for micro aerial vehicles capable of running efficiently on a 168 MHz Cortex M4F processor and a state of the art flight control stack supporting multicopter and fixed wing aircraft. It can be easily used for experimental air (and ground) vehicles as well, as the application on a spherical blimp shows.
+
+http://pixhawk.org
+
+
+*/
\ No newline at end of file
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
index 4ef150da1..ecca04dd7 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
@@ -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
@@ -34,12 +34,14 @@
 /**
  * @file KalmanNav.cpp
  *
- * kalman filter navigation code
+ * Kalman filter navigation code
  */
 
 #include <poll.h>
 
 #include "KalmanNav.hpp"
+#include <systemlib/err.h>
+#include <geo/geo.h>
 
 // constants
 // Titterton pg. 52
@@ -58,8 +60,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	P0(9, 9),
 	V(6, 6),
 	// attitude measurement ekf matrices
-	HAtt(6, 9),
-	RAtt(6, 6),
+	HAtt(4, 9),
+	RAtt(4, 4),
 	// position measurement ekf matrices
 	HPos(6, 9),
 	RPos(6, 6),
@@ -72,6 +74,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	_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()),
@@ -88,6 +91,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	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"),
@@ -123,8 +127,15 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	lon = 0.0f;
 	alt = 0.0f;
 
-	// initialize quaternions
-	q = Quaternion(EulerAngles(phi, theta, psi));
+	// 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);
@@ -141,6 +152,45 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	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;
@@ -174,15 +224,12 @@ void KalmanNav::update()
 	updateSubscriptions();
 
 	// initialize attitude when sensors online
-	if (!_attitudeInitialized && sensorsUpdate &&
-	    _sensors.accelerometer_counter > 10 &&
-	    _sensors.gyro_counter > 10 &&
-	    _sensors.magnetometer_counter > 10) {
+	if (!_attitudeInitialized && sensorsUpdate) {
 		if (correctAtt() == ret_ok) _attitudeInitCounter++;
 
 		if (_attitudeInitCounter > 100) {
-			printf("[kalman_demo] initialized EKF attitude\n");
-			printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n",
+			warnx("initialized EKF attitude\n");
+			warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f\n",
 			       double(phi), double(theta), double(psi));
 			_attitudeInitialized = true;
 		}
@@ -201,14 +248,24 @@ void KalmanNav::update()
 		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;
-		printf("[kalman_demo] initialized EKF state with GPS\n");
-		printf("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
+		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, alt);
+		       lat, lon, double(alt));
 	}
 
-	// prediciton step
+	// 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));
@@ -233,7 +290,7 @@ void KalmanNav::update()
 	// attitude correction step
 	if (_attitudeInitialized 								// initialized
 	    && sensorsUpdate 								// new data
-	    && _sensors.timestamp - _attTimeStamp > 1e6 / 20 	// 20 Hz
+	    && _sensors.timestamp - _attTimeStamp > 1e6 / 50 	// 50 Hz
 	   ) {
 		_attTimeStamp = _sensors.timestamp;
 		correctAtt();
@@ -260,7 +317,7 @@ void KalmanNav::updatePublications()
 {
 	using namespace math;
 
-	// position publication
+	// global position publication
 	_pos.timestamp = _pubTimeStamp;
 	_pos.time_gps_usec = _gps.timestamp_position;
 	_pos.valid = true;
@@ -271,7 +328,32 @@ void KalmanNav::updatePublications()
 	_pos.vx = vN;
 	_pos.vy = vE;
 	_pos.vz = vD;
-	_pos.hdg = psi;
+	_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;
@@ -296,8 +378,10 @@ void KalmanNav::updatePublications()
 
 	// selectively update publications,
 	// do NOT call superblock do-all method
-	if (_positionInitialized)
+	if (_positionInitialized) {
 		_pos.update();
+		_localPos.update();
+	}
 
 	if (_attitudeInitialized)
 		_att.update();
@@ -480,26 +564,32 @@ int KalmanNav::correctAtt()
 	// trig
 	float cosPhi = cosf(phi);
 	float cosTheta = cosf(theta);
-	float cosPsi = cosf(psi);
+	// float cosPsi = cosf(psi);
 	float sinPhi = sinf(phi);
 	float sinTheta = sinf(theta);
-	float sinPsi = sinf(psi);
-
-	// mag measurement
-	Vector3 zMag(_sensors.magnetometer_ga);
-	//float magNorm = zMag.norm();
-	zMag = zMag.unit();
+	// 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 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
-	float bN = cosf(dip) * cosf(dec);
-	float bE = cosf(dip) * sinf(dec);
-	float bD = sinf(dip);
-	Vector3 bNav(bN, bE, bD);
-	Vector3 zMagHat = (C_nb.transpose() * bNav).unit();
+
+	// 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);
@@ -512,9 +602,9 @@ int KalmanNav::correctAtt()
 	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;
-		RAttAdjust(4, 4) = 1.0e10;
-		RAttAdjust(5, 5) = 1.0e10;
 
 	} else {
 		//printf("correcting attitude with accel\n");
@@ -523,58 +613,25 @@ int KalmanNav::correctAtt()
 	// accel predicted measurement
 	Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit();
 
-	// combined measurement
-	Vector zAtt(6);
-	Vector zAttHat(6);
+	// 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);
 
-	for (int i = 0; i < 3; i++) {
-		zAtt(i) = zMag(i);
-		zAtt(i + 3) = zAccel(i);
-		zAttHat(i) = zMagHat(i);
-		zAttHat(i + 3) = zAccelHat(i);
-	}
+	// HMag
+	HAtt(0, 2) = 1;
 
-	// HMag , HAtt (0-2,:)
-	float tmp1 =
-		cosPsi * cosTheta * bN +
-		sinPsi * cosTheta * bE -
-		sinTheta * bD;
-	HAtt(0, 1) = -(
-			     cosPsi * sinTheta * bN +
-			     sinPsi * sinTheta * bE +
-			     cosTheta * bD
-		     );
-	HAtt(0, 2) = -cosTheta * (sinPsi * bN - cosPsi * bE);
-	HAtt(1, 0) =
-		(cosPhi * cosPsi * sinTheta + sinPhi * sinPsi) * bN +
-		(cosPhi * sinPsi * sinTheta - sinPhi * cosPsi) * bE +
-		cosPhi * cosTheta * bD;
-	HAtt(1, 1) = sinPhi * tmp1;
-	HAtt(1, 2) = -(
-			     (sinPhi * sinPsi * sinTheta + cosPhi * cosPsi) * bN -
-			     (sinPhi * cosPsi * sinTheta - cosPhi * sinPsi) * bE
-		     );
-	HAtt(2, 0) = -(
-			     (sinPhi * cosPsi * sinTheta - cosPhi * sinPsi) * bN +
-			     (sinPhi * sinPsi * sinTheta + cosPhi * cosPsi) * bE +
-			     (sinPhi * cosTheta) * bD
-		     );
-	HAtt(2, 1) = cosPhi * tmp1;
-	HAtt(2, 2) = -(
-			     (cosPhi * sinPsi * sinTheta - sinPhi * cosTheta) * bN -
-			     (cosPhi * cosPsi * sinTheta + sinPhi * sinPsi) * bE
-		     );
-
-	// HAccel , HAtt (3-5,:)
-	HAtt(3, 1) = cosTheta;
-	HAtt(4, 0) = -cosPhi * cosTheta;
-	HAtt(4, 1) = sinPhi * sinTheta;
-	HAtt(5, 0) = sinPhi * cosTheta;
-	HAtt(5, 1) = cosPhi * sinTheta;
+	// 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
-	Vector y = zAtt - zAttHat; // residual
 	Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance
 	Matrix K = P * HAtt.transpose() * S.inverse();
 	Vector xCorrect = K * y;
@@ -585,7 +642,7 @@ int KalmanNav::correctAtt()
 
 		if (isnan(val) || isinf(val)) {
 			// abort correction and return
-			printf("[kalman_demo] numerical failure in att correction\n");
+			warnx("numerical failure in att correction\n");
 			// reset P matrix to P0
 			P = P0;
 			return ret_error;
@@ -615,11 +672,8 @@ int KalmanNav::correctAtt()
 	float beta = y.dot(S.inverse() * y);
 
 	if (beta > _faultAtt.get()) {
-		printf("fault in attitude: beta = %8.4f\n", (double)beta);
-		printf("y:\n"); y.print();
-		printf("zMagHat:\n"); zMagHat.print();
-		printf("zMag:\n"); zMag.print();
-		printf("bNav:\n"); bNav.print();
+		warnx("fault in attitude: beta = %8.4f", (double)beta);
+		warnx("y:"); y.print();
 	}
 
 	// update quaternions from euler
@@ -637,10 +691,10 @@ int KalmanNav::correctPos()
 	Vector y(6);
 	y(0) = _gps.vel_n_m_s - vN;
 	y(1) = _gps.vel_e_m_s - vE;
-	y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG;
-	y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG;
-	y(4) = double(_gps.alt) / 1.0e3 - alt;
-	y(5) = double(_sensors.baro_alt_meter) - alt;
+	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
@@ -652,9 +706,9 @@ int KalmanNav::correctPos()
 	for (size_t i = 0; i < xCorrect.getRows(); i++) {
 		float val = xCorrect(i);
 
-		if (isnan(val) || isinf(val)) {
+		if (!isfinite(val)) {
 			// abort correction and return
-			printf("[kalman_demo] numerical failure in gps correction\n");
+			warnx("numerical failure in gps correction\n");
 			// fallback to GPS
 			vN = _gps.vel_n_m_s;
 			vE = _gps.vel_e_m_s;
@@ -674,7 +728,7 @@ int KalmanNav::correctPos()
 	vD += xCorrect(VD);
 	lat += double(xCorrect(LAT));
 	lon += double(xCorrect(LON));
-	alt += double(xCorrect(ALT));
+	alt += xCorrect(ALT);
 
 	// update state covariance
 	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
@@ -683,9 +737,10 @@ int KalmanNav::correctPos()
 	// fault detetcion
 	float beta = y.dot(S.inverse() * y);
 
-	if (beta > _faultPos.get()) {
-		printf("fault in gps: beta = %8.4f\n", (double)beta);
-		printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
+	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))),
@@ -693,6 +748,7 @@ int KalmanNav::correctPos()
 		       double(y(4) / sqrtf(RPos(4, 4))),
 		       double(y(5) / sqrtf(RPos(5, 5))));
 	}
+	counter++;
 
 	return ret_ok;
 }
@@ -720,8 +776,6 @@ void KalmanNav::updateParams()
 	if (noiseMagSq < noiseMin) noiseMagSq = noiseMin;
 
 	RAtt(0, 0) = noiseMagSq; // normalized direction
-	RAtt(1, 1) = noiseMagSq;
-	RAtt(2, 2) = noiseMagSq;
 
 	// accelerometer noise
 	float noiseAccelSq = _rAccel.get() * _rAccel.get();
@@ -729,9 +783,9 @@ void KalmanNav::updateParams()
 	// bound noise to prevent singularities
 	if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin;
 
-	RAtt(3, 3) = noiseAccelSq; // normalized direction
-	RAtt(4, 4) = noiseAccelSq;
-	RAtt(5, 5) = noiseAccelSq;
+	RAtt(1, 1) = noiseAccelSq; // normalized direction
+	RAtt(2, 2) = noiseAccelSq;
+	RAtt(3, 3) = noiseAccelSq;
 
 	// gps noise
 	float R = R0 + float(alt);
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
index c2bf18115..a69bde1a6 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
@@ -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
@@ -47,15 +47,20 @@
 #include <mathlib/mathlib.h>
 #include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
-#include <controllib/block/UOrbSubscription.hpp>
-#include <controllib/block/UOrbPublication.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>
@@ -78,6 +83,9 @@ public:
 	 */
 
 	virtual ~KalmanNav() {};
+
+	math::Quaternion init(float ax, float ay, float az, float mx, float my, float mz);
+
 	/**
 	 * The main callback function for the class
 	 */
@@ -135,6 +143,7 @@ protected:
 	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 */
@@ -151,21 +160,24 @@ protected:
 	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, alt;                   /**< lat, lon, alt, radians */
+	double lat, lon;                   	/**< lat, lon radians */
 	// parameters
-	control::BlockParam<float> _vGyro;      /**< gyro process noise */
-	control::BlockParam<float> _vAccel;     /**< accelerometer process noise  */
-	control::BlockParam<float> _rMag;       /**< magnetometer measurement noise  */
-	control::BlockParam<float> _rGpsVel;    /**< gps velocity measurement noise */
-	control::BlockParam<float> _rGpsPos;    /**< gps position measurement noise */
-	control::BlockParam<float> _rGpsAlt;    /**< gps altitude measurement noise */
-	control::BlockParam<float> _rPressAlt;  /**< press altitude measurement noise */
-	control::BlockParam<float> _rAccel;     /**< accelerometer measurement noise */
-	control::BlockParam<float> _magDip;     /**< magnetic inclination with level */
-	control::BlockParam<float> _magDec;     /**< magnetic declination, clockwise rotation */
-	control::BlockParam<float> _g;          /**< gravitational constant */
-	control::BlockParam<float> _faultPos;   /**< fault detection threshold for position */
-	control::BlockParam<float> _faultAtt;   /**< fault detection threshold for attitude */
+	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;
diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp
index 3b411031a..372b2d162 100644
--- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp
+++ b/src/modules/att_pos_estimator_ekf/kalman_main.cpp
@@ -1,7 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Example User <mail@example.com>
+ *   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
@@ -33,8 +33,10 @@
  ****************************************************************************/
 
 /**
- * @file kalman_demo.cpp
- * Demonstration of control library
+ * @file kalman_main.cpp
+ * Combined attitude / position estimator.
+ *
+ * @author James Goppert
  */
 
 #include <nuttx/config.h>
@@ -44,13 +46,14 @@
 #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 deamon_task;             /**< Handle of deamon task / thread */
+static int daemon_task;             /**< Handle of deamon task / thread */
 
 /**
  * Deamon management function.
@@ -73,7 +76,7 @@ usage(const char *reason)
 	if (reason)
 		fprintf(stderr, "%s\n", reason);
 
-	fprintf(stderr, "usage: kalman_demo {start|stop|status} [-p <additional params>]\n\n");
+	warnx("usage: att_pos_estimator_ekf {start|stop|status} [-p <additional params>]");
 	exit(1);
 }
 
@@ -94,15 +97,16 @@ int att_pos_estimator_ekf_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			printf("kalman_demo already running\n");
+			warnx("already running");
 			/* this is not an error */
 			exit(0);
 		}
 
 		thread_should_exit = false;
-		deamon_task = task_spawn_cmd("kalman_demo",
+
+		daemon_task = task_spawn_cmd("att_pos_estimator_ekf",
 					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 5,
+					 SCHED_PRIORITY_MAX - 30,
 					 4096,
 					 kalman_demo_thread_main,
 					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
@@ -116,13 +120,14 @@ int att_pos_estimator_ekf_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			printf("\tkalman_demo app is running\n");
+			warnx("is running\n");
+			exit(0);
 
 		} else {
-			printf("\tkalman_demo app not started\n");
+			warnx("not started\n");
+			exit(1);
 		}
 
-		exit(0);
 	}
 
 	usage("unrecognized command");
@@ -132,7 +137,7 @@ int att_pos_estimator_ekf_main(int argc, char *argv[])
 int kalman_demo_thread_main(int argc, char *argv[])
 {
 
-	printf("[kalman_demo] starting\n");
+	warnx("starting");
 
 	using namespace math;
 
@@ -144,7 +149,7 @@ int kalman_demo_thread_main(int argc, char *argv[])
 		nav.update();
 	}
 
-	printf("[kalman_demo] exiting.\n");
+	warnx("exiting.");
 
 	thread_running = false;
 
diff --git a/src/modules/att_pos_estimator_ekf/module.mk b/src/modules/att_pos_estimator_ekf/module.mk
index 21b7c9166..8d4a40d95 100644
--- a/src/modules/att_pos_estimator_ekf/module.mk
+++ b/src/modules/att_pos_estimator_ekf/module.mk
@@ -37,9 +37,6 @@
 
 MODULE_COMMAND	= att_pos_estimator_ekf
 
-# XXX this might be intended for the spawned deamon, validate
-MODULE_PRIORITY	 = "SCHED_PRIORITY_MAX-30"
-
 SRCS		= kalman_main.cpp \
 		  KalmanNav.cpp \
 		  params.c
diff --git a/src/modules/att_pos_estimator_ekf/params.c b/src/modules/att_pos_estimator_ekf/params.c
index 50642f067..4af5edead 100644
--- a/src/modules/att_pos_estimator_ekf/params.c
+++ b/src/modules/att_pos_estimator_ekf/params.c
@@ -1,12 +1,45 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
 #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, 1.0f);
-PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
-PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 5.0f);
-PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 5.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);
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 7e3eca085..a70a14fe4 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -57,7 +57,7 @@
 #include <uORB/topics/debug_key_value.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/parameter_update.h>
 #include <drivers/drv_hrt.h>
 
@@ -126,7 +126,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
 		attitude_estimator_ekf_task = task_spawn_cmd("attitude_estimator_ekf",
 					      SCHED_DEFAULT,
 					      SCHED_PRIORITY_MAX - 5,
-					      12400,
+					      14000,
 					      attitude_estimator_ekf_thread_main,
 					      (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
@@ -139,10 +139,12 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			printf("\tattitude_estimator_ekf app is running\n");
+			warnx("running");
+			exit(0);
 
 		} else {
-			printf("\tattitude_estimator_ekf app not started\n");
+			warnx("not started");
+			exit(1);
 		}
 
 		exit(0);
@@ -214,27 +216,26 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	memset(&raw, 0, sizeof(raw));
 	struct vehicle_attitude_s att;
 	memset(&att, 0, sizeof(att));
-	struct vehicle_status_s state;
-	memset(&state, 0, sizeof(state));
+	struct vehicle_control_mode_s control_mode;
+	memset(&control_mode, 0, sizeof(control_mode));
 
 	uint64_t last_data = 0;
 	uint64_t last_measurement = 0;
 
 	/* subscribe to raw data */
 	int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
-	/* rate-limit raw data updates to 200Hz */
-	orb_set_interval(sub_raw, 4);
+	/* 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 param changes */
 	int sub_params = orb_subscribe(ORB_ID(parameter_update));
 
-	/* subscribe to system state*/
-	int sub_state = orb_subscribe(ORB_ID(vehicle_status));
+	/* subscribe to control mode*/
+	int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
 
 	/* advertise attitude */
 	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
 
-
 	int loopcounter = 0;
 	int printcounter = 0;
 
@@ -281,9 +282,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 			/* XXX this is seriously bad - should be an emergency */
 		} else if (ret == 0) {
 			/* check if we're in HIL - not getting sensor data is fine then */
-			orb_copy(ORB_ID(vehicle_status), sub_state, &state);
+			orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);
 
-			if (!state.flag_hil_enabled) {
+			if (!control_mode.flag_system_hil_enabled) {
 				fprintf(stderr,
 					"[att ekf] WARNING: Not getting sensors - sensor app running?\n");
 			}
@@ -307,18 +308,20 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 				orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);
 
 				if (!initialized) {
+					// XXX disabling init for now
+					initialized = true;
 
-					gyro_offsets[0] += raw.gyro_rad_s[0];
-					gyro_offsets[1] += raw.gyro_rad_s[1];
-					gyro_offsets[2] += raw.gyro_rad_s[2];
-					offset_count++;
+					// gyro_offsets[0] += raw.gyro_rad_s[0];
+					// gyro_offsets[1] += raw.gyro_rad_s[1];
+					// gyro_offsets[2] += raw.gyro_rad_s[2];
+					// offset_count++;
 
-					if (hrt_absolute_time() - start_time > 3000000LL) {
-						initialized = true;
-						gyro_offsets[0] /= offset_count;
-						gyro_offsets[1] /= offset_count;
-						gyro_offsets[2] /= offset_count;
-					}
+					// if (hrt_absolute_time() - start_time > 3000000LL) {
+					// 	initialized = true;
+					// 	gyro_offsets[0] /= offset_count;
+					// 	gyro_offsets[1] /= offset_count;
+					// 	gyro_offsets[2] /= offset_count;
+					// }
 
 				} else {
 
@@ -382,7 +385,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					static bool const_initialized = false;
 
 					/* initialize with good values once we have a reasonable dt estimate */
-					if (!const_initialized && dt < 0.05f && dt > 0.005f) {
+					if (!const_initialized && dt < 0.05f && dt > 0.001f) {
 						dt = 0.005f;
 						parameters_update(&ekf_param_handles, &ekf_params);
 
@@ -422,7 +425,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						continue;
 					}
 
-					if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
+					if (last_data > 0 && raw.timestamp - last_data > 12000)
+						printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
 
 					last_data = raw.timestamp;
 
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 7d3812abd..3cfddf28e 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
@@ -44,42 +44,44 @@
 /* Extended Kalman Filter covariances */
 
 /* gyro process noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q0, 1e-4f);
-PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q1, 0.08f);
-PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q2, 0.009f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f);
 /* gyro offsets process noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q3, 0.005f);
-PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q4, 0.0f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f);
 
 /* gyro measurement noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_V2_R0, 0.0008f);
-PARAM_DEFINE_FLOAT(EKF_ATT_V2_R1, 0.8f);
-PARAM_DEFINE_FLOAT(EKF_ATT_V2_R2, 1.0f);
-/* accelerometer measurement noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_V2_R3, 0.0f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f);
+/* accel measurement noise */
+PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f);
+/* mag measurement noise */
+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_OFFS, 0.0f);
-PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f);
-PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f);
+PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f);
+PARAM_DEFINE_FLOAT(ATT_PITCH_OFF3, 0.0f);
+PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f);
 
 int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 {
 	/* PID parameters */
-	h->q0 	=	param_find("EKF_ATT_V2_Q0");
-	h->q1 	=	param_find("EKF_ATT_V2_Q1");
-	h->q2 	=	param_find("EKF_ATT_V2_Q2");
-	h->q3 	=	param_find("EKF_ATT_V2_Q3");
-	h->q4 	=	param_find("EKF_ATT_V2_Q4");
+	h->q0 	=	param_find("EKF_ATT_V3_Q0");
+	h->q1 	=	param_find("EKF_ATT_V3_Q1");
+	h->q2 	=	param_find("EKF_ATT_V3_Q2");
+	h->q3 	=	param_find("EKF_ATT_V3_Q3");
+	h->q4 	=	param_find("EKF_ATT_V3_Q4");
 
-	h->r0 	=	param_find("EKF_ATT_V2_R0");
-	h->r1 	=	param_find("EKF_ATT_V2_R1");
-	h->r2 	=	param_find("EKF_ATT_V2_R2");
-	h->r3 	=	param_find("EKF_ATT_V2_R3");
+	h->r0 	=	param_find("EKF_ATT_V4_R0");
+	h->r1 	=	param_find("EKF_ATT_V4_R1");
+	h->r2 	=	param_find("EKF_ATT_V4_R2");
+	h->r3 	=	param_find("EKF_ATT_V4_R3");
 
-	h->roll_off  =	param_find("ATT_ROLL_OFFS");
-	h->pitch_off =	param_find("ATT_PITCH_OFFS");
-	h->yaw_off   =	param_find("ATT_YAW_OFFS");
+	h->roll_off  =	param_find("ATT_ROLL_OFF3");
+	h->pitch_off =	param_find("ATT_PITCH_OFF3");
+	h->yaw_off   =	param_find("ATT_YAW_OFF3");
 
 	return OK;
 }
diff --git a/src/modules/attitude_estimator_so3/README b/src/modules/attitude_estimator_so3/README
new file mode 100644
index 000000000..02ab66ee4
--- /dev/null
+++ b/src/modules/attitude_estimator_so3/README
@@ -0,0 +1,3 @@
+Synopsis
+
+ nsh> attitude_estimator_so3_comp start
\ No newline at end of file
diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
new file mode 100755
index 000000000..e79726613
--- /dev/null
+++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
@@ -0,0 +1,678 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Hyon Lim <limhyon@gmail.com>
+ *           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 attitude_estimator_so3_main.cpp
+ *
+ * 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.
+ *
+ * Theory of nonlinear complementary filters on the SO(3) is based on [1].
+ * Quaternion realization of [1] is based on [2].
+ * Optmized quaternion update code is based on Sebastian Madgwick's implementation.
+ *
+ * References
+ *  [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
+ *  [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <poll.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/debug_key_value.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/parameter_update.h>
+#include <drivers/drv_hrt.h>
+
+#include <systemlib/systemlib.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+#include "attitude_estimator_so3_params.h"
+#ifdef __cplusplus
+}
+#endif
+
+extern "C" __EXPORT int attitude_estimator_so3_main(int argc, char *argv[]);
+
+static bool thread_should_exit = false;		/**< Deamon exit flag */
+static bool thread_running = false;		/**< Deamon status flag */
+static int attitude_estimator_so3_task;				/**< Handle of deamon task / thread */
+
+//! Auxiliary variables to reduce number of repeated operations
+static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;	/** quaternion of sensor frame relative to auxiliary frame */
+static float dq0 = 0.0f, dq1 = 0.0f, dq2 = 0.0f, dq3 = 0.0f;	/** quaternion of sensor frame relative to auxiliary frame */
+static float gyro_bias[3] = {0.0f, 0.0f, 0.0f}; /** bias estimation */
+static float q0q0, q0q1, q0q2, q0q3;
+static float q1q1, q1q2, q1q3;
+static float q2q2, q2q3;
+static float q3q3;
+static bool bFilterInit = false;
+
+/**
+ * Mainloop of attitude_estimator_so3.
+ */
+int attitude_estimator_so3_thread_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+/* Function prototypes */
+float invSqrt(float number);
+void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, float mz);
+void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt);
+
+static void
+usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+
+	fprintf(stderr, "usage: attitude_estimator_so3 {start|stop|status}\n");
+	exit(1);
+}
+
+/**
+ * The attitude_estimator_so3 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 attitude_estimator_so3_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		attitude_estimator_so3_task = task_spawn_cmd("attitude_estimator_so3",
+					      SCHED_DEFAULT,
+					      SCHED_PRIORITY_MAX - 5,
+					      14000,
+					      attitude_estimator_so3_thread_main,
+					      (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+
+		while (thread_running){
+			usleep(200000);
+		}
+		
+		warnx("stopped");
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("running");
+			exit(0);
+
+		} else {
+			warnx("not started");
+			exit(1);
+		}
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+//---------------------------------------------------------------------------------------------------
+// Fast inverse square-root
+// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
+float invSqrt(float number) 
+{
+    volatile long i;
+    volatile float x, y;
+    volatile const float f = 1.5F;
+
+    x = number * 0.5F;
+    y = number;
+    i = * (( long * ) &y);
+    i = 0x5f375a86 - ( i >> 1 );
+    y = * (( float * ) &i);
+    y = y * ( f - ( x * y * y ) );
+    return y;
+}
+
+//! Using accelerometer, sense the gravity vector.
+//! Using magnetometer, sense yaw.
+void NonlinearSO3AHRSinit(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);
+
+    q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;
+    q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
+    q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
+    q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
+
+    // auxillary variables to reduce number of repeated operations, for 1st pass
+    q0q0 = q0 * q0;
+    q0q1 = q0 * q1;
+    q0q2 = q0 * q2;
+    q0q3 = q0 * q3;
+    q1q1 = q1 * q1;
+    q1q2 = q1 * q2;
+    q1q3 = q1 * q3;
+    q2q2 = q2 * q2;
+    q2q3 = q2 * q3;
+    q3q3 = q3 * q3;
+}
+
+void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt) 
+{
+	float recipNorm;
+	float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
+
+	// Make filter converge to initial solution faster
+	// This function assumes you are in static position.
+	// WARNING : in case air reboot, this can cause problem. But this is very unlikely happen.
+	if(bFilterInit == false) {
+		NonlinearSO3AHRSinit(ax,ay,az,mx,my,mz);
+		bFilterInit = true;
+	}
+        	
+	//! If magnetometer measurement is available, use it.
+	if(!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) {
+		float hx, hy, hz, bx, bz;
+		float halfwx, halfwy, halfwz;
+	
+		// Normalise magnetometer measurement
+		// Will sqrt work better? PX4 system is powerful enough?
+    	recipNorm = invSqrt(mx * mx + my * my + mz * mz);
+    	mx *= recipNorm;
+    	my *= recipNorm;
+    	mz *= recipNorm;
+    
+    	// Reference direction of Earth's magnetic field
+    	hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
+    	hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
+		hz = 2.0f * mx * (q1q3 - q0q2) + 2.0f * my * (q2q3 + q0q1) + 2.0f * mz * (0.5f - q1q1 - q2q2);
+    	bx = sqrt(hx * hx + hy * hy);
+    	bz = hz;
+    
+    	// Estimated direction of magnetic field
+    	halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
+    	halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
+    	halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
+    
+    	// Error is sum of cross product between estimated direction and measured direction of field vectors
+    	halfex += (my * halfwz - mz * halfwy);
+    	halfey += (mz * halfwx - mx * halfwz);
+    	halfez += (mx * halfwy - my * halfwx);
+	}
+
+	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+		float halfvx, halfvy, halfvz;
+	
+		// Normalise accelerometer measurement
+		recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+		ax *= recipNorm;
+		ay *= recipNorm;
+		az *= recipNorm;
+
+		// Estimated direction of gravity and magnetic field
+		halfvx = q1q3 - q0q2;
+		halfvy = q0q1 + q2q3;
+		halfvz = q0q0 - 0.5f + q3q3;
+	
+		// Error is sum of cross product between estimated direction and measured direction of field vectors
+		halfex += ay * halfvz - az * halfvy;
+		halfey += az * halfvx - ax * halfvz;
+		halfez += ax * halfvy - ay * halfvx;
+	}
+
+	// Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
+	if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
+		// Compute and apply integral feedback if enabled
+		if(twoKi > 0.0f) {
+			gyro_bias[0] += twoKi * halfex * dt;	// integral error scaled by Ki
+			gyro_bias[1] += twoKi * halfey * dt;
+			gyro_bias[2] += twoKi * halfez * dt;
+			
+			// apply integral feedback
+			gx += gyro_bias[0];
+			gy += gyro_bias[1];
+			gz += gyro_bias[2];
+		}
+		else {
+			gyro_bias[0] = 0.0f;	// prevent integral windup
+			gyro_bias[1] = 0.0f;
+			gyro_bias[2] = 0.0f;
+		}
+
+		// Apply proportional feedback
+		gx += twoKp * halfex;
+		gy += twoKp * halfey;
+		gz += twoKp * halfez;
+	}
+	
+	//! Integrate rate of change of quaternion
+#if 0
+	gx *= (0.5f * dt);		// pre-multiply common factors
+	gy *= (0.5f * dt);
+	gz *= (0.5f * dt);
+#endif 
+
+	// Time derivative of quaternion. q_dot = 0.5*q\otimes omega.
+	//! q_k = q_{k-1} + dt*\dot{q}
+	//! \dot{q} = 0.5*q \otimes P(\omega)
+	dq0 = 0.5f*(-q1 * gx - q2 * gy - q3 * gz);
+	dq1 = 0.5f*(q0 * gx + q2 * gz - q3 * gy);
+	dq2 = 0.5f*(q0 * gy - q1 * gz + q3 * gx);
+	dq3 = 0.5f*(q0 * gz + q1 * gy - q2 * gx); 
+
+	q0 += dt*dq0;
+	q1 += dt*dq1;
+	q2 += dt*dq2;
+	q3 += dt*dq3;
+	
+	// Normalise quaternion
+	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+	q0 *= recipNorm;
+	q1 *= recipNorm;
+	q2 *= recipNorm;
+	q3 *= recipNorm;
+
+	// Auxiliary variables to avoid repeated arithmetic
+    q0q0 = q0 * q0;
+    q0q1 = q0 * q1;
+    q0q2 = q0 * q2;
+    q0q3 = q0 * q3;
+    q1q1 = q1 * q1;
+    q1q2 = q1 * q2;
+   	q1q3 = q1 * q3;
+    q2q2 = q2 * q2;
+    q2q3 = q2 * q3;
+    q3q3 = q3 * q3;   
+}
+
+/*
+ * Nonliner complementary filter on SO(3), attitude estimator main function.
+ *
+ * Estimates the attitude once started.
+ *
+ * @param argc number of commandline arguments (plus command name)
+ * @param argv strings containing the arguments
+ */
+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;
+	
+	/* output euler angles */
+	float euler[3] = {0.0f, 0.0f, 0.0f};
+	
+	/* Initialization */
+	float Rot_matrix[9] = {1.f,  0.0f,  0.0f, 0.0f,  1.f,  0.0f, 0.0f,  0.0f,  1.f };		/**< init: identity matrix */
+	float acc[3] = {0.0f, 0.0f, 0.0f};
+	float gyro[3] = {0.0f, 0.0f, 0.0f};
+	float mag[3] = {0.0f, 0.0f, 0.0f};
+
+	warnx("main thread started");
+
+	struct sensor_combined_s raw;
+	memset(&raw, 0, sizeof(raw));
+
+	//! Initialize attitude vehicle uORB message.
+	struct vehicle_attitude_s att;
+	memset(&att, 0, sizeof(att));
+
+	struct vehicle_control_mode_s control_mode;
+	memset(&control_mode, 0, sizeof(control_mode));
+
+	uint64_t last_data = 0;
+	uint64_t last_measurement = 0;
+
+	/* 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 param changes */
+	int sub_params = orb_subscribe(ORB_ID(parameter_update));
+
+	/* subscribe to control mode */
+	int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
+
+	/* advertise attitude */
+	//orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
+	//orb_advert_t att_pub = -1;
+	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;
+	struct attitude_estimator_so3_param_handles so3_comp_param_handles;
+
+	/* initialize parameter handles */
+	parameters_init(&so3_comp_param_handles);
+	parameters_update(&so3_comp_param_handles, &so3_comp_params);
+
+	uint64_t start_time = hrt_absolute_time();
+	bool initialized = false;
+	bool state_initialized = false;
+
+	float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
+	unsigned offset_count = 0;
+
+	/* register the perf counter */
+	perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_so3");
+
+	/* Main loop*/
+	while (!thread_should_exit) {
+
+		struct pollfd fds[2];
+		fds[0].fd = sub_raw;
+		fds[0].events = POLLIN;
+		fds[1].fd = sub_params;
+		fds[1].events = POLLIN;
+		int ret = poll(fds, 2, 1000);
+
+		if (ret < 0) {
+			/* XXX this is seriously bad - should be an emergency */
+		} else if (ret == 0) {
+			/* check if we're in HIL - not getting sensor data is fine then */
+			orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);
+
+			if (!control_mode.flag_system_hil_enabled) {
+				warnx("WARNING: Not getting sensors - sensor app running?");
+			}
+		} else {
+			/* only update parameters if they changed */
+			if (fds[1].revents & POLLIN) {
+				/* read from param to clear updated flag */
+				struct parameter_update_s update;
+				orb_copy(ORB_ID(parameter_update), sub_params, &update);
+
+				/* update parameters */
+				parameters_update(&so3_comp_param_handles, &so3_comp_params);
+			}
+
+			/* only run filter if sensor values changed */
+			if (fds[0].revents & POLLIN) {
+
+				/* get latest measurements */
+				orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);
+
+				if (!initialized) {
+
+					gyro_offsets[0] += raw.gyro_rad_s[0];
+					gyro_offsets[1] += raw.gyro_rad_s[1];
+					gyro_offsets[2] += raw.gyro_rad_s[2];
+					offset_count++;
+
+					if (hrt_absolute_time() > start_time + 3000000l) {
+						initialized = true;
+						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]);
+					}
+
+				} else {
+
+					perf_begin(so3_comp_loop_perf);
+
+					/* 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]);
+						sensor_last_timestamp[0] = raw.timestamp;
+					}
+
+					gyro[0] = raw.gyro_rad_s[0] - gyro_offsets[0];
+					gyro[1] = raw.gyro_rad_s[1] - gyro_offsets[1];
+					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;
+					}
+
+					acc[0] = raw.accelerometer_m_s2[0];
+					acc[1] = raw.accelerometer_m_s2[1];
+					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;
+					}
+
+					mag[0] = raw.magnetometer_ga[0];
+					mag[1] = raw.magnetometer_ga[1];
+					mag[2] = raw.magnetometer_ga[2];
+
+					/* initialize with good values once we have a reasonable dt estimate */
+					if (!state_initialized && dt < 0.05f && dt > 0.001f) {
+						state_initialized = true;
+						warnx("state initialized");
+					}
+
+					/* do not execute the filter if not initialized */
+					if (!state_initialized) {
+						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],
+										-acc[0], -acc[1], -acc[2],
+										mag[0], mag[1], mag[2],
+										so3_comp_params.Kp,
+										so3_comp_params.Ki, 
+										dt);
+
+					// Convert q->R, This R converts inertial frame to body frame.
+					Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11
+					Rot_matrix[1] = 2.f * (q1*q2 + q0*q3);	// 12
+					Rot_matrix[2] = 2.f * (q1*q3 - q0*q2);	// 13
+					Rot_matrix[3] = 2.f * (q1*q2 - q0*q3);	// 21
+					Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22
+					Rot_matrix[5] = 2.f * (q2*q3 + q0*q1);	// 23
+					Rot_matrix[6] = 2.f * (q1*q3 + q0*q2);	// 31
+					Rot_matrix[7] = 2.f * (q2*q3 - q0*q1);	// 32
+					Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33
+
+					//1-2-3 Representation.
+					//Equation (290) 
+					//Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors, James Diebel.
+					// Existing PX4 EKF code was generated by MATLAB which uses coloum major order matrix.
+					euler[0] = atan2f(Rot_matrix[5], Rot_matrix[8]);	//! Roll
+					euler[1] = -asinf(Rot_matrix[2]);	//! Pitch
+					euler[2] = atan2f(Rot_matrix[1], Rot_matrix[0]);		//! Yaw
+					
+					/* swap values for next iteration, check for fatal inputs */
+					if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
+						// Publish only finite euler angles
+						att.roll = euler[0] - so3_comp_params.roll_off;
+						att.pitch = euler[1] - so3_comp_params.pitch_off;
+						att.yaw = euler[2] - so3_comp_params.yaw_off;
+					} else {
+						/* 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]);
+						// Don't publish anything
+						continue;
+					}
+
+					if (last_data > 0 && raw.timestamp > last_data + 12000) {
+						warnx("sensor data missed");
+					}
+
+					last_data = raw.timestamp;
+
+					/* send out */
+					att.timestamp = raw.timestamp;
+					
+					// Quaternion
+					att.q[0] = q0;
+					att.q[1] = q1;
+					att.q[2] = q2;
+					att.q[3] = q3;
+					att.q_valid = true;
+
+					// Euler angle rate. But it needs to be investigated again.
+					/*
+					att.rollspeed = 2.0f*(-q1*dq0 + q0*dq1 - q3*dq2 + q2*dq3);
+					att.pitchspeed = 2.0f*(-q2*dq0 + q3*dq1 + q0*dq2 - q1*dq3);
+					att.yawspeed = 2.0f*(-q3*dq0 -q2*dq1 + q1*dq2 + q0*dq3);
+					*/
+					att.rollspeed = gyro[0];
+					att.pitchspeed = gyro[1];
+					att.yawspeed = gyro[2];
+
+					att.rollacc = 0;
+					att.pitchacc = 0;
+					att.yawacc = 0;
+
+					/* TODO: Bias estimation required */
+					memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets));
+
+					/* copy rotation matrix */
+					memcpy(&att.R, Rot_matrix, sizeof(float)*9);
+					att.R_valid = true;
+					
+					// Publish
+					if (att_pub > 0) {
+						orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
+					} else {
+						warnx("NaN in roll/pitch/yaw estimate!");
+						 orb_advertise(ORB_ID(vehicle_attitude), &att);
+					}
+
+					perf_end(so3_comp_loop_perf);
+				}
+			}
+		}
+
+		loopcounter++;
+	}
+
+	thread_running = false;
+
+	return 0;
+}
diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c b/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c
new file mode 100755
index 000000000..0c8d522b4
--- /dev/null
+++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c
@@ -0,0 +1,86 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Hyon Lim <limhyon@gmail.com>
+ *           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 attitude_estimator_so3_params.c
+ *
+ * Parameters for nonlinear complementary filters on the SO(3).
+ */
+
+#include "attitude_estimator_so3_params.h"
+
+/* This is filter gain for nonlinear SO3 complementary filter */
+/* NOTE : How to tune the gain? First of all, stick with this default gain. And let the quad in stable place.
+   Log the steady state reponse of filter. If it is too slow, increase SO3_COMP_KP.
+   If you are flying from ground to high altitude in short amount of time, please increase SO3_COMP_KI which
+   will compensate gyro bias which depends on temperature and vibration of your vehicle */
+PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); //! This parameter will give you about 15 seconds convergence time.
+                                       //! You can set this gain higher if you want more fast response.
+                                       //! But note that higher gain will give you also higher overshoot.
+PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.05f); //! This gain will incorporate slow time-varying bias (e.g., temperature change)
+					//! This gain is depend on your vehicle status.
+
+/* offsets in roll, pitch and yaw of sensor plane and body */
+PARAM_DEFINE_FLOAT(SO3_ROLL_OFFS, 0.0f);
+PARAM_DEFINE_FLOAT(SO3_PITCH_OFFS, 0.0f);
+PARAM_DEFINE_FLOAT(SO3_YAW_OFFS, 0.0f);
+
+int parameters_init(struct attitude_estimator_so3_param_handles *h)
+{
+	/* Filter gain parameters */
+	h->Kp = 	param_find("SO3_COMP_KP");
+	h->Ki = 	param_find("SO3_COMP_KI");
+
+	/* Attitude offset (WARNING: Do not change if you do not know what exactly this variable wil lchange) */
+	h->roll_off  =	param_find("SO3_ROLL_OFFS");
+	h->pitch_off =	param_find("SO3_PITCH_OFFS");
+	h->yaw_off   =	param_find("SO3_YAW_OFFS");
+
+	return OK;
+}
+
+int parameters_update(const struct attitude_estimator_so3_param_handles *h, struct attitude_estimator_so3_params *p)
+{
+	/* Update filter gain */
+	param_get(h->Kp, &(p->Kp));
+	param_get(h->Ki, &(p->Ki));
+
+	/* Update attitude offset */
+	param_get(h->roll_off, &(p->roll_off));
+	param_get(h->pitch_off, &(p->pitch_off));
+	param_get(h->yaw_off, &(p->yaw_off));
+
+	return OK;
+}
diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h b/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h
new file mode 100755
index 000000000..dfb4cad05
--- /dev/null
+++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h
@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Hyon Lim <limhyon@gmail.com>
+ *           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 attitude_estimator_so3_params.h
+ *
+ * Parameters for nonlinear complementary filters on the SO(3).
+ */
+
+#include <systemlib/param/param.h>
+
+struct attitude_estimator_so3_params {
+	float Kp;
+	float Ki;
+	float roll_off;
+	float pitch_off;
+	float yaw_off;
+};
+
+struct attitude_estimator_so3_param_handles {
+	param_t Kp, Ki;
+	param_t roll_off, pitch_off, yaw_off;
+};
+
+/**
+ * Initialize all parameter handles and values
+ *
+ */
+int parameters_init(struct attitude_estimator_so3_param_handles *h);
+
+/**
+ * Update all parameters
+ *
+ */
+int parameters_update(const struct attitude_estimator_so3_param_handles *h, struct attitude_estimator_so3_params *p);
diff --git a/src/modules/attitude_estimator_so3/module.mk b/src/modules/attitude_estimator_so3/module.mk
new file mode 100644
index 000000000..e29bb16a6
--- /dev/null
+++ b/src/modules/attitude_estimator_so3/module.mk
@@ -0,0 +1,8 @@
+#
+# Attitude estimator (Nonlinear SO(3) complementary Filter)
+#
+
+MODULE_COMMAND	 = attitude_estimator_so3
+
+SRCS		 = attitude_estimator_so3_main.cpp \
+		   attitude_estimator_so3_params.c
diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.c
deleted file mode 100644
index d79dd93dd..000000000
--- a/src/modules/commander/accelerometer_calibration.c
+++ /dev/null
@@ -1,414 +0,0 @@
-/*
- * accelerometer_calibration.c
- *
- *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
- *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
- *
- * Transform acceleration vector to true orientation and scale
- *
- * * * * Model * * *
- * accel_corr = accel_T * (accel_raw - accel_offs)
- *
- * accel_corr[3] - fully corrected acceleration vector in body frame
- * accel_T[3][3] - accelerometers transform matrix, rotation and scaling transform
- * accel_raw[3]  - raw acceleration vector
- * accel_offs[3] - acceleration offset vector
- *
- * * * * Calibration * * *
- *
- * Reference vectors
- * accel_corr_ref[6][3] = [  g  0  0 ]     // nose up
- *                        | -g  0  0 |     // nose down
- *                        |  0  g  0 |     // left side down
- *                        |  0 -g  0 |     // right side down
- *                        |  0  0  g |     // on back
- *                        [  0  0 -g ]     // level
- * accel_raw_ref[6][3]
- *
- * accel_corr_ref[i] = accel_T * (accel_raw_ref[i] - accel_offs), i = 0...5
- *
- * 6 reference vectors * 3 axes = 18 equations
- * 9 (accel_T) + 3 (accel_offs) = 12 unknown constants
- *
- * Find accel_offs
- *
- * accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2
- *
- *
- * Find accel_T
- *
- * 9 unknown constants
- * need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations
- *
- * accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2
- *
- * Solve separate system for each row of accel_T:
- *
- * accel_corr_ref[j*2][i] = accel_T[i] * (accel_raw_ref[j*2] - accel_offs), j = 0...2
- *
- * A * x = b
- *
- * x = [ accel_T[0][i] ]
- *     | accel_T[1][i] |
- *     [ accel_T[2][i] ]
- *
- * b = [ accel_corr_ref[0][i] ]	// One measurement per axis is enough
- *     | accel_corr_ref[2][i] |
- *     [ accel_corr_ref[4][i] ]
- *
- * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0...2
- *
- * Matrix A is common for all three systems:
- * A = [ a[0][0]  a[0][1]  a[0][2] ]
- *     | a[2][0]  a[2][1]  a[2][2] |
- *     [ a[4][0]  a[4][1]  a[4][2] ]
- *
- * x = A^-1 * b
- *
- * accel_T = A^-1 * g
- * g = 9.80665
- */
-
-#include "accelerometer_calibration.h"
-
-#include <poll.h>
-#include <drivers/drv_hrt.h>
-#include <uORB/topics/sensor_combined.h>
-#include <drivers/drv_accel.h>
-#include <systemlib/conversions.h>
-#include <mavlink/mavlink_log.h>
-
-void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd);
-int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]);
-int detect_orientation(int mavlink_fd, int sub_sensor_combined);
-int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num);
-int mat_invert3(float src[3][3], float dst[3][3]);
-int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g);
-
-void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd) {
-	/* announce change */
-	mavlink_log_info(mavlink_fd, "accel calibration started");
-	/* set to accel calibration mode */
-	status->flag_preflight_accel_calibration = true;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	/* measure and calculate offsets & scales */
-	float accel_offs[3];
-	float accel_scale[3];
-	int res = do_accel_calibration_mesurements(mavlink_fd, accel_offs, accel_scale);
-
-	if (res == OK) {
-		/* measurements complete successfully, set parameters */
-		if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0]))
-			|| param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1]))
-			|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2]))
-			|| param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0]))
-			|| param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1]))
-			|| param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) {
-			mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed");
-		}
-
-		int fd = open(ACCEL_DEVICE_PATH, 0);
-		struct accel_scale ascale = {
-			accel_offs[0],
-			accel_scale[0],
-			accel_offs[1],
-			accel_scale[1],
-			accel_offs[2],
-			accel_scale[2],
-		};
-
-		if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
-			warn("WARNING: failed to set scale / offsets for accel");
-
-		close(fd);
-
-		/* auto-save to EEPROM */
-		int save_ret = param_save_default();
-
-		if (save_ret != 0) {
-			warn("WARNING: auto-save of params to storage failed");
-		}
-
-		mavlink_log_info(mavlink_fd, "accel calibration done");
-		tune_confirm();
-		sleep(2);
-		tune_confirm();
-		sleep(2);
-		/* third beep by cal end routine */
-	} else {
-		/* measurements error */
-		mavlink_log_info(mavlink_fd, "accel calibration aborted");
-		tune_error();
-		sleep(2);
-	}
-
-	/* exit accel calibration mode */
-	status->flag_preflight_accel_calibration = false;
-	state_machine_publish(status_pub, status, mavlink_fd);
-}
-
-int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) {
-	const int samples_num = 2500;
-	float accel_ref[6][3];
-	bool data_collected[6] = { false, false, false, false, false, false };
-	const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" };
-
-	/* reset existing calibration */
-	int fd = open(ACCEL_DEVICE_PATH, 0);
-	struct accel_scale ascale_null = {
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-	};
-	int ioctl_res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null);
-	close(fd);
-
-	if (OK != ioctl_res) {
-		warn("ERROR: failed to set scale / offsets for accel");
-		return ERROR;
-	}
-
-	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
-	while (true) {
-		bool done = true;
-		char str[80];
-		int str_ptr;
-		str_ptr = sprintf(str, "keep vehicle still:");
-		for (int i = 0; i < 6; i++) {
-			if (!data_collected[i]) {
-				str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]);
-				done = false;
-			}
-		}
-		if (done)
-			break;
-		mavlink_log_info(mavlink_fd, str);
-
-		int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
-		if (orient < 0)
-			return ERROR;
-
-		sprintf(str, "meas started: %s", orientation_strs[orient]);
-		mavlink_log_info(mavlink_fd, str);
-		read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
-		str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], accel_ref[orient][0], accel_ref[orient][1], accel_ref[orient][2]);
-		mavlink_log_info(mavlink_fd, str);
-		data_collected[orient] = true;
-		tune_confirm();
-	}
-	close(sensor_combined_sub);
-
-	/* calculate offsets and rotation+scale matrix */
-	float accel_T[3][3];
-	int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
-	if (res != 0) {
-		mavlink_log_info(mavlink_fd, "ERROR: calibration values calc error");
-		return ERROR;
-	}
-
-	/* convert accel transform matrix to scales,
-	 * rotation part of transform matrix is not used by now
-	 */
-	for (int i = 0; i < 3; i++) {
-		accel_scale[i] = accel_T[i][i];
-	}
-
-	return OK;
-}
-
-/*
- * Wait for vehicle become still and detect it's orientation.
- *
- * @return 0..5 according to orientation when vehicle is still and ready for measurements,
- * ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2
- */
-int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
-	struct sensor_combined_s sensor;
-	/* exponential moving average of accel */
-	float accel_ema[3] = { 0.0f, 0.0f, 0.0f };
-	/* max-hold dispersion of accel */
-	float accel_disp[3] = { 0.0f, 0.0f, 0.0f };
-	/* EMA time constant in seconds*/
-	float ema_len = 0.2f;
-	/* set "still" threshold to 0.1 m/s^2 */
-	float still_thr2 = pow(0.1f, 2);
-	/* set accel error threshold to 5m/s^2 */
-	float accel_err_thr = 5.0f;
-	/* still time required in us */
-	int64_t still_time = 2000000;
-	struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
-
-	hrt_abstime t_start = hrt_absolute_time();
-	/* set timeout to 30s */
-	hrt_abstime timeout = 30000000;
-	hrt_abstime t_timeout = t_start + timeout;
-	hrt_abstime t = t_start;
-	hrt_abstime t_prev = t_start;
-	hrt_abstime t_still = 0;
-	while (true) {
-		/* wait blocking for new data */
-		int poll_ret = poll(fds, 1, 1000);
-		if (poll_ret) {
-			orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &sensor);
-			t = hrt_absolute_time();
-			float dt = (t - t_prev) / 1000000.0f;
-			t_prev = t;
-			float w = dt / ema_len;
-			for (int i = 0; i < 3; i++) {
-				accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w;
-				float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i];
-				d = d * d;
-				accel_disp[i] = accel_disp[i] * (1.0f - w);
-				if (d > accel_disp[i])
-					accel_disp[i] = d;
-			}
-			/* still detector with hysteresis */
-			if (  accel_disp[0] < still_thr2 &&
-				  accel_disp[1] < still_thr2 &&
-				  accel_disp[2] < still_thr2 ) {
-				/* is still now */
-				if (t_still == 0) {
-					/* first time */
-					mavlink_log_info(mavlink_fd, "still...");
-					t_still = t;
-					t_timeout = t + timeout;
-				} else {
-					/* still since t_still */
-					if ((int64_t) t - (int64_t) t_still > still_time) {
-						/* vehicle is still, exit from the loop to detection of its orientation */
-						break;
-					}
-				}
-			} else if ( accel_disp[0] > still_thr2 * 2.0f ||
-					    accel_disp[1] > still_thr2 * 2.0f ||
-					    accel_disp[2] > still_thr2 * 2.0f) {
-				/* not still, reset still start time */
-				if (t_still != 0) {
-					mavlink_log_info(mavlink_fd, "moving...");
-					t_still = 0;
-				}
-			}
-		} else if (poll_ret == 0) {
-			/* any poll failure for 1s is a reason to abort */
-			mavlink_log_info(mavlink_fd, "ERROR: poll failure");
-			return -3;
-		}
-		if (t > t_timeout) {
-			mavlink_log_info(mavlink_fd, "ERROR: timeout");
-			return -1;
-		}
-	}
-
-	if (  fabs(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
-		  fabs(accel_ema[1]) < accel_err_thr &&
-		  fabs(accel_ema[2]) < accel_err_thr  )
-		return 0;	// [ g, 0, 0 ]
-	if (  fabs(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
-		  fabs(accel_ema[1]) < accel_err_thr &&
-		  fabs(accel_ema[2]) < accel_err_thr  )
-		return 1;	// [ -g, 0, 0 ]
-	if (  fabs(accel_ema[0]) < accel_err_thr &&
-		  fabs(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
-		  fabs(accel_ema[2]) < accel_err_thr  )
-		return 2;	// [ 0, g, 0 ]
-	if (  fabs(accel_ema[0]) < accel_err_thr &&
-		  fabs(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
-		  fabs(accel_ema[2]) < accel_err_thr  )
-		return 3;	// [ 0, -g, 0 ]
-	if (  fabs(accel_ema[0]) < accel_err_thr &&
-		  fabs(accel_ema[1]) < accel_err_thr &&
-		  fabs(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr  )
-		return 4;	// [ 0, 0, g ]
-	if (  fabs(accel_ema[0]) < accel_err_thr &&
-		  fabs(accel_ema[1]) < accel_err_thr &&
-		  fabs(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr  )
-		return 5;	// [ 0, 0, -g ]
-
-	mavlink_log_info(mavlink_fd, "ERROR: invalid orientation");
-
-	return -2;	// Can't detect orientation
-}
-
-/*
- * Read specified number of accelerometer samples, calculate average and dispersion.
- */
-int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) {
-	struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } };
-	int count = 0;
-	float accel_sum[3] = { 0.0f, 0.0f, 0.0f };
-
-	while (count < samples_num) {
-		int poll_ret = poll(fds, 1, 1000);
-		if (poll_ret == 1) {
-			struct sensor_combined_s sensor;
-			orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
-			for (int i = 0; i < 3; i++)
-				accel_sum[i] += sensor.accelerometer_m_s2[i];
-			count++;
-		} else {
-			return ERROR;
-		}
-	}
-
-	for (int i = 0; i < 3; i++) {
-		accel_avg[i] = accel_sum[i] / count;
-	}
-
-	return OK;
-}
-
-int mat_invert3(float src[3][3], float dst[3][3]) {
-	float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) -
-			    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.0)
-		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;
-	dst[2][0] = (src[1][0] * src[2][1] - src[1][1] * src[2][0]) / det;
-	dst[0][1] = (src[0][2] * src[2][1] - src[0][1] * src[2][2]) / det;
-	dst[1][1] = (src[0][0] * src[2][2] - src[0][2] * src[2][0]) / det;
-	dst[2][1] = (src[0][1] * src[2][0] - src[0][0] * src[2][1]) / det;
-	dst[0][2] = (src[0][1] * src[1][2] - src[0][2] * src[1][1]) / det;
-	dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det;
-	dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det;
-
-	return OK;
-}
-
-int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) {
-	/* calculate offsets */
-	for (int i = 0; i < 3; i++) {
-		accel_offs[i] = (accel_ref[i * 2][i] + accel_ref[i * 2 + 1][i]) / 2;
-	}
-
-	/* fill matrix A for linear equations system*/
-	float mat_A[3][3];
-	memset(mat_A, 0, sizeof(mat_A));
-	for (int i = 0; i < 3; i++) {
-		for (int j = 0; j < 3; j++) {
-			float a = accel_ref[i * 2][j] - accel_offs[j];
-			mat_A[i][j] = a;
-		}
-	}
-
-	/* calculate inverse matrix for A */
-	float mat_A_inv[3][3];
-	if (mat_invert3(mat_A, mat_A_inv) != OK)
-		return ERROR;
-
-	/* copy results to accel_T */
-	for (int i = 0; i < 3; i++) {
-		for (int j = 0; j < 3; j++) {
-			/* simplify matrices mult because b has only one non-zero element == g at index i */
-			accel_T[j][i] = mat_A_inv[j][i] * g;
-		}
-	}
-
-	return OK;
-}
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
new file mode 100644
index 000000000..5eeca5a1a
--- /dev/null
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -0,0 +1,566 @@
+/****************************************************************************
+ *
+ *   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 accelerometer_calibration.cpp
+ *
+ * Implementation of accelerometer calibration.
+ *
+ * Transform acceleration vector to true orientation, scale and offset
+ *
+ * ===== Model =====
+ * accel_corr = accel_T * (accel_raw - accel_offs)
+ *
+ * accel_corr[3] - fully corrected acceleration vector in body frame
+ * accel_T[3][3] - accelerometers transform matrix, rotation and scaling transform
+ * accel_raw[3]  - raw acceleration vector
+ * accel_offs[3] - acceleration offset vector
+ *
+ * ===== Calibration =====
+ *
+ * Reference vectors
+ * accel_corr_ref[6][3] = [  g  0  0 ]     // nose up
+ *                        | -g  0  0 |     // nose down
+ *                        |  0  g  0 |     // left side down
+ *                        |  0 -g  0 |     // right side down
+ *                        |  0  0  g |     // on back
+ *                        [  0  0 -g ]     // level
+ * accel_raw_ref[6][3]
+ *
+ * accel_corr_ref[i] = accel_T * (accel_raw_ref[i] - accel_offs), i = 0...5
+ *
+ * 6 reference vectors * 3 axes = 18 equations
+ * 9 (accel_T) + 3 (accel_offs) = 12 unknown constants
+ *
+ * Find accel_offs
+ *
+ * accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2
+ *
+ * Find accel_T
+ *
+ * 9 unknown constants
+ * need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations
+ *
+ * accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2
+ *
+ * Solve separate system for each row of accel_T:
+ *
+ * accel_corr_ref[j*2][i] = accel_T[i] * (accel_raw_ref[j*2] - accel_offs), j = 0...2
+ *
+ * A * x = b
+ *
+ * x = [ accel_T[0][i] ]
+ *     | accel_T[1][i] |
+ *     [ accel_T[2][i] ]
+ *
+ * b = [ accel_corr_ref[0][i] ]	// One measurement per axis is enough
+ *     | accel_corr_ref[2][i] |
+ *     [ accel_corr_ref[4][i] ]
+ *
+ * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0...2
+ *
+ * Matrix A is common for all three systems:
+ * A = [ a[0][0]  a[0][1]  a[0][2] ]
+ *     | a[2][0]  a[2][1]  a[2][2] |
+ *     [ a[4][0]  a[4][1]  a[4][2] ]
+ *
+ * x = A^-1 * b
+ *
+ * accel_T = A^-1 * g
+ * g = 9.80665
+ *
+ * ===== Rotation =====
+ *
+ * Calibrating using model:
+ * accel_corr = accel_T_r * (rot * accel_raw - accel_offs_r)
+ *
+ * Actual correction:
+ * accel_corr = rot * accel_T * (accel_raw - accel_offs)
+ *
+ * Known: accel_T_r, accel_offs_r, rot
+ * Unknown: accel_T, accel_offs
+ *
+ * Solution:
+ * accel_T_r * (rot * accel_raw - accel_offs_r) = rot * accel_T * (accel_raw - accel_offs)
+ * rot^-1 * accel_T_r * (rot * accel_raw - accel_offs_r) = accel_T * (accel_raw - accel_offs)
+ * rot^-1 * accel_T_r * rot * accel_raw - rot^-1 * accel_T_r * accel_offs_r = accel_T * accel_raw - accel_T * accel_offs)
+ * => accel_T = rot^-1 * accel_T_r * rot
+ * => accel_offs = rot^-1 * accel_offs_r
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include "accelerometer_calibration.h"
+#include "calibration_messages.h"
+#include "commander_helper.h"
+
+#include <unistd.h>
+#include <stdio.h>
+#include <poll.h>
+#include <fcntl.h>
+#include <sys/prctl.h>
+#include <math.h>
+#include <mathlib/mathlib.h>
+#include <string.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/sensor_combined.h>
+#include <drivers/drv_accel.h>
+#include <geo/geo.h>
+#include <conversion/rotation.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <mavlink/mavlink_log.h>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+static const char *sensor_name = "accel";
+
+int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]);
+int detect_orientation(int mavlink_fd, int sub_sensor_combined);
+int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num);
+int mat_invert3(float src[3][3], float dst[3][3]);
+int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g);
+
+int do_accel_calibration(int mavlink_fd)
+{
+	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
+
+	struct accel_scale accel_scale = {
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+	};
+
+	int res = OK;
+
+	/* reset all offsets to zero and all scales to one */
+	int fd = open(ACCEL_DEVICE_PATH, 0);
+	res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
+	close(fd);
+
+	if (res != OK) {
+		mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
+	}
+
+	float accel_offs[3];
+	float accel_T[3][3];
+
+	if (res == OK) {
+		/* measure and calculate offsets & scales */
+		res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T);
+	}
+
+	if (res == OK) {
+		/* measurements completed successfully, rotate calibration values */
+		param_t board_rotation_h = param_find("SENS_BOARD_ROT");
+		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);
+		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;
+
+		accel_scale.x_offset = accel_offs_rotated(0);
+		accel_scale.x_scale = accel_T_rotated(0, 0);
+		accel_scale.y_offset = accel_offs_rotated(1);
+		accel_scale.y_scale = accel_T_rotated(1, 1);
+		accel_scale.z_offset = accel_offs_rotated(2);
+		accel_scale.z_scale = accel_T_rotated(2, 2);
+
+		/* set parameters */
+		if (param_set(param_find("SENS_ACC_XOFF"), &(accel_scale.x_offset))
+		    || param_set(param_find("SENS_ACC_YOFF"), &(accel_scale.y_offset))
+		    || param_set(param_find("SENS_ACC_ZOFF"), &(accel_scale.z_offset))
+		    || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale.x_scale))
+		    || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale.y_scale))
+		    || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale.z_scale))) {
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
+			res = ERROR;
+		}
+	}
+
+	if (res == OK) {
+		/* apply new scaling and offsets */
+		int fd = open(ACCEL_DEVICE_PATH, 0);
+		res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
+		close(fd);
+
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
+		}
+	}
+
+	if (res == OK) {
+		/* auto-save to EEPROM */
+		res = param_save_default();
+
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
+		}
+	}
+
+	if (res == OK) {
+		mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
+
+	} else {
+		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+	}
+
+	return res;
+}
+
+int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3])
+{
+	const int samples_num = 2500;
+	float accel_ref[6][3];
+	bool data_collected[6] = { false, false, false, false, false, false };
+	const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" };
+
+	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
+
+	unsigned done_count = 0;
+	int res = OK;
+
+	while (true) {
+		bool done = true;
+		unsigned old_done_count = done_count;
+		done_count = 0;
+
+		for (int i = 0; i < 6; i++) {
+			if (data_collected[i]) {
+				done_count++;
+
+			} else {
+				done = false;
+			}
+		}
+
+		if (old_done_count != done_count)
+			mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count);
+
+		if (done)
+			break;
+
+		mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s",
+				 (!data_collected[0]) ? "x+ " : "",
+				 (!data_collected[1]) ? "x- " : "",
+				 (!data_collected[2]) ? "y+ " : "",
+				 (!data_collected[3]) ? "y- " : "",
+				 (!data_collected[4]) ? "z+ " : "",
+				 (!data_collected[5]) ? "z- " : "");
+
+		int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
+
+		if (orient < 0) {
+			res = ERROR;
+			break;
+		}
+
+		if (data_collected[orient]) {
+			mavlink_log_info(mavlink_fd, "%s done, rotate to a different axis", orientation_strs[orient]);
+			continue;
+		}
+
+		mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]);
+		read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
+		mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient],
+				 (double)accel_ref[orient][0],
+				 (double)accel_ref[orient][1],
+				 (double)accel_ref[orient][2]);
+
+		data_collected[orient] = true;
+		tune_neutral();
+	}
+
+	close(sensor_combined_sub);
+
+	if (res == OK) {
+		/* calculate offsets and transform matrix */
+		res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
+
+		if (res != OK) {
+			mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error");
+		}
+	}
+
+	return res;
+}
+
+/*
+ * Wait for vehicle become still and detect it's orientation.
+ *
+ * @return 0..5 according to orientation when vehicle is still and ready for measurements,
+ * ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2
+ */
+int detect_orientation(int mavlink_fd, int sub_sensor_combined)
+{
+	struct sensor_combined_s sensor;
+	/* exponential moving average of accel */
+	float accel_ema[3] = { 0.0f, 0.0f, 0.0f };
+	/* max-hold dispersion of accel */
+	float accel_disp[3] = { 0.0f, 0.0f, 0.0f };
+	/* EMA time constant in seconds*/
+	float ema_len = 0.5f;
+	/* set "still" threshold to 0.25 m/s^2 */
+	float still_thr2 = pow(0.25f, 2);
+	/* set accel error threshold to 5m/s^2 */
+	float accel_err_thr = 5.0f;
+	/* still time required in us */
+	hrt_abstime still_time = 2000000;
+	struct pollfd fds[1];
+	fds[0].fd = sub_sensor_combined;
+	fds[0].events = POLLIN;
+
+	hrt_abstime t_start = hrt_absolute_time();
+	/* set timeout to 30s */
+	hrt_abstime timeout = 30000000;
+	hrt_abstime t_timeout = t_start + timeout;
+	hrt_abstime t = t_start;
+	hrt_abstime t_prev = t_start;
+	hrt_abstime t_still = 0;
+
+	unsigned poll_errcount = 0;
+
+	while (true) {
+		/* wait blocking for new data */
+		int poll_ret = poll(fds, 1, 1000);
+
+		if (poll_ret) {
+			orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &sensor);
+			t = hrt_absolute_time();
+			float dt = (t - t_prev) / 1000000.0f;
+			t_prev = t;
+			float w = dt / ema_len;
+
+			for (int i = 0; i < 3; i++) {
+				float d = sensor.accelerometer_m_s2[i] - accel_ema[i];
+				accel_ema[i] += d * w;
+				d = d * d;
+				accel_disp[i] = accel_disp[i] * (1.0f - w);
+
+				if (d > still_thr2 * 8.0f)
+					d = still_thr2 * 8.0f;
+
+				if (d > accel_disp[i])
+					accel_disp[i] = d;
+			}
+
+			/* still detector with hysteresis */
+			if (accel_disp[0] < still_thr2 &&
+			    accel_disp[1] < still_thr2 &&
+			    accel_disp[2] < still_thr2) {
+				/* is still now */
+				if (t_still == 0) {
+					/* first time */
+					mavlink_log_info(mavlink_fd, "detected rest position, waiting...");
+					t_still = t;
+					t_timeout = t + timeout;
+
+				} else {
+					/* still since t_still */
+					if (t > t_still + still_time) {
+						/* vehicle is still, exit from the loop to detection of its orientation */
+						break;
+					}
+				}
+
+			} else if (accel_disp[0] > still_thr2 * 4.0f ||
+				   accel_disp[1] > still_thr2 * 4.0f ||
+				   accel_disp[2] > still_thr2 * 4.0f) {
+				/* not still, reset still start time */
+				if (t_still != 0) {
+					mavlink_log_info(mavlink_fd, "detected motion, hold still...");
+					t_still = 0;
+				}
+			}
+
+		} else if (poll_ret == 0) {
+			poll_errcount++;
+		}
+
+		if (t > t_timeout) {
+			poll_errcount++;
+		}
+
+		if (poll_errcount > 1000) {
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
+			return ERROR;
+		}
+	}
+
+	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 ]
+
+	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 ]
+
+	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 ]
+
+	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 ]
+
+	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 ]
+
+	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 ]
+
+	mavlink_log_critical(mavlink_fd, "ERROR: invalid orientation");
+
+	return ERROR;	// Can't detect orientation
+}
+
+/*
+ * Read specified number of accelerometer samples, calculate average and dispersion.
+ */
+int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num)
+{
+	struct pollfd fds[1];
+	fds[0].fd = sensor_combined_sub;
+	fds[0].events = POLLIN;
+	int count = 0;
+	float accel_sum[3] = { 0.0f, 0.0f, 0.0f };
+
+	int errcount = 0;
+
+	while (count < samples_num) {
+		int poll_ret = poll(fds, 1, 1000);
+
+		if (poll_ret == 1) {
+			struct sensor_combined_s sensor;
+			orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
+
+			for (int i = 0; i < 3; i++)
+				accel_sum[i] += sensor.accelerometer_m_s2[i];
+
+			count++;
+
+		} else {
+			errcount++;
+			continue;
+		}
+
+		if (errcount > samples_num / 10)
+			return ERROR;
+	}
+
+	for (int i = 0; i < 3; i++) {
+		accel_avg[i] = accel_sum[i] / count;
+	}
+
+	return OK;
+}
+
+int mat_invert3(float src[3][3], float dst[3][3])
+{
+	float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) -
+		    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
+
+	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;
+	dst[2][0] = (src[1][0] * src[2][1] - src[1][1] * src[2][0]) / det;
+	dst[0][1] = (src[0][2] * src[2][1] - src[0][1] * src[2][2]) / det;
+	dst[1][1] = (src[0][0] * src[2][2] - src[0][2] * src[2][0]) / det;
+	dst[2][1] = (src[0][1] * src[2][0] - src[0][0] * src[2][1]) / det;
+	dst[0][2] = (src[0][1] * src[1][2] - src[0][2] * src[1][1]) / det;
+	dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det;
+	dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det;
+
+	return OK;
+}
+
+int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g)
+{
+	/* calculate offsets */
+	for (int i = 0; i < 3; i++) {
+		accel_offs[i] = (accel_ref[i * 2][i] + accel_ref[i * 2 + 1][i]) / 2;
+	}
+
+	/* fill matrix A for linear equations system*/
+	float mat_A[3][3];
+	memset(mat_A, 0, sizeof(mat_A));
+
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++) {
+			float a = accel_ref[i * 2][j] - accel_offs[j];
+			mat_A[i][j] = a;
+		}
+	}
+
+	/* calculate inverse matrix for A */
+	float mat_A_inv[3][3];
+
+	if (mat_invert3(mat_A, mat_A_inv) != OK)
+		return ERROR;
+
+	/* copy results to accel_T */
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++) {
+			/* simplify matrices mult because b has only one non-zero element == g at index i */
+			accel_T[j][i] = mat_A_inv[j][i] * g;
+		}
+	}
+
+	return OK;
+}
diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h
index a11cf93d3..1cf9c0977 100644
--- a/src/modules/commander/accelerometer_calibration.h
+++ b/src/modules/commander/accelerometer_calibration.h
@@ -1,16 +1,50 @@
-/*
- * accelerometer_calibration.h
+/****************************************************************************
  *
- *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
- *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ *   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 accelerometer_calibration.h
+ *
+ * Definition of accelerometer calibration.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #ifndef ACCELEROMETER_CALIBRATION_H_
 #define ACCELEROMETER_CALIBRATION_H_
 
 #include <stdint.h>
-#include <uORB/topics/vehicle_status.h>
 
-void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd);
+int do_accel_calibration(int mavlink_fd);
 
 #endif /* ACCELEROMETER_CALIBRATION_H_ */
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
new file mode 100644
index 000000000..1809f9688
--- /dev/null
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -0,0 +1,154 @@
+/****************************************************************************
+ *
+ *   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 airspeed_calibration.cpp
+ * Airspeed sensor calibration routine
+ */
+
+#include "airspeed_calibration.h"
+#include "calibration_messages.h"
+#include "commander_helper.h"
+
+#include <stdio.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <math.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_airspeed.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/differential_pressure.h>
+#include <mavlink/mavlink_log.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+static const char *sensor_name = "dpress";
+
+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;
+
+	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,
+		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;
+		}
+		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;
+	}
+
+	while (calibration_counter < calibration_count) {
+
+		/* 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);
+			diff_pres_offset += diff_pres.differential_pressure_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);
+
+		} 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;
+		}
+	}
+
+	diff_pres_offset = diff_pres_offset / calibration_count;
+
+	if (isfinite(diff_pres_offset)) {
+
+		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;
+		}
+
+		/* auto-save to EEPROM */
+		int save_ret = param_save_default();
+
+		if (save_ret != 0) {
+			warn("WARNING: auto-save of params to storage failed");
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
+			close(diff_pres_sub);
+			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;
+	}
+}
diff --git a/src/modules/commander/airspeed_calibration.h b/src/modules/commander/airspeed_calibration.h
new file mode 100644
index 000000000..71c701fc2
--- /dev/null
+++ b/src/modules/commander/airspeed_calibration.h
@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ *   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 gyro_calibration.h
+ * Airspeed sensor calibration routine
+ */
+
+#ifndef AIRSPEED_CALIBRATION_H_
+#define AIRSPEED_CALIBRATION_H_
+
+#include <stdint.h>
+
+int do_airspeed_calibration(int mavlink_fd);
+
+#endif /* AIRSPEED_CALIBRATION_H_ */
diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp
new file mode 100644
index 000000000..3123c4087
--- /dev/null
+++ b/src/modules/commander/baro_calibration.cpp
@@ -0,0 +1,60 @@
+/****************************************************************************
+ *
+ *   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 baro_calibration.cpp
+ * Barometer calibration routine
+ */
+
+#include "baro_calibration.h"
+
+#include <poll.h>
+#include <math.h>
+#include <fcntl.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/sensor_combined.h>
+#include <drivers/drv_baro.h>
+#include <mavlink/mavlink_log.h>
+#include <systemlib/param/param.h>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+int do_baro_calibration(int mavlink_fd)
+{
+	// TODO implement this
+	return ERROR;
+}
diff --git a/src/modules/commander/baro_calibration.h b/src/modules/commander/baro_calibration.h
new file mode 100644
index 000000000..bc42bc6cf
--- /dev/null
+++ b/src/modules/commander/baro_calibration.h
@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ *   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 mag_calibration.h
+ * Barometer calibration routine
+ */
+
+#ifndef BARO_CALIBRATION_H_
+#define BARO_CALIBRATION_H_
+
+#include <stdint.h>
+
+int do_baro_calibration(int mavlink_fd);
+
+#endif /* BARO_CALIBRATION_H_ */
diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h
new file mode 100644
index 000000000..fd8b8564d
--- /dev/null
+++ b/src/modules/commander/calibration_messages.h
@@ -0,0 +1,57 @@
+/****************************************************************************
+ *
+ *   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 calibration_messages.h
+ *
+ * Common calibration messages.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef CALIBRATION_MESSAGES_H_
+#define CALIBRATION_MESSAGES_H_
+
+#define CAL_STARTED_MSG	"%s calibration: started"
+#define CAL_DONE_MSG	"%s calibration: done"
+#define CAL_FAILED_MSG	"%s calibration: failed"
+#define CAL_PROGRESS_MSG	"%s calibration: progress <%u>"
+
+#define CAL_FAILED_SENSOR_MSG	"ERROR: failed reading sensor"
+#define CAL_FAILED_RESET_CAL_MSG	"ERROR: failed to reset calibration"
+#define CAL_FAILED_APPLY_CAL_MSG	"ERROR: failed to apply calibration"
+#define CAL_FAILED_SET_PARAMS_MSG	"ERROR: failed to set parameters"
+#define CAL_FAILED_SAVE_PARAMS_MSG	"ERROR: failed to save parameters"
+
+#endif /* CALIBRATION_MESSAGES_H_ */
diff --git a/src/modules/commander/calibration_routines.c b/src/modules/commander/calibration_routines.cpp
index a26938637..be38ea104 100644
--- a/src/modules/commander/calibration_routines.c
+++ b/src/modules/commander/calibration_routines.cpp
@@ -33,7 +33,7 @@
  ****************************************************************************/
 
 /**
- * @file calibration_routines.c
+ * @file calibration_routines.cpp
  * Calibration routines implementations.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
@@ -217,3 +217,4 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
 
 	return 0;
 }
+
diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c
deleted file mode 100644
index 67f053e22..000000000
--- a/src/modules/commander/commander.c
+++ /dev/null
@@ -1,2078 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 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>
- *
- * 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 commander.c
- * Main system state machine implementation.
- *
- * @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>
- *
- */
-
-#include "commander.h"
-
-#include <nuttx/config.h>
-#include <pthread.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <stdbool.h>
-#include <string.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <debug.h>
-#include <sys/prctl.h>
-#include <string.h>
-#include <drivers/drv_led.h>
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_tone_alarm.h>
-#include "state_machine_helper.h"
-#include "systemlib/systemlib.h"
-#include <math.h>
-#include <poll.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/battery_status.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/offboard_control_setpoint.h>
-#include <uORB/topics/home_position.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_command.h>
-#include <uORB/topics/subsystem_info.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/differential_pressure.h>
-#include <mavlink/mavlink_log.h>
-
-#include <systemlib/param/param.h>
-#include <systemlib/systemlib.h>
-#include <systemlib/err.h>
-
-/* XXX MOVE CALIBRATION TO SENSORS APP THREAD */
-#include <drivers/drv_accel.h>
-#include <drivers/drv_gyro.h>
-#include <drivers/drv_mag.h>
-#include <drivers/drv_baro.h>
-
-#include "calibration_routines.h"
-#include "accelerometer_calibration.h"
-
-PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0);	/**< Go into low-level failsafe after 0 ms */
-//PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0);	/**< Go into high-level failsafe after 0 ms */
-PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
-PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
-PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
-
-#include <systemlib/cpuload.h>
-extern struct system_load_s system_load;
-
-/* Decouple update interval and hysteris counters, all depends on intervals */
-#define COMMANDER_MONITORING_INTERVAL 50000
-#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))
-#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-
-#define STICK_ON_OFF_LIMIT 0.75f
-#define STICK_THRUST_RANGE 1.0f
-#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
-#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-
-#define GPS_FIX_TYPE_2D 2
-#define GPS_FIX_TYPE_3D 3
-#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000
-#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-
-/* File descriptors */
-static int leds;
-static int buzzer;
-static int mavlink_fd;
-static bool commander_initialized = false;
-static struct vehicle_status_s current_status; /**< Main state machine */
-static orb_advert_t stat_pub;
-
-// static uint16_t nofix_counter = 0;
-// static uint16_t gotfix_counter = 0;
-
-static unsigned int failsafe_lowlevel_timeout_ms;
-
-static bool thread_should_exit = false;		/**< daemon exit flag */
-static bool thread_running = false;		/**< daemon status flag */
-static int daemon_task;				/**< Handle of daemon task / thread */
-
-/* pthread loops */
-static void *orb_receive_loop(void *arg);
-
-__EXPORT int commander_main(int argc, char *argv[]);
-
-/**
- * Mainloop of commander.
- */
-int commander_thread_main(int argc, char *argv[]);
-
-static int buzzer_init(void);
-static void buzzer_deinit(void);
-static int led_init(void);
-static void led_deinit(void);
-static int led_toggle(int led);
-static int led_on(int led);
-static int led_off(int led);
-static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status);
-static void do_mag_calibration(int status_pub, struct vehicle_status_s *status);
-static void do_rc_calibration(int status_pub, struct vehicle_status_s *status);
-static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd);
-
-int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state);
-
-
-
-/**
- * Print the correct usage.
- */
-static void usage(const char *reason);
-
-/**
- * Sort calibration values.
- *
- * Sorts the calibration values with bubble sort.
- *
- * @param a 	The array to sort
- * @param n 	The number of entries in the array
- */
-// static void cal_bsort(float a[], int n);
-
-static int buzzer_init()
-{
-	buzzer = open("/dev/tone_alarm", O_WRONLY);
-
-	if (buzzer < 0) {
-		warnx("Buzzer: open fail\n");
-		return ERROR;
-	}
-
-	return 0;
-}
-
-static void buzzer_deinit()
-{
-	close(buzzer);
-}
-
-
-static int led_init()
-{
-	leds = open(LED_DEVICE_PATH, 0);
-
-	if (leds < 0) {
-		warnx("LED: open fail\n");
-		return ERROR;
-	}
-
-	if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) {
-		warnx("LED: ioctl fail\n");
-		return ERROR;
-	}
-
-	return 0;
-}
-
-static void led_deinit()
-{
-	close(leds);
-}
-
-static int led_toggle(int led)
-{
-	static int last_blue = LED_ON;
-	static int last_amber = LED_ON;
-
-	if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON;
-
-	if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON;
-
-	return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led);
-}
-
-static int led_on(int led)
-{
-	return ioctl(leds, LED_ON, led);
-}
-
-static int led_off(int led)
-{
-	return ioctl(leds, LED_OFF, led);
-}
-
-enum AUDIO_PATTERN {
-	AUDIO_PATTERN_ERROR = 2,
-	AUDIO_PATTERN_NOTIFY_POSITIVE = 3,
-	AUDIO_PATTERN_NOTIFY_NEUTRAL = 4,
-	AUDIO_PATTERN_NOTIFY_NEGATIVE = 5,
-	AUDIO_PATTERN_NOTIFY_CHARGE = 6
-};
-
-int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state)
-{
-
-	/* Trigger alarm if going into any error state */
-	if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) ||
-	    ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) {
-		ioctl(buzzer, TONE_SET_ALARM, 0);
-		ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR);
-	}
-
-	/* Trigger neutral on arming / disarming */
-	if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) {
-		ioctl(buzzer, TONE_SET_ALARM, 0);
-		ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL);
-	}
-
-	/* Trigger Tetris on being bored */
-
-	return 0;
-}
-
-void tune_confirm(void)
-{
-	ioctl(buzzer, TONE_SET_ALARM, 3);
-}
-
-void tune_error(void)
-{
-	ioctl(buzzer, TONE_SET_ALARM, 4);
-}
-
-void do_rc_calibration(int status_pub, struct vehicle_status_s *status)
-{
-	if (current_status.offboard_control_signal_lost) {
-		mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal.");
-		return;
-	}
-
-	int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
-	struct manual_control_setpoint_s sp;
-	orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
-
-	/* set parameters */
-
-	float p = sp.roll;
-	param_set(param_find("TRIM_ROLL"), &p);
-	p = sp.pitch;
-	param_set(param_find("TRIM_PITCH"), &p);
-	p = sp.yaw;
-	param_set(param_find("TRIM_YAW"), &p);
-
-	/* store to permanent storage */
-	/* auto-save to EEPROM */
-	int save_ret = param_save_default();
-
-	if (save_ret != 0) {
-		mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed");
-	}
-
-	mavlink_log_info(mavlink_fd, "trim calibration done");
-}
-
-void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
-{
-
-	/* set to mag calibration mode */
-	status->flag_preflight_mag_calibration = true;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
-	struct mag_report mag;
-
-	/* 45 seconds */
-	uint64_t calibration_interval = 45 * 1000 * 1000;
-
-	/* maximum 2000 values */
-	const unsigned int calibration_maxcount = 500;
-	unsigned int calibration_counter = 0;
-
-	/* limit update rate to get equally spaced measurements over time (in ms) */
-	orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
-
-	// XXX old cal
-	//  * FLT_MIN is not the most negative float number,
-	//  * but the smallest number by magnitude float can
-	//  * represent. Use -FLT_MAX to initialize the most
-	//  * negative number
-
-	// float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX};
-	// float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX};
-
-	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
-
-	/* erase old calibration */
-	struct mag_scale mscale_null = {
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-	};
-
-	if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
-		warn("WARNING: failed to set scale / offsets for mag");
-		mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag");
-	}
-
-	/* calibrate range */
-	if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) {
-		warnx("failed to calibrate scale");
-	}
-
-	close(fd);
-
-	/* calibrate offsets */
-
-	// uint64_t calibration_start = hrt_absolute_time();
-
-	uint64_t axis_deadline = hrt_absolute_time();
-	uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
-
-	const char axislabels[3] = { 'X', 'Y', 'Z'};
-	int axis_index = -1;
-
-	float *x = (float *)malloc(sizeof(float) * calibration_maxcount);
-	float *y = (float *)malloc(sizeof(float) * calibration_maxcount);
-	float *z = (float *)malloc(sizeof(float) * calibration_maxcount);
-
-	if (x == NULL || y == NULL || z == NULL) {
-		warnx("mag cal failed: out of memory");
-		mavlink_log_info(mavlink_fd, "mag cal failed: out of memory");
-		warnx("x:%p y:%p z:%p\n", x, y, z);
-		return;
-	}
-
-	tune_confirm();
-	sleep(2);
-	tune_confirm();
-
-	while (hrt_absolute_time() < calibration_deadline &&
-	       calibration_counter < calibration_maxcount) {
-
-		/* wait blocking for new data */
-		struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } };
-
-		/* user guidance */
-		if (hrt_absolute_time() >= axis_deadline &&
-		    axis_index < 3) {
-
-			axis_index++;
-
-			char buf[50];
-			sprintf(buf, "Please rotate around %c", axislabels[axis_index]);
-			mavlink_log_info(mavlink_fd, buf);
-			tune_confirm();
-
-			axis_deadline += calibration_interval / 3;
-		}
-
-		if (!(axis_index < 3)) {
-			break;
-		}
-
-		// int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time();
-
-		// if ((axis_left / 1000) == 0 && axis_left > 0) {
-		// 	char buf[50];
-		// 	sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]);
-		// 	mavlink_log_info(mavlink_fd, buf);
-		// }
-
-		int poll_ret = poll(fds, 1, 1000);
-
-		if (poll_ret) {
-			orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
-
-			x[calibration_counter] = mag.x;
-			y[calibration_counter] = mag.y;
-			z[calibration_counter] = mag.z;
-
-			/* get min/max values */
-
-			// if (mag.x < mag_min[0]) {
-			// 	mag_min[0] = mag.x;
-			// }
-			// else if (mag.x > mag_max[0]) {
-			// 	mag_max[0] = mag.x;
-			// }
-
-			// if (raw.magnetometer_ga[1] < mag_min[1]) {
-			// 	mag_min[1] = raw.magnetometer_ga[1];
-			// }
-			// else if (raw.magnetometer_ga[1] > mag_max[1]) {
-			// 	mag_max[1] = raw.magnetometer_ga[1];
-			// }
-
-			// if (raw.magnetometer_ga[2] < mag_min[2]) {
-			// 	mag_min[2] = raw.magnetometer_ga[2];
-			// }
-			// else if (raw.magnetometer_ga[2] > mag_max[2]) {
-			// 	mag_max[2] = raw.magnetometer_ga[2];
-			// }
-
-			calibration_counter++;
-
-		} else if (poll_ret == 0) {
-			/* any poll failure for 1s is a reason to abort */
-			mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)");
-			break;
-		}
-	}
-
-	float sphere_x;
-	float sphere_y;
-	float sphere_z;
-	float sphere_radius;
-
-	sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
-
-	free(x);
-	free(y);
-	free(z);
-
-	if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) {
-
-		fd = open(MAG_DEVICE_PATH, 0);
-
-		struct mag_scale mscale;
-
-		if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale))
-			warn("WARNING: failed to get scale / offsets for mag");
-
-		mscale.x_offset = sphere_x;
-		mscale.y_offset = sphere_y;
-		mscale.z_offset = sphere_z;
-
-		if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
-			warn("WARNING: failed to set scale / offsets for mag");
-
-		close(fd);
-
-		/* announce and set new offset */
-
-		if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
-			warnx("Setting X mag offset failed!\n");
-		}
-
-		if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
-			warnx("Setting Y mag offset failed!\n");
-		}
-
-		if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
-			warnx("Setting Z mag offset failed!\n");
-		}
-
-		if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
-			warnx("Setting X mag scale failed!\n");
-		}
-
-		if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
-			warnx("Setting Y mag scale failed!\n");
-		}
-
-		if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
-			warnx("Setting Z mag scale failed!\n");
-		}
-
-		/* auto-save to EEPROM */
-		int save_ret = param_save_default();
-
-		if (save_ret != 0) {
-			warn("WARNING: auto-save of params to storage failed");
-			mavlink_log_info(mavlink_fd, "FAILED storing calibration");
-		}
-
-		warnx("\tscale: %.6f %.6f %.6f\n         \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n",
-		       (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale,
-		       (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius);
-
-		char buf[52];
-		sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
-			(double)mscale.y_offset, (double)mscale.z_offset);
-		mavlink_log_info(mavlink_fd, buf);
-
-		sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
-			(double)mscale.y_scale, (double)mscale.z_scale);
-		mavlink_log_info(mavlink_fd, buf);
-
-		mavlink_log_info(mavlink_fd, "mag calibration done");
-
-		tune_confirm();
-		sleep(2);
-		tune_confirm();
-		sleep(2);
-		/* third beep by cal end routine */
-
-	} else {
-		mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
-	}
-
-	/* disable calibration mode */
-	status->flag_preflight_mag_calibration = false;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	close(sub_mag);
-}
-
-void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
-{
-	/* set to gyro calibration mode */
-	status->flag_preflight_gyro_calibration = true;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	const int calibration_count = 5000;
-
-	int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
-	struct sensor_combined_s raw;
-
-	int calibration_counter = 0;
-	float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
-
-	/* set offsets to zero */
-	int fd = open(GYRO_DEVICE_PATH, 0);
-	struct gyro_scale gscale_null = {
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-	};
-
-	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null))
-		warn("WARNING: failed to set scale / offsets for gyro");
-
-	close(fd);
-
-	while (calibration_counter < calibration_count) {
-
-		/* wait blocking for new data */
-		struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
-
-		int poll_ret = poll(fds, 1, 1000);
-
-		if (poll_ret) {
-			orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
-			gyro_offset[0] += raw.gyro_rad_s[0];
-			gyro_offset[1] += raw.gyro_rad_s[1];
-			gyro_offset[2] += raw.gyro_rad_s[2];
-			calibration_counter++;
-
-		} else if (poll_ret == 0) {
-			/* any poll failure for 1s is a reason to abort */
-			mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
-			return;
-		}
-	}
-
-	gyro_offset[0] = gyro_offset[0] / calibration_count;
-	gyro_offset[1] = gyro_offset[1] / calibration_count;
-	gyro_offset[2] = gyro_offset[2] / calibration_count;
-
-	/* exit gyro calibration mode */
-	status->flag_preflight_gyro_calibration = false;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) {
-
-		if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))
-			|| param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))
-			|| param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) {
-			mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!");
-		}
-
-		/* set offsets to actual value */
-		fd = open(GYRO_DEVICE_PATH, 0);
-		struct gyro_scale gscale = {
-			gyro_offset[0],
-			1.0f,
-			gyro_offset[1],
-			1.0f,
-			gyro_offset[2],
-			1.0f,
-		};
-
-		if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
-			warn("WARNING: failed to set scale / offsets for gyro");
-
-		close(fd);
-
-		/* auto-save to EEPROM */
-		int save_ret = param_save_default();
-
-		if (save_ret != 0) {
-			warn("WARNING: auto-save of params to storage failed");
-		}
-
-		// char buf[50];
-		// sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
-		// mavlink_log_info(mavlink_fd, buf);
-		mavlink_log_info(mavlink_fd, "gyro calibration done");
-
-		tune_confirm();
-		sleep(2);
-		tune_confirm();
-		sleep(2);
-		/* third beep by cal end routine */
-
-	} else {
-		mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)");
-	}
-
-	close(sub_sensor_combined);
-}
-
-void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
-{
-	/* announce change */
-
-	mavlink_log_info(mavlink_fd, "keep it still");
-	/* set to accel calibration mode */
-	status->flag_preflight_airspeed_calibration = true;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	const int calibration_count = 2500;
-
-	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;
-
-	while (calibration_counter < calibration_count) {
-
-		/* wait blocking for new data */
-		struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } };
-
-		int poll_ret = poll(fds, 1, 1000);
-
-		if (poll_ret) {
-			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
-			diff_pres_offset += diff_pres.differential_pressure_pa;
-			calibration_counter++;
-
-		} else if (poll_ret == 0) {
-			/* any poll failure for 1s is a reason to abort */
-			mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
-			return;
-		}
-	}
-
-	diff_pres_offset = diff_pres_offset / calibration_count;
-
-	if (isfinite(diff_pres_offset)) {
-
-		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
-			mavlink_log_critical(mavlink_fd, "Setting offs failed!");
-		}
-
-		/* auto-save to EEPROM */
-		int save_ret = param_save_default();
-
-		if (save_ret != 0) {
-			warn("WARNING: auto-save of params to storage failed");
-		}
-
-		//char buf[50];
-		//sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]);
-		//mavlink_log_info(mavlink_fd, buf);
-		mavlink_log_info(mavlink_fd, "airspeed calibration done");
-
-		tune_confirm();
-		sleep(2);
-		tune_confirm();
-		sleep(2);
-		/* third beep by cal end routine */
-
-	} else {
-		mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
-	}
-
-	/* exit airspeed calibration mode */
-	status->flag_preflight_airspeed_calibration = false;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	close(diff_pres_sub);
-}
-
-
-
-void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd)
-{
-	/* result of the command */
-	uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-
-	/* announce command handling */
-	tune_confirm();
-
-
-	/* supported command handling start */
-
-	/* request to set different system mode */
-	switch (cmd->command) {
-	case VEHICLE_CMD_DO_SET_MODE: {
-			if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) {
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-			} else {
-				result = VEHICLE_CMD_RESULT_DENIED;
-			}
-		}
-		break;
-
-	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
-			/* request to arm */
-			if ((int)cmd->param1 == 1) {
-				if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				/* request to disarm */
-
-			} else if ((int)cmd->param1 == 0) {
-				if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-			}
-		}
-		break;
-
-		/* request for an autopilot reboot */
-	case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: {
-			if ((int)cmd->param1 == 1) {
-				if (OK == do_state_update(status_pub, current_vehicle_status, mavlink_fd, SYSTEM_STATE_REBOOT)) {
-					/* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					/* system may return here */
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-			}
-		}
-		break;
-
-//		/* request to land */
-//		case VEHICLE_CMD_NAV_LAND:
-//		 {
-//				//TODO: add check if landing possible
-//				//TODO: add landing maneuver
-//
-//				if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) {
-//					result = VEHICLE_CMD_RESULT_ACCEPTED;
-//		}		}
-//		break;
-//
-//		/* request to takeoff */
-//		case VEHICLE_CMD_NAV_TAKEOFF:
-//		{
-//			//TODO: add check if takeoff possible
-//			//TODO: add takeoff maneuver
-//
-//			if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) {
-//				result = VEHICLE_CMD_RESULT_ACCEPTED;
-//			}
-//		}
-//		break;
-//
-		/* preflight calibration */
-	case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
-			bool handled = false;
-
-			/* gyro calibration */
-			if ((int)(cmd->param1) == 1) {
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "starting gyro cal");
-					tune_confirm();
-					do_gyro_calibration(status_pub, &current_status);
-					mavlink_log_info(mavlink_fd, "finished gyro cal");
-					tune_confirm();
-					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING gyro cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* magnetometer calibration */
-			if ((int)(cmd->param2) == 1) {
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "starting mag cal");
-					tune_confirm();
-					do_mag_calibration(status_pub, &current_status);
-					mavlink_log_info(mavlink_fd, "finished mag cal");
-					tune_confirm();
-					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING mag cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* zero-altitude pressure calibration */
-			if ((int)(cmd->param3) == 1) {
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented");
-					tune_confirm();
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* trim calibration */
-			if ((int)(cmd->param4) == 1) {
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "starting trim cal");
-					tune_confirm();
-					do_rc_calibration(status_pub, &current_status);
-					mavlink_log_info(mavlink_fd, "finished trim cal");
-					tune_confirm();
-					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING trim cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* accel calibration */
-			if ((int)(cmd->param5) == 1) {
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "CMD starting accel cal");
-					tune_confirm();
-					do_accel_calibration(status_pub, &current_status, mavlink_fd);
-					tune_confirm();
-					mavlink_log_info(mavlink_fd, "CMD finished accel cal");
-					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING accel cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* airspeed calibration */
-			if ((int)(cmd->param6) == 1) { //xxx: this is not defined by the mavlink protocol
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "CMD starting airspeed cal");
-					tune_confirm();
-					do_airspeed_calibration(status_pub, &current_status);
-					tune_confirm();
-					mavlink_log_info(mavlink_fd, "CMD finished airspeed cal");
-					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING airspeed cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* none found */
-			if (!handled) {
-				//warnx("refusing unsupported calibration request\n");
-				mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request");
-				result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-			}
-		}
-		break;
-
-	case VEHICLE_CMD_PREFLIGHT_STORAGE: {
-			if (current_status.flag_system_armed &&
-			    ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
-			     (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
-			     (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) {
-				/* do not perform expensive memory tasks on multirotors in flight */
-				// XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed
-				mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed");
-
-			} else {
-
-				// XXX move this to LOW PRIO THREAD of commander app
-				/* Read all parameters from EEPROM to RAM */
-
-				if (((int)(cmd->param1)) == 0)	{
-
-					/* read all parameters from EEPROM to RAM */
-					int read_ret = param_load_default();
-
-					if (read_ret == OK) {
-						//warnx("[mavlink pm] Loaded EEPROM params in RAM\n");
-						mavlink_log_info(mavlink_fd, "OK loading params from");
-						mavlink_log_info(mavlink_fd, param_get_default_file());
-						result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-					} else if (read_ret == 1) {
-						mavlink_log_info(mavlink_fd, "OK no changes in");
-						mavlink_log_info(mavlink_fd, param_get_default_file());
-						result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-					} else {
-						if (read_ret < -1) {
-							mavlink_log_info(mavlink_fd, "ERR loading params from");
-							mavlink_log_info(mavlink_fd, param_get_default_file());
-
-						} else {
-							mavlink_log_info(mavlink_fd, "ERR no param file named");
-							mavlink_log_info(mavlink_fd, param_get_default_file());
-						}
-
-						result = VEHICLE_CMD_RESULT_FAILED;
-					}
-
-				} else if (((int)(cmd->param1)) == 1)	{
-
-					/* write all parameters from RAM to EEPROM */
-					int write_ret = param_save_default();
-
-					if (write_ret == OK) {
-						mavlink_log_info(mavlink_fd, "OK saved param file");
-						mavlink_log_info(mavlink_fd, param_get_default_file());
-						result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-					} else {
-						if (write_ret < -1) {
-							mavlink_log_info(mavlink_fd, "ERR params file does not exit:");
-							mavlink_log_info(mavlink_fd, param_get_default_file());
-
-						} else {
-							mavlink_log_info(mavlink_fd, "ERR writing params to");
-							mavlink_log_info(mavlink_fd, param_get_default_file());
-						}
-
-						result = VEHICLE_CMD_RESULT_FAILED;
-					}
-
-				} else {
-					mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request");
-					result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-				}
-			}
-		}
-		break;
-
-	default: {
-			mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command");
-			result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-			/* announce command rejection */
-			ioctl(buzzer, TONE_SET_ALARM, 4);
-		}
-		break;
-	}
-
-	/* supported command handling stop */
-	if (result == VEHICLE_CMD_RESULT_FAILED ||
-	    result == VEHICLE_CMD_RESULT_DENIED ||
-	    result == VEHICLE_CMD_RESULT_UNSUPPORTED) {
-		ioctl(buzzer, TONE_SET_ALARM, 5);
-
-	} else if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
-		tune_confirm();
-	}
-
-	/* send any requested ACKs */
-	if (cmd->confirmation > 0) {
-		/* send acknowledge command */
-		// XXX TODO
-	}
-
-}
-
-static void *orb_receive_loop(void *arg)  //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal
-{
-	/* Set thread name */
-	prctl(PR_SET_NAME, "commander orb rcv", getpid());
-
-	/* Subscribe to command topic */
-	int subsys_sub = orb_subscribe(ORB_ID(subsystem_info));
-	struct subsystem_info_s info;
-
-	struct vehicle_status_s *vstatus = (struct vehicle_status_s *)arg;
-
-	while (!thread_should_exit) {
-		struct pollfd fds[1] = { { .fd = subsys_sub, .events = POLLIN } };
-
-		if (poll(fds, 1, 5000) == 0) {
-			/* timeout, but this is no problem, silently ignore */
-		} else {
-			/* got command */
-			orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
-
-			warnx("Subsys changed: %d\n", (int)info.subsystem_type);
-
-			/* mark / unmark as present */
-			if (info.present) {
-				vstatus->onboard_control_sensors_present |= info.subsystem_type;
-
-			} else {
-				vstatus->onboard_control_sensors_present &= ~info.subsystem_type;
-			}
-
-			/* mark / unmark as enabled */
-			if (info.enabled) {
-				vstatus->onboard_control_sensors_enabled |= info.subsystem_type;
-
-			} else {
-				vstatus->onboard_control_sensors_enabled &= ~info.subsystem_type;
-			}
-
-			/* mark / unmark as ok */
-			if (info.ok) {
-				vstatus->onboard_control_sensors_health |= info.subsystem_type;
-
-			} else {
-				vstatus->onboard_control_sensors_health &= ~info.subsystem_type;
-			}
-		}
-	}
-
-	close(subsys_sub);
-
-	return NULL;
-}
-
-/*
- * Provides a coarse estimate of remaining battery power.
- *
- * The estimate is very basic and based on decharging voltage curves.
- *
- * @return the estimated remaining capacity in 0..1
- */
-float battery_remaining_estimate_voltage(float voltage);
-
-PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f);
-PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f);
-PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3);
-
-float battery_remaining_estimate_voltage(float voltage)
-{
-	float ret = 0;
-	static param_t bat_volt_empty;
-	static param_t bat_volt_full;
-	static param_t bat_n_cells;
-	static bool initialized = false;
-	static unsigned int counter = 0;
-	static float ncells = 3;
-	// XXX change cells to int (and param to INT32)
-
-	if (!initialized) {
-		bat_volt_empty = param_find("BAT_V_EMPTY");
-		bat_volt_full = param_find("BAT_V_FULL");
-		bat_n_cells = param_find("BAT_N_CELLS");
-		initialized = true;
-	}
-
-	static float chemistry_voltage_empty = 3.2f;
-	static float chemistry_voltage_full = 4.05f;
-
-	if (counter % 100 == 0) {
-		param_get(bat_volt_empty, &chemistry_voltage_empty);
-		param_get(bat_volt_full, &chemistry_voltage_full);
-		param_get(bat_n_cells, &ncells);
-	}
-
-	counter++;
-
-	ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty));
-
-	/* limit to sane values */
-	ret = (ret < 0) ? 0 : ret;
-	ret = (ret > 1) ? 1 : ret;
-	return ret;
-}
-
-static void
-usage(const char *reason)
-{
-	if (reason)
-		fprintf(stderr, "%s\n", reason);
-
-	fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
-	exit(1);
-}
-
-/**
- * The daemon 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 commander_main(int argc, char *argv[])
-{
-	if (argc < 1)
-		usage("missing command");
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			warnx("commander already running\n");
-			/* this is not an error */
-			exit(0);
-		}
-
-		thread_should_exit = false;
-		daemon_task = task_spawn_cmd("commander",
-					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 40,
-					 3000,
-					 commander_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("\tcommander is running\n");
-
-		} else {
-			warnx("\tcommander not started\n");
-		}
-
-		exit(0);
-	}
-
-	usage("unrecognized command");
-	exit(1);
-}
-
-int commander_thread_main(int argc, char *argv[])
-{
-	/* not yet initialized */
-	commander_initialized = false;
-	bool home_position_set = false;
-
-	/* set parameters */
-	failsafe_lowlevel_timeout_ms = 0;
-	param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms);
-
-	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");
-
-	/* welcome user */
-	warnx("I am in command now!\n");
-
-	/* pthreads for command and subsystem info handling */
-	// pthread_t command_handling_thread;
-	pthread_t subsystem_info_thread;
-
-	/* initialize */
-	if (led_init() != 0) {
-		warnx("ERROR: Failed to initialize leds\n");
-	}
-
-	if (buzzer_init() != 0) {
-		warnx("ERROR: Failed to initialize buzzer\n");
-	}
-
-	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-
-	if (mavlink_fd < 0) {
-		warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n");
-	}
-
-	/* make sure we are in preflight state */
-	memset(&current_status, 0, sizeof(current_status));
-	current_status.state_machine = SYSTEM_STATE_PREFLIGHT;
-	current_status.flag_system_armed = false;
-	/* neither manual nor offboard control commands have been received */
-	current_status.offboard_control_signal_found_once = false;
-	current_status.rc_signal_found_once = false;
-	/* mark all signals lost as long as they haven't been found */
-	current_status.rc_signal_lost = true;
-	current_status.offboard_control_signal_lost = true;
-	/* allow manual override initially */
-	current_status.flag_external_manual_override_ok = true;
-	/* flag position info as bad, do not allow auto mode */
-	current_status.flag_vector_flight_mode_ok = false;
-	/* set battery warning flag */
-	current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
-
-	/* advertise to ORB */
-	stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
-	/* publish current state machine */
-	state_machine_publish(stat_pub, &current_status, mavlink_fd);
-
-	/* home position */
-	orb_advert_t home_pub = -1;
-	struct home_position_s home;
-	memset(&home, 0, sizeof(home));
-
-	if (stat_pub < 0) {
-		warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
-		warnx("exiting.");
-		exit(ERROR);
-	}
-
-	mavlink_log_info(mavlink_fd, "system is running");
-
-	/* create pthreads */
-	pthread_attr_t subsystem_info_attr;
-	pthread_attr_init(&subsystem_info_attr);
-	pthread_attr_setstacksize(&subsystem_info_attr, 2048);
-	pthread_create(&subsystem_info_thread, &subsystem_info_attr, orb_receive_loop, &current_status);
-
-	/* Start monitoring loop */
-	uint16_t counter = 0;
-	uint8_t flight_env;
-
-	/* Initialize to 0.0V */
-	float battery_voltage = 0.0f;
-	bool battery_voltage_valid = false;
-	bool low_battery_voltage_actions_done = false;
-	bool critical_battery_voltage_actions_done = false;
-	uint8_t low_voltage_counter = 0;
-	uint16_t critical_voltage_counter = 0;
-	int16_t mode_switch_rc_value;
-	float bat_remain = 1.0f;
-
-	uint16_t stick_off_counter = 0;
-	uint16_t stick_on_counter = 0;
-
-	/* Subscribe to manual control data */
-	int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	struct manual_control_setpoint_s sp_man;
-	memset(&sp_man, 0, sizeof(sp_man));
-
-	/* 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));
-
-	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));
-	uint64_t last_global_position_time = 0;
-
-	int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
-	struct vehicle_local_position_s local_position;
-	memset(&local_position, 0, sizeof(local_position));
-	uint64_t last_local_position_time = 0;
-
-	/* 
-	 * The home position is set based on GPS only, to prevent a dependency between
-	 * position estimator and commander. RAW GPS is more than good enough for a
-	 * non-flying vehicle.
-	 */
-	int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	struct vehicle_gps_position_s gps_position;
-	memset(&gps_position, 0, sizeof(gps_position));
-
-	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	struct sensor_combined_s sensors;
-	memset(&sensors, 0, sizeof(sensors));
-
-	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
-	struct differential_pressure_s diff_pres;
-	memset(&diff_pres, 0, sizeof(diff_pres));
-	uint64_t last_diff_pres_time = 0;
-
-	/* Subscribe to command topic */
-	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
-	struct vehicle_command_s cmd;
-	memset(&cmd, 0, sizeof(cmd));
-
-	/* Subscribe to parameters changed topic */
-	int param_changed_sub = orb_subscribe(ORB_ID(parameter_update));
-	struct parameter_update_s param_changed;
-	memset(&param_changed, 0, sizeof(param_changed));
-
-	/* subscribe to battery topic */
-	int battery_sub = orb_subscribe(ORB_ID(battery_status));
-	struct battery_status_s battery;
-	memset(&battery, 0, sizeof(battery));
-	battery.voltage_v = 0.0f;
-
-	// uint8_t vehicle_state_previous = current_status.state_machine;
-	float voltage_previous = 0.0f;
-
-	uint64_t last_idle_time = 0;
-
-	/* now initialized */
-	commander_initialized = true;
-	thread_running = true;
-
-	uint64_t start_time = hrt_absolute_time();
-	uint64_t failsave_ll_start_time = 0;
-
-	bool state_changed = true;
-	bool param_init_forced = true;
-
-	while (!thread_should_exit) {
-
-		/* Get current values */
-		bool new_data;
-		orb_check(sp_man_sub, &new_data);
-
-		if (new_data) {
-			orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
-		}
-
-		orb_check(sp_offboard_sub, &new_data);
-
-		if (new_data) {
-			orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
-		}
-
-		orb_check(sensor_sub, &new_data);
-
-		if (new_data) {
-			orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
-		}
-
-		orb_check(diff_pres_sub, &new_data);
-
-		if (new_data) {
-			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
-			last_diff_pres_time = diff_pres.timestamp;
-		}
-
-		orb_check(cmd_sub, &new_data);
-
-		if (new_data) {
-			/* got command */
-			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
-
-			/* handle it */
-			handle_command(stat_pub, &current_status, &cmd);
-		}
-
-		/* update parameters */
-		orb_check(param_changed_sub, &new_data);
-
-		if (new_data || param_init_forced) {
-			param_init_forced = false;
-			/* parameters changed */
-			orb_copy(ORB_ID(parameter_update), param_changed_sub, &param_changed);
-
-
-			/* update parameters */
-			if (!current_status.flag_system_armed) {
-				if (param_get(_param_sys_type, &(current_status.system_type)) != OK) {
-					warnx("failed setting new system type");
-				}
-
-				/* disable manual override for all systems that rely on electronic stabilization */
-				if (current_status.system_type == VEHICLE_TYPE_QUADROTOR ||
-				    current_status.system_type == VEHICLE_TYPE_HEXAROTOR ||
-				    current_status.system_type == VEHICLE_TYPE_OCTOROTOR) {
-					current_status.flag_external_manual_override_ok = false;
-
-				} else {
-					current_status.flag_external_manual_override_ok = true;
-				}
-
-				/* check and update system / component ID */
-				param_get(_param_system_id, &(current_status.system_id));
-				param_get(_param_component_id, &(current_status.component_id));
-
-			}
-		}
-
-		/* update global position estimate */
-		orb_check(global_position_sub, &new_data);
-
-		if (new_data) {
-			/* position changed */
-			orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
-			last_global_position_time = global_position.timestamp;
-		}
-
-		/* update local position estimate */
-		orb_check(local_position_sub, &new_data);
-
-		if (new_data) {
-			/* position changed */
-			orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
-			last_local_position_time = local_position.timestamp;
-		}
-
-		/* update battery status */
-		orb_check(battery_sub, &new_data);
-		if (new_data) {
-			orb_copy(ORB_ID(battery_status), battery_sub, &battery);
-			battery_voltage = battery.voltage_v;
-			battery_voltage_valid = true;
-
-			/*
-			 * Only update battery voltage estimate if system has
-			 * been running for two and a half seconds.
-			 */
-			if (hrt_absolute_time() - start_time > 2500000) {
-				bat_remain = battery_remaining_estimate_voltage(battery_voltage);
-			}
-		}
-
-		/* Slow but important 8 Hz checks */
-		if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) {
-			/* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */
-			if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY ||
-			     current_status.state_machine == SYSTEM_STATE_AUTO  ||
-			     current_status.state_machine == SYSTEM_STATE_MANUAL)) {
-				/* armed, solid */
-				led_on(LED_AMBER);
-
-			} else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
-				/* not armed */
-				led_toggle(LED_AMBER);
-			}
-
-			if (hrt_absolute_time() - gps_position.timestamp_position < 2000000) {
-
-				/* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */
-				if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000)
-					&& (gps_position.fix_type == GPS_FIX_TYPE_3D)) {
-					/* GPS lock */
-					led_on(LED_BLUE);
-
-				} else if ((counter + 4) % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
-					/* no GPS lock, but GPS module is aquiring lock */
-					led_toggle(LED_BLUE);
-				}
-
-			} else {
-				/* no GPS info, don't light the blue led */
-				led_off(LED_BLUE);
-			}
-
-			/* toggle GPS led at 5 Hz in HIL mode */
-			if (current_status.flag_hil_enabled) {
-				/* hil enabled */
-				led_toggle(LED_BLUE);
-
-			} else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) {
-				/* toggle arming (red) at 5 Hz on low battery or error */
-				led_toggle(LED_AMBER);
-
-			} else {
-				// /* Constant error indication in standby mode without GPS */
-				// if (!current_status.gps_valid) {
-				// 	led_on(LED_AMBER);
-
-				// } else {
-				// 	led_off(LED_AMBER);
-				// }
-			}
-
-			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)
-					current_status.load = 1000 - (interval_runtime / 1000);	//system load is time spent in non-idle
-
-				last_idle_time = system_load.tasks[0].total_runtime;
-			}
-		}
-
-		// // XXX Export patterns and threshold to parameters
-		/* Trigger audio event for low battery */
-		if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 0)) {
-			/* For less than 10%, start be really annoying at 5 Hz */
-			ioctl(buzzer, TONE_SET_ALARM, 0);
-			ioctl(buzzer, TONE_SET_ALARM, 3);
-
-		} else if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 2)) {
-			ioctl(buzzer, TONE_SET_ALARM, 0);
-
-		} else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 0)) {
-			/* For less than 20%, start be slightly annoying at 1 Hz */
-			ioctl(buzzer, TONE_SET_ALARM, 0);
-			tune_confirm();
-
-		} else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 2)) {
-			ioctl(buzzer, TONE_SET_ALARM, 0);
-		}
-
-		/* Check battery voltage */
-		/* write to sys_status */
-		if (battery_voltage_valid) {
-			current_status.voltage_battery = battery_voltage;
-
-		} else {
-			current_status.voltage_battery = 0.0f;
-		}
-
-		/* if battery voltage is getting lower, warn using buzzer, etc. */
-		if (battery_voltage_valid && (bat_remain < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
-
-			if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
-				low_battery_voltage_actions_done = true;
-				mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!");
-				current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING;
-			}
-
-			low_voltage_counter++;
-		}
-
-		/* Critical, this is rather an emergency, kill signal to sdlog and change state machine */
-		else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) {
-			if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
-				critical_battery_voltage_actions_done = true;
-				mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!");
-				current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT;
-				state_machine_emergency(stat_pub, &current_status, mavlink_fd);
-			}
-
-			critical_voltage_counter++;
-
-		} else {
-			low_voltage_counter = 0;
-			critical_voltage_counter = 0;
-		}
-
-		/* End battery voltage check */
-
-
-		/*
-		 * Check for valid position information.
-		 *
-		 * If the system has a valid position source from an onboard
-		 * position estimator, it is safe to operate it autonomously.
-		 * The flag_vector_flight_mode_ok flag indicates that a minimum
-		 * set of position measurements is available.
-		 */
-
-		/* store current state to reason later about a state change */
-		bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok;
-		bool global_pos_valid = current_status.flag_global_position_valid;
-		bool local_pos_valid = current_status.flag_local_position_valid;
-		bool airspeed_valid = current_status.flag_airspeed_valid;
-
-		/* check for global or local position updates, set a timeout of 2s */
-		if (hrt_absolute_time() - last_global_position_time < 2000000) {
-			current_status.flag_global_position_valid = true;
-			// XXX check for controller status and home position as well
-
-		} else {
-			current_status.flag_global_position_valid = false;
-		}
-
-		if (hrt_absolute_time() - last_local_position_time < 2000000) {
-			current_status.flag_local_position_valid = true;
-			// XXX check for controller status and home position as well
-
-		} else {
-			current_status.flag_local_position_valid = false;
-		}
-
-		/* Check for valid airspeed/differential pressure measurements */
-		if (hrt_absolute_time() - last_diff_pres_time < 2000000) {
-			current_status.flag_airspeed_valid = true;
-
-		} else {
-			current_status.flag_airspeed_valid = false;
-		}
-
-		/*
-		 * Consolidate global position and local position valid flags
-		 * for vector flight mode.
-		 */
-		if (current_status.flag_local_position_valid ||
-		    current_status.flag_global_position_valid) {
-			current_status.flag_vector_flight_mode_ok = true;
-
-		} else {
-			current_status.flag_vector_flight_mode_ok = false;
-		}
-
-		/* consolidate state change, flag as changed if required */
-		if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok ||
-		    global_pos_valid != current_status.flag_global_position_valid ||
-		    local_pos_valid != current_status.flag_local_position_valid ||
-		    airspeed_valid != current_status.flag_airspeed_valid) {
-			state_changed = true;
-		}
-
-		/*
-		 * Mark the position of the first position lock as return to launch (RTL)
-		 * position. The MAV will return here on command or emergency.
-		 *
-		 * Conditions:
-		 *
-		 * 	1) The system aquired position lock just now
-		 *	2) The system has not aquired position lock before
-		 *	3) The system is not armed (on the ground)
-		 */
-		if (!current_status.flag_valid_launch_position &&
-		    !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok &&
-		    !current_status.flag_system_armed) {
-			/* first time a valid position, store it and emit it */
-
-			// XXX implement storage and publication of RTL position
-			current_status.flag_valid_launch_position = true;
-			current_status.flag_auto_flight_mode_ok = true;
-			state_changed = true;
-		}
-
-		if (orb_check(ORB_ID(vehicle_gps_position), &new_data)) {
-
-			orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
-
-			/* check for first, long-term and valid GPS lock -> set home position */
-			float hdop_m = gps_position.eph_m;
-			float vdop_m = gps_position.epv_m;
-
-			/* check if gps fix is ok */
-			// 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.
-			 */
-
-			if (gps_position.fix_type == GPS_FIX_TYPE_3D
-				&& (hdop_m < hdop_threshold_m)
-				&& (vdop_m < vdop_threshold_m)
-				&& !home_position_set
-				&& (hrt_absolute_time() - gps_position.timestamp_position < 2000000)
-				&& !current_status.flag_system_armed) {
-				warnx("setting home position");
-
-				/* copy position data to uORB home message, store it locally as well */
-				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;
-
-				/* 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 */
-				home_position_set = true;
-				tune_confirm();
-			}
-		}
-
-		/* ignore RC signals if in offboard control mode */
-		if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) {
-			/* Start RC state check */
-
-			if ((hrt_absolute_time() - sp_man.timestamp) < 100000) {
-
-				// /*
-				//  * Check if manual control modes have to be switched
-				//  */
-				// if (!isfinite(sp_man.manual_mode_switch)) {
-				// 	warnx("man mode sw not finite\n");
-
-				// 	/* this switch is not properly mapped, set default */
-				// 	if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
-				// 		(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
-				// 		(current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) {
-
-				// 		/* assuming a rotary wing, fall back to SAS */
-				// 		current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-				// 		current_status.flag_control_attitude_enabled = true;
-				// 		current_status.flag_control_rates_enabled = true;
-				// 	} else {
-
-				// 		/* assuming a fixed wing, fall back to direct pass-through */
-				// 		current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
-				// 		current_status.flag_control_attitude_enabled = false;
-				// 		current_status.flag_control_rates_enabled = false;
-				// 	}
-
-				// } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) {
-
-				// 	/* bottom stick position, set direct mode for vehicles supporting it */
-				// 	if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
-				// 		(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
-				// 		(current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) {
-
-				// 		/* assuming a rotary wing, fall back to SAS */
-				// 		current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-				// 		current_status.flag_control_attitude_enabled = true;
-				// 		current_status.flag_control_rates_enabled = true;
-				// 	} else {
-
-				// 		/* assuming a fixed wing, set to direct pass-through as requested */
-				// 		current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
-				// 		current_status.flag_control_attitude_enabled = false;
-				// 		current_status.flag_control_rates_enabled = false;
-				// 	}
-
-				// } else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) {
-
-				// 	/* top stick position, set SAS for all vehicle types */
-				// 	current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-				// 	current_status.flag_control_attitude_enabled = true;
-				// 	current_status.flag_control_rates_enabled = true;
-
-				// } else {
-				// 	/* center stick position, set rate control */
-				// 	current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES;
-				// 	current_status.flag_control_attitude_enabled = false;
-				// 	current_status.flag_control_rates_enabled = true;
-				// }
-
-				// warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode);
-
-				/*
-				 * Check if manual stability control modes have to be switched
-				 */
-				if (!isfinite(sp_man.manual_sas_switch)) {
-
-					/* this switch is not properly mapped, set default */
-					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
-
-				} else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) {
-
-					/* bottom stick position, set altitude hold */
-					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE;
-
-				} else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) {
-
-					/* top stick position */
-					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE;
-
-				} else {
-					/* center stick position, set default */
-					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
-				}
-
-				/*
-				 * Check if left stick is in lower left position --> switch to standby state.
-				 * Do this only for multirotors, not for fixed wing aircraft.
-				 */
-				if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
-				     (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
-				     (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
-				    ) &&
-				    ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) &&
-				    (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) {
-					if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-						update_state_machine_disarm(stat_pub, &current_status, mavlink_fd);
-						stick_on_counter = 0;
-
-					} else {
-						stick_off_counter++;
-						stick_on_counter = 0;
-					}
-				}
-
-				/* check if left stick is in lower right position --> arm */
-				if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) {
-					if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-						update_state_machine_arm(stat_pub, &current_status, mavlink_fd);
-						stick_on_counter = 0;
-
-					} else {
-						stick_on_counter++;
-						stick_off_counter = 0;
-					}
-				}
-
-				/* check manual override switch - switch to manual or auto mode */
-				if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) {
-					/* enable manual override */
-					update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
-
-				} else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) {
-					// /* check auto mode switch for correct mode */
-					// if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) {
-					// 	/* enable guided mode */
-					// 	update_state_machine_mode_guided(stat_pub, &current_status, mavlink_fd);
-
-					// } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) {
-						// XXX hardcode to auto for now
-						update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
-
-					// }
-
-				} else {
-					/* center stick position, set SAS for all vehicle types */
-					update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
-				}
-
-				/* handle the case where RC signal was regained */
-				if (!current_status.rc_signal_found_once) {
-					current_status.rc_signal_found_once = true;
-					mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME.");
-
-				} else {
-					if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!");
-				}
-
-				current_status.rc_signal_cutting_off = false;
-				current_status.rc_signal_lost = false;
-				current_status.rc_signal_lost_interval = 0;
-
-			} else {
-				static uint64_t last_print_time = 0;
-
-				/* print error message for first RC glitch and then every 5 s / 5000 ms) */
-				if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) {
-					/* only complain if the offboard control is NOT active */
-					current_status.rc_signal_cutting_off = true;
-					mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!");
-					last_print_time = hrt_absolute_time();
-				}
-
-				/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
-				current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp;
-
-				/* if the RC signal is gone for a full second, consider it lost */
-				if (current_status.rc_signal_lost_interval > 1000000) {
-					current_status.rc_signal_lost = true;
-					current_status.failsave_lowlevel = true;
-					state_changed = true;
-				}
-
-				// if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) {
-				// 	publish_armed_status(&current_status);
-				// }
-			}
-		}
-
-
-
-
-		/* End mode switch */
-
-		/* END RC state check */
-
-
-		/* State machine update for offboard control */
-		if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) {
-			if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) {
-
-				/* decide about attitude control flag, enable in att/pos/vel */
-				bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE ||
-							      sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY ||
-							      sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION);
-
-				/* decide about rate control flag, enable it always XXX (for now) */
-				bool rates_ctrl_enabled = true;
-
-				/* set up control mode */
-				if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) {
-					current_status.flag_control_attitude_enabled = attitude_ctrl_enabled;
-					state_changed = true;
-				}
-
-				if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) {
-					current_status.flag_control_rates_enabled = rates_ctrl_enabled;
-					state_changed = true;
-				}
-
-				/* handle the case where offboard control signal was regained */
-				if (!current_status.offboard_control_signal_found_once) {
-					current_status.offboard_control_signal_found_once = true;
-					/* enable offboard control, disable manual input */
-					current_status.flag_control_manual_enabled = false;
-					current_status.flag_control_offboard_enabled = true;
-					state_changed = true;
-					tune_confirm();
-
-					mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST");
-
-				} else {
-					if (current_status.offboard_control_signal_lost) {
-						mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL");
-						state_changed = true;
-						tune_confirm();
-					}
-				}
-
-				current_status.offboard_control_signal_weak = false;
-				current_status.offboard_control_signal_lost = false;
-				current_status.offboard_control_signal_lost_interval = 0;
-
-				/* arm / disarm on request */
-				if (sp_offboard.armed && !current_status.flag_system_armed) {
-					update_state_machine_arm(stat_pub, &current_status, mavlink_fd);
-					/* switch to stabilized mode = takeoff */
-					update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
-
-				} else if (!sp_offboard.armed && current_status.flag_system_armed) {
-					update_state_machine_disarm(stat_pub, &current_status, mavlink_fd);
-				}
-
-			} else {
-				static uint64_t last_print_time = 0;
-
-				/* print error message for first RC glitch and then every 5 s / 5000 ms) */
-				if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) {
-					current_status.offboard_control_signal_weak = true;
-					mavlink_log_critical(mavlink_fd, "CRIT:NO OFFBOARD CONTROL!");
-					last_print_time = hrt_absolute_time();
-				}
-
-				/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
-				current_status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp;
-
-				/* if the signal is gone for 0.1 seconds, consider it lost */
-				if (current_status.offboard_control_signal_lost_interval > 100000) {
-					current_status.offboard_control_signal_lost = true;
-					current_status.failsave_lowlevel_start_time = hrt_absolute_time();
-					tune_confirm();
-
-					/* kill motors after timeout */
-					if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) {
-						current_status.failsave_lowlevel = true;
-						state_changed = true;
-					}
-				}
-			}
-		}
-
-
-		current_status.counter++;
-		current_status.timestamp = hrt_absolute_time();
-
-
-		/* If full run came back clean, transition to standby */
-		if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT &&
-		    current_status.flag_preflight_gyro_calibration == false &&
-		    current_status.flag_preflight_mag_calibration == false &&
-		    current_status.flag_preflight_accel_calibration == false) {
-			/* All ok, no calibration going on, go to standby */
-			do_state_update(stat_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-		}
-
-		/* publish at least with 1 Hz */
-		if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) {
-			publish_armed_status(&current_status);
-			orb_publish(ORB_ID(vehicle_status), stat_pub, &current_status);
-			state_changed = false;
-		}
-
-		/* Store old modes to detect and act on state transitions */
-		voltage_previous = current_status.voltage_battery;
-
-		fflush(stdout);
-		counter++;
-		usleep(COMMANDER_MONITORING_INTERVAL);
-	}
-
-	/* wait for threads to complete */
-	// pthread_join(command_handling_thread, NULL);
-	pthread_join(subsystem_info_thread, NULL);
-
-	/* close fds */
-	led_deinit();
-	buzzer_deinit();
-	close(sp_man_sub);
-	close(sp_offboard_sub);
-	close(global_position_sub);
-	close(sensor_sub);
-	close(cmd_sub);
-
-	warnx("exiting..\n");
-	fflush(stdout);
-
-	thread_running = false;
-
-	return 0;
-}
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
new file mode 100644
index 000000000..54219a34a
--- /dev/null
+++ b/src/modules/commander/commander.cpp
@@ -0,0 +1,1918 @@
+/****************************************************************************
+ *
+ *   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>
+ *
+ * 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 commander.cpp
+ * Main system state machine implementation.
+ *
+ */
+
+#include <nuttx/config.h>
+#include <pthread.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <systemlib/err.h>
+#include <debug.h>
+#include <sys/prctl.h>
+#include <sys/stat.h>
+#include <string.h>
+#include <math.h>
+#include <poll.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/offboard_control_setpoint.h>
+#include <uORB/topics/home_position.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_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 <drivers/drv_led.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_tone_alarm.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/param/param.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/err.h>
+#include <systemlib/cpuload.h>
+#include <systemlib/rc_check.h>
+
+#include "px4_custom_mode.h"
+#include "commander_helper.h"
+#include "state_machine_helper.h"
+#include "calibration_routines.h"
+#include "accelerometer_calibration.h"
+#include "gyro_calibration.h"
+#include "mag_calibration.h"
+#include "baro_calibration.h"
+#include "rc_calibration.h"
+#include "airspeed_calibration.h"
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+extern struct system_load_s system_load;
+
+/* Decouple update interval and hysteris counters, all depends on intervals */
+#define COMMANDER_MONITORING_INTERVAL 50000
+#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))
+
+#define MAVLINK_OPEN_INTERVAL 50000
+
+#define STICK_ON_OFF_LIMIT 0.75f
+#define STICK_THRUST_RANGE 1.0f
+#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 PRINT_INTERVAL	5000000
+#define PRINT_MODE_REJECT_INTERVAL	2000000
+
+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, /*  | */
+};
+
+/* Mavlink file descriptors */
+static int mavlink_fd;
+
+/* flags */
+static bool commander_initialized = false;
+static volatile bool thread_should_exit = false;		/**< daemon exit flag */
+static volatile bool thread_running = false;		/**< daemon status flag */
+static int daemon_task;				/**< Handle of daemon task / thread */
+
+static unsigned int leds_counter;
+/* To remember when last notification was sent */
+static uint64_t last_print_mode_reject_time = 0;
+/* if connected via USB */
+static bool on_usb_power = false;
+
+static float takeoff_alt = 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;
+
+/* tasks waiting for low prio thread */
+typedef enum {
+	LOW_PRIO_TASK_NONE = 0,
+	LOW_PRIO_TASK_PARAM_SAVE,
+	LOW_PRIO_TASK_PARAM_LOAD,
+	LOW_PRIO_TASK_GYRO_CALIBRATION,
+	LOW_PRIO_TASK_MAG_CALIBRATION,
+	LOW_PRIO_TASK_ALTITUDE_CALIBRATION,
+	LOW_PRIO_TASK_RC_CALIBRATION,
+	LOW_PRIO_TASK_ACCEL_CALIBRATION,
+	LOW_PRIO_TASK_AIRSPEED_CALIBRATION
+} low_prio_task_t;
+
+static low_prio_task_t low_prio_task = LOW_PRIO_TASK_NONE;
+
+/**
+ * The daemon 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().
+ *
+ * @ingroup apps
+ */
+extern "C" __EXPORT int commander_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+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);
+
+/**
+ * Mainloop of commander.
+ */
+int commander_thread_main(int argc, char *argv[]);
+
+void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed);
+
+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);
+
+transition_result_t check_main_state_machine(struct vehicle_status_s *current_status);
+
+void print_reject_mode(struct vehicle_status_s *current_status, const char *msg);
+
+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);
+
+/**
+ * Loop that runs at a lower rate and priority for calibration and parameter tasks.
+ */
+void *commander_low_prio_loop(void *arg);
+
+void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result);
+
+
+int commander_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("commander already running");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		daemon_task = task_spawn_cmd("commander",
+					     SCHED_DEFAULT,
+					     SCHED_PRIORITY_MAX - 40,
+					     3000,
+					     commander_thread_main,
+					     (argv) ? (const char **)&argv[2] : (const char **)NULL);
+
+		while (!thread_running) {
+			usleep(200);
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+
+		if (!thread_running)
+			errx(0, "commander already stopped");
+
+		thread_should_exit = true;
+
+		while (thread_running) {
+			usleep(200000);
+			warnx(".");
+		}
+
+		warnx("terminated.");
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("\tcommander is running");
+			print_status();
+
+		} else {
+			warnx("\tcommander not started");
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "arm")) {
+		arm();
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "disarm")) {
+		disarm();
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+void usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+
+	fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
+	exit(1);
+}
+
+void print_status()
+{
+	warnx("usb powered: %s", (on_usb_power) ? "yes" : "no");
+
+	/* read all relevant states */
+	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
+	struct vehicle_status_s state;
+	orb_copy(ORB_ID(vehicle_status), state_sub, &state);
+
+	const char *armed_str;
+
+	switch (state.arming_state) {
+	case ARMING_STATE_INIT:
+		armed_str = "INIT";
+		break;
+
+	case ARMING_STATE_STANDBY:
+		armed_str = "STANDBY";
+		break;
+
+	case ARMING_STATE_ARMED:
+		armed_str = "ARMED";
+		break;
+
+	case ARMING_STATE_ARMED_ERROR:
+		armed_str = "ARMED_ERROR";
+		break;
+
+	case ARMING_STATE_STANDBY_ERROR:
+		armed_str = "STANDBY_ERROR";
+		break;
+
+	case ARMING_STATE_REBOOT:
+		armed_str = "REBOOT";
+		break;
+
+	case ARMING_STATE_IN_AIR_RESTORE:
+		armed_str = "IN_AIR_RESTORE";
+		break;
+
+	default:
+		armed_str = "ERR: UNKNOWN STATE";
+		break;
+	}
+
+	close(state_sub);
+
+
+	warnx("arming: %s", armed_str);
+}
+
+static orb_advert_t control_mode_pub;
+static orb_advert_t status_pub;
+
+int arm()
+{
+	int arming_res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed);
+
+	if (arming_res == TRANSITION_CHANGED) {
+		mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
+		return 0;
+	} else {
+		return 1;
+	}
+}
+
+int disarm()
+{
+	int arming_res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
+
+	if (arming_res == TRANSITION_CHANGED) {
+		mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
+		return 0;
+	} else {
+		return 1;
+	}
+}
+
+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)
+{
+	/* result of the command */
+	uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
+
+	/* only handle high-priority commands here */
+
+	/* 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);
+
+				if (arming_res != TRANSITION_DENIED) {
+					mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby");
+
+				} 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);
+				}
+
+				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 main state */
+			transition_result_t main_res = TRANSITION_DENIED;
+
+			if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
+				/* use autopilot-specific mode */
+				if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
+					/* MANUAL */
+					main_res = main_state_transition(status, MAIN_STATE_MANUAL);
+
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) {
+					/* SEATBELT */
+					main_res = main_state_transition(status, MAIN_STATE_SEATBELT);
+
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) {
+					/* EASY */
+					main_res = main_state_transition(status, MAIN_STATE_EASY);
+
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
+					/* AUTO */
+					main_res = main_state_transition(status, MAIN_STATE_AUTO);
+				}
+
+			} else {
+				/* use base mode */
+				if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
+					/* AUTO */
+					main_res = main_state_transition(status, MAIN_STATE_AUTO);
+
+				} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
+					if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
+						/* EASY */
+						main_res = main_state_transition(status, MAIN_STATE_EASY);
+
+					} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
+						/* MANUAL */
+						main_res = main_state_transition(status, MAIN_STATE_MANUAL);
+					}
+				}
+			}
+
+			if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) {
+				result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+			} else {
+				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			}
+
+			break;
+		}
+
+	case VEHICLE_CMD_NAV_TAKEOFF: {
+			if (armed->armed) {
+				transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
+
+				if (nav_res == TRANSITION_CHANGED) {
+					mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command");
+				}
+
+				if (nav_res != TRANSITION_DENIED) {
+					result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+				} else {
+					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				}
+
+			} else {
+				/* reject TAKEOFF not armed */
+				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			}
+
+			break;
+		}
+
+	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
+			transition_result_t arming_res = TRANSITION_NOT_CHANGED;
+
+			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;
+
+				} else {
+					arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
+				}
+
+				if (arming_res == TRANSITION_CHANGED) {
+					mavlink_log_critical(mavlink_fd, "#audio: ARMED by component arm cmd");
+					result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+				} else {
+					mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
+					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				}
+			}
+		}
+		break;
+
+	default:
+		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);
+
+		}
+	}
+
+	/* send any requested ACKs */
+	if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
+		/* send acknowledge command */
+		// XXX TODO
+	}
+
+}
+
+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;
+
+	/* 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");
+
+	/* welcome user */
+	warnx("starting");
+
+	/* pthread for slow low prio thread */
+	pthread_t commander_low_prio_thread;
+
+	/* initialize */
+	if (led_init() != 0) {
+		warnx("ERROR: Failed to initialize leds");
+	}
+
+	if (buzzer_init() != OK) {
+		warnx("ERROR: Failed to initialize buzzer");
+	}
+
+	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+
+	/* Main state machine */
+	/* make sure we are in preflight state */
+	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.arming_state = ARMING_STATE_INIT;
+	status.hil_state = HIL_STATE_OFF;
+
+	/* neither manual nor offboard control commands have been received */
+	status.offboard_control_signal_found_once = false;
+	status.rc_signal_found_once = false;
+
+	/* 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;
+
+	/* set battery warning flag */
+	status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
+	status.condition_battery_voltage_valid = false;
+
+	// 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;
+
+	/* advertise to ORB */
+	status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
+	/* publish current state machine */
+
+	/* publish initial state */
+	status.counter++;
+	status.timestamp = hrt_absolute_time();
+	orb_publish(ORB_ID(vehicle_status), status_pub, &status);
+
+	armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
+
+	control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
+
+	/* 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);
+	}
+
+	mavlink_log_info(mavlink_fd, "[cmd] started");
+
+	int ret;
+
+	pthread_attr_t commander_low_prio_attr;
+	pthread_attr_init(&commander_low_prio_attr);
+	pthread_attr_setstacksize(&commander_low_prio_attr, 2992);
+
+	struct sched_param param;
+	(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
+
+	/* low priority */
+	param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
+	(void)pthread_attr_setschedparam(&commander_low_prio_attr, &param);
+	pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL);
+	pthread_attr_destroy(&commander_low_prio_attr);
+
+	/* Start monitoring loop */
+	unsigned counter = 0;
+	unsigned stick_off_counter = 0;
+	unsigned stick_on_counter = 0;
+
+	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;
+
+	bool status_changed = true;
+	bool param_init_forced = true;
+
+	bool updated = false;
+
+	bool rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
+
+	/* Subscribe to safety topic */
+	int safety_sub = orb_subscribe(ORB_ID(safety));
+	memset(&safety, 0, sizeof(safety));
+	safety.safety_switch_available = false;
+	safety.safety_off = false;
+
+	/* Subscribe to manual control data */
+	int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	struct manual_control_setpoint_s sp_man;
+	memset(&sp_man, 0, sizeof(sp_man));
+
+	/* 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 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));
+
+	/* Subscribe to local position data */
+	int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
+	struct vehicle_local_position_s local_position;
+	memset(&local_position, 0, sizeof(local_position));
+
+	/*
+	 * The home position is set based on GPS only, to prevent a dependency between
+	 * position estimator and commander. RAW GPS is more than good enough for a
+	 * non-flying vehicle.
+	 */
+
+	/* Subscribe to GPS topic */
+	int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	struct vehicle_gps_position_s gps_position;
+	memset(&gps_position, 0, sizeof(gps_position));
+
+	/* Subscribe to sensor topic */
+	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	struct sensor_combined_s sensors;
+	memset(&sensors, 0, sizeof(sensors));
+
+	/* Subscribe to differential pressure topic */
+	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
+	struct differential_pressure_s diff_pres;
+	memset(&diff_pres, 0, sizeof(diff_pres));
+
+	/* Subscribe to command topic */
+	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
+	struct vehicle_command_s cmd;
+	memset(&cmd, 0, sizeof(cmd));
+
+	/* Subscribe to parameters changed topic */
+	int param_changed_sub = orb_subscribe(ORB_ID(parameter_update));
+	struct parameter_update_s param_changed;
+	memset(&param_changed, 0, sizeof(param_changed));
+
+	/* Subscribe to battery topic */
+	int battery_sub = orb_subscribe(ORB_ID(battery_status));
+	struct battery_status_s battery;
+	memset(&battery, 0, sizeof(battery));
+
+	/* Subscribe to subsystem info topic */
+	int subsys_sub = orb_subscribe(ORB_ID(subsystem_info));
+	struct subsystem_info_s info;
+	memset(&info, 0, sizeof(info));
+
+	control_status_leds(&status, &armed, true);
+
+	/* now initialized */
+	commander_initialized = true;
+	thread_running = true;
+
+	start_time = hrt_absolute_time();
+
+	while (!thread_should_exit) {
+
+		if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) {
+			/* try to open the mavlink log device every once in a while */
+			mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+		}
+
+		/* update parameters */
+		orb_check(param_changed_sub, &updated);
+
+		if (updated || param_init_forced) {
+			param_init_forced = false;
+			/* parameters changed */
+			orb_copy(ORB_ID(parameter_update), param_changed_sub, &param_changed);
+
+			/* update parameters */
+			if (!armed.armed) {
+				if (param_get(_param_sys_type, &(status.system_type)) != OK) {
+					warnx("failed getting new system type");
+				}
+
+				/* disable manual override for all systems that rely on electronic stabilization */
+				if (status.system_type == VEHICLE_TYPE_COAXIAL ||
+				    status.system_type == VEHICLE_TYPE_HELICOPTER ||
+				    status.system_type == VEHICLE_TYPE_TRICOPTER ||
+				    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_changed = true;
+
+				/* re-check RC calibration */
+				rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
+
+				/* navigation parameters */
+				param_get(_param_takeoff_alt, &takeoff_alt);
+			}
+		}
+
+		orb_check(sp_man_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
+		}
+
+		orb_check(sp_offboard_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
+		}
+
+		orb_check(sensor_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
+		}
+
+		orb_check(diff_pres_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
+		}
+
+		check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed);
+
+		/* update safety topic */
+		orb_check(safety_sub, &updated);
+
+		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);
+			// }
+		}
+
+		/* update global position estimate */
+		orb_check(global_position_sub, &updated);
+
+		if (updated) {
+			/* position changed */
+			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);
+
+		if (updated) {
+			/* position changed */
+			orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
+		}
+
+		/* update condition_local_position_valid and condition_local_altitude_valid */
+		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(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_landed != local_position.landed) {
+				status.condition_landed = local_position.landed;
+				status_changed = true;
+
+				if (status.condition_landed) {
+					mavlink_log_critical(mavlink_fd, "#audio: LANDED");
+
+				} else {
+					mavlink_log_critical(mavlink_fd, "#audio: IN AIR");
+				}
+			}
+		}
+
+		/* update battery status */
+		orb_check(battery_sub, &updated);
+
+		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;
+				status.battery_current = battery.current_a;
+				status.condition_battery_voltage_valid = true;
+				status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah);
+			}
+		}
+
+		/* update subsystem */
+		orb_check(subsys_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
+
+			warnx("subsystem changed: %d\n", (int)info.subsystem_type);
+
+			/* mark / unmark as present */
+			if (info.present) {
+				status.onboard_control_sensors_present |= info.subsystem_type;
+
+			} else {
+				status.onboard_control_sensors_present &= ~info.subsystem_type;
+			}
+
+			/* mark / unmark as enabled */
+			if (info.enabled) {
+				status.onboard_control_sensors_enabled |= info.subsystem_type;
+
+			} else {
+				status.onboard_control_sensors_enabled &= ~info.subsystem_type;
+			}
+
+			/* mark / unmark as ok */
+			if (info.ok) {
+				status.onboard_control_sensors_health |= info.subsystem_type;
+
+			} else {
+				status.onboard_control_sensors_health &= ~info.subsystem_type;
+			}
+
+			status_changed = true;
+		}
+
+		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
+
+			last_idle_time = system_load.tasks[0].total_runtime;
+
+			/* check if board is connected via USB */
+			//struct stat statbuf;
+			//on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
+		}
+
+		/* 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");
+			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");
+			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);
+
+			} else {
+				arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed);
+			}
+
+			status_changed = true;
+		}
+
+		/* End battery voltage check */
+
+		/* 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);
+
+		} else {
+			// XXX: Add emergency stuff if sensors are lost
+		}
+
+
+		/*
+		 * Check for valid position information.
+		 *
+		 * If the system has a valid position source from an onboard
+		 * position estimator, it is safe to operate it autonomously.
+		 * The flag_vector_flight_mode_ok flag indicates that a minimum
+		 * set of position measurements is available.
+		 */
+
+		orb_check(gps_sub, &updated);
+
+		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);
+				}
+
+				/* mark home position as set */
+				home_position_set = true;
+				tune_positive();
+			}
+		}
+
+		/* 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;
+
+				} else {
+					if (status.rc_signal_lost) {
+						mavlink_log_critical(mavlink_fd, "#audio: 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++;
+					}
+
+				} 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.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_on_counter++;
+					}
+
+				} else {
+					stick_on_counter = 0;
+				}
+
+				if (res == TRANSITION_CHANGED) {
+					if (status.arming_state == ARMING_STATE_ARMED) {
+						mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
+
+					} else {
+						mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
+					}
+
+				} 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);
+				}
+
+				/* fill current_status according to mode switches */
+				check_mode_switches(&sp_man, &status);
+
+				/* evaluate the main state machine */
+				res = check_main_state_machine(&status);
+
+				if (res == TRANSITION_CHANGED) {
+					//mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state);
+					tune_positive();
+
+				} 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, "#audio: CRITICAL: RC SIGNAL LOST");
+					status.rc_signal_lost = true;
+					status_changed = true;
+				}
+			}
+		}
+
+
+		/* handle commands last, as the system needs to be updated to handle them */
+		orb_check(cmd_sub, &updated);
+
+		if (updated) {
+			/* got command */
+			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
+
+			/* handle it */
+			handle_command(&status, &safety, &control_mode, &cmd, &armed);
+		}
+
+		/* evaluate the navigation state machine */
+		transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position);
+
+		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);
+		}
+
+		/* 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();
+
+		hrt_abstime t1 = hrt_absolute_time();
+
+		if (navigation_state_changed || arming_state_changed) {
+			control_mode.flag_armed = armed.armed;	// copy armed state to vehicle_control_mode topic
+		}
+
+		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);
+			status_changed = true;
+		}
+
+		/* 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);
+			control_mode.timestamp = t1;
+			orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
+			armed.timestamp = t1;
+			orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
+		}
+
+		/* 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;
+
+		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
+			/* play tune on battery critical */
+			if (tune_critical_bat() == OK)
+				battery_tune_played = true;
+
+		} else if (battery_tune_played) {
+			tune_stop();
+			battery_tune_played = false;
+		}
+
+		/* reset arm_tune_played when disarmed */
+		if (status.arming_state != ARMING_STATE_ARMED || (safety.safety_switch_available && !safety.safety_off)) {
+			arm_tune_played = false;
+		}
+
+		fflush(stdout);
+		counter++;
+
+		int blink_state = blink_msg_state();
+
+		if (blink_state > 0) {
+			/* blinking LED message, don't touch LEDs */
+			if (blink_state == 2) {
+				/* blinking LED message completed, restore normal state */
+				control_status_leds(&status, &armed, true);
+			}
+
+		} else {
+			/* normal state */
+			control_status_leds(&status, &armed, status_changed);
+		}
+
+		status_changed = false;
+
+		usleep(COMMANDER_MONITORING_INTERVAL);
+	}
+
+	/* wait for threads to complete */
+	ret = pthread_join(commander_low_prio_thread, NULL);
+
+	if (ret) {
+		warn("join failed: %d", ret);
+	}
+
+	rgbled_set_mode(RGBLED_MODE_OFF);
+
+	/* close fds */
+	led_deinit();
+	buzzer_deinit();
+	close(sp_man_sub);
+	close(sp_offboard_sub);
+	close(local_position_sub);
+	close(global_position_sub);
+	close(gps_sub);
+	close(sensor_sub);
+	close(safety_sub);
+	close(cmd_sub);
+	close(subsys_sub);
+	close(diff_pres_sub);
+	close(param_changed_sub);
+	close(battery_sub);
+
+	thread_running = false;
+
+	return 0;
+}
+
+void
+check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed)
+{
+	hrt_abstime t = hrt_absolute_time();
+	bool valid_new = (t < timestamp + timeout && t > timeout && valid_in);
+
+	if (*valid_out != valid_new) {
+		*valid_out = valid_new;
+		*changed = true;
+	}
+}
+
+void
+control_status_leds(vehicle_status_s *status, 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) {
+			rgbled_set_mode(RGBLED_MODE_ON);
+			set_normal_color = true;
+
+		} else if (status->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) {
+			rgbled_set_mode(RGBLED_MODE_BREATHE);
+			set_normal_color = true;
+
+		} else {	// STANDBY_ERROR and other states
+			rgbled_set_mode(RGBLED_MODE_BLINK_NORMAL);
+			rgbled_set_color(RGBLED_COLOR_RED);
+		}
+
+		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);
+				}
+
+				/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
+
+			} else {
+				if (status->condition_local_position_valid) {
+					rgbled_set_color(RGBLED_COLOR_GREEN);
+
+				} else {
+					rgbled_set_color(RGBLED_COLOR_BLUE);
+				}
+			}
+		}
+	}
+
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+
+	/* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */
+	if (actuator_armed->armed) {
+		/* armed, solid */
+		led_on(LED_BLUE);
+
+	} else if (actuator_armed->ready_to_arm) {
+		/* ready to arm, blink at 1Hz */
+		if (leds_counter % 20 == 0)
+			led_toggle(LED_BLUE);
+
+	} else {
+		/* not ready to arm, blink at 10Hz */
+		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)
+			led_toggle(LED_AMBER);
+
+	} else {
+		led_off(LED_AMBER);
+	}
+
+	leds_counter++;
+}
+
+void
+check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status)
+{
+	/* 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;
+
+	} else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) {
+		current_status->mode_switch = MODE_SWITCH_MANUAL;
+
+	} else {
+		current_status->mode_switch = MODE_SWITCH_ASSISTED;
+	}
+
+	/* 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;
+
+	} else {
+		current_status->return_switch = RETURN_SWITCH_NONE;
+	}
+
+	/* assisted switch */
+	if (!isfinite(sp_man->assisted_switch)) {
+		current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
+
+	} else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) {
+		current_status->assisted_switch = ASSISTED_SWITCH_EASY;
+
+	} else {
+		current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
+	}
+
+	/* mission switch  */
+	if (!isfinite(sp_man->mission_switch)) {
+		current_status->mission_switch = MISSION_SWITCH_MISSION;
+
+	} else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) {
+		current_status->mission_switch = MISSION_SWITCH_NONE;
+
+	} else {
+		current_status->mission_switch = MISSION_SWITCH_MISSION;
+	}
+}
+
+transition_result_t
+check_main_state_machine(struct vehicle_status_s *current_status)
+{
+	/* evaluate the main state machine */
+	transition_result_t res = TRANSITION_DENIED;
+
+	switch (current_status->mode_switch) {
+	case MODE_SWITCH_MANUAL:
+		res = main_state_transition(current_status, 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);
+
+			if (res != TRANSITION_DENIED)
+				break;	// changed successfully or already in this state
+
+			// else fallback to SEATBELT
+			print_reject_mode(current_status, "EASY");
+		}
+
+		res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
+
+		if (res != TRANSITION_DENIED)
+			break;	// changed successfully or already in this mode
+
+		if (current_status->assisted_switch != ASSISTED_SWITCH_EASY)	// don't print both messages
+			print_reject_mode(current_status, "SEATBELT");
+
+		// else fallback to MANUAL
+		res = main_state_transition(current_status, MAIN_STATE_MANUAL);
+		// TRANSITION_DENIED is not possible here
+		break;
+
+	case MODE_SWITCH_AUTO:
+		res = main_state_transition(current_status, MAIN_STATE_AUTO);
+
+		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);
+
+		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);
+		// TRANSITION_DENIED is not possible here
+		break;
+
+	default:
+		break;
+	}
+
+	return res;
+}
+
+void
+print_reject_mode(struct vehicle_status_s *current_status, 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: 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 {
+
+			// Always show the led indication
+			led_negative();
+		}
+	}
+}
+
+void
+print_reject_arm(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();
+	}
+}
+
+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 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;
+
+			default:
+				break;
+			}
+		}
+	}
+
+	return res;
+}
+
+void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result)
+{
+	switch (result) {
+	case VEHICLE_CMD_RESULT_ACCEPTED:
+			tune_positive();
+		break;
+
+	case VEHICLE_CMD_RESULT_DENIED:
+		mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command);
+		tune_negative();
+		break;
+
+	case VEHICLE_CMD_RESULT_FAILED:
+		mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command);
+		tune_negative();
+		break;
+
+	case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
+		mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command);
+		tune_negative();
+		break;
+
+	case VEHICLE_CMD_RESULT_UNSUPPORTED:
+		mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command);
+		tune_negative();
+		break;
+
+	default:
+		break;
+	}
+}
+
+void *commander_low_prio_loop(void *arg)
+{
+	/* Set thread name */
+	prctl(PR_SET_NAME, "commander_low_prio", getpid());
+
+	/* Subscribe to command topic */
+	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
+	struct vehicle_command_s cmd;
+	memset(&cmd, 0, sizeof(cmd));
+
+	/* wakeup source(s) */
+	struct pollfd fds[1];
+
+	/* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */
+	fds[0].fd = cmd_sub;
+	fds[0].events = POLLIN;
+
+	while (!thread_should_exit) {
+		/* wait for up to 200ms for data */
+		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200);
+
+		/* timed out - periodic check for thread_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 error %d, %d", pret, errno);
+			continue;
+		}
+
+		/* if we reach here, we have a valid command */
+		orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
+
+		/* 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)
+			continue;
+
+		/* only handle low-priority commands here */
+		switch (cmd.command) {
+
+		case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
+			if (is_safe(&status, &safety, &armed)) {
+
+				if (((int)(cmd.param1)) == 1) {
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+					usleep(100000);
+					/* reboot */
+					systemreset(false);
+
+				} else if (((int)(cmd.param1)) == 3) {
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+					usleep(100000);
+					/* reboot to bootloader */
+					systemreset(true);
+
+				} else {
+					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+				}
+
+			} else {
+				answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+			}
+
+			break;
+
+		case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
+
+				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)) {
+					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+					break;
+				}
+
+				if ((int)(cmd.param1) == 1) {
+					/* gyro calibration */
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+					calib_ret = do_gyro_calibration(mavlink_fd);
+
+				} else if ((int)(cmd.param2) == 1) {
+					/* magnetometer calibration */
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+					calib_ret = do_mag_calibration(mavlink_fd);
+
+				} else if ((int)(cmd.param3) == 1) {
+					/* zero-altitude pressure calibration */
+					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+
+				} else if ((int)(cmd.param4) == 1) {
+					/* RC calibration */
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+					/* disable RC control input completely */
+					status.rc_input_blocked = true;
+					calib_ret = OK;
+					mavlink_log_info(mavlink_fd, "CAL: Disabling RC IN");
+
+				} else if ((int)(cmd.param4) == 2) {
+					/* RC trim calibration */
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+					calib_ret = do_trim_calibration(mavlink_fd);
+
+				} else if ((int)(cmd.param5) == 1) {
+					/* accelerometer calibration */
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+					calib_ret = do_accel_calibration(mavlink_fd);
+
+				} else if ((int)(cmd.param6) == 1) {
+					/* 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) {
+						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+						/* enable RC control input */
+						status.rc_input_blocked = false;
+						mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN");
+					}
+
+					/* this always succeeds */
+					calib_ret = OK;
+
+				}
+
+				if (calib_ret == OK)
+					tune_positive();
+				else
+					tune_negative();
+
+				arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
+
+				break;
+			}
+
+		case VEHICLE_CMD_PREFLIGHT_STORAGE: {
+
+				if (((int)(cmd.param1)) == 0) {
+					int ret = param_load_default();
+
+					if (ret == OK) {
+						mavlink_log_info(mavlink_fd, "[cmd] parameters loaded");
+						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+
+					} else {
+						mavlink_log_critical(mavlink_fd, "#audio: parameters load ERROR");
+
+						/* convenience as many parts of NuttX use negative errno */
+						if (ret < 0)
+							ret = -ret;
+
+						if (ret < 1000)
+							mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret));
+
+						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
+					}
+
+				} else if (((int)(cmd.param1)) == 1) {
+					int ret = param_save_default();
+
+					if (ret == OK) {
+						mavlink_log_info(mavlink_fd, "[cmd] parameters saved");
+						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+
+					} else {
+						mavlink_log_critical(mavlink_fd, "#audio: parameters save error");
+
+						/* convenience as many parts of NuttX use negative errno */
+						if (ret < 0)
+							ret = -ret;
+
+						if (ret < 1000)
+							mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret));
+
+						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
+					}
+				}
+
+				break;
+			}
+
+		case VEHICLE_CMD_START_RX_PAIR:
+		/* handled in the IO driver */
+		break;
+
+		default:
+			answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
+			break;
+		}
+
+		/* send any requested ACKs */
+		if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE
+		    && cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) {
+			/* send acknowledge command */
+			// XXX TODO
+		}
+	}
+
+	close(cmd_sub);
+
+	return NULL;
+}
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
new file mode 100644
index 000000000..033e7dc88
--- /dev/null
+++ b/src/modules/commander/commander_helper.cpp
@@ -0,0 +1,306 @@
+/****************************************************************************
+ *
+ *   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>
+ *
+ * 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 commander_helper.cpp
+ * Commander helper functions implementations
+ */
+
+#include <stdio.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <fcntl.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 <systemlib/err.h>
+#include <systemlib/param/param.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_tone_alarm.h>
+#include <drivers/drv_led.h>
+#include <drivers/drv_rgbled.h>
+
+#include "commander_helper.h"
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+#define BLINK_MSG_TIME	700000	// 3 fast blinks
+
+bool is_multirotor(const struct vehicle_status_s *current_status)
+{
+	return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
+		(current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
+		(current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
+		(current_status->system_type == VEHICLE_TYPE_TRICOPTER));
+}
+
+bool is_rotary_wing(const struct vehicle_status_s *current_status)
+{
+	return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER)
+	       || (current_status->system_type == VEHICLE_TYPE_COAXIAL);
+}
+
+static int buzzer;
+static hrt_abstime blink_msg_end;
+
+int buzzer_init()
+{
+	buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY);
+
+	if (buzzer < 0) {
+		warnx("Buzzer: open fail\n");
+		return ERROR;
+	}
+
+	return OK;
+}
+
+void buzzer_deinit()
+{
+	close(buzzer);
+}
+
+void tune_error()
+{
+	ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE);
+}
+
+void tune_positive()
+{
+	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);
+}
+
+void tune_neutral()
+{
+	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);
+}
+
+void led_negative()
+{
+	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);
+}
+
+int blink_msg_state()
+{
+	if (blink_msg_end == 0) {
+		return 0;
+
+	} else if (hrt_absolute_time() > blink_msg_end) {
+		return 2;
+
+	} else {
+		return 1;
+	}
+}
+
+static int leds;
+static int rgbleds;
+
+int led_init()
+{
+	blink_msg_end = 0;
+
+	/* first open normal LEDs */
+	leds = open(LED_DEVICE_PATH, 0);
+
+	if (leds < 0) {
+		warnx("LED: open fail\n");
+		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;
+	}
+
+#endif
+
+	if (ioctl(leds, LED_ON, LED_AMBER)) {
+		warnx("Amber LED: ioctl fail\n");
+		return ERROR;
+	}
+
+	/* 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
+	}
+
+	return 0;
+}
+
+void led_deinit()
+{
+	close(leds);
+
+	if (rgbleds != -1) {
+		close(rgbleds);
+	}
+}
+
+int led_toggle(int led)
+{
+	return ioctl(leds, LED_TOGGLE, led);
+}
+
+int led_on(int led)
+{
+	return ioctl(leds, LED_ON, led);
+}
+
+int led_off(int led)
+{
+	return ioctl(leds, LED_OFF, led);
+}
+
+void rgbled_set_color(rgbled_color_t color)
+{
+
+	if (rgbleds != -1)
+		ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color);
+}
+
+void rgbled_set_mode(rgbled_mode_t mode)
+{
+
+	if (rgbleds != -1)
+		ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode);
+}
+
+void rgbled_set_pattern(rgbled_pattern_t *pattern)
+{
+
+	if (rgbleds != -1)
+		ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
+}
+
+float battery_remaining_estimate_voltage(float voltage, float discharged)
+{
+	float ret = 0;
+	static param_t bat_v_empty_h;
+	static param_t bat_v_full_h;
+	static param_t bat_n_cells_h;
+	static param_t bat_capacity_h;
+	static float bat_v_empty = 3.2f;
+	static float bat_v_full = 4.0f;
+	static int bat_n_cells = 3;
+	static float bat_capacity = -1.0f;
+	static bool initialized = false;
+	static unsigned int counter = 0;
+
+	if (!initialized) {
+		bat_v_empty_h = param_find("BAT_V_EMPTY");
+		bat_v_full_h = param_find("BAT_V_FULL");
+		bat_n_cells_h = param_find("BAT_N_CELLS");
+		bat_capacity_h = param_find("BAT_CAPACITY");
+		initialized = true;
+	}
+
+	if (counter % 100 == 0) {
+		param_get(bat_v_empty_h, &bat_v_empty);
+		param_get(bat_v_full_h, &bat_v_full);
+		param_get(bat_n_cells_h, &bat_n_cells);
+		param_get(bat_capacity_h, &bat_capacity);
+	}
+
+	counter++;
+
+	/* remaining charge estimate based on voltage */
+	float remaining_voltage = (voltage - bat_n_cells * bat_v_empty) / (bat_n_cells * (bat_v_full - bat_v_empty));
+
+	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;
+	}
+
+	/* limit to sane values */
+	ret = (ret < 0.0f) ? 0.0f : ret;
+	ret = (ret > 1.0f) ? 1.0f : ret;
+	return ret;
+}
diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h
new file mode 100644
index 000000000..af25a5e97
--- /dev/null
+++ b/src/modules/commander/commander_helper.h
@@ -0,0 +1,90 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * 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 commander_helper.h
+ * Commander helper functions definitions
+ */
+
+#ifndef COMMANDER_HELPER_H_
+#define COMMANDER_HELPER_H_
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <drivers/drv_rgbled.h>
+
+
+bool is_multirotor(const struct vehicle_status_s *current_status);
+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();
+
+int blink_msg_state();
+
+int led_init(void);
+void led_deinit(void);
+int led_toggle(int led);
+int led_on(int led);
+int led_off(int led);
+
+void rgbled_set_color(rgbled_color_t color);
+void rgbled_set_mode(rgbled_mode_t mode);
+void rgbled_set_pattern(rgbled_pattern_t *pattern);
+
+/**
+ * Estimate remaining battery charge.
+ *
+ * Use integral of current if battery capacity known (BAT_CAPACITY parameter set),
+ * else use simple estimate based on voltage.
+ *
+ * @return the estimated remaining capacity in 0..1
+ */
+float battery_remaining_estimate_voltage(float voltage, float discharged);
+
+#endif /* COMMANDER_HELPER_H_ */
diff --git a/src/modules/commander/commander.h b/src/modules/commander/commander_params.c
index 04b4e72ab..e10b7f18d 100644
--- a/src/modules/commander/commander.h
+++ b/src/modules/commander/commander_params.c
@@ -1,10 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 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>
+ *   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
@@ -36,23 +33,24 @@
  ****************************************************************************/
 
 /**
- * @file commander.h
- * Main system state machine definition.
+ * @file commander_params.c
+ *
+ * Parameters defined by the sensors task.
  *
- * @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>
- *
  */
 
-#ifndef COMMANDER_H_
-#define COMMANDER_H_
-
-#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
-#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
-
-void tune_confirm(void);
-void tune_error(void);
+#include <nuttx/config.h>
+#include <systemlib/param/param.h>
 
-#endif /* COMMANDER_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);
+PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
+PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f);
+PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
+PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
diff --git a/src/modules/commander/commander_tests/commander_tests.cpp b/src/modules/commander/commander_tests/commander_tests.cpp
new file mode 100644
index 000000000..6e72cf0d9
--- /dev/null
+++ b/src/modules/commander/commander_tests/commander_tests.cpp
@@ -0,0 +1,55 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@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 commander_tests.cpp
+ * Commander unit tests. Run the tests as follows:
+ *   nsh> commander_tests
+ *
+ */
+
+#include <systemlib/err.h>
+
+#include "state_machine_helper_test.h"
+
+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();
+
+	return 0;
+}
diff --git a/src/modules/commander/commander_tests/module.mk b/src/modules/commander/commander_tests/module.mk
new file mode 100644
index 000000000..4d10275d1
--- /dev/null
+++ b/src/modules/commander/commander_tests/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.
+#
+############################################################################
+
+#
+# System state machine tests.
+#
+
+MODULE_COMMAND		= commander_tests
+SRCS			= commander_tests.cpp \
+			state_machine_helper_test.cpp \
+			../state_machine_helper.cpp
diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
new file mode 100644
index 000000000..40bedd9f3
--- /dev/null
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
@@ -0,0 +1,247 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@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 state_machine_helper_test.cpp
+ * System state machine unit test.
+ *
+ */
+
+#include "state_machine_helper_test.h"
+
+#include "../state_machine_helper.h"
+#include <unit_test/unit_test.h>
+
+class StateMachineHelperTest : public UnitTest
+{
+public:
+	StateMachineHelperTest();
+	virtual ~StateMachineHelperTest();
+
+	virtual const char*	run_tests();
+
+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();
+};
+
+StateMachineHelperTest::StateMachineHelperTest() {
+}
+
+StateMachineHelperTest::~StateMachineHelperTest() {
+}
+
+const char*
+StateMachineHelperTest::arming_state_transition_test()
+{
+	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 actuator_armed_s armed;
+
+	// TODO(sjwilks): ARM then DISARM.
+	return 0;
+}
+
+const char*
+StateMachineHelperTest::main_state_transition_test()
+{
+	struct vehicle_status_s current_state;
+	main_state_t new_main_state;
+	
+	// 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;
+}
+
+const char*
+StateMachineHelperTest::is_safe_test()
+{
+	struct vehicle_status_s current_state;
+	struct safety_s safety;
+	struct actuator_armed_s armed;
+
+	armed.armed = false;
+	armed.lockdown = false;
+	safety.safety_switch_available = true;
+	safety.safety_off = false;
+	mu_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));
+
+	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));
+
+	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));
+
+	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));
+
+	return 0;
+}
+
+const char*
+StateMachineHelperTest::run_tests()
+{
+	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;
+}
+
+void
+state_machine_helper_test()
+{
+	StateMachineHelperTest* test = new StateMachineHelperTest();
+	test->UnitTest::print_results(test->run_tests());
+}
diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/modules/commander/commander_tests/state_machine_helper_test.h
new file mode 100644
index 000000000..10a68e602
--- /dev/null
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.h
@@ -0,0 +1,44 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@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 state_machine_helper_test.h
+ */
+
+#ifndef STATE_MACHINE_HELPER_TEST_H_
+#define STATE_MACHINE_HELPER_TEST_
+
+void state_machine_helper_test();
+
+#endif /* STATE_MACHINE_HELPER_TEST_H_ */
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
new file mode 100644
index 000000000..30cd0d48d
--- /dev/null
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -0,0 +1,305 @@
+/****************************************************************************
+ *
+ *   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 gyro_calibration.cpp
+ *
+ * Gyroscope calibration routine
+ */
+
+#include "gyro_calibration.h"
+#include "calibration_messages.h"
+#include "commander_helper.h"
+
+#include <stdio.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <math.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/sensor_combined.h>
+#include <drivers/drv_gyro.h>
+#include <mavlink/mavlink_log.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+static const char *sensor_name = "gyro";
+
+int do_gyro_calibration(int mavlink_fd)
+{
+	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
+	mavlink_log_info(mavlink_fd, "don't move system");
+
+	struct gyro_scale gyro_scale = {
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+	};
+
+	int res = OK;
+
+	/* reset all offsets to zero and all scales to one */
+	int fd = open(GYRO_DEVICE_PATH, 0);
+	res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
+	close(fd);
+
+	if (res != OK) {
+		mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
+	}
+
+	if (res == OK) {
+		/* determine gyro mean values */
+		const unsigned calibration_count = 5000;
+		unsigned calibration_counter = 0;
+		unsigned poll_errcount = 0;
+
+		/* subscribe to gyro sensor topic */
+		int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro));
+		struct gyro_report gyro_report;
+
+		while (calibration_counter < calibration_count) {
+			/* wait blocking for new data */
+			struct pollfd fds[1];
+			fds[0].fd = sub_sensor_gyro;
+			fds[0].events = POLLIN;
+
+			int poll_ret = poll(fds, 1, 1000);
+
+			if (poll_ret > 0) {
+				orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report);
+				gyro_scale.x_offset += gyro_report.x;
+				gyro_scale.y_offset += gyro_report.y;
+				gyro_scale.z_offset += gyro_report.z;
+				calibration_counter++;
+
+				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++;
+			}
+
+			if (poll_errcount > 1000) {
+				mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
+				res = ERROR;
+				break;
+			}
+		}
+
+		close(sub_sensor_gyro);
+
+		gyro_scale.x_offset /= calibration_count;
+		gyro_scale.y_offset /= calibration_count;
+		gyro_scale.z_offset /= calibration_count;
+	}
+
+	if (res == OK) {
+		/* check offsets */
+		if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) {
+			mavlink_log_critical(mavlink_fd, "ERROR: offset is NaN");
+			res = ERROR;
+		}
+	}
+
+	if (res == OK) {
+		/* set offset parameters to new values */
+		if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset))
+		    || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset))
+		    || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) {
+			mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params");
+			res = ERROR;
+		}
+	}
+
+#if 0
+	/* beep on offset calibration end */
+	mavlink_log_info(mavlink_fd, "gyro offset calibration done");
+	tune_neutral();
+
+	/* scale calibration */
+	/* this was only a proof of concept and is currently not working. scaling will be set to 1.0 for now. */
+
+	mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip.");
+	warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip.");
+
+	/* apply new offsets */
+	fd = open(GYRO_DEVICE_PATH, 0);
+
+	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale))
+		warn("WARNING: failed to apply new offsets for gyro");
+
+	close(fd);
+
+
+	unsigned rotations_count = 30;
+	float gyro_integral = 0.0f;
+	float baseline_integral = 0.0f;
+
+	// XXX change to mag topic
+	orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
+
+	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;
+
+
+	uint64_t last_time = hrt_absolute_time();
+	uint64_t start_time = hrt_absolute_time();
+
+	while ((int)fabsf(baseline_integral / (2.0f * M_PI_F)) < rotations_count) {
+
+		/* abort this loop if not rotated more than 180 degrees within 5 seconds */
+		if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f)
+		    && (hrt_absolute_time() - start_time > 5 * 1e6)) {
+			mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done");
+			close(sub_sensor_combined);
+			return OK;
+		}
+
+		/* wait blocking for new data */
+		struct pollfd fds[1];
+		fds[0].fd = sub_sensor_combined;
+		fds[0].events = POLLIN;
+
+		int poll_ret = poll(fds, 1, 1000);
+
+		if (poll_ret) {
+
+			float dt_ms = (hrt_absolute_time() - last_time) / 1e3f;
+			last_time = hrt_absolute_time();
+
+			orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
+
+			// XXX this is just a proof of concept and needs world / body
+			// transformation and more
+
+			//math::Vector2f magNav(raw.magnetometer_ga);
+
+			// calculate error between estimate and measurement
+			// apply declination correction for true heading as well.
+			//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;
+
+			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;
+
+			baseline_integral += diff;
+			mag_last = mag;
+			// Jump through some timing scale hoops to avoid
+			// operating near the 1e6/1e8 max sane resolution of float.
+			gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f;
+
+//			warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral);
+
+			// } else if (poll_ret == 0) {
+			// 	/* any poll failure for 1s is a reason to abort */
+			// 	mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
+			// 	return;
+		}
+	}
+
+	float gyro_scale = baseline_integral / gyro_integral;
+
+	warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale);
+	mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale);
+
+
+	if (!isfinite(gyro_scale.x_scale) || !isfinite(gyro_scale.y_scale) || !isfinite(gyro_scale.z_scale)) {
+		mavlink_log_info(mavlink_fd, "gyro scale calibration FAILED (NaN)");
+		close(sub_sensor_gyro);
+		mavlink_log_critical(mavlink_fd, "gyro calibration failed");
+		return ERROR;
+	}
+
+	/* beep on calibration end */
+	mavlink_log_info(mavlink_fd, "gyro scale calibration done");
+	tune_neutral();
+
+#endif
+
+	if (res == OK) {
+		/* set scale parameters to new values */
+		if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale))
+		    || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale))
+		    || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) {
+			mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params");
+			res = ERROR;
+		}
+	}
+
+	if (res == OK) {
+		/* apply new scaling and offsets */
+		fd = open(GYRO_DEVICE_PATH, 0);
+		res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
+		close(fd);
+
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
+		}
+	}
+
+	if (res == OK) {
+		/* auto-save to EEPROM */
+		res = param_save_default();
+
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
+		}
+	}
+
+	if (res == OK) {
+		mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
+
+	} else {
+		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+	}
+
+	return res;
+}
diff --git a/src/modules/commander/gyro_calibration.h b/src/modules/commander/gyro_calibration.h
new file mode 100644
index 000000000..59c32a15e
--- /dev/null
+++ b/src/modules/commander/gyro_calibration.h
@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ *   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 gyro_calibration.h
+ * Gyroscope calibration routine
+ */
+
+#ifndef GYRO_CALIBRATION_H_
+#define GYRO_CALIBRATION_H_
+
+#include <stdint.h>
+
+int do_gyro_calibration(int mavlink_fd);
+
+#endif /* GYRO_CALIBRATION_H_ */
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
new file mode 100644
index 000000000..4ebf266f4
--- /dev/null
+++ b/src/modules/commander/mag_calibration.cpp
@@ -0,0 +1,285 @@
+/****************************************************************************
+ *
+ *   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 mag_calibration.cpp
+ *
+ * Magnetometer calibration routine
+ */
+
+#include "mag_calibration.h"
+#include "commander_helper.h"
+#include "calibration_routines.h"
+#include "calibration_messages.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <poll.h>
+#include <math.h>
+#include <fcntl.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/sensor_combined.h>
+#include <drivers/drv_mag.h>
+#include <mavlink/mavlink_log.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+static const char *sensor_name = "mag";
+
+int do_mag_calibration(int mavlink_fd)
+{
+	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
+	mavlink_log_info(mavlink_fd, "don't move system");
+
+	/* 45 seconds */
+	uint64_t calibration_interval = 45 * 1000 * 1000;
+
+	/* maximum 500 values */
+	const unsigned int calibration_maxcount = 500;
+	unsigned int calibration_counter;
+
+	struct mag_scale mscale_null = {
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+	};
+
+	int res = OK;
+
+	/* erase old calibration */
+	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+	res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
+
+	if (res != OK) {
+		mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
+	}
+
+	if (res == OK) {
+		/* calibrate range */
+		res = ioctl(fd, MAGIOCCALIBRATE, fd);
+
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, "Skipped scale calibration");
+			/* this is non-fatal - mark it accordingly */
+			res = OK;
+		}
+	}
+
+	close(fd);
+
+	float *x = NULL;
+	float *y = NULL;
+	float *z = NULL;
+
+	if (res == OK) {
+		/* allocate memory */
+		mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20);
+
+		x = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
+		y = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
+		z = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
+
+		if (x == NULL || y == NULL || z == NULL) {
+			mavlink_log_critical(mavlink_fd, "ERROR: out of memory");
+			res = ERROR;
+			return res;
+		}
+	} else {
+		/* exit */
+		return ERROR;
+	}
+
+	if (res == OK) {
+		int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
+		struct mag_report mag;
+
+		/* limit update rate to get equally spaced measurements over time (in ms) */
+		orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
+
+		/* calibrate offsets */
+		uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
+		unsigned poll_errcount = 0;
+
+		mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis");
+
+		calibration_counter = 0;
+
+		while (hrt_absolute_time() < calibration_deadline &&
+		       calibration_counter < calibration_maxcount) {
+
+			/* wait blocking for new data */
+			struct pollfd fds[1];
+			fds[0].fd = sub_mag;
+			fds[0].events = POLLIN;
+
+			int poll_ret = poll(fds, 1, 1000);
+
+			if (poll_ret > 0) {
+				orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
+
+				x[calibration_counter] = mag.x;
+				y[calibration_counter] = mag.y;
+				z[calibration_counter] = mag.z;
+
+				calibration_counter++;
+
+				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++;
+			}
+
+			if (poll_errcount > 1000) {
+				mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
+				res = ERROR;
+				break;
+			}
+		}
+
+		close(sub_mag);
+	}
+
+	float sphere_x;
+	float sphere_y;
+	float sphere_z;
+	float sphere_radius;
+
+	if (res == OK) {
+
+		/* sphere fit */
+		mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70);
+		sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
+		mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 80);
+
+		if (!isfinite(sphere_x) || !isfinite(sphere_y) || !isfinite(sphere_z)) {
+			mavlink_log_critical(mavlink_fd, "ERROR: NaN in sphere fit");
+			res = ERROR;
+		}
+	}
+
+	if (x != NULL)
+		free(x);
+
+	if (y != NULL)
+		free(y);
+
+	if (z != NULL)
+		free(z);
+
+	if (res == OK) {
+		/* apply calibration and set parameters */
+		struct mag_scale mscale;
+
+		fd = open(MAG_DEVICE_PATH, 0);
+		res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale);
+
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, "ERROR: failed to get current calibration");
+		}
+
+		if (res == OK) {
+			mscale.x_offset = sphere_x;
+			mscale.y_offset = sphere_y;
+			mscale.z_offset = sphere_z;
+
+			res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale);
+
+			if (res != OK) {
+				mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
+			}
+		}
+
+		close(fd);
+
+		if (res == OK) {
+			/* set parameters */
+			if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset)))
+				res = ERROR;
+
+			if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset)))
+				res = ERROR;
+
+			if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset)))
+				res = ERROR;
+
+			if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale)))
+				res = ERROR;
+
+			if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale)))
+				res = ERROR;
+
+			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);
+			}
+
+			mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 90);
+		}
+
+		if (res == OK) {
+			/* auto-save to EEPROM */
+			res = param_save_default();
+
+			if (res != OK) {
+				mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
+			}
+		}
+
+		mavlink_log_info(mavlink_fd, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
+				 (double)mscale.y_offset, (double)mscale.z_offset);
+		mavlink_log_info(mavlink_fd, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
+				 (double)mscale.y_scale, (double)mscale.z_scale);
+
+		if (res == OK) {
+			mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
+
+		} else {
+			mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+		}
+	}
+
+	return res;
+}
diff --git a/src/modules/commander/mag_calibration.h b/src/modules/commander/mag_calibration.h
new file mode 100644
index 000000000..a101cd7b1
--- /dev/null
+++ b/src/modules/commander/mag_calibration.h
@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ *   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 mag_calibration.h
+ * Magnetometer calibration routine
+ */
+
+#ifndef MAG_CALIBRATION_H_
+#define MAG_CALIBRATION_H_
+
+#include <stdint.h>
+
+int do_mag_calibration(int mavlink_fd);
+
+#endif /* MAG_CALIBRATION_H_ */
diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk
index fe44e955a..554dfcb08 100644
--- a/src/modules/commander/module.mk
+++ b/src/modules/commander/module.mk
@@ -36,8 +36,14 @@
 #
 
 MODULE_COMMAND	 	= commander
-SRCS		 	= commander.c \
-			state_machine_helper.c \
-			calibration_routines.c \
-			accelerometer_calibration.c
-
+SRCS		 	= commander.cpp \
+			commander_params.c \
+			state_machine_helper.cpp \
+			commander_helper.cpp \
+			calibration_routines.cpp \
+			accelerometer_calibration.cpp \
+			gyro_calibration.cpp \
+			mag_calibration.cpp \
+			baro_calibration.cpp \
+			rc_calibration.cpp \
+			airspeed_calibration.cpp
diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h
new file mode 100644
index 000000000..b60a7e0b9
--- /dev/null
+++ b/src/modules/commander/px4_custom_mode.h
@@ -0,0 +1,37 @@
+/*
+ * px4_custom_mode.h
+ *
+ *  Created on: 09.08.2013
+ *      Author: ton
+ */
+
+#ifndef PX4_CUSTOM_MODE_H_
+#define PX4_CUSTOM_MODE_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_AUTO,
+};
+
+enum PX4_CUSTOM_SUB_MODE_AUTO {
+	PX4_CUSTOM_SUB_MODE_AUTO_READY = 1,
+	PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF,
+	PX4_CUSTOM_SUB_MODE_AUTO_LOITER,
+	PX4_CUSTOM_SUB_MODE_AUTO_MISSION,
+	PX4_CUSTOM_SUB_MODE_AUTO_RTL,
+	PX4_CUSTOM_SUB_MODE_AUTO_LAND,
+};
+
+union px4_custom_mode {
+	struct {
+		uint16_t reserved;
+		uint8_t main_mode;
+		uint8_t sub_mode;
+	};
+	uint32_t data;
+	float data_float;
+};
+
+#endif /* PX4_CUSTOM_MODE_H_ */
diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp
new file mode 100644
index 000000000..41f3ca0aa
--- /dev/null
+++ b/src/modules/commander/rc_calibration.cpp
@@ -0,0 +1,92 @@
+/****************************************************************************
+ *
+ *   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 rc_calibration.cpp
+ * Remote Control calibration routine
+ */
+
+#include "rc_calibration.h"
+#include "commander_helper.h"
+
+#include <poll.h>
+#include <unistd.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <mavlink/mavlink_log.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+int do_trim_calibration(int mavlink_fd)
+{
+	int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
+	usleep(200000);
+	struct manual_control_setpoint_s sp;
+	bool changed;
+	orb_check(sub_man, &changed);
+
+	if (!changed) {
+		mavlink_log_critical(mavlink_fd, "no inputs, aborting");
+		return ERROR;
+	}
+
+	orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
+
+	/* set parameters */
+	float p = sp.roll;
+	param_set(param_find("TRIM_ROLL"), &p);
+	p = sp.pitch;
+	param_set(param_find("TRIM_PITCH"), &p);
+	p = sp.yaw;
+	param_set(param_find("TRIM_YAW"), &p);
+
+	/* store to permanent storage */
+	/* auto-save */
+	int save_ret = param_save_default();
+
+	if (save_ret != 0) {
+		mavlink_log_critical(mavlink_fd, "TRIM: SAVE FAIL");
+		close(sub_man);
+		return ERROR;
+	}
+
+	mavlink_log_info(mavlink_fd, "trim cal done");
+	close(sub_man);
+	return OK;
+}
diff --git a/src/modules/commander/rc_calibration.h b/src/modules/commander/rc_calibration.h
new file mode 100644
index 000000000..45efedf55
--- /dev/null
+++ b/src/modules/commander/rc_calibration.h
@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ *   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 rc_calibration.h
+ * Remote Control calibration routine
+ */
+
+#ifndef RC_CALIBRATION_H_
+#define RC_CALIBRATION_H_
+
+#include <stdint.h>
+
+int do_trim_calibration(int mavlink_fd);
+
+#endif /* RC_CALIBRATION_H_ */
diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c
deleted file mode 100644
index ab728c7bb..000000000
--- a/src/modules/commander/state_machine_helper.c
+++ /dev/null
@@ -1,757 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * 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_machine_helper.c
- * State machine helper functions implementations
- */
-
-#include <stdio.h>
-#include <unistd.h>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/actuator_controls.h>
-#include <systemlib/systemlib.h>
-#include <drivers/drv_hrt.h>
-#include <mavlink/mavlink_log.h>
-
-#include "state_machine_helper.h"
-
-static const char *system_state_txt[] = {
-	"SYSTEM_STATE_PREFLIGHT",
-	"SYSTEM_STATE_STANDBY",
-	"SYSTEM_STATE_GROUND_READY",
-	"SYSTEM_STATE_MANUAL",
-	"SYSTEM_STATE_STABILIZED",
-	"SYSTEM_STATE_AUTO",
-	"SYSTEM_STATE_MISSION_ABORT",
-	"SYSTEM_STATE_EMCY_LANDING",
-	"SYSTEM_STATE_EMCY_CUTOFF",
-	"SYSTEM_STATE_GROUND_ERROR",
-	"SYSTEM_STATE_REBOOT",
-
-};
-
-/**
- * Transition from one state to another
- */
-int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state)
-{
-	int invalid_state = false;
-	int ret = ERROR;
-
-	commander_state_machine_t old_state = current_status->state_machine;
-
-	switch (new_state) {
-	case SYSTEM_STATE_MISSION_ABORT: {
-			/* Indoor or outdoor */
-			// if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
-			ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING);
-
-			// } else {
-			// 	ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF);
-			// }
-		}
-		break;
-
-	case SYSTEM_STATE_EMCY_LANDING:
-		/* Tell the controller to land */
-
-		/* set system flags according to state */
-		current_status->flag_system_armed = true;
-
-		warnx("EMERGENCY LANDING!\n");
-		mavlink_log_critical(mavlink_fd, "EMERGENCY LANDING!");
-		break;
-
-	case SYSTEM_STATE_EMCY_CUTOFF:
-		/* Tell the controller to cutoff the motors (thrust = 0) */
-
-		/* set system flags according to state */
-		current_status->flag_system_armed = false;
-
-		warnx("EMERGENCY MOTOR CUTOFF!\n");
-		mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!");
-		break;
-
-	case SYSTEM_STATE_GROUND_ERROR:
-
-		/* set system flags according to state */
-
-		/* prevent actuators from arming */
-		current_status->flag_system_armed = false;
-
-		warnx("GROUND ERROR, locking down propulsion system\n");
-		mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system");
-		break;
-
-	case SYSTEM_STATE_PREFLIGHT:
-		if (current_status->state_machine == SYSTEM_STATE_STANDBY
-		    || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
-			/* set system flags according to state */
-			current_status->flag_system_armed = false;
-			mavlink_log_critical(mavlink_fd, "Switched to PREFLIGHT state");
-
-		} else {
-			invalid_state = true;
-			mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state");
-		}
-
-		break;
-
-	case SYSTEM_STATE_REBOOT:
-		if (current_status->state_machine == SYSTEM_STATE_STANDBY
-			|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT
-			|| current_status->flag_hil_enabled) {
-			invalid_state = false;
-			/* set system flags according to state */
-			current_status->flag_system_armed = false;
-			mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM");
-			usleep(500000);
-			up_systemreset();
-			/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
-
-		} else {
-			invalid_state = true;
-			mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT");
-		}
-
-		break;
-
-	case SYSTEM_STATE_STANDBY:
-		/* set system flags according to state */
-
-		/* standby enforces disarmed */
-		current_status->flag_system_armed = false;
-
-		mavlink_log_critical(mavlink_fd, "Switched to STANDBY state");
-		break;
-
-	case SYSTEM_STATE_GROUND_READY:
-
-		/* set system flags according to state */
-
-		/* ground ready has motors / actuators armed */
-		current_status->flag_system_armed = true;
-
-		mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state");
-		break;
-
-	case SYSTEM_STATE_AUTO:
-
-		/* set system flags according to state */
-
-		/* auto is airborne and in auto mode, motors armed */
-		current_status->flag_system_armed = true;
-
-		mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode");
-		break;
-
-	case SYSTEM_STATE_STABILIZED:
-
-		/* set system flags according to state */
-		current_status->flag_system_armed = true;
-
-		mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode");
-		break;
-
-	case SYSTEM_STATE_MANUAL:
-
-		/* set system flags according to state */
-		current_status->flag_system_armed = true;
-
-		mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode");
-		break;
-
-	default:
-		invalid_state = true;
-		break;
-	}
-
-	if (invalid_state == false || old_state != new_state) {
-		current_status->state_machine = new_state;
-		state_machine_publish(status_pub, current_status, mavlink_fd);
-		publish_armed_status(current_status);
-		ret = OK;
-	}
-
-	if (invalid_state) {
-		mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition");
-		ret = ERROR;
-	}
-
-	return ret;
-}
-
-void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	/* publish the new state */
-	current_status->counter++;
-	current_status->timestamp = hrt_absolute_time();
-
-	/* assemble state vector based on flag values */
-	if (current_status->flag_control_rates_enabled) {
-		current_status->onboard_control_sensors_present |= 0x400;
-
-	} else {
-		current_status->onboard_control_sensors_present &= ~0x400;
-	}
-
-	current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
-	current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
-	current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
-	current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
-
-	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0;
-	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
-	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
-	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
-	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
-
-	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-	printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]);
-}
-
-void publish_armed_status(const struct vehicle_status_s *current_status)
-{
-	struct actuator_armed_s armed;
-	armed.armed = current_status->flag_system_armed;
-
-	/* XXX allow arming by external components on multicopters only if not yet armed by RC */
-	/* XXX allow arming only if core sensors are ok */
-	armed.ready_to_arm = true;
-
-	/* lock down actuators if required, only in HIL */
-	armed.lockdown = (current_status->flag_hil_enabled) ? true : false;
-	orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
-	orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
-}
-
-
-/*
- * Private functions, update the state machine
- */
-void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	warnx("EMERGENCY HANDLER\n");
-	/* Depending on the current state go to one of the error states */
-
-	if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR);
-
-	} else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) {
-
-		// DO NOT abort mission
-		//do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT);
-
-	} else {
-		warnx("Unknown system state: #%d\n", current_status->state_machine);
-	}
-}
-
-void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors
-{
-	if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself
-		state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd);
-
-	} else {
-		//global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL);
-	}
-
-}
-
-
-
-// /*
-//  * 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;
-// 	}
-
-// }
-
-
-// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_health |= 1 << *subsystem_type;
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// 	switch (*subsystem_type) {
-// 	case SUBSYSTEM_TYPE_GYRO:
-// 		//TODO state machine change (recovering)
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_ACC:
-// 		//TODO state machine change
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_MAG:
-// 		//TODO state machine change
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_GPS:
-// 		//TODO state machine change
-// 		break;
-
-// 	default:
-// 		break;
-// 	}
-
-
-// }
-
-
-// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	bool previosly_healthy = (bool)(current_status->onboard_control_sensors_health & 1 << *subsystem_type);
-// 	current_status->onboard_control_sensors_health &= ~(1 << *subsystem_type);
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// 	/* if we received unhealthy message more than *_HEALTH_COUNTER_LIMIT, switch to error state */
-
-// 	switch (*subsystem_type) {
-// 	case SUBSYSTEM_TYPE_GYRO:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: gyro unhealthy", MAV_SEVERITY_CRITICAL);
-
-// 		if (previosly_healthy) 	//only throw emergency if previously healthy
-// 			state_machine_emergency_always_critical(status_pub, current_status);
-
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_ACC:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: accelerometer unhealthy", MAV_SEVERITY_CRITICAL);
-
-// 		if (previosly_healthy) 	//only throw emergency if previously healthy
-// 			state_machine_emergency_always_critical(status_pub, current_status);
-
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_MAG:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: magnetometer unhealthy", MAV_SEVERITY_CRITICAL);
-
-// 		if (previosly_healthy) 	//only throw emergency if previously healthy
-// 			state_machine_emergency_always_critical(status_pub, current_status);
-
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_GPS:
-// //			//TODO: remove this block
-// //			break;
-// //			///////////////////
-// 		//global_data_send_mavlink_statustext_message_out("Commander: GPS unhealthy", MAV_SEVERITY_CRITICAL);
-
-// //				printf("previosly_healthy = %u\n", previosly_healthy);
-// 		if (previosly_healthy) 	//only throw emergency if previously healthy
-// 			state_machine_emergency(status_pub, current_status);
-
-// 		break;
-
-// 	default:
-// 		break;
-// 	}
-
-// }
-
-
-/* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
-
-
-void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	/* Depending on the current state switch state */
-	if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
-	}
-}
-
-void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	/* Depending on the current state switch state */
-	if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		state_machine_emergency(status_pub, current_status, mavlink_fd);
-	}
-}
-
-void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	if (current_status->state_machine == SYSTEM_STATE_STANDBY) {
-		printf("[cmd] arming\n");
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
-	}
-}
-
-void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
-		printf("[cmd] going standby\n");
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
-
-	} else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		printf("[cmd] MISSION ABORT!\n");
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
-	}
-}
-
-void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	int old_mode = current_status->flight_mode;
-	current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
-
-	current_status->flag_control_manual_enabled = true;
-
-	/* set behaviour based on airframe */
-	if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
-	    (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
-	    (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) {
-
-		/* assuming a rotary wing, set to SAS */
-		current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-		current_status->flag_control_attitude_enabled = true;
-		current_status->flag_control_rates_enabled = true;
-
-	} else {
-
-		/* assuming a fixed wing, set to direct pass-through */
-		current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
-		current_status->flag_control_attitude_enabled = false;
-		current_status->flag_control_rates_enabled = false;
-	}
-
-	if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
-
-	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		printf("[cmd] manual mode\n");
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
-	}
-}
-
-void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		int old_mode = current_status->flight_mode;
-		int old_manual_control_mode = current_status->manual_control_mode;
-		current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
-		current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-		current_status->flag_control_attitude_enabled = true;
-		current_status->flag_control_rates_enabled = true;
-		current_status->flag_control_manual_enabled = true;
-
-		if (old_mode != current_status->flight_mode ||
-		    old_manual_control_mode != current_status->manual_control_mode) {
-			printf("[cmd] att stabilized mode\n");
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
-			state_machine_publish(status_pub, current_status, mavlink_fd);
-		}
-
-	}
-}
-
-void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	if (!current_status->flag_vector_flight_mode_ok) {
-		mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE");
-		tune_error();
-		return;
-	}
-
-	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		printf("[cmd] position guided mode\n");
-		int old_mode = current_status->flight_mode;
-		current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB;
-		current_status->flag_control_manual_enabled = false;
-		current_status->flag_control_attitude_enabled = true;
-		current_status->flag_control_rates_enabled = true;
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED);
-
-		if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
-
-	}
-}
-
-void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	if (!current_status->flag_vector_flight_mode_ok) {
-		mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE");
-		return;
-	}
-
-	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) {
-		printf("[cmd] auto mode\n");
-		int old_mode = current_status->flight_mode;
-		current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
-		current_status->flag_control_manual_enabled = false;
-		current_status->flag_control_attitude_enabled = true;
-		current_status->flag_control_rates_enabled = true;
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
-
-		if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
-	}
-}
-
-
-uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
-{
-	uint8_t 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_system_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_system_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_system_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;
-}
-
-uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations
-{
-	commander_state_machine_t current_system_state = current_status->state_machine;
-
-	uint8_t ret = 1;
-
-	switch (custom_mode) {
-	case SYSTEM_STATE_GROUND_READY:
-		break;
-
-	case SYSTEM_STATE_STANDBY:
-		break;
-
-	case SYSTEM_STATE_REBOOT:
-		printf("try to reboot\n");
-
-		if (current_system_state == SYSTEM_STATE_STANDBY
-				|| current_system_state == SYSTEM_STATE_PREFLIGHT
-				|| current_status->flag_hil_enabled) {
-			printf("system will reboot\n");
-			mavlink_log_critical(mavlink_fd, "Rebooting..");
-			usleep(200000);
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT);
-			ret = 0;
-		}
-
-		break;
-
-	case SYSTEM_STATE_AUTO:
-		printf("try to switch to auto/takeoff\n");
-
-		if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_MANUAL) {
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
-			printf("state: auto\n");
-			ret = 0;
-		}
-
-		break;
-
-	case SYSTEM_STATE_MANUAL:
-		printf("try to switch to manual\n");
-
-		if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_AUTO) {
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
-			printf("state: manual\n");
-			ret = 0;
-		}
-
-		break;
-
-	default:
-		break;
-	}
-
-	return ret;
-}
-
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
new file mode 100644
index 000000000..7ae61d9ef
--- /dev/null
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -0,0 +1,738 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * 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_machine_helper.cpp
+ * State machine helper functions implementations
+ */
+
+#include <stdio.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <dirent.h>
+#include <fcntl.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 <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_device.h>
+#include <mavlink/mavlink_log.h>
+
+#include "state_machine_helper.h"
+#include "commander_helper.h"
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+static bool arming_state_changed = true;
+static bool main_state_changed = true;
+static bool navigation_state_changed = true;
+
+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)
+{
+	/*
+	 * Perform an atomic state update
+	 */
+	irqstate_t flags = irqsave();
+
+	transition_result_t ret = TRANSITION_DENIED;
+
+	/* only check transition if the new state is actually different from the current one */
+	if (new_arming_state == status->arming_state) {
+		ret = TRANSITION_NOT_CHANGED;
+
+	} else {
+
+		/* enforce lockdown in HIL */
+		if (control_mode->flag_system_hil_enabled) {
+			armed->lockdown = true;
+		} else {
+			armed->lockdown = false;
+		}
+
+		switch (new_arming_state) {
+		case ARMING_STATE_INIT:
+
+			/* 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;
+			}
+
+			break;
+
+		case ARMING_STATE_STANDBY:
+
+			/* 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) {
+
+				/* 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;
+
+		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;
+			}
+
+			break;
+
+		case ARMING_STATE_STANDBY_ERROR:
+
+			/* 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;
+			}
+
+			break;
+
+		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;
+			}
+
+			break;
+
+		case ARMING_STATE_IN_AIR_RESTORE:
+
+			/* XXX implement */
+			break;
+
+		default:
+			break;
+		}
+
+		if (ret == TRANSITION_CHANGED) {
+			status->arming_state = new_arming_state;
+			arming_state_changed = true;
+		}
+	}
+
+	/* end of atomic state update */
+	irqrestore(flags);
+
+	if (ret == TRANSITION_DENIED)
+		warnx("arming transition rejected");
+
+	return ret;
+}
+
+bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safety, const struct actuator_armed_s *armed)
+{
+	// System is safe if:
+	// 1) Not armed
+	// 2) Armed, but in software lockdown (HIL)
+	// 3) Safety switch is present AND engaged -> actuators locked
+	if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) {
+		return true;
+
+	} else {
+		return false;
+	}
+}
+
+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)
+{
+	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:
+			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;
+}
+
+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:
+			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;
+
+		case NAVIGATION_STATE_AUTO_TAKEOFF:
+			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_LOITER:
+			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;
+
+		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 NAVIGATION_STATE_AUTO_RTL:
+			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;
+		}
+	}
+
+	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)
+{
+	bool valid_transition = false;
+	int ret = ERROR;
+
+	warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state);
+
+	if (current_status->hil_state == new_state) {
+		warnx("Hil state not changed");
+		valid_transition = true;
+
+	} 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;
+
+			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
+
+				/* list directory */
+				DIR		*d;
+				struct dirent	*direntry;
+				d = opendir("/dev");
+				if (d) {
+
+					while ((direntry = readdir(d)) != NULL) {
+
+						int sensfd = ::open(direntry->d_name, 0);
+						int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0);
+						close(sensfd);
+
+						printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL");
+					}
+
+					closedir(d);
+
+					warnx("directory listing ok (FS mounted and readable)");
+
+				} else {
+					/* failed opening dir */
+					warnx("FAILED LISTING DEVICE ROOT DIRECTORY");
+					return 1;
+				}
+			}
+
+			break;
+
+		default:
+			warnx("Unknown hil state");
+			break;
+		}
+	}
+
+	if (valid_transition) {
+		current_status->hil_state = new_state;
+
+		current_status->timestamp = hrt_absolute_time();
+		orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+
+		current_control_mode->timestamp = hrt_absolute_time();
+		orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode);
+
+		// XXX also set lockdown here
+
+		ret = OK;
+
+	} else {
+		mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition");
+	}
+
+	return ret;
+}
+
+
+
+// /*
+//  * Wrapper functions (to be used in the commander), all functions assume lock on current_status
+//  */
+
+// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR
+//  *
+//  * START SUBSYSTEM/EMERGENCY FUNCTIONS
+//  * */
+
+// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
+// {
+// 	current_status->onboard_control_sensors_present |= 1 << *subsystem_type;
+// 	current_status->counter++;
+// 	current_status->timestamp = hrt_absolute_time();
+// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+// }
+
+// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
+// {
+// 	current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type);
+// 	current_status->counter++;
+// 	current_status->timestamp = hrt_absolute_time();
+// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+
+// 	/* if a subsystem was removed something went completely wrong */
+
+// 	switch (*subsystem_type) {
+// 	case SUBSYSTEM_TYPE_GYRO:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_ACC:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_MAG:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_GPS:
+// 		{
+// 			uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV];
+
+// 			if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
+// 				//global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY);
+// 				state_machine_emergency(status_pub, current_status);
+// 			}
+// 		}
+// 		break;
+
+// 	default:
+// 		break;
+// 	}
+
+// }
+
+// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
+// {
+// 	current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type;
+// 	current_status->counter++;
+// 	current_status->timestamp = hrt_absolute_time();
+// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+// }
+
+// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
+// {
+// 	current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type);
+// 	current_status->counter++;
+// 	current_status->timestamp = hrt_absolute_time();
+// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+
+// 	/* if a subsystem was disabled something went completely wrong */
+
+// 	switch (*subsystem_type) {
+// 	case SUBSYSTEM_TYPE_GYRO:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_ACC:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_MAG:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_GPS:
+// 		{
+// 			uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
+
+// 			if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
+// 				//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY);
+// 				state_machine_emergency(status_pub, current_status);
+// 			}
+// 		}
+// 		break;
+
+// 	default:
+// 		break;
+// 	}
+
+// }
+
+
+///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
+//
+//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
+//{
+//	int ret = 1;
+//
+////	/* Switch on HIL if in standby and not already in HIL mode */
+////	if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
+////	    && !current_status->flag_hil_enabled) {
+////		if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) {
+////			/* Enable HIL on request */
+////			current_status->flag_hil_enabled = true;
+////			ret = OK;
+////			state_machine_publish(status_pub, current_status, mavlink_fd);
+////			publish_armed_status(current_status);
+////			printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
+////
+////		} else if (current_status->state_machine != SYSTEM_STATE_STANDBY &&
+////			   current_status->flag_fmu_armed) {
+////
+////			mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!")
+////
+////		} else {
+////
+////			mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.")
+////		}
+////	}
+//
+//	/* switch manual / auto */
+//	if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
+//		update_state_machine_mode_auto(status_pub, current_status, mavlink_fd);
+//
+//	} else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) {
+//		update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd);
+//
+//	} else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
+//		update_state_machine_mode_guided(status_pub, current_status, mavlink_fd);
+//
+//	} else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
+//		update_state_machine_mode_manual(status_pub, current_status, mavlink_fd);
+//	}
+//
+//	/* vehicle is disarmed, mode requests arming */
+//	if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
+//		/* only arm in standby state */
+//		// XXX REMOVE
+//		if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
+//			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
+//			ret = OK;
+//			printf("[cmd] arming due to command request\n");
+//		}
+//	}
+//
+//	/* vehicle is armed, mode requests disarming */
+//	if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
+//		/* only disarm in ground ready */
+//		if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
+//			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
+//			ret = OK;
+//			printf("[cmd] disarming due to command request\n");
+//		}
+//	}
+//
+//	/* NEVER actually switch off HIL without reboot */
+//	if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
+//		warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
+//		mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL");
+//		ret = ERROR;
+//	}
+//
+//	return ret;
+//}
+
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 2f2ccc729..0bfdf36a8 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
  *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
  *           Julian Oes <joes@student.ethz.ch>
  *
@@ -46,164 +46,34 @@
 
 #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>
 
-/**
- * Switch to new state with no checking.
- *
- * do_state_update: this is the functions that all other functions have to call in order to update the state.
- * the function does not question the state change, this must be done before
- * The function performs actions that are connected with the new state (buzzer, reboot, ...)
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- *
- * @return 0 (macro OK) or 1 on error (macro ERROR)
- */
-int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state);
+typedef enum {
+	TRANSITION_DENIED = -1,
+	TRANSITION_NOT_CHANGED = 0,
+	TRANSITION_CHANGED
 
-/* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR */
-// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
+} transition_result_t;
 
-// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
+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);
 
-// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
+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();
 
-/**
- * Handle state machine if got position fix
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);
 
-/**
- * Handle state machine if position fix lost
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+bool check_main_state_changed();
 
-/**
- * Handle state machine if user wants to arm
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+transition_result_t navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode);
 
-/**
- * Handle state machine if user wants to disarm
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if mode switch is manual
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if mode switch is stabilized
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if mode switch is guided
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if mode switch is auto
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Publish current state information
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-
-/*
- *  Functions that handle incoming requests to change the state machine or a parameter (probably from the mavlink app).
- *  If the request is obeyed the functions return 0
- *
- */
-
-/**
- * Handles *incoming request* to switch to a specific state, if state change is successful returns 0
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode);
-
-/**
- * Handles *incoming request* to switch to a specific custom state, if state change is successful returns 0
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode);
-
-/**
- * Always switches to critical mode under any circumstances.
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Switches to emergency if required.
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Publish the armed state depending on the current system state
- *
- * @param current_status the current system status
- */
-void publish_armed_status(const struct vehicle_status_s *current_status);
+bool check_navigation_state_changed();
 
+void set_navigation_state_changed();
 
+int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, 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 5994d2315..b964d40a3 100644
--- a/src/modules/controllib/block/Block.cpp
+++ b/src/modules/controllib/block/Block.cpp
@@ -43,8 +43,8 @@
 
 #include "Block.hpp"
 #include "BlockParam.hpp"
-#include "UOrbSubscription.hpp"
-#include "UOrbPublication.hpp"
+#include "../uorb/UOrbSubscription.hpp"
+#include "../uorb/UOrbPublication.hpp"
 
 namespace control
 {
diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp
index 58a9bfc0d..36bc8c24b 100644
--- a/src/modules/controllib/block/BlockParam.hpp
+++ b/src/modules/controllib/block/BlockParam.hpp
@@ -69,22 +69,39 @@ protected:
 /**
  * Parameters that are tied to blocks for updating and nameing.
  */
-template<class T>
-class __EXPORT BlockParam : public BlockParamBase
+
+class __EXPORT BlockParamFloat : 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);
+	}
+protected:
+	float _val;
+};
+
+class __EXPORT BlockParamInt : public BlockParamBase
 {
 public:
-	BlockParam(Block *block, const char *name, bool parent_prefix=true) :
+	BlockParamInt(Block *block, const char *name, bool parent_prefix=true) :
 		BlockParamBase(block, name, parent_prefix),
 		_val() {
 		update();
 	}
-	T get() { return _val; }
-	void set(T val) { _val = val; }
+	int get() { return _val; }
+	void set(int val) { _val = val; }
 	void update() {
 		if (_handle != PARAM_INVALID) param_get(_handle, &_val);
 	}
 protected:
-	T _val;
+	int _val;
 };
 
 } // namespace control
diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp
index 7a785d12e..66e929038 100644
--- a/src/modules/controllib/blocks.hpp
+++ b/src/modules/controllib/blocks.hpp
@@ -42,6 +42,7 @@
 #include <assert.h>
 #include <time.h>
 #include <stdlib.h>
+#include <math.h>
 #include <mathlib/math/test/test.hpp>
 
 #include "block/Block.hpp"
@@ -73,8 +74,8 @@ public:
 	float getMax() { return _max.get(); }
 protected:
 // attributes
-	BlockParam<float> _min;
-	BlockParam<float> _max;
+	control::BlockParamFloat _min;
+	control::BlockParamFloat _max;
 };
 
 int __EXPORT blockLimitTest();
@@ -98,7 +99,7 @@ public:
 	float getMax() { return _max.get(); }
 protected:
 // attributes
-	BlockParam<float> _max;
+	control::BlockParamFloat _max;
 };
 
 int __EXPORT blockLimitSymTest();
@@ -125,7 +126,7 @@ public:
 protected:
 // attributes
 	float _state;
-	BlockParam<float> _fCut;
+	control::BlockParamFloat _fCut;
 };
 
 int __EXPORT blockLowPassTest();
@@ -156,7 +157,7 @@ protected:
 // attributes
 	float _u; /**< previous input */
 	float _y; /**< previous output */
-	BlockParam<float> _fCut; /**< cut-off frequency, Hz */
+	control::BlockParamFloat _fCut; /**< cut-off frequency, Hz */
 };
 
 int __EXPORT blockHighPassTest();
@@ -272,7 +273,7 @@ public:
 // accessors
 	float getKP() { return _kP.get(); }
 protected:
-	BlockParam<float> _kP;
+	control::BlockParamFloat _kP;
 };
 
 int __EXPORT blockPTest();
@@ -302,8 +303,8 @@ public:
 	BlockIntegral &getIntegral() { return _integral; }
 private:
 	BlockIntegral _integral;
-	BlockParam<float> _kP;
-	BlockParam<float> _kI;
+	control::BlockParamFloat _kP;
+	control::BlockParamFloat _kI;
 };
 
 int __EXPORT blockPITest();
@@ -333,8 +334,8 @@ public:
 	BlockDerivative &getDerivative() { return _derivative; }
 private:
 	BlockDerivative _derivative;
-	BlockParam<float> _kP;
-	BlockParam<float> _kD;
+	control::BlockParamFloat _kP;
+	control::BlockParamFloat _kD;
 };
 
 int __EXPORT blockPDTest();
@@ -371,9 +372,9 @@ private:
 // attributes
 	BlockIntegral _integral;
 	BlockDerivative _derivative;
-	BlockParam<float> _kP;
-	BlockParam<float> _kI;
-	BlockParam<float> _kD;
+	control::BlockParamFloat _kP;
+	control::BlockParamFloat _kI;
+	control::BlockParamFloat _kD;
 };
 
 int __EXPORT blockPIDTest();
@@ -403,7 +404,7 @@ public:
 	float get() { return _val; }
 private:
 // attributes
-	BlockParam<float> _trim;
+	control::BlockParamFloat _trim;
 	BlockLimit _limit;
 	float _val;
 };
@@ -438,8 +439,8 @@ public:
 	float getMax() { return _max.get(); }
 private:
 // attributes
-	BlockParam<float> _min;
-	BlockParam<float> _max;
+	control::BlockParamFloat _min;
+	control::BlockParamFloat _max;
 };
 
 int __EXPORT blockRandUniformTest();
@@ -485,8 +486,8 @@ public:
 	float getStdDev() { return _stdDev.get(); }
 private:
 // attributes
-	BlockParam<float> _mean;
-	BlockParam<float> _stdDev;
+	control::BlockParamFloat _mean;
+	control::BlockParamFloat _stdDev;
 };
 
 int __EXPORT blockRandGaussTest();
diff --git a/src/modules/controllib/module.mk b/src/modules/controllib/module.mk
index 13d1069c7..d815a8feb 100644
--- a/src/modules/controllib/module.mk
+++ b/src/modules/controllib/module.mk
@@ -37,7 +37,7 @@
 SRCS		 = test_params.c \
 		   block/Block.cpp \
 		   block/BlockParam.cpp \
-		   block/UOrbPublication.cpp \
-		   block/UOrbSubscription.cpp \
-		   blocks.cpp \
-		   fixedwing.cpp
+		   uorb/UOrbPublication.cpp \
+		   uorb/UOrbSubscription.cpp \
+		   uorb/blocks.cpp \
+		   blocks.cpp
diff --git a/src/modules/controllib/block/UOrbPublication.cpp b/src/modules/controllib/uorb/UOrbPublication.cpp
index f69b39d90..f69b39d90 100644
--- a/src/modules/controllib/block/UOrbPublication.cpp
+++ b/src/modules/controllib/uorb/UOrbPublication.cpp
diff --git a/src/modules/controllib/block/UOrbPublication.hpp b/src/modules/controllib/uorb/UOrbPublication.hpp
index a36f4429f..6f1f3fc1c 100644
--- a/src/modules/controllib/block/UOrbPublication.hpp
+++ b/src/modules/controllib/uorb/UOrbPublication.hpp
@@ -39,8 +39,8 @@
 #pragma once
 
 #include <uORB/uORB.h>
-#include "Block.hpp"
-#include "List.hpp"
+#include "../block/Block.hpp"
+#include "../block/List.hpp"
 
 
 namespace control
@@ -60,11 +60,15 @@ public:
 		List<UOrbPublicationBase *> * list,
 		const struct orb_metadata *meta) :
 		_meta(meta),
-		_handle() {
+		_handle(-1) {
 		if (list != NULL) list->add(this);
 	}
 	void update() {
-		orb_publish(getMeta(), getHandle(), getDataVoidPtr());
+		if (_handle > 0) {
+			orb_publish(getMeta(), getHandle(), getDataVoidPtr());
+		} else {
+			setHandle(orb_advertise(getMeta(), getDataVoidPtr()));
+		}
 	}
 	virtual void *getDataVoidPtr() = 0;
 	virtual ~UOrbPublicationBase() {
@@ -99,10 +103,6 @@ public:
 		const struct orb_metadata *meta) :
 		T(), // initialize data structure to zero
 		UOrbPublicationBase(list, meta) {
-		// It is important that we call T()
-		// before we publish the data, so we
-		// call this here instead of the base class
-		setHandle(orb_advertise(getMeta(), getDataVoidPtr()));
 	}
 	virtual ~UOrbPublication() {}
 	/*
diff --git a/src/modules/controllib/block/UOrbSubscription.cpp b/src/modules/controllib/uorb/UOrbSubscription.cpp
index 022cadd24..022cadd24 100644
--- a/src/modules/controllib/block/UOrbSubscription.cpp
+++ b/src/modules/controllib/uorb/UOrbSubscription.cpp
diff --git a/src/modules/controllib/block/UOrbSubscription.hpp b/src/modules/controllib/uorb/UOrbSubscription.hpp
index 22cc2e114..d337d89a8 100644
--- a/src/modules/controllib/block/UOrbSubscription.hpp
+++ b/src/modules/controllib/uorb/UOrbSubscription.hpp
@@ -39,8 +39,8 @@
 #pragma once
 
 #include <uORB/uORB.h>
-#include "Block.hpp"
-#include "List.hpp"
+#include "../block/Block.hpp"
+#include "../block/List.hpp"
 
 
 namespace control
diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp
new file mode 100644
index 000000000..448a42a99
--- /dev/null
+++ b/src/modules/controllib/uorb/blocks.cpp
@@ -0,0 +1,101 @@
+/****************************************************************************
+ *
+ *   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 uorb_blocks.cpp
+ *
+ * uorb block library code
+ */
+
+#include "blocks.hpp"
+
+namespace control
+{
+
+BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_xtYawLimit(this, "XT2YAW"),
+	_xt2Yaw(this, "XT2YAW"),
+	_psiCmd(0)
+{
+}
+
+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)
+{
+
+	// 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);
+
+	// 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);
+
+	_psiCmd = _wrap_2pi(psiTrack -
+			    _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
+}
+
+BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	// subscriptions
+	_att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20),
+	_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),
+	_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
+	// publications
+	_actuators(&getPublications(), ORB_ID(actuator_controls_0))
+{
+}
+
+BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {};
+
+} // namespace control
+
diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp
new file mode 100644
index 000000000..46dc1bec2
--- /dev/null
+++ b/src/modules/controllib/uorb/blocks.hpp
@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   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 uorb_blocks.h
+ *
+ * uorb block library code
+ */
+
+#pragma once
+
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#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/manual_control_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/parameter_update.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <drivers/drv_hrt.h>
+#include <poll.h>
+
+extern "C" {
+#include <geo/geo.h>
+}
+
+#include "../blocks.hpp"
+#include "UOrbSubscription.hpp"
+#include "UOrbPublication.hpp"
+
+namespace control
+{
+
+/**
+ * Waypoint Guidance block
+ */
+class __EXPORT BlockWaypointGuidance : public SuperBlock
+{
+private:
+	BlockLimitSym _xtYawLimit;
+	BlockP _xt2Yaw;
+	float _psiCmd;
+public:
+	BlockWaypointGuidance(SuperBlock *parent, const char *name);
+	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);
+	float getPsiCmd() { return _psiCmd; }
+};
+
+/**
+ * UorbEnabledAutopilot
+ */
+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;
+	// publications
+	UOrbPublication<actuator_controls_s> _actuators;
+public:
+	BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name);
+	virtual ~BlockUorbEnabledAutopilot();
+};
+
+} // namespace control
+
diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c
index 769b8b0a8..2aeca3a98 100644
--- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c
+++ b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c
@@ -128,8 +128,8 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
 	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); //P Controller
-		pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller
+		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;
 	}
 
diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
index 6c9c137bb..b6b4546c2 100644
--- a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
+++ b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
@@ -53,6 +53,7 @@
 #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>
@@ -116,6 +117,8 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 	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));
 
@@ -137,7 +140,8 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 	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 vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+	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};
@@ -178,30 +182,16 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 		}
 
 		orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
-		orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
+		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;
 
-		/* control */
-
-		if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
-		    vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
-			/* 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;
-
-		} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
-			if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
+		/* 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) {
@@ -234,15 +224,6 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 
 				att_sp.timestamp = hrt_absolute_time();
 
-				/* 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;
-
 				/* pass through flaps */
 				if (isfinite(manual_sp.flaps)) {
 					actuators.control[4] = manual_sp.flaps;
@@ -251,7 +232,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 					actuators.control[4] = 0.0f;
 				}
 
-			} else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
+			} else {
 				/* directly pass through values */
 				actuators.control[0] = manual_sp.roll;
 				/* positive pitch means negative actuator -> pull up */
@@ -267,6 +248,22 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 				}
 			}
 		}
+		
+		/* 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);
diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
index 4eccc118c..cdab39edc 100644
--- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
+++ b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
@@ -179,9 +179,9 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	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); // 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); // 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); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
+		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;
 	}
 
diff --git a/src/modules/controllib/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp
index 7be38015c..6dc19df41 100644
--- a/src/modules/controllib/fixedwing.cpp
+++ b/src/modules/fixedwing_backside/fixedwing.cpp
@@ -86,61 +86,6 @@ void BlockStabilization::update(float pCmd, float qCmd, float rCmd,
 	_yawDamper.update(rCmd, r, outputScale);
 }
 
-BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
-	SuperBlock(parent, name),
-	_xtYawLimit(this, "XT2YAW"),
-	_xt2Yaw(this, "XT2YAW"),
-	_psiCmd(0)
-{
-}
-
-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)
-{
-
-	// 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);
-
-	// 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);
-
-	_psiCmd = _wrap_2pi(psiTrack -
-			    _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
-}
-
-BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
-	SuperBlock(parent, name),
-	// subscriptions
-	_att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20),
-	_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_setpoint), 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
-	// publications
-	_actuators(&getPublications(), ORB_ID(actuator_controls_0))
-{
-}
-
-BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {};
-
 BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) :
 	BlockUorbEnabledAutopilot(parent, name),
 	_stabilization(this, ""), // no name needed, already unique
@@ -211,9 +156,10 @@ void BlockMultiModeBacksideAutopilot::update()
 		_actuators.control[i] = 0.0f;
 
 	// only update guidance in auto mode
-	if (_status.state_machine == SYSTEM_STATE_AUTO) {
+	if (_status.main_state == MAIN_STATE_AUTO) {
+		// TODO use vehicle_control_mode here?
 		// update guidance
-		_guide.update(_pos, _att, _posCmd, _lastPosCmd);
+		_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
 	}
 
 	// XXX handle STABILIZED (loiter on spot) as well
@@ -221,14 +167,11 @@ void BlockMultiModeBacksideAutopilot::update()
 	// the setpoint should update to loitering around this position
 
 	// handle autopilot modes
-	if (_status.state_machine == SYSTEM_STATE_AUTO ||
-	    _status.state_machine == SYSTEM_STATE_STABILIZED) {
+	if (_status.main_state == MAIN_STATE_AUTO) {
 
-		// update guidance
-		_guide.update(_pos, _att, _posCmd, _lastPosCmd);
-
-		// calculate velocity, XXX should be airspeed, but using ground speed for now
-		// for the purpose of control we will limit the velocity feedback between
+		// 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 +
@@ -239,7 +182,7 @@ void BlockMultiModeBacksideAutopilot::update()
 		float vCmd = _vLimit.update(_vCmd.get());
 
 		// altitude hold
-		float dThrottle = _h2Thr.update(_posCmd.altitude - _pos.alt);
+		float dThrottle = _h2Thr.update(_posCmd.current.altitude - _pos.alt);
 
 		// heading hold
 		float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
@@ -273,90 +216,87 @@ void BlockMultiModeBacksideAutopilot::update()
 		// a first binary release can be targeted.
 		// This is not a hack, but a design choice.
 
-		/* do not limit in HIL */
-		if (!_status.flag_hil_enabled) {
+		// do not limit in HIL
+		if (_status.hil_state != HIL_STATE_ON) {
 			/* limit to value of manual throttle */
 			_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
 						     _actuators.control[CH_THR] : _manual.throttle;
 		}
 
-	} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
-
-		if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
-			_actuators.control[CH_AIL] = _manual.roll;
-			_actuators.control[CH_ELV] = _manual.pitch;
-			_actuators.control[CH_RDR] = _manual.yaw;
-			_actuators.control[CH_THR] = _manual.throttle;
-
-		} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
-
-			// 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));
-
-			// pitch channel -> rate of climb
-			// TODO, might want to put a gain on this, otherwise commanding
-			// from +1 -> -1 m/s for rate of climb
-			//float dThrottle = _cr2Thr.update(
-			//_crMax.get()*_manual.pitch - _pos.vz);
-
-			// roll channel -> bank angle
-			float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
-			float pCmd = _phi2P.update(phiCmd - _att.roll);
-
-			// throttle channel -> velocity
-			// negative sign because nose over to increase speed
-			float vCmd = _vLimit.update(_manual.throttle *
-					(_vLimit.getMax() - _vLimit.getMin()) +
-					_vLimit.getMin());
-			float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
-			float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
-
-			// yaw rate cmd
-			float rCmd = 0;
-
-			// stabilization
-			_stabilization.update(pCmd, qCmd, rCmd,
-					      _att.rollspeed, _att.pitchspeed, _att.yawspeed);
-
-			// output
-			_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
-			_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
-			_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
-
-			// currenlty using manual throttle
-			// XXX if you enable this watch out, vz might be very noisy
-			//_actuators.control[CH_THR] = dThrottle + _trimThr.get();
-			_actuators.control[CH_THR] = _manual.throttle;
-
-			// XXX limit throttle to manual setting (safety) for now.
-			// If it turns out to be confusing, it can be removed later once
-			// a first binary release can be targeted.
-			// This is not a hack, but a design choice.
-
-			/* do not limit in HIL */
-			if (!_status.flag_hil_enabled) {
-				/* limit to value of manual throttle */
-				_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
-							     _actuators.control[CH_THR] : _manual.throttle;
-			}
-		}
+	} else if (_status.main_state == MAIN_STATE_MANUAL) {
+		_actuators.control[CH_AIL] = _manual.roll;
+		_actuators.control[CH_ELV] = _manual.pitch;
+		_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 */) {
+
+		// 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));
+
+		// pitch channel -> rate of climb
+		// TODO, might want to put a gain on this, otherwise commanding
+		// from +1 -> -1 m/s for rate of climb
+		//float dThrottle = _cr2Thr.update(
+		//_crMax.get()*_manual.pitch - _pos.vz);
+
+		// roll channel -> bank angle
+		float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
+		float pCmd = _phi2P.update(phiCmd - _att.roll);
+
+		// throttle channel -> velocity
+		// negative sign because nose over to increase speed
+		float vCmd = _vLimit.update(_manual.throttle *
+				(_vLimit.getMax() - _vLimit.getMin()) +
+				_vLimit.getMin());
+		float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
+		float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
+
+		// yaw rate cmd
+		float rCmd = 0;
+
+		// stabilization
+		_stabilization.update(pCmd, qCmd, rCmd,
+					  _att.rollspeed, _att.pitchspeed, _att.yawspeed);
 
-		// body rates controller, disabled for now
-		else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) {
+		// output
+		_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
+		_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
+		_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
 
-			_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
-					      _att.rollspeed, _att.pitchspeed, _att.yawspeed);
+		// currently using manual throttle
+		// XXX if you enable this watch out, vz might be very noisy
+		//_actuators.control[CH_THR] = dThrottle + _trimThr.get();
+		_actuators.control[CH_THR] = _manual.throttle;
+
+		// XXX limit throttle to manual setting (safety) for now.
+		// If it turns out to be confusing, it can be removed later once
+		// a first binary release can be targeted.
+		// This is not a hack, but a design choice.
 
-			_actuators.control[CH_AIL] = _stabilization.getAileron();
-			_actuators.control[CH_ELV] = _stabilization.getElevator();
-			_actuators.control[CH_RDR] = _stabilization.getRudder();
-			_actuators.control[CH_THR] = _manual.throttle;
+		/* do not limit in HIL */
+		if (_status.hil_state != HIL_STATE_ON) {
+			/* limit to value of manual throttle */
+			_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
+							 _actuators.control[CH_THR] : _manual.throttle;
 		}
+	// body rates controller, disabled for now
+	// TODO
+	} else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) {	// TODO use vehicle_control_mode here?
+
+		_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
+					  _att.rollspeed, _att.pitchspeed, _att.yawspeed);
+
+		_actuators.control[CH_AIL] = _stabilization.getAileron();
+		_actuators.control[CH_ELV] = _stabilization.getElevator();
+		_actuators.control[CH_RDR] = _stabilization.getRudder();
+		_actuators.control[CH_THR] = _manual.throttle;
 	}
 
 	// update all publications
diff --git a/src/modules/controllib/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp
index 53d0cf893..567efeb35 100644
--- a/src/modules/controllib/fixedwing.hpp
+++ b/src/modules/fixedwing_backside/fixedwing.hpp
@@ -39,31 +39,8 @@
 
 #pragma once
 
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#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_setpoint.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/parameter_update.h>
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <math.h>
-
-#include <drivers/drv_hrt.h>
-#include <poll.h>
-
-#include "blocks.hpp"
-#include "block/UOrbSubscription.hpp"
-#include "block/UOrbPublication.hpp"
-
-extern "C" {
-#include <systemlib/geo/geo.h>
-}
+#include <controllib/blocks.hpp>
+#include <controllib/uorb/blocks.hpp>
 
 namespace control
 {
@@ -251,47 +228,6 @@ public:
  */
 
 /**
- * Waypoint Guidance block
- */
-class __EXPORT BlockWaypointGuidance : public SuperBlock
-{
-private:
-	BlockLimitSym _xtYawLimit;
-	BlockP _xt2Yaw;
-	float _psiCmd;
-public:
-	BlockWaypointGuidance(SuperBlock *parent, const char *name);
-	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);
-	float getPsiCmd() { return _psiCmd; }
-};
-
-/**
- * UorbEnabledAutopilot
- */
-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_setpoint_s> _posCmd;
-	UOrbSubscription<manual_control_setpoint_s> _manual;
-	UOrbSubscription<vehicle_status_s> _status;
-	UOrbSubscription<parameter_update_s> _param_update;
-	// publications
-	UOrbPublication<actuator_controls_s> _actuators;
-public:
-	BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name);
-	virtual ~BlockUorbEnabledAutopilot();
-};
-
-/**
  * Multi-mode Autopilot
  */
 class __EXPORT BlockMultiModeBacksideAutopilot : public BlockUorbEnabledAutopilot
@@ -319,16 +255,16 @@ private:
 	BlockWaypointGuidance _guide;
 
 	// block params
-	BlockParam<float> _trimAil;
-	BlockParam<float> _trimElv;
-	BlockParam<float> _trimRdr;
-	BlockParam<float> _trimThr;
-	BlockParam<float> _trimV;
-	BlockParam<float> _vCmd;
-	BlockParam<float> _crMax;
+	BlockParamFloat _trimAil;
+	BlockParamFloat _trimElv;
+	BlockParamFloat _trimRdr;
+	BlockParamFloat _trimThr;
+	BlockParamFloat _trimV;
+	BlockParamFloat _vCmd;
+	BlockParamFloat _crMax;
 
 	struct pollfd _attPoll;
-	vehicle_global_position_setpoint_s _lastPosCmd;
+	vehicle_global_position_set_triplet_s _lastPosCmd;
 	enum {CH_AIL, CH_ELV, CH_RDR, CH_THR};
 	uint64_t _timeStamp;
 public:
diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
index e2330427d..f4ea05088 100644
--- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
+++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
@@ -45,11 +45,13 @@
 #include <stdlib.h>
 #include <string.h>
 #include <systemlib/systemlib.h>
-#include <controllib/fixedwing.hpp>
 #include <systemlib/param/param.h>
+#include <systemlib/err.h>
 #include <drivers/drv_hrt.h>
 #include <math.h>
 
+#include "fixedwing.hpp"
+
 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 */
@@ -80,7 +82,7 @@ usage(const char *reason)
 	if (reason)
 		fprintf(stderr, "%s\n", reason);
 
-	fprintf(stderr, "usage: control_demo {start|stop|status} [-p <additional params>]\n\n");
+	fprintf(stderr, "usage: fixedwing_backside {start|stop|status} [-p <additional params>]\n\n");
 	exit(1);
 }
 
@@ -101,13 +103,14 @@ int fixedwing_backside_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			printf("control_demo already running\n");
+			warnx("already running");
 			/* this is not an error */
 			exit(0);
 		}
 
 		thread_should_exit = false;
-		deamon_task = task_spawn_cmd("control_demo",
+
+		deamon_task = task_spawn_cmd("fixedwing_backside",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 10,
 					 5120,
@@ -128,10 +131,10 @@ int fixedwing_backside_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			printf("\tcontrol_demo app is running\n");
+			warnx("is running");
 
 		} else {
-			printf("\tcontrol_demo app not started\n");
+			warnx("not started");
 		}
 
 		exit(0);
@@ -144,7 +147,7 @@ int fixedwing_backside_main(int argc, char *argv[])
 int control_demo_thread_main(int argc, char *argv[])
 {
 
-	printf("[control_Demo] starting\n");
+	warnx("starting");
 
 	using namespace control;
 
@@ -156,7 +159,7 @@ int control_demo_thread_main(int argc, char *argv[])
 		autopilot.update();
 	}
 
-	printf("[control_demo] exiting.\n");
+	warnx("exiting.");
 
 	thread_running = false;
 
@@ -165,6 +168,6 @@ int control_demo_thread_main(int argc, char *argv[])
 
 void test()
 {
-	printf("beginning control lib test\n");
+	warnx("beginning control lib test");
 	control::basicBlocksTest();
 }
diff --git a/src/modules/fixedwing_backside/module.mk b/src/modules/fixedwing_backside/module.mk
index ec958d7cb..133728781 100644
--- a/src/modules/fixedwing_backside/module.mk
+++ b/src/modules/fixedwing_backside/module.mk
@@ -38,4 +38,5 @@
 MODULE_COMMAND	= fixedwing_backside
 
 SRCS		= fixedwing_backside_main.cpp \
+		  fixedwing.cpp \
 		  params.c
diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
index 48c0b9f9d..73df3fb9e 100644
--- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
+++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
@@ -239,10 +239,10 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 
 	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); //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);
-	pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE);
-	pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value
+	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");
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
new file mode 100644
index 000000000..60c902ce5
--- /dev/null
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -0,0 +1,782 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	Lorenz Meier
+ *
+ * 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 fw_att_control_main.c
+ * Implementation of a generic attitude controller based on classic orthogonal PIDs.
+ *
+ * @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 <drivers/drv_hrt.h>
+#include <drivers/drv_accel.h>
+#include <arch/board/board.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/airspeed.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/parameter_update.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <systemlib/pid/pid.h>
+#include <geo/geo.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/systemlib.h>
+#include <mathlib/mathlib.h>
+
+#include <ecl/attitude_fw/ecl_pitch_controller.h>
+#include <ecl/attitude_fw/ecl_roll_controller.h>
+#include <ecl/attitude_fw/ecl_yaw_controller.h>
+
+/**
+ * Fixedwing attitude control app start / stop handling function
+ *
+ * @ingroup apps
+ */
+extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]);
+
+class FixedwingAttitudeControl
+{
+public:
+	/**
+	 * Constructor
+	 */
+	FixedwingAttitudeControl();
+
+	/**
+	 * Destructor, also kills the sensors task.
+	 */
+	~FixedwingAttitudeControl();
+
+	/**
+	 * 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		_att_sub;			/**< vehicle attitude subscription */
+	int		_accel_sub;			/**< accelerometer subscription */
+	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 		_params_sub;			/**< notification of parameter updates */
+	int 		_manual_sub;			/**< notification of manual control updates */
+
+	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 */
+
+	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
+	struct accel_report				_accel;			/**< body frame accelerations */
+	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_control_mode_s			_vcontrol_mode;		/**< vehicle control mode */
+	struct actuator_controls_s			_actuators;		/**< actuator control inputs */
+
+	perf_counter_t	_loop_perf;			/**< loop performance counter */
+
+	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_rmax_pos;
+		float p_rmax_neg;
+		float p_integrator_max;
+		float p_roll_feedforward;
+		float r_p;
+		float r_d;
+		float r_i;
+		float r_integrator_max;
+		float r_rmax;
+		float y_p;
+		float y_i;
+		float y_d;
+		float y_roll_feedforward;
+		float y_integrator_max;
+
+		float airspeed_min;
+		float airspeed_trim;
+		float airspeed_max;
+	}		_parameters;			/**< local copies of interesting parameters */
+
+	struct {
+
+		param_t tconst;
+		param_t p_p;
+		param_t p_d;
+		param_t p_i;
+		param_t p_rmax_pos;
+		param_t p_rmax_neg;
+		param_t p_integrator_max;
+		param_t p_roll_feedforward;
+		param_t r_p;
+		param_t r_d;
+		param_t r_i;
+		param_t r_integrator_max;
+		param_t r_rmax;
+		param_t y_p;
+		param_t y_i;
+		param_t y_d;
+		param_t y_roll_feedforward;
+		param_t y_integrator_max;
+
+		param_t airspeed_min;
+		param_t airspeed_trim;
+		param_t airspeed_max;
+	}		_parameter_handles;		/**< handles for interesting parameters */
+
+
+	ECL_RollController				_roll_ctrl;
+	ECL_PitchController				_pitch_ctrl;
+	ECL_YawController				_yaw_ctrl;
+
+
+	/**
+	 * Update our local parameter cache.
+	 */
+	int		parameters_update();
+
+	/**
+	 * Update control outputs
+	 *
+	 */
+	void		control_update();
+
+	/**
+	 * Check for changes in vehicle control mode.
+	 */
+	void		vehicle_control_mode_poll();
+
+	/**
+	 * Check for changes in manual inputs.
+	 */
+	void		vehicle_manual_poll();
+
+
+	/**
+	 * Check for airspeed updates.
+	 */
+	bool		vehicle_airspeed_poll();
+
+	/**
+	 * Check for accel updates.
+	 */
+	void		vehicle_accel_poll();
+
+	/**
+	 * Check for set triplet updates.
+	 */
+	void		vehicle_setpoint_poll();
+
+	/**
+	 * 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 att_control
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+FixedwingAttitudeControl	*g_control;
+}
+
+FixedwingAttitudeControl::FixedwingAttitudeControl() :
+
+	_task_should_exit(false),
+	_control_task(-1),
+
+/* subscriptions */
+	_att_sub(-1),
+	_accel_sub(-1),
+	_airspeed_sub(-1),
+	_vcontrol_mode_sub(-1),
+	_params_sub(-1),
+	_manual_sub(-1),
+
+/* publications */
+	_rate_sp_pub(-1),
+	_actuators_0_pub(-1),
+	_attitude_sp_pub(-1),
+
+/* performance counters */
+	_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
+/* states */
+	_setpoint_valid(false),
+	_airspeed_valid(false)
+{
+	_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_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_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_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.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");
+
+	/* fetch initial parameter values */
+	parameters_update();
+}
+
+FixedwingAttitudeControl::~FixedwingAttitudeControl()
+{
+	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);
+	}
+
+	att_control::g_control = nullptr;
+}
+
+int
+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_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_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_integrator_max, &(_parameters.y_integrator_max));
+
+	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));
+
+	/* 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_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));
+
+	/* 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_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));
+
+	return OK;
+}
+
+void
+FixedwingAttitudeControl::vehicle_control_mode_poll()
+{
+	bool vcontrol_mode_updated;
+
+	/* Check HIL state if vehicle status has changed */
+	orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated);
+
+	if (vcontrol_mode_updated) {
+
+		orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode);
+	}
+}
+
+void
+FixedwingAttitudeControl::vehicle_manual_poll()
+{
+	bool manual_updated;
+
+	/* get pilots inputs */
+	orb_check(_manual_sub, &manual_updated);
+
+	if (manual_updated) {
+
+		orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual);
+	}
+}
+
+bool
+FixedwingAttitudeControl::vehicle_airspeed_poll()
+{
+	/* check if there is a new position */
+	bool airspeed_updated;
+	orb_check(_airspeed_sub, &airspeed_updated);
+
+	if (airspeed_updated) {
+		orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
+		return true;
+	}
+
+	return false;
+}
+
+void
+FixedwingAttitudeControl::vehicle_accel_poll()
+{
+	/* check if there is a new position */
+	bool accel_updated;
+	orb_check(_accel_sub, &accel_updated);
+
+	if (accel_updated) {
+		orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
+	}
+}
+
+void
+FixedwingAttitudeControl::vehicle_setpoint_poll()
+{
+	/* check if there is a new setpoint */
+	bool att_sp_updated;
+	orb_check(_att_sp_sub, &att_sp_updated);
+
+	if (att_sp_updated) {
+		orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
+		_setpoint_valid = true;
+	}
+}
+
+void
+FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[])
+{
+	att_control::g_control->task_main();
+}
+
+void
+FixedwingAttitudeControl::task_main()
+{
+
+	/* inform about start */
+	warnx("Initializing..");
+	fflush(stdout);
+
+	/*
+	 * do subscriptions
+	 */
+	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
+	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
+	_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));
+
+	/* rate limit vehicle status updates to 5Hz */
+	orb_set_interval(_vcontrol_mode_sub, 200);
+	/* rate limit attitude control to 50 Hz (with some margin, so 17 ms) */
+	orb_set_interval(_att_sub, 17);
+
+	parameters_update();
+
+	/* get an initial update for all sensor and status data */
+	(void)vehicle_airspeed_poll();
+	vehicle_setpoint_poll();
+	vehicle_accel_poll();
+	vehicle_control_mode_poll();
+	vehicle_manual_poll();
+
+	/* wakeup source(s) */
+	struct pollfd fds[2];
+
+	/* Setup of loop */
+	fds[0].fd = _params_sub;
+	fds[0].events = POLLIN;
+	fds[1].fd = _att_sub;
+	fds[1].events = POLLIN;
+
+	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 error %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 controller if attitude 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_attitude), _att_sub, &_att);
+
+			_airspeed_valid = vehicle_airspeed_poll();
+
+			vehicle_setpoint_poll();
+
+			vehicle_accel_poll();
+
+			vehicle_control_mode_poll();
+
+			vehicle_manual_poll();
+
+			/* lock integrator until control is started */
+			bool lock_integrator;
+
+			if (_vcontrol_mode.flag_control_attitude_enabled) {
+				lock_integrator = false;
+
+			} else {
+				lock_integrator = true;
+			}
+
+			/* 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;
+
+				} else {
+					airspeed = _airspeed.indicated_airspeed_m_s;
+				}
+
+				float airspeed_scaling = _parameters.airspeed_trim / airspeed;
+				//warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling);
+
+				float roll_sp, pitch_sp;
+				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;
+					throttle_sp = _att_sp.thrust;
+
+					/* reset integrals where needed */
+					if (_att_sp.roll_reset_integral)
+						_roll_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.
+					 *
+					 * 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.
+					 */
+					roll_sp = _manual.roll * 0.75f;
+					pitch_sp = _manual.pitch * 0.75f;
+					throttle_sp = _manual.throttle;
+					_actuators.control[4] = _manual.flaps;
+
+					/*
+					 * in manual mode no external source should / does emit attitude setpoints.
+					 * emit the manual setpoint here to allow attitude controller tuning
+					 * in attitude control mode.
+					 */
+					struct vehicle_attitude_setpoint_s att_sp;
+					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.thrust = throttle_sp;
+
+					/* lazily publish the setpoint only once available */
+					if (_attitude_sp_pub > 0) {
+						/* publish the attitude setpoint */
+						orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp);
+
+					} else {
+						/* advertise and publish */
+						_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
+					}
+				}
+
+				if (isfinite(roll_sp) && isfinite(pitch_sp)) {
+
+					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;
+
+					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 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;
+
+					/* throttle passed through */
+					_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
+
+					// 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
+
+					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);
+
+					} 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[4] = _manual.flaps;
+			}
+
+			_actuators.control[5] = _manual.aux1;
+			_actuators.control[6] = _manual.aux2;
+			_actuators.control[7] = _manual.aux3;
+
+			/* lazily publish the setpoint only once available */
+			_actuators.timestamp = hrt_absolute_time();
+
+			if (_actuators_0_pub > 0) {
+				/* publish the attitude setpoint */
+				orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
+
+			} else {
+				/* advertise and publish */
+				_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
+			}
+
+		}
+
+		perf_end(_loop_perf);
+	}
+
+	warnx("exiting.\n");
+
+	_control_task = -1;
+	_exit(0);
+}
+
+int
+FixedwingAttitudeControl::start()
+{
+	ASSERT(_control_task == -1);
+
+	/* start the task */
+	_control_task = task_spawn_cmd("fw_att_control",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2048,
+				       (main_t)&FixedwingAttitudeControl::task_main_trampoline,
+				       nullptr);
+
+	if (_control_task < 0) {
+		warn("task start failed");
+		return -errno;
+	}
+
+	return OK;
+}
+
+int fw_att_control_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		errx(1, "usage: fw_att_control {start|stop|status}");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (att_control::g_control != nullptr)
+			errx(1, "already running");
+
+		att_control::g_control = new FixedwingAttitudeControl;
+
+		if (att_control::g_control == nullptr)
+			errx(1, "alloc failed");
+
+		if (OK != att_control::g_control->start()) {
+			delete att_control::g_control;
+			att_control::g_control = nullptr;
+			err(1, "start failed");
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		if (att_control::g_control == nullptr)
+			errx(1, "not running");
+
+		delete att_control::g_control;
+		att_control::g_control = nullptr;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (att_control::g_control) {
+			errx(0, "running");
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}
diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c
new file mode 100644
index 000000000..be76524da
--- /dev/null
+++ b/src/modules/fw_att_control/fw_att_control_params.c
@@ -0,0 +1,136 @@
+/****************************************************************************
+ *
+ *   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 fw_att_control_params.c
+ *
+ * Parameters defined by the fixed-wing attitude control task
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#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
+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);
+
+// @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);
+
+// @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
+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
+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);
diff --git a/src/modules/fw_att_control/module.mk b/src/modules/fw_att_control/module.mk
new file mode 100644
index 000000000..1e600757e
--- /dev/null
+++ b/src/modules/fw_att_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.
+#
+############################################################################
+
+#
+# Fixedwing attitude control application
+#
+
+MODULE_COMMAND	= fw_att_control
+
+SRCS		= fw_att_control_main.cpp \
+		  fw_att_control_params.c
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
new file mode 100644
index 000000000..a9648b207
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -0,0 +1,1217 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	Lorenz Meier
+ *
+ * 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 fw_pos_control_l1_main.c
+ * Implementation of a generic position controller based on the L1 norm. Outputs a bank / roll
+ * angle, equivalent to a lateral motion (for copters and rovers).
+ *
+ * Original publication for horizontal control class:
+ *    S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking,"
+ *    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)
+ *
+ * More details and acknowledgements in the referenced library headers.
+ *
+ * @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 <drivers/drv_hrt.h>
+#include <drivers/drv_accel.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/vehicle_control_mode.h>
+#include <uORB/topics/navigation_capabilities.h>
+#include <uORB/topics/parameter_update.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <systemlib/pid/pid.h>
+#include <geo/geo.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/systemlib.h>
+#include <mathlib/mathlib.h>
+
+#include <ecl/l1/ecl_l1_pos_controller.h>
+#include <external_lgpl/tecs/tecs.h>
+
+/**
+ * L1 control app start / stop handling function
+ *
+ * @ingroup apps
+ */
+extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]);
+
+class FixedwingPositionControl
+{
+public:
+	/**
+	 * Constructor
+	 */
+	FixedwingPositionControl();
+
+	/**
+	 * Destructor, also kills the sensors task.
+	 */
+	~FixedwingPositionControl();
+
+	/**
+	 * 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		_global_pos_sub;
+	int		_global_set_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 		_params_sub;			/**< notification of parameter updates */
+	int 		_manual_control_sub;		/**< notification of manual control updates */
+	int		_accel_sub;			/**< body frame accelerations */
+
+	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 */
+
+	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		_loiter_hold_alt;
+	bool		_loiter_hold;
+
+	float		_launch_lat;
+	float		_launch_lon;
+	float		_launch_alt;
+	bool		_launch_valid;
+
+	/* land states */
+	/* not in non-abort mode for landing yet */
+	bool land_noreturn;
+	/* heading hold */
+	float target_bearing;
+
+	/* 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
+
+	ECL_L1_Pos_Controller				_l1_control;
+	TECS						_tecs;
+
+	struct {
+		float l1_period;
+		float l1_damping;
+
+		float time_const;
+		float min_sink_rate;
+		float max_sink_rate;
+		float max_climb_rate;
+		float throttle_damp;
+		float integrator_gain;
+		float vertical_accel_limit;
+		float height_comp_filter_omega;
+		float speed_comp_filter_omega;
+		float roll_throttle_compensation;
+		float speed_weight;
+		float pitch_damping;
+
+		float airspeed_min;
+		float airspeed_trim;
+		float airspeed_max;
+
+		float pitch_limit_min;
+		float pitch_limit_max;
+		float roll_limit;
+		float throttle_min;
+		float throttle_max;
+		float throttle_cruise;
+
+		float throttle_land_max;
+
+		float loiter_hold_radius;
+	}		_parameters;			/**< local copies of interesting parameters */
+
+	struct {
+
+		param_t l1_period;
+		param_t l1_damping;
+
+		param_t time_const;
+		param_t min_sink_rate;
+		param_t max_sink_rate;
+		param_t max_climb_rate;
+		param_t throttle_damp;
+		param_t integrator_gain;
+		param_t vertical_accel_limit;
+		param_t height_comp_filter_omega;
+		param_t speed_comp_filter_omega;
+		param_t roll_throttle_compensation;
+		param_t speed_weight;
+		param_t pitch_damping;
+
+		param_t airspeed_min;
+		param_t airspeed_trim;
+		param_t airspeed_max;
+
+		param_t pitch_limit_min;
+		param_t pitch_limit_max;
+		param_t roll_limit;
+		param_t throttle_min;
+		param_t throttle_max;
+		param_t throttle_cruise;
+
+		param_t throttle_land_max;
+
+		param_t loiter_hold_radius;
+	}		_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_control_mode_poll();
+
+	/**
+	 * Check for airspeed updates.
+	 */
+	bool		vehicle_airspeed_poll();
+
+	/**
+	 * Check for position updates.
+	 */
+	void		vehicle_attitude_poll();
+
+	/**
+	 * Check for accel updates.
+	 */
+	void		vehicle_accel_poll();
+
+	/**
+	 * Check for set triplet updates.
+	 */
+	void		vehicle_setpoint_poll();
+
+	/**
+	 * 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);
+
+	float calculate_target_airspeed(float airspeed_demand);
+	void calculate_gndspeed_undershoot();
+
+	/**
+	 * 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 l1_control
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+FixedwingPositionControl	*g_control;
+}
+
+FixedwingPositionControl::FixedwingPositionControl() :
+
+	_task_should_exit(false),
+	_control_task(-1),
+
+/* subscriptions */
+	_global_pos_sub(-1),
+	_global_set_triplet_sub(-1),
+	_att_sub(-1),
+	_airspeed_sub(-1),
+	_control_mode_sub(-1),
+	_params_sub(-1),
+	_manual_control_sub(-1),
+
+/* publications */
+	_attitude_sp_pub(-1),
+	_nav_capabilities_pub(-1),
+
+/* performance counters */
+	_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
+/* states */
+	_setpoint_valid(false),
+	_loiter_hold(false),
+	_airspeed_error(0.0f),
+	_airspeed_valid(false),
+	_groundspeed_undershoot(0.0f),
+	_global_pos_valid(false),
+	land_noreturn(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");
+	_parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX");
+
+	_parameter_handles.pitch_limit_min = param_find("FW_P_LIM_MIN");
+	_parameter_handles.pitch_limit_max = param_find("FW_P_LIM_MAX");
+	_parameter_handles.roll_limit = param_find("FW_R_LIM");
+	_parameter_handles.throttle_min = param_find("FW_THR_MIN");
+	_parameter_handles.throttle_max = param_find("FW_THR_MAX");
+	_parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE");
+	_parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX");
+
+	_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");
+	_parameter_handles.max_climb_rate =			param_find("FW_T_CLMB_MAX");
+	_parameter_handles.throttle_damp = 			param_find("FW_T_THR_DAMP");
+	_parameter_handles.integrator_gain =			param_find("FW_T_INTEG_GAIN");
+	_parameter_handles.vertical_accel_limit =		param_find("FW_T_VERT_ACC");
+	_parameter_handles.height_comp_filter_omega =		param_find("FW_T_HGT_OMEGA");
+	_parameter_handles.speed_comp_filter_omega =		param_find("FW_T_SPD_OMEGA");
+	_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");
+
+	/* fetch initial parameter values */
+	parameters_update();
+}
+
+FixedwingPositionControl::~FixedwingPositionControl()
+{
+	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);
+	}
+
+	l1_control::g_control = nullptr;
+}
+
+int
+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));
+	param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max));
+
+	param_get(_parameter_handles.pitch_limit_min, &(_parameters.pitch_limit_min));
+	param_get(_parameter_handles.pitch_limit_max, &(_parameters.pitch_limit_max));
+	param_get(_parameter_handles.roll_limit, &(_parameters.roll_limit));
+	param_get(_parameter_handles.throttle_min, &(_parameters.throttle_min));
+	param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max));
+	param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise));
+
+	param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max));
+
+	param_get(_parameter_handles.time_const, &(_parameters.time_const));
+	param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate));
+	param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate));
+	param_get(_parameter_handles.throttle_damp, &(_parameters.throttle_damp));
+	param_get(_parameter_handles.integrator_gain, &(_parameters.integrator_gain));
+	param_get(_parameter_handles.vertical_accel_limit, &(_parameters.vertical_accel_limit));
+	param_get(_parameter_handles.height_comp_filter_omega, &(_parameters.height_comp_filter_omega));
+	param_get(_parameter_handles.speed_comp_filter_omega, &(_parameters.speed_comp_filter_omega));
+	param_get(_parameter_handles.roll_throttle_compensation, &(_parameters.roll_throttle_compensation));
+	param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight));
+	param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping));
+	param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate));
+
+	_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 ||
+	    _parameters.airspeed_min > 100.0f ||
+	    _parameters.airspeed_trim < _parameters.airspeed_min ||
+	    _parameters.airspeed_trim > _parameters.airspeed_max) {
+		warnx("error: airspeed parameters invalid");
+		return 1;
+	}
+
+	return OK;
+}
+
+void
+FixedwingPositionControl::vehicle_control_mode_poll()
+{
+	bool vstatus_updated;
+
+	/* Check HIL state if vehicle status has changed */
+	orb_check(_control_mode_sub, &vstatus_updated);
+
+	if (vstatus_updated) {
+
+		bool was_armed = _control_mode.flag_armed;
+
+		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_alt = _global_pos.alt;
+			_launch_valid = true;
+		}
+	}
+}
+
+bool
+FixedwingPositionControl::vehicle_airspeed_poll()
+{
+	/* check if there is an airspeed update or if it timed out */
+	bool airspeed_updated;
+	orb_check(_airspeed_sub, &airspeed_updated);
+
+	if (airspeed_updated) {
+		orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
+		_airspeed_valid = true;
+		_airspeed_last_valid = hrt_absolute_time();
+
+	} else {
+
+		/* no airspeed updates for one second */
+		if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) {
+			_airspeed_valid = false;
+		}
+	}
+
+	/* update TECS state */
+	_tecs.enable_airspeed(_airspeed_valid);
+
+	return airspeed_updated;
+}
+
+void
+FixedwingPositionControl::vehicle_attitude_poll()
+{
+	/* 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);
+
+		/* set rotation matrix */
+		for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
+				_R_nb(i, j) = _att.R[i][j];
+	}
+}
+
+void
+FixedwingPositionControl::vehicle_accel_poll()
+{
+	/* check if there is a new position */
+	bool accel_updated;
+	orb_check(_accel_sub, &accel_updated);
+
+	if (accel_updated) {
+		orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
+	}
+}
+
+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);
+
+	if (global_sp_updated) {
+		orb_copy(ORB_ID(vehicle_global_position_set_triplet), _global_set_triplet_sub, &_global_triplet);
+		_setpoint_valid = true;
+	}
+}
+
+void
+FixedwingPositionControl::task_main_trampoline(int argc, char *argv[])
+{
+	l1_control::g_control->task_main();
+}
+
+float
+FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand)
+{
+	float airspeed;
+
+	if (_airspeed_valid) {
+		airspeed = _airspeed.true_airspeed_m_s;
+
+	} else {
+		airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f;
+	}
+
+	/* cruise airspeed for all modes unless modified below */
+	float target_airspeed = airspeed_demand;
+
+	/* add minimum ground speed undershoot (only non-zero in presence of sufficient wind) */
+	target_airspeed += _groundspeed_undershoot;
+
+	if (0/* throttle nudging enabled */) {
+		//target_airspeed += nudge term.
+	}
+
+	/* sanity check: limit to range */
+	target_airspeed = math::constrain(target_airspeed, _parameters.airspeed_min, _parameters.airspeed_max);
+
+	/* plain airspeed error */
+	_airspeed_error = target_airspeed - airspeed;
+
+	return target_airspeed;
+}
+
+void
+FixedwingPositionControl::calculate_gndspeed_undershoot()
+{
+
+	if (_global_pos_valid) {
+		/* get ground speed vector */
+		math::Vector2f ground_speed_vector(_global_pos.vx, _global_pos.vy);
+
+		/* rotate with current attitude */
+		math::Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
+		yaw_vector.normalize();
+		float ground_speed_body = yaw_vector * ground_speed_vector;
+
+		/*
+		 * Ground speed undershoot is the amount of ground velocity not reached
+		 * by the plane. Consequently it is zero if airspeed is >= min ground speed
+		 * and positive if airspeed < min ground speed.
+		 *
+		 * This error value ensures that a plane (as long as its throttle capability is
+		 * 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);
+
+	} else {
+		_groundspeed_undershoot = 0;
+	}
+}
+
+bool
+FixedwingPositionControl::control_position(const math::Vector2f &current_position, const math::Vector2f &ground_speed,
+		const struct vehicle_global_position_set_triplet_s &global_triplet)
+{
+	bool setpoint = true;
+
+	calculate_gndspeed_undershoot();
+
+	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;
+
+	_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;
+
+	/* no throttle limit as default */
+	float throttle_max = 1.0f;
+
+	/* AUTONOMOUS FLIGHT */
+
+	// 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) {
+
+		/* get circle mode */
+		bool was_circle_mode = _l1_control.circle_mode();
+
+		/* restore speed weight, in case changed intermittently (e.g. in landing handling) */
+		_tecs.set_speed_weight(_parameters.speed_weight);
+
+		/* execute navigation once we have a setpoint */
+		if (_setpoint_valid) {
+
+			/* current waypoint (the one currently heading for) */
+			math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f);
+
+			/* previous 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 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);
+
+			}
+
+			// XXX add RTL switch
+			if (global_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH && _launch_valid) {
+
+				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();
+
+				_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));
+
+				// XXX handle case when having arrived at home (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();
+
+				_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));
+
+			} 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) {
+
+				/* 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();
+
+				_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));
+
+			} else if (global_triplet.current.nav_cmd == NAV_CMD_LAND) {
+
+				/* switch to heading hold for the last meters, continue heading hold after */
+
+				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) {
+
+					/* heading hold, along the line connecting this and the last waypoint */
+					
+
+					// 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 {
+
+					if (!land_noreturn)
+						target_bearing = _att.yaw;
+					//}
+
+					warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
+
+					_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed);
+
+					if (altitude_error > -5.0f)
+						land_noreturn = true;
+
+				} else {
+
+					/* normal navigation */
+					_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
+				}
+
+				/* do not go down too early */
+				if (wp_distance > 50.0f) {
+					altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt;
+				}
+
+
+				_att_sp.roll_body = _l1_control.nav_roll();
+				_att_sp.yaw_body = _l1_control.nav_bearing();
+
+				/* 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_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;
+
+				if (altitude_error > -4.0f) {
+
+					/* land with minimal speed */
+
+					/* 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 = math::min(throttle_max, _parameters.throttle_land_max);
+
+					/* 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) {
+
+					/* minimize speed to approach speed */
+
+					_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));
+
+				} else {
+
+					/* normal cruise speed */
+
+					_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));
+				}
+
+			} else if (global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
+
+				_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();
+
+				/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
+				if (altitude_error > 10.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));
+
+					/* 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));
+				}
+			}
+
+			// 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);
+			}
+
+			_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));
+
+			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));
+			}
+		}
+
+		/* reset land state */
+		if (global_triplet.current.nav_cmd != NAV_CMD_LAND) {
+			land_noreturn = false;
+		}
+
+		if (was_circle_mode && !_l1_control.circle_mode()) {
+			/* just kicked out of loiter, reset roll integrals */
+			_att_sp.roll_reset_integral = true;
+		}
+
+	} else if (0/* easy mode enabled */) {
+
+		/** EASY FLIGHT **/
+
+		if (0/* switched from another mode to easy */) {
+			_seatbelt_hold_heading = _att.yaw;
+		}
+
+		if (0/* easy on and manual control yaw non-zero */) {
+			_seatbelt_hold_heading = _att.yaw + _manual.yaw;
+		}
+
+		/* climb out control */
+		bool climb_out = false;
+
+		/* user wants to climb out */
+		if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
+			climb_out = true;
+		}
+
+		/* if in seatbelt mode, set airspeed based on manual control */
+
+		// XXX check if ground speed undershoot should be applied here
+		float seatbelt_airspeed = _parameters.airspeed_min +
+					  (_parameters.airspeed_max - _parameters.airspeed_min) *
+					  _manual.throttle;
+
+		_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed);
+		_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 */) {
+
+		/** SEATBELT FLIGHT **/
+
+		if (0/* switched from another mode to seatbelt */) {
+			_seatbelt_hold_heading = _att.yaw;
+		}
+
+		if (0/* seatbelt on and manual control yaw non-zero */) {
+			_seatbelt_hold_heading = _att.yaw + _manual.yaw;
+		}
+
+		/* if in seatbelt mode, set airspeed based on manual control */
+
+		// XXX check if ground speed undershoot should be applied here
+		float seatbelt_airspeed = _parameters.airspeed_min +
+					  (_parameters.airspeed_max - _parameters.airspeed_min) *
+					  _manual.throttle;
+
+		/* user switched off throttle */
+		if (_manual.throttle < 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) {
+			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);
+
+	} else {
+
+		/** MANUAL FLIGHT **/
+
+		/* no flight mode applies, do not publish an attitude setpoint */
+		setpoint = false;
+	}
+
+	_att_sp.pitch_body = _tecs.get_pitch_demand();
+	_att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max);
+
+	return setpoint;
+}
+
+void
+FixedwingPositionControl::task_main()
+{
+
+	/* inform about start */
+	warnx("Initializing..");
+	fflush(stdout);
+
+	/*
+	 * 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));
+	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
+	_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));
+
+	/* rate limit vehicle status updates to 5Hz */
+	orb_set_interval(_control_mode_sub, 200);
+	/* rate limit position updates to 50 Hz */
+	orb_set_interval(_global_pos_sub, 20);
+
+	/* abort on a nonzero return value from the parameter init */
+	if (parameters_update()) {
+		/* parameter setup went wrong, abort */
+		warnx("aborting startup due to errors.");
+		_task_should_exit = true;
+	}
+
+	/* wakeup source(s) */
+	struct pollfd fds[2];
+
+	/* Setup of loop */
+	fds[0].fd = _params_sub;
+	fds[0].events = POLLIN;
+	fds[1].fd = _global_pos_sub;
+	fds[1].events = POLLIN;
+
+	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 error %d, %d", pret, errno);
+			continue;
+		}
+
+		perf_begin(_loop_perf);
+
+		/* check vehicle status for changes to publication state */
+		vehicle_control_mode_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();
+		}
+
+		/* 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);
+
+			// XXX add timestamp check
+			_global_pos_valid = true;
+
+			vehicle_attitude_poll();
+			vehicle_setpoint_poll();
+			vehicle_accel_poll();
+			vehicle_airspeed_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);
+
+			/*
+			 * Attempt to control position, on success (= sensors present and not in manual mode),
+			 * publish setpoint.
+			 */
+			if (control_position(current_position, ground_speed, _global_triplet)) {
+				_att_sp.timestamp = hrt_absolute_time();
+
+				/* lazily publish the setpoint only once available */
+				if (_attitude_sp_pub > 0) {
+					/* publish the attitude setpoint */
+					orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp);
+
+				} else {
+					/* advertise and publish */
+					_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);
+
+				/* lazily publish navigation capabilities */
+				if (turn_distance != _nav_capabilities.turn_distance && 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);
+					}
+				}
+
+			}
+
+		}
+
+		perf_end(_loop_perf);
+	}
+
+	warnx("exiting.\n");
+
+	_control_task = -1;
+	_exit(0);
+}
+
+int
+FixedwingPositionControl::start()
+{
+	ASSERT(_control_task == -1);
+
+	/* start the task */
+	_control_task = task_spawn_cmd("fw_pos_control_l1",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       4048,
+				       (main_t)&FixedwingPositionControl::task_main_trampoline,
+				       nullptr);
+
+	if (_control_task < 0) {
+		warn("task start failed");
+		return -errno;
+	}
+
+	return OK;
+}
+
+int fw_pos_control_l1_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		errx(1, "usage: fw_pos_control_l1 {start|stop|status}");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (l1_control::g_control != nullptr)
+			errx(1, "already running");
+
+		l1_control::g_control = new FixedwingPositionControl;
+
+		if (l1_control::g_control == nullptr)
+			errx(1, "alloc failed");
+
+		if (OK != l1_control::g_control->start()) {
+			delete l1_control::g_control;
+			l1_control::g_control = nullptr;
+			err(1, "start failed");
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		if (l1_control::g_control == nullptr)
+			errx(1, "not running");
+
+		delete l1_control::g_control;
+		l1_control::g_control = nullptr;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (l1_control::g_control) {
+			errx(0, "running");
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}
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
new file mode 100644
index 000000000..31a9cdefa
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -0,0 +1,166 @@
+/****************************************************************************
+ *
+ *   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 fw_pos_control_l1_params.c
+ *
+ * Parameters defined by the L1 position control task
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * Controller parameters, accessible via MAVLink
+ *
+ */
+
+/**
+ * L1 period
+ *
+ * This is the L1 distance and defines the tracking
+ * point ahead of the aircraft its following.
+ * A value of 25 meters works for most aircraft. Shorten
+ * slowly during tuning until response is sharp without oscillation.
+ *
+ * @min 1.0
+ * @max 100.0
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 25.0f);
+
+/**
+ * L1 damping
+ *
+ * Damping factor for L1 control.
+ *
+ * @min 0.6
+ * @max 0.9
+ * @group L1 Control
+ */
+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.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.7f);
+
+/**
+ * Negative pitch limit
+ *
+ * The minimum negative pitch the controller will output.
+ *
+ * @unit degrees
+ * @min -60.0
+ * @max 0.0
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f);
+
+/**
+ * Positive pitch limit
+ *
+ * The maximum positive pitch the controller will output.
+ *
+ * @unit degrees
+ * @min 0.0
+ * @max 60.0
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f);
+
+
+PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f);
+
+
+PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
+
+
+PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
+
+PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f);
+
+PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
+
+
+PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
+
+
+PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
+
+
+PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
+
+
+PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
+
+
+PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
+
+
+PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
+
+
+PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
+
+
+PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
+
+
+PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
+
+
+PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
+
+
+PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk
new file mode 100644
index 000000000..b00b9aa5a
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/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.
+#
+############################################################################
+
+#
+# Fixedwing L1 position control application
+#
+
+MODULE_COMMAND	= fw_pos_control_l1
+
+SRCS		= fw_pos_control_l1_main.cpp \
+		  fw_pos_control_l1_params.c
diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c
index a80bf9cb8..d383146f9 100644
--- a/src/modules/gpio_led/gpio_led.c
+++ b/src/modules/gpio_led/gpio_led.c
@@ -48,22 +48,29 @@
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 #include <systemlib/systemlib.h>
+#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>
 
 struct gpio_led_s {
 	struct work_s work;
 	int gpio_fd;
+	bool use_io;
 	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;
 };
 
 static struct gpio_led_s gpio_led_data;
+static bool gpio_led_started = false;
 
 __EXPORT int gpio_led_main(int argc, char *argv[]);
 
@@ -73,48 +80,128 @@ void gpio_led_cycle(FAR void *arg);
 
 int gpio_led_main(int argc, char *argv[])
 {
-	int pin = GPIO_EXT_1;
+	if (argc < 2) {
+		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"
+		     "\t\t2\tPX4FMU GPIO_EXT2\n"
+		     "\t\ta1\tPX4IO ACC1\n"
+		     "\t\ta2\tPX4IO ACC2\n"
+		     "\t\tr1\tPX4IO RELAY1\n"
+		     "\t\tr2\tPX4IO RELAY2");
 
-	if (argc > 1) {
-		if (!strcmp(argv[1], "-p")) {
-			if (!strcmp(argv[2], "1")) {
-				pin = GPIO_EXT_1;
+	} else {
+
+		if (!strcmp(argv[1], "start")) {
+			if (gpio_led_started) {
+				errx(1, "already running");
+			}
+
+			bool use_io = false;
+			int pin = GPIO_EXT_1;
+
+			if (argc > 2) {
+				if (!strcmp(argv[2], "-p")) {
+					if (!strcmp(argv[3], "1")) {
+						use_io = false;
+						pin = GPIO_EXT_1;
+
+					} else if (!strcmp(argv[3], "2")) {
+						use_io = false;
+						pin = GPIO_EXT_2;
 
-			} else if (!strcmp(argv[2], "2")) {
-				pin = GPIO_EXT_2;
+					} else if (!strcmp(argv[3], "a1")) {
+						use_io = true;
+						pin = PX4IO_P_SETUP_RELAYS_ACC1;
+
+					} else if (!strcmp(argv[3], "a2")) {
+						use_io = true;
+						pin = PX4IO_P_SETUP_RELAYS_ACC2;
+
+					} else if (!strcmp(argv[3], "r1")) {
+						use_io = true;
+						pin = PX4IO_P_SETUP_RELAYS_POWER1;
+
+					} else if (!strcmp(argv[3], "r2")) {
+						use_io = true;
+						pin = PX4IO_P_SETUP_RELAYS_POWER2;
+
+					} else {
+						errx(1, "unsupported pin: %s", argv[3]);
+					}
+				}
+			}
+
+			memset(&gpio_led_data, 0, sizeof(gpio_led_data));
+			gpio_led_data.use_io = use_io;
+			gpio_led_data.pin = pin;
+			int ret = work_queue(LPWORK, &gpio_led_data.work, gpio_led_start, &gpio_led_data, 0);
+
+			if (ret != 0) {
+				errx(1, "failed to queue work: %d", ret);
 
 			} else {
-				printf("[gpio_led] Unsupported pin: %s\n", argv[2]);
-				exit(1);
+				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);
 			}
-		}
-	}
 
-	memset(&gpio_led_data, 0, sizeof(gpio_led_data));
-	gpio_led_data.pin = pin;
-	int ret = work_queue(LPWORK, &gpio_led_data.work, gpio_led_start, &gpio_led_data, 0);
+			exit(0);
 
-	if (ret != 0) {
-		printf("[gpio_led] Failed to queue work: %d\n", ret);
-		exit(1);
-	}
 
-	exit(0);
+		} else if (!strcmp(argv[1], "stop")) {
+			if (gpio_led_started) {
+				gpio_led_started = false;
+				warnx("stop");
+
+			} else {
+				errx(1, "not running");
+			}
+
+		} else {
+			errx(1, "unrecognized command '%s', only supporting 'start' or 'stop'", argv[1]);
+		}
+	}
 }
 
 void gpio_led_start(FAR void *arg)
 {
 	FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg;
 
+	char *gpio_dev;
+
+	if (priv->use_io) {
+		gpio_dev = PX4IO_DEVICE_PATH;
+	} else {
+		gpio_dev = PX4FMU_DEVICE_PATH;
+	}
+
 	/* open GPIO device */
-	priv->gpio_fd = open(GPIO_DEVICE_PATH, 0);
+	priv->gpio_fd = open(gpio_dev, 0);
 
 	if (priv->gpio_fd < 0) {
-		printf("[gpio_led] GPIO: open fail\n");
+		// TODO find way to print errors
+		//printf("gpio_led: GPIO device \"%s\" open fail\n", gpio_dev);
+		gpio_led_started = false;
 		return;
 	}
 
 	/* configure GPIO pin */
+	/* 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 */
@@ -125,11 +212,11 @@ void gpio_led_start(FAR void *arg)
 	int ret = work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, 0);
 
 	if (ret != 0) {
-		printf("[gpio_led] Failed to queue work: %d\n", ret);
+		// TODO find way to print errors
+		//printf("gpio_led: failed to queue work: %d\n", ret);
+		gpio_led_started = false;
 		return;
 	}
-
-	printf("[gpio_led] Started, using pin GPIO_EXT%i\n", priv->pin);
 }
 
 void gpio_led_cycle(FAR void *arg)
@@ -143,10 +230,15 @@ void gpio_led_cycle(FAR void *arg)
 	if (status_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->status.flag_system_armed) {
+	if (priv->armed.armed) {
 		if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
 			pattern = 0x3f;	// ****** solid (armed)
 
@@ -155,11 +247,10 @@ void gpio_led_cycle(FAR void *arg)
 		}
 
 	} else {
-		if (priv->status.state_machine == SYSTEM_STATE_PREFLIGHT) {
+		if (priv->armed.ready_to_arm) {
 			pattern = 0x00;	// ______ off (disarmed, preflight check)
 
-		} else if (priv->status.state_machine == SYSTEM_STATE_STANDBY &&
-			   priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
+		} else if (priv->armed.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
 			pattern = 0x38;	// ***___ slow blink (disarmed, ready)
 
 		} else {
@@ -175,7 +266,6 @@ 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);
 		}
@@ -186,6 +276,12 @@ void gpio_led_cycle(FAR void *arg)
 	if (priv->counter > 5)
 		priv->counter = 0;
 
-	/* repeat cycle at 5 Hz*/
-	work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(200000));
+	/* repeat cycle at 5 Hz */
+	if (gpio_led_started) {
+		work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(200000));
+
+	} else {
+		/* switch off LED on stop */
+		ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);
+	}
 }
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index c72a53fee..20853379d 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -49,7 +49,6 @@
 #include <mqueue.h>
 #include <string.h>
 #include "mavlink_bridge_header.h"
-#include <v1.0/common/mavlink.h>
 #include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
@@ -65,6 +64,7 @@
 #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"
@@ -144,14 +144,6 @@ set_hil_on_off(bool hil_enabled)
 	/* Enable HIL */
 	if (hil_enabled && !mavlink_hil_enabled) {
 
-		/* Advertise topics */
-		pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
-		pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
-
-		/* sensore level hil */
-		pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
-		pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
-
 		mavlink_hil_enabled = true;
 
 		/* ramp up some HIL-related subscriptions */
@@ -190,102 +182,80 @@ set_hil_on_off(bool hil_enabled)
 }
 
 void
-get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
+get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
 {
 	/* reset MAVLink mode bitfield */
-	*mavlink_mode = 0;
+	*mavlink_base_mode = 0;
+	*mavlink_custom_mode = 0;
 
-	/* set mode flags independent of system state */
+	/**
+	 * Set mode flags
+	 **/
 
 	/* HIL */
-	if (v_status.flag_hil_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
+	if (v_status.hil_state == HIL_STATE_ON) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
 	}
 
-	/* manual input */
-	if (v_status.flag_control_manual_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_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;
 	}
 
-	/* attitude or rate control */
-	if (v_status.flag_control_attitude_enabled ||
-	    v_status.flag_control_rates_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
-	}
-
-	/* vector control */
-	if (v_status.flag_control_velocity_enabled ||
-	    v_status.flag_control_position_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
-	}
-
-	/* autonomous mode */
-	if (v_status.state_machine == SYSTEM_STATE_AUTO) {
-		*mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
-	}
-
-	/* set arming state */
-	if (armed.armed) {
-		*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
-
-	} else {
-		*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
-	}
-
-	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;
+	/* 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;
 		}
-
-		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;
-		break;
-
-	case SYSTEM_STATE_STABILIZED:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
-
-	case SYSTEM_STATE_AUTO:
+	}
+	*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;
-		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:
+	} 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;
-		break;
+	} else {
+		warnx("Unknown mavlink state");
+		*mavlink_state = MAV_STATE_CRITICAL;
 	}
-
 }
 
 
@@ -321,6 +291,9 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m
 		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:
@@ -429,12 +402,12 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf
 	case 921600: speed = B921600; break;
 
 	default:
-		fprintf(stderr, "[mavlink] ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud);
+		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 */
-	printf("[mavlink] UART is %s, baudrate is %d\n", uart_name, baud);
+	warnx("UART is %s, baudrate is %d\n", uart_name, baud);
 	uart = open(uart_name, O_RDWR | O_NOCTTY);
 
 	/* Try to set baud rate */
@@ -442,44 +415,42 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf
 	int termios_state;
 	*is_usb = false;
 
-	/* make some wild guesses including that USB serial is indicated by either /dev/ttyACM0 or /dev/console */
-	if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/console") != OK) {
-		/* Back up the original uart configuration to restore it after exit */
-		if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
-			fprintf(stderr, "[mavlink] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
-			close(uart);
-			return -1;
-		}
+	/* 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);
+	/* 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;
+	/* 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) {
-			fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
+			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) {
-			fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
-			close(uart);
-			return -1;
-		}
-
-	} else {
-		*is_usb = true;
+	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, uint8_t *ch, int length)
+mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
 {
 	write(uart, ch, (size_t)(sizeof(uint8_t) * length));
 }
@@ -487,7 +458,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length)
 /*
  * Internal function to give access to the channel status for each channel
  */
-mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
+extern 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];
@@ -496,7 +467,7 @@ mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
 /*
  * Internal function to give access to the channel buffer for each channel
  */
-mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel)
+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];
@@ -545,7 +516,7 @@ void mavlink_update_system(void)
 int mavlink_thread_main(int argc, char *argv[])
 {
 	/* initialize mavlink text message buffering */
-	mavlink_logbuffer_init(&lb, 5);
+	mavlink_logbuffer_init(&lb, 10);
 
 	int ch;
 	char *device_name = "/dev/ttyS1";
@@ -560,7 +531,7 @@ int mavlink_thread_main(int argc, char *argv[])
 		case 'b':
 			baudrate = strtoul(optarg, NULL, 10);
 
-			if (baudrate == 0)
+			if (baudrate < 9600 || baudrate > 921600)
 				errx(1, "invalid baud rate '%s'", optarg);
 
 			break;
@@ -579,6 +550,7 @@ int mavlink_thread_main(int argc, char *argv[])
 
 		default:
 			usage();
+			break;
 		}
 	}
 
@@ -685,24 +657,28 @@ int mavlink_thread_main(int argc, char *argv[])
 
 			/* 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(&mavlink_state, &mavlink_mode);
+			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_mode, v_status.state_machine, mavlink_state);
+			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state);
 
 			/* switch HIL mode if required */
-			set_hil_on_off(v_status.flag_hil_enabled);
+			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,
-						    v_status.voltage_battery * 1000.0f,
-						    v_status.current_battery * 1000.0f,
-						    v_status.battery_remaining,
+						    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,
@@ -714,21 +690,26 @@ int mavlink_thread_main(int argc, char *argv[])
 
 		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);
+		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();
 		}
@@ -755,12 +736,14 @@ int mavlink_thread_main(int argc, char *argv[])
 	pthread_join(uorb_receive_thread, NULL);
 
 	/* Reset the UART flags to original state */
-	if (!usb_uart)
-		tcsetattr(uart, TCSANOW, &uart_config_original);
+	tcsetattr(uart, TCSANOW, &uart_config_original);
+
+	/* destroy log buffer */
+	//mavlink_logbuffer_destroy(&lb);
 
 	thread_running = false;
 
-	exit(0);
+	return 0;
 }
 
 static void
@@ -784,7 +767,7 @@ int mavlink_main(int argc, char *argv[])
 
 		/* this is not an error */
 		if (thread_running)
-			errx(0, "mavlink already running\n");
+			errx(0, "mavlink already running");
 
 		thread_should_exit = false;
 		mavlink_task = task_spawn_cmd("mavlink",
@@ -793,15 +776,25 @@ int mavlink_main(int argc, char *argv[])
 					  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);
-			printf(".");
+			warnx(".");
 		}
 
 		warnx("terminated.");
diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h
index 0010bb341..149efda60 100644
--- a/src/modules/mavlink/mavlink_bridge_header.h
+++ b/src/modules/mavlink/mavlink_bridge_header.h
@@ -73,9 +73,11 @@ 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, uint8_t *ch, int length);
+extern void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length);
 
-mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
-mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
+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>
 
 #endif /* MAVLINK_BRIDGE_HEADER_H */
diff --git a/src/modules/mavlink/mavlink_hil.h b/src/modules/mavlink/mavlink_hil.h
index 8c7a5b514..744ed7d94 100644
--- a/src/modules/mavlink/mavlink_hil.h
+++ b/src/modules/mavlink/mavlink_hil.h
@@ -41,15 +41,6 @@
 
 extern bool mavlink_hil_enabled;
 
-extern struct vehicle_global_position_s hil_global_pos;
-extern struct vehicle_attitude_s hil_attitude;
-extern struct sensor_combined_s hil_sensors;
-extern struct vehicle_gps_position_s hil_gps;
-extern orb_advert_t pub_hil_global_pos;
-extern orb_advert_t pub_hil_attitude;
-extern orb_advert_t pub_hil_sensors;
-extern orb_advert_t pub_hil_gps;
-
 /**
  * Enable / disable Hardware in the Loop simulation mode.
  *
diff --git a/src/modules/mavlink/mavlink_parameters.c b/src/modules/mavlink/mavlink_parameters.c
index 819f3441b..18ca7a854 100644
--- a/src/modules/mavlink/mavlink_parameters.c
+++ b/src/modules/mavlink/mavlink_parameters.c
@@ -40,7 +40,6 @@
  */
 
 #include "mavlink_bridge_header.h"
-#include <v1.0/common/mavlink.h>
 #include "mavlink_parameters.h"
 #include <uORB/uORB.h>
 #include "math.h" /* isinf / isnan checks */
diff --git a/src/modules/mavlink/mavlink_receiver.c b/src/modules/mavlink/mavlink_receiver.cpp
index 940d030b2..7b6fad658 100644
--- a/src/modules/mavlink/mavlink_receiver.c
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -49,9 +49,11 @@
 #include <fcntl.h>
 #include <mqueue.h>
 #include <string.h>
-#include "mavlink_bridge_header.h"
-#include <v1.0/common/mavlink.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_mag.h>
+#include <drivers/drv_baro.h>
 #include <time.h>
 #include <float.h>
 #include <unistd.h>
@@ -62,10 +64,19 @@
 #include <stdlib.h>
 #include <poll.h>
 
+#include <mathlib/mathlib.h>
+
 #include <systemlib/param/param.h>
 #include <systemlib/systemlib.h>
+#include <systemlib/err.h>
+#include <systemlib/airspeed.h>
 #include <mavlink/mavlink_log.h>
+#include <commander/px4_custom_mode.h>
+#include <geo/geo.h>
+
+__BEGIN_DECLS
 
+#include "mavlink_bridge_header.h"
 #include "waypoints.h"
 #include "orb_topics.h"
 #include "missionlib.h"
@@ -73,8 +84,12 @@
 #include "mavlink_parameters.h"
 #include "util.h"
 
+extern bool gcs_link;
+
+__END_DECLS
+
 /* XXX should be in a header somewhere */
-pthread_t receive_start(int uart);
+extern "C" pthread_t receive_start(int uart);
 
 static void handle_message(mavlink_message_t *msg);
 static void *receive_thread(void *arg);
@@ -85,21 +100,39 @@ 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;
-orb_advert_t pub_hil_global_pos = -1;
-orb_advert_t pub_hil_attitude = -1;
-orb_advert_t pub_hil_gps = -1;
-orb_advert_t pub_hil_sensors = -1;
+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;
 
-extern bool gcs_link;
+// 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)
@@ -131,7 +164,8 @@ handle_message(mavlink_message_t *msg)
 				vcmd.param5 = cmd_mavlink.param5;
 				vcmd.param6 = cmd_mavlink.param6;
 				vcmd.param7 = cmd_mavlink.param7;
-				vcmd.command = cmd_mavlink.command;
+				// 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;
@@ -141,10 +175,11 @@ handle_message(mavlink_message_t *msg)
 				/* 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);
+				} else {
+					/* publish */
+					orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
+				}
 			}
 		}
 	}
@@ -179,15 +214,17 @@ handle_message(mavlink_message_t *msg)
 		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 = new_mode.custom_mode;
+		vcmd.param2 = custom_mode.main_mode;
 		vcmd.param3 = 0;
 		vcmd.param4 = 0;
 		vcmd.param5 = 0;
 		vcmd.param6 = 0;
 		vcmd.param7 = 0;
-		vcmd.command = MAV_CMD_DO_SET_MODE;
+		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;
@@ -281,7 +318,7 @@ handle_message(mavlink_message_t *msg)
 			}
 
 			offboard_control_sp.armed = ml_armed;
-			offboard_control_sp.mode = ml_mode;
+			offboard_control_sp.mode = static_cast<enum OFFBOARD_CONTROL_MODE>(ml_mode);
 
 			offboard_control_sp.timestamp = hrt_absolute_time();
 
@@ -296,6 +333,33 @@ handle_message(mavlink_message_t *msg)
 		}
 	}
 
+	/* handle status updates of the radio */
+	if (msg->msgid == MAVLINK_MSG_ID_RADIO_STATUS) {
+
+		struct telemetry_status_s tstatus;
+
+		mavlink_radio_status_t rstatus;
+		mavlink_msg_radio_status_decode(msg, &rstatus);
+
+		/* publish telemetry status topic */
+		tstatus.timestamp = hrt_absolute_time();
+		tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
+		tstatus.rssi = rstatus.rssi;
+		tstatus.remote_rssi = rstatus.remrssi;
+		tstatus.txbuf = rstatus.txbuf;
+		tstatus.noise = rstatus.noise;
+		tstatus.remote_noise = rstatus.remnoise;
+		tstatus.rxerrors = rstatus.rxerrors;
+		tstatus.fixed = rstatus.fixed;
+
+		if (telemetry_status_pub == 0) {
+			telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
+
+		} else {
+			orb_publish(ORB_ID(telemetry_status), telemetry_status_pub, &tstatus);
+		}
+	}
+
 	/*
 	 * Only decode hil messages in HIL mode.
 	 *
@@ -308,31 +372,25 @@ handle_message(mavlink_message_t *msg)
 
 		uint64_t timestamp = hrt_absolute_time();
 
-		if (msg->msgid == MAVLINK_MSG_ID_HIGHRES_IMU) {
+		if (msg->msgid == MAVLINK_MSG_ID_HIL_SENSOR) {
 
-			mavlink_highres_imu_t imu;
-			mavlink_msg_highres_imu_decode(msg, &imu);
-
-			/* packet counter */
-			static uint16_t hil_counter = 0;
-			static uint16_t hil_frames = 0;
-			static uint64_t old_timestamp = 0;
+			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_counter = hil_counter;
 			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 */
-			hil_sensors.accelerometer_counter = hil_counter;
 			static const float mg2ms2 = 9.8f / 1000.0f;
 			hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
 			hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
@@ -342,15 +400,15 @@ handle_message(mavlink_message_t *msg)
 			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;
-			hil_sensors.adc_voltage_v[1] = 0;
-			hil_sensors.adc_voltage_v[2] = 0;
+			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_counter = hil_counter;
 			hil_sensors.magnetometer_raw[0] = imu.xmag / mga2ga;
 			hil_sensors.magnetometer_raw[1] = imu.ymag / mga2ga;
 			hil_sensors.magnetometer_raw[2] = imu.zmag / mga2ga;
@@ -360,18 +418,141 @@ handle_message(mavlink_message_t *msg)
 			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;
 
-			hil_sensors.gyro_counter = hil_counter;
-			hil_sensors.magnetometer_counter = hil_counter;
-			hil_sensors.accelerometer_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;
 
-			/* publish */
-			orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
+			/* 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);
+
+			} else {
+				orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
+			}
+
+			//warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s);
+
+			/* 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();
+
+			if (pub_hil_gyro < 0) {
+				pub_hil_gyro = orb_advertise(ORB_ID(sensor_gyro), &gyro);
+
+			} else {
+				orb_publish(ORB_ID(sensor_gyro), pub_hil_gyro, &gyro);
+			}
+
+			struct accel_report accel;
+
+			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;
+
+			accel.timestamp = hrt_absolute_time();
+
+			if (pub_hil_accel < 0) {
+				pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel);
+
+			} else {
+				orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
+			}
+
+			struct mag_report mag;
+
+			mag.x_raw = imu.xmag / mga2ga;
+
+			mag.y_raw = imu.ymag / mga2ga;
+
+			mag.z_raw = imu.zmag / mga2ga;
+
+			mag.x = imu.xmag;
+
+			mag.y = imu.ymag;
+
+			mag.z = imu.zmag;
+
+			mag.timestamp = hrt_absolute_time();
+
+			if (pub_hil_mag < 0) {
+				pub_hil_mag = orb_advertise(ORB_ID(sensor_mag), &mag);
+
+			} else {
+				orb_publish(ORB_ID(sensor_mag), pub_hil_mag, &mag);
+			}
+
+			struct baro_report baro;
+
+			baro.pressure = imu.abs_pressure;
+
+			baro.altitude = imu.pressure_alt;
+
+			baro.temperature = imu.temperature;
+
+			baro.timestamp = hrt_absolute_time();
+
+			if (pub_hil_baro < 0) {
+				pub_hil_baro = orb_advertise(ORB_ID(sensor_baro), &baro);
+
+			} else {
+				orb_publish(ORB_ID(sensor_baro), pub_hil_baro, &baro);
+			}
+
+			/* publish combined sensor topic */
+			if (pub_hil_sensors > 0) {
+				orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
+
+			} else {
+				pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
+			}
+
+			/* 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;
+
+			/* lazily publish the battery voltage */
+			if (pub_hil_battery > 0) {
+				orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status);
+
+			} else {
+				pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
+			}
 
 			// increment counters
 			hil_counter++;
@@ -379,21 +560,16 @@ handle_message(mavlink_message_t *msg)
 
 			// output
 			if ((timestamp - old_timestamp) > 10000000) {
-				printf("receiving hil imu at %d hz\n", hil_frames/10);
+				printf("receiving hil sensor at %d hz\n", hil_frames / 10);
 				old_timestamp = timestamp;
 				hil_frames = 0;
 			}
 		}
 
-		if (msg->msgid == MAVLINK_MSG_ID_GPS_RAW_INT) {
-
-			mavlink_gps_raw_int_t gps;
-			mavlink_msg_gps_raw_int_decode(msg, &gps);
+		if (msg->msgid == MAVLINK_MSG_ID_HIL_GPS) {
 
-			/* packet counter */
-			static uint16_t hil_counter = 0;
-			static uint16_t hil_frames = 0;
-			static uint64_t old_timestamp = 0;
+			mavlink_hil_gps_t gps;
+			mavlink_msg_hil_gps_decode(msg, &gps);
 
 			/* gps */
 			hil_gps.timestamp_position = gps.time_usec;
@@ -409,80 +585,174 @@ handle_message(mavlink_message_t *msg)
 
 			/* 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 = (float)gps.vel * 1e-2f * cosf(heading_rad);
-			hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(heading_rad);
-			hil_gps.vel_d_m_s = 0.0f;
+
+			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 speced as -PI..+PI */
+			/* 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 */
-			orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps);
+			/* publish GPS measurement data */
+			if (pub_hil_gps > 0) {
+				orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps);
 
-			// increment counters
-			hil_counter += 1 ;
-			hil_frames += 1 ;
+			} else {
+				pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
+			}
 
-			// output
-			// if ((timestamp - old_timestamp) > 10000000) {
-			// 	printf("receiving hil gps at %d hz\n", hil_frames/10);
-			// 	old_timestamp = timestamp;
-			// 	hil_frames = 0;
-			// }
 		}
 
-		if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) {
-
-			mavlink_hil_state_t hil_state;
-			mavlink_msg_hil_state_decode(msg, &hil_state);
+		if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE_QUATERNION) {
 
-			/* Calculate Rotation Matrix */
-			//TODO: better clarification which app does this, atm we have a ekf for quadrotors which does this, but there is no such thing if fly in fixed wing mode
-
-			if (mavlink_system.type == MAV_TYPE_FIXED_WING) {
-				//TODO: assuming low pitch and roll values for now
-				hil_attitude.R[0][0] = cosf(hil_state.yaw);
-				hil_attitude.R[0][1] = sinf(hil_state.yaw);
-				hil_attitude.R[0][2] = 0.0f;
+			mavlink_hil_state_quaternion_t hil_state;
+			mavlink_msg_hil_state_quaternion_decode(msg, &hil_state);
 
-				hil_attitude.R[1][0] = -sinf(hil_state.yaw);
-				hil_attitude.R[1][1] = cosf(hil_state.yaw);
-				hil_attitude.R[1][2] = 0.0f;
+			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;
 
-				hil_attitude.R[2][0] = 0.0f;
-				hil_attitude.R[2][1] = 0.0f;
-				hil_attitude.R[2][2] = 1.0f;
+			if (pub_hil_airspeed < 0) {
+				pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed);
 
-				hil_attitude.R_valid = true;
+			} else {
+				orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
 			}
 
-			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;
+			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;
 
+			} else {
+				pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
+			}
 
-			/* set timestamp and notify processes (broadcast) */
-			hil_global_pos.timestamp = hrt_absolute_time();
-			orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &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);
+			}
 
-			hil_attitude.roll = hil_state.roll;
-			hil_attitude.pitch = hil_state.pitch;
-			hil_attitude.yaw = hil_state.yaw;
+			/* Calculate Rotation Matrix */
+			math::Quaternion q(hil_state.attitude_quaternion);
+			math::Dcm C_nb(q);
+			math::EulerAngles euler(C_nb);
+
+			/* 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);
+
+			hil_attitude.R_valid = true;
+
+			/* 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;
+
+			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;
 
 			/* set timestamp and notify processes (broadcast) */
 			hil_attitude.timestamp = hrt_absolute_time();
-			orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude);
+
+			if (pub_hil_attitude > 0) {
+				orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude);
+
+			} else {
+				pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
+			}
+
+			struct accel_report accel;
+
+			accel.x_raw = hil_state.xacc / 9.81f * 1e3f;
+
+			accel.y_raw = hil_state.yacc / 9.81f * 1e3f;
+
+			accel.z_raw = hil_state.zacc / 9.81f * 1e3f;
+
+			accel.x = hil_state.xacc;
+
+			accel.y = hil_state.yacc;
+
+			accel.z = hil_state.zacc;
+
+			accel.temperature = 25.0f;
+
+			accel.timestamp = hrt_absolute_time();
+
+			if (pub_hil_accel < 0) {
+				pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel);
+
+			} else {
+				orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
+			}
+
+			/* 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;
+
+			/* lazily publish the battery voltage */
+			if (pub_hil_battery > 0) {
+				orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status);
+
+			} else {
+				pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
+			}
 		}
 
 		if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
@@ -549,15 +819,23 @@ receive_thread(void *arg)
 
 	mavlink_message_t msg;
 
-	prctl(PR_SET_NAME, "mavlink uart rcv", getpid());
+	prctl(PR_SET_NAME, "mavlink_uart_rcv", getpid());
 
-	while (!thread_should_exit) {
+	struct pollfd fds[1];
+	fds[0].fd = uart_fd;
+	fds[0].events = POLLIN;
 
-		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 */
-			ssize_t nread = read(uart_fd, buf, sizeof(buf));
+			nread = read(uart_fd, buf, sizeof(buf));
 
 			/* if read failed, this loop won't execute */
 			for (ssize_t i = 0; i < nread; i++) {
@@ -565,10 +843,10 @@ receive_thread(void *arg)
 					/* handle generic messages and commands */
 					handle_message(&msg);
 
-					/* Handle packet with waypoint component */
+					/* handle packet with waypoint component */
 					mavlink_wpm_message_handler(&msg, &global_pos, &local_pos);
 
-					/* Handle packet with parameter component */
+					/* handle packet with parameter component */
 					mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
 				}
 			}
@@ -589,12 +867,15 @@ receive_start(int uart)
 	fcntl(uart, 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, 2048);
+	pthread_attr_setstacksize(&receiveloop_attr, 3000);
 
 	pthread_t thread;
 	pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
+
+	pthread_attr_destroy(&receiveloop_attr);
 	return thread;
 }
diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c
index d369e05ff..124b3b2ae 100644
--- a/src/modules/mavlink/missionlib.c
+++ b/src/modules/mavlink/missionlib.c
@@ -48,7 +48,6 @@
 #include <mqueue.h>
 #include <string.h>
 #include "mavlink_bridge_header.h"
-#include <v1.0/common/mavlink.h>
 #include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
@@ -60,10 +59,12 @@
 #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"
@@ -73,6 +74,10 @@
 #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)
@@ -123,6 +128,74 @@ uint64_t mavlink_missionlib_get_system_timestamp()
 }
 
 /**
+ * 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
@@ -133,9 +206,13 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 		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 */
@@ -144,9 +221,10 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 		sp.lon = param6_lon_y * 1e7f;
 		sp.altitude = param7_alt_z;
 		sp.altitude_is_relative = false;
-		sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
+		sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
+		set_special_fields(param1, param2, param3, param4, command, &sp);
 
-		/* Initialize publication if necessary */
+		/* Initialize setpoint publication if necessary */
 		if (global_position_setpoint_pub < 0) {
 			global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp);
 
@@ -154,6 +232,108 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 			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) {
@@ -163,7 +343,8 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 		sp.lon = param6_lon_y * 1e7f;
 		sp.altitude = param7_alt_z;
 		sp.altitude_is_relative = true;
-		sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
+		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) {
@@ -173,6 +354,8 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 			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) {
@@ -181,7 +364,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 		sp.x = param5_lat_x;
 		sp.y = param6_lon_y;
 		sp.z = param7_alt_z;
-		sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
+		sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
 
 		/* Initialize publication if necessary */
 		if (local_position_setpoint_pub < 0) {
@@ -192,8 +375,15 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 		}
 
 		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
index c2ca735b3..c7d8f90c5 100644
--- a/src/modules/mavlink/missionlib.h
+++ b/src/modules/mavlink/missionlib.h
@@ -39,7 +39,7 @@
 
 #pragma once
 
-#include <v1.0/common/mavlink.h>
+#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);
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index cbf08aeb2..5d3d6a73c 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -39,8 +39,7 @@ MODULE_COMMAND	 = mavlink
 SRCS		 += mavlink.c \
 			   missionlib.c \
 			   mavlink_parameters.c \
-			   mavlink_log.c \
-			   mavlink_receiver.c \
+			   mavlink_receiver.cpp \
 			   orb_listener.c \
 			   waypoints.c
 
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index 295cd5e28..e1dabfd21 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -47,7 +47,6 @@
 #include <fcntl.h>
 #include <string.h>
 #include "mavlink_bridge_header.h"
-#include <v1.0/common/mavlink.h>
 #include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
@@ -55,6 +54,7 @@
 #include <sys/prctl.h>
 #include <stdlib.h>
 #include <poll.h>
+#include <lib/geo/geo.h>
 
 #include <mavlink/mavlink_log.h>
 
@@ -68,10 +68,14 @@ 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;
 
@@ -89,6 +93,8 @@ static unsigned int gps_counter;
  */
 static uint64_t last_sensor_timestamp;
 
+static hrt_abstime last_sent_vfr = 0;
+
 static void	*uorb_receive_thread(void *arg);
 
 struct listener {
@@ -97,6 +103,8 @@ struct listener {
 	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);
@@ -116,6 +124,8 @@ 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},
@@ -140,10 +150,25 @@ static const struct listener listeners[] = {
 	{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)
 {
@@ -192,7 +217,7 @@ l_sensor_combined(const struct listener *l)
 					     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, 0 /* no diff pressure yet */,
+					     raw.baro_pres_mbar, raw.differential_pressure_pa,
 					     raw.baro_alt_meter, raw.baro_temp_celcius,
 					     fields_updated);
 
@@ -202,13 +227,10 @@ l_sensor_combined(const struct listener *l)
 void
 l_vehicle_attitude(const struct listener *l)
 {
-	struct vehicle_attitude_s att;
-
-
 	/* copy attitude data into local buffer */
 	orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att);
 
-	if (gcs_link)
+	if (gcs_link) {
 		/* send sensor values */
 		mavlink_msg_attitude_send(MAVLINK_COMM_0,
 					  last_sensor_timestamp / 1000,
@@ -218,6 +240,30 @@ l_vehicle_attitude(const struct listener *l)
 					  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++;
 }
@@ -231,11 +277,7 @@ l_vehicle_gps_position(const struct listener *l)
 	orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps);
 
 	/* GPS COG is 0..2PI in degrees * 1e2 */
-	float cog_deg = gps.cog_rad;
-	if (cog_deg > M_PI_F)
-		cog_deg -= 2.0f * M_PI_F;
-	cog_deg *= M_RAD_TO_DEG_F;
-
+	float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F;
 
 	/* GPS position */
 	mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0,
@@ -244,10 +286,10 @@ l_vehicle_gps_position(const struct listener *l)
 				     gps.lat,
 				     gps.lon,
 				     gps.alt,
-				     gps.eph_m * 1e2f, // from m to cm
-				     gps.epv_m * 1e2f, // from m to cm
+				     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 rad to deg * 100
+				     cog_deg * 1e2f, // from deg to deg * 100
 				     gps.satellites_visible);
 
 	/* update SAT info every 10 seconds */
@@ -272,19 +314,23 @@ l_vehicle_status(const struct listener *l)
 	orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
 
 	/* enable or disable HIL */
-	set_hil_on_off(v_status.flag_hil_enabled);
+	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_mode = 0;
-	get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
+	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_mode,
-				   v_status.state_machine,
+				   mavlink_base_mode,
+				   mavlink_custom_mode,
 				   mavlink_state);
 }
 
@@ -302,20 +348,26 @@ 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)
-		/* Channels are sent in MAVLink main loop at a fixed interval */
-		mavlink_msg_rc_channels_raw_send(chan,
-						 rc_raw.timestamp / 1000,
-						 0,
-						 (rc_raw.channel_count > 0) ? rc_raw.values[0] : UINT16_MAX,
-						 (rc_raw.channel_count > 1) ? rc_raw.values[1] : UINT16_MAX,
-						 (rc_raw.channel_count > 2) ? rc_raw.values[2] : UINT16_MAX,
-						 (rc_raw.channel_count > 3) ? rc_raw.values[3] : UINT16_MAX,
-						 (rc_raw.channel_count > 4) ? rc_raw.values[4] : UINT16_MAX,
-						 (rc_raw.channel_count > 5) ? rc_raw.values[5] : UINT16_MAX,
-						 (rc_raw.channel_count > 6) ? rc_raw.values[6] : UINT16_MAX,
-						 (rc_raw.channel_count > 7) ? rc_raw.values[7] : UINT16_MAX,
-						 255);
+	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
@@ -324,28 +376,16 @@ 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);
 
-	uint64_t timestamp = global_pos.timestamp;
-	int32_t lat = global_pos.lat;
-	int32_t lon = global_pos.lon;
-	int32_t alt = (int32_t)(global_pos.alt * 1000);
-	int32_t relative_alt = (int32_t)(global_pos.relative_alt * 1000.0f);
-	int16_t vx = (int16_t)(global_pos.vx * 100.0f);
-	int16_t vy = (int16_t)(global_pos.vy * 100.0f);
-	int16_t vz = (int16_t)(global_pos.vz * 100.0f);
-
-	/* heading in degrees * 10, from 0 to 36.000) */
-	uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f);
-
 	mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
-					     timestamp / 1000,
-					     lat,
-					     lon,
-					     alt,
-					     relative_alt,
-					     vx,
-					     vy,
-					     vz,
-					     hdg);
+						 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
@@ -383,8 +423,8 @@ l_global_position_setpoint(const struct listener *l)
 				coordinate_frame,
 				global_sp.lat,
 				global_sp.lon,
-				global_sp.altitude,
-				global_sp.yaw);
+				global_sp.altitude * 1000.0f,
+				global_sp.yaw * M_RAD_TO_DEG_F * 100.0f);
 }
 
 void
@@ -455,7 +495,8 @@ l_actuator_outputs(const struct listener *l)
 
 	if (gcs_link) {
 		mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000,
-						  l->arg /* port number */,
+						  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],
@@ -465,13 +506,14 @@ l_actuator_outputs(const struct listener *l)
 						  act_outputs.output[6],
 						  act_outputs.output[7]);
 
-		/* only send in HIL mode */
-		if (mavlink_hil_enabled && armed.armed) {
+		/* 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_mode = 0;
-			get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
+			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 */
 
@@ -488,7 +530,7 @@ l_actuator_outputs(const struct listener *l)
 							      -1,
 							      -1,
 							      -1,
-							      mavlink_mode,
+							      mavlink_base_mode,
 							      0);
 
 			} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
@@ -502,7 +544,7 @@ l_actuator_outputs(const struct listener *l)
 							      ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
 							      -1,
 							      -1,
-							      mavlink_mode,
+							      mavlink_base_mode,
 							      0);
 
 			} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
@@ -516,7 +558,7 @@ l_actuator_outputs(const struct listener *l)
 							      ((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_mode,
+							      mavlink_base_mode,
 							      0);
 
 			} else {
@@ -530,7 +572,7 @@ l_actuator_outputs(const struct listener *l)
 							      (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_mode,
+							      mavlink_base_mode,
 							      0);
 			}
 		}
@@ -564,28 +606,26 @@ l_manual_control_setpoint(const struct listener *l)
 void
 l_vehicle_attitude_controls(const struct listener *l)
 {
-	struct actuator_controls_effective_s actuators;
-
-	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators);
+	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,
-						   "eff ctrl0    ",
-						   actuators.control_effective[0]);
+						   "ctrl0    ",
+						   actuators_0.control[0]);
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
-						   "eff ctrl1    ",
-						   actuators.control_effective[1]);
+						   "ctrl1    ",
+						   actuators_0.control[1]);
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
-						   "eff ctrl2     ",
-						   actuators.control_effective[2]);
+						   "ctrl2     ",
+						   actuators_0.control[2]);
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
-						   "eff ctrl3     ",
-						   actuators.control_effective[3]);
+						   "ctrl3     ",
+						   actuators_0.control[3]);
 	}
 }
 
@@ -626,11 +666,30 @@ l_home(const struct listener *l)
 	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());
+	prctl(PR_SET_NAME, "mavlink_orb_rcv", getpid());
 
 	/*
 	 * set up poll to block for new data,
@@ -659,7 +718,7 @@ uorb_receive_thread(void *arg)
 
 		/* handle the poll result */
 		if (poll_ret == 0) {
-			mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s");
+			/* silent */
 
 		} else if (poll_ret < 0) {
 			mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data");
@@ -754,7 +813,7 @@ uorb_receive_start(void)
 	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_EFFECTIVE);
+	mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
 	orb_set_interval(mavlink_subs.actuators_sub, 100);	/* 10Hz updates */
 
 	/* --- DEBUG VALUE OUTPUT --- */
@@ -765,6 +824,15 @@ uorb_receive_start(void)
 	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);
@@ -774,5 +842,7 @@ uorb_receive_start(void)
 
 	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/orb_topics.h b/src/modules/mavlink/orb_topics.h
index d61cd43dc..91773843b 100644
--- a/src/modules/mavlink/orb_topics.h
+++ b/src/modules/mavlink/orb_topics.h
@@ -52,15 +52,22 @@
 #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_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>
 #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 <drivers/drv_rc_input.h>
+#include <uORB/topics/navigation_capabilities.h>
 
 struct mavlink_subscriptions {
 	int sensor_sub;
@@ -72,8 +79,10 @@ struct mavlink_subscriptions {
 	int act_3_sub;
 	int gps_sub;
 	int man_control_sp_sub;
-	int armed_sub;
+	int safety_sub;
 	int actuators_sub;
+	int armed_sub;
+	int actuators_effective_sub;
 	int local_pos_sub;
 	int spa_sub;
 	int spl_sub;
@@ -83,6 +92,8 @@ struct mavlink_subscriptions {
 	int optical_flow;
 	int rates_setpoint_sub;
 	int home_sub;
+	int airspeed_sub;
+	int navigation_capabilities_sub;
 };
 
 extern struct mavlink_subscriptions mavlink_subs;
@@ -93,6 +104,9 @@ extern struct vehicle_global_position_s global_pos;
 /** Local position */
 extern struct vehicle_local_position_s local_pos;
 
+/** navigation capabilities */
+extern struct navigation_capabilities_s nav_cap;
+
 /** Vehicle status */
 extern struct vehicle_status_s v_status;
 
diff --git a/src/modules/mavlink/util.h b/src/modules/mavlink/util.h
index a4ff06a88..5e5ee8261 100644
--- a/src/modules/mavlink/util.h
+++ b/src/modules/mavlink/util.h
@@ -51,4 +51,4 @@ extern mavlink_wpm_storage *wpm;
 /**
  * Translate the custom state into standard mavlink modes and state.
  */
-extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode);
+extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index a131b143b..7e4a2688f 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -45,6 +45,7 @@
 #include <unistd.h>
 #include <stdio.h>
 
+#include "mavlink_bridge_header.h"
 #include "missionlib.h"
 #include "waypoints.h"
 #include "util.h"
@@ -245,64 +246,19 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq)
 	// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
 }
 
-//float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z)
-//{
-//    if (seq < wpm->size)
-//    {
-//        mavlink_mission_item_t *cur = waypoints->at(seq);
-//
-//        const PxVector3 A(cur->x, cur->y, cur->z);
-//        const PxVector3 C(x, y, z);
-//
-//        // seq not the second last waypoint
-//        if ((uint16_t)(seq+1) < wpm->size)
-//        {
-//            mavlink_mission_item_t *next = waypoints->at(seq+1);
-//            const PxVector3 B(next->x, next->y, next->z);
-//            const float r = (B-A).dot(C-A) / (B-A).lengthSquared();
-//            if (r >= 0 && r <= 1)
-//            {
-//                const PxVector3 P(A + r*(B-A));
-//                return (P-C).length();
-//            }
-//            else if (r < 0.f)
-//            {
-//                return (C-A).length();
-//            }
-//            else
-//            {
-//                return (C-B).length();
-//            }
-//        }
-//        else
-//        {
-//            return (C-A).length();
-//        }
-//    }
-//    else
-//    {
-//        // if (verbose) // printf("ERROR: index out of bounds\n");
-//    }
-//    return -1.f;
-//}
-
 /*
  * 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 mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt, float *dist_xy, float *dist_z)
 {
-	//TODO: implement for z once altidude contoller is implemented
 
-	static uint16_t counter;
-
-//	if(counter % 10 == 0) printf(" x = %.10f, y = %.10f\n", x, y);
 	if (seq < wpm->size) {
-		mavlink_mission_item_t *cur = &(wpm->waypoints[seq]);
+		mavlink_mission_item_t *wp = &(wpm->waypoints[seq]);
 
-		double current_x_rad = cur->x / 180.0 * M_PI;
-		double current_y_rad = cur->y / 180.0 * M_PI;
+		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;
 
@@ -314,20 +270,24 @@ float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float
 
 		const double radius_earth = 6371000.0;
 
-		return radius_earth * c;
+		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;
 	}
 
-	counter++;
-
 }
 
 /*
  * Calculate distance in local frame (NED)
  */
-float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z)
+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]);
@@ -336,88 +296,182 @@ float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float
 		float dy = (cur->y - y);
 		float dz = (cur->z - z);
 
-		return sqrt(dx * dx + dy * dy + dz * dz);
+		*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)
+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;
 
-	// Do not flood the precious wireless link with debug data
-	// if (wpm->size > 0 && counter % 10 == 0) {
-	// 	printf("Currect active waypoint id: %i\n", wpm->current_active_wp_id);
-	// }
-
+	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 = wpm->waypoints[wpm->current_active_wp_id].param2;
+		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 = 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, global_pos->lat, global_pos->lon, global_pos->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 = 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 <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw
+		if (dist >= 0.f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) {
 			wpm->pos_reached = true;
-
-			if (counter % 100 == 0)
-				printf("Setpoint reached: %0.4f, orbit: %.4f\n", dist, orbit);
 		}
 
-//		else
-//		{
-//			if(counter % 100 == 0)
-//				printf("Setpoint not reached yet: %0.4f, orbit: %.4f, coordinate frame: %d\n",dist, orbit, coordinate_frame);
-//		}
+		// 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->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
-				printf("Reached waypoint %u for the first time \n", cur_wp->seq);
 				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))
-			if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param2 * 1000) {
-				printf("Reached waypoint %u long enough \n", cur_wp->seq);
+
+			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;
 
-					if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) {
-						/* the last waypoint was reached, if auto continue is
-						 * activated restart the waypoint list from the beginning
-						 */
-						wpm->current_active_wp_id = 0;
+					cur_wp->current = 0;
 
-					} else {
-						if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size)
-							wpm->current_active_wp_id++;
+					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);
@@ -438,7 +492,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 }
 
 
-int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position)
+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) {
@@ -461,12 +515,7 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio
 		}
 	}
 
-	// Do NOT continously send the current WP, since we're not loosing messages
-	// if (now - wpm->timestamp_last_send_setpoint > wpm->delay_setpoint && wpm->current_active_wp_id < wpm->size) {
-	// 	mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
-	// }
-
-	check_waypoints_reached(now, global_position , local_position);
+	check_waypoints_reached(now, global_position, local_position, nav_cap->turn_distance);
 
 	return OK;
 }
@@ -477,115 +526,6 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 	uint64_t now = mavlink_missionlib_get_system_timestamp();
 
 	switch (msg->msgid) {
-//		case MAVLINK_MSG_ID_ATTITUDE:
-//        {
-//            if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size)
-//            {
-//                mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]);
-//                if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED)
-//                {
-//                    mavlink_attitude_t att;
-//                    mavlink_msg_attitude_decode(msg, &att);
-//                    float yaw_tolerance = wpm->accept_range_yaw;
-//                    //compare current yaw
-//                    if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*FM_PI)
-//                    {
-//                        if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4)
-//                            wpm->yaw_reached = true;
-//                    }
-//                    else if(att.yaw - yaw_tolerance < 0.0f)
-//                    {
-//                        float lowerBound = 360.0f + att.yaw - yaw_tolerance;
-//                        if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance)
-//                            wpm->yaw_reached = true;
-//                    }
-//                    else
-//                    {
-//                        float upperBound = att.yaw + yaw_tolerance - 2.f*FM_PI;
-//                        if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound)
-//                            wpm->yaw_reached = true;
-//                    }
-//                }
-//            }
-//            break;
-//        }
-//
-//		case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
-//        {
-//            if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size)
-//            {
-//                mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]);
-//
-//                if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED)
-//                {
-//                    mavlink_local_position_ned_t pos;
-//                    mavlink_msg_local_position_ned_decode(msg, &pos);
-//                    //// if (debug) // printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z);
-//
-//                    wpm->pos_reached = false;
-//
-//                    // compare current position (given in message) with current waypoint
-//                    float orbit = wp->param1;
-//
-//                    float dist;
-////                    if (wp->param2 == 0)
-////                    {
-////						// FIXME segment distance
-////                        //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z);
-////                    }
-////                    else
-////                    {
-//                        dist = mavlink_wpm_distance_to_point(wpm->current_active_wp_id, pos.x, pos.y, pos.z);
-////                    }
-//
-//                    if (dist >= 0.f && dist <= orbit && wpm->yaw_reached)
-//                    {
-//                        wpm->pos_reached = true;
-//                    }
-//                }
-//            }
-//            break;
-//        }
-
-//		case MAVLINK_MSG_ID_CMD: // special action from ground station
-//        {
-//            mavlink_cmd_t action;
-//            mavlink_msg_cmd_decode(msg, &action);
-//            if(action.target == mavlink_system.sysid)
-//            {
-//                // if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl;
-//                switch (action.action)
-//                {
-//						//				case MAV_ACTION_LAUNCH:
-//						//					// if (verbose) std::cerr << "Launch received" << std::endl;
-//						//					current_active_wp_id = 0;
-//						//					if (wpm->size>0)
-//						//					{
-//						//						setActive(waypoints[current_active_wp_id]);
-//						//					}
-//						//					else
-//						//						// if (verbose) std::cerr << "No launch, waypointList empty" << std::endl;
-//						//					break;
-//
-//						//				case MAV_ACTION_CONTINUE:
-//						//					// if (verbose) std::c
-//						//					err << "Continue received" << std::endl;
-//						//					idle = false;
-//						//					setActive(waypoints[current_active_wp_id]);
-//						//					break;
-//
-//						//				case MAV_ACTION_HALT:
-//						//					// if (verbose) std::cerr << "Halt received" << std::endl;
-//						//					idle = true;
-//						//					break;
-//
-//						//				default:
-//						//					// if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl;
-//						//					break;
-//                }
-//            }
-//            break;
-//        }
 
 	case MAVLINK_MSG_ID_MISSION_ACK: {
 			mavlink_mission_ack_t wpa;
@@ -596,26 +536,16 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 
 				if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) {
 					if (wpm->current_wp_id == wpm->size - 1) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE");
-#else
 
-						if (MAVLINK_WPM_VERBOSE) printf("Received ACK after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n");
+						mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE");
 
-#endif
 						wpm->current_state = MAVLINK_WPM_STATE_IDLE;
 						wpm->current_wp_id = 0;
 					}
 				}
 
 			} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
 				mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
-#else
-
-				if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
-
-#endif
 			}
 
 			break;
@@ -643,13 +573,8 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 							}
 						}
 
-#ifdef MAVLINK_WPM_NO_PRINTF
 						mavlink_missionlib_send_gcs_string("NEW WP SET");
-#else
 
-						if (MAVLINK_WPM_VERBOSE) printf("New current waypoint %u\n", wpm->current_active_wp_id);
-
-#endif
 						wpm->yaw_reached = false;
 						wpm->pos_reached = false;
 						mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id);
@@ -657,33 +582,16 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 						wpm->timestamp_firstinside_orbit = 0;
 
 					} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
 						mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT: Index out of bounds\n");
-
-#endif
 					}
 
 				} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
 					mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
-#else
-
-					if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\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;
@@ -1130,5 +1038,5 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 		}
 	}
 
-	check_waypoints_reached(now, global_pos, local_pos);
+	// check_waypoints_reached(now, global_pos, local_pos);
 }
diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h
index c32ab32e5..d7d6b31dc 100644
--- a/src/modules/mavlink/waypoints.h
+++ b/src/modules/mavlink/waypoints.h
@@ -47,14 +47,15 @@
 
 #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 <v1.0/common/mavlink.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 {
@@ -120,7 +121,7 @@ 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 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);
 
diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c
index cb6d6b16a..0edb53a3e 100644
--- a/src/modules/mavlink_onboard/mavlink.c
+++ b/src/modules/mavlink_onboard/mavlink.c
@@ -49,7 +49,6 @@
 #include <mqueue.h>
 #include <string.h>
 #include "mavlink_bridge_header.h"
-#include <v1.0/common/mavlink.h>
 #include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
@@ -168,12 +167,12 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf
 	case 921600: speed = B921600; break;
 
 	default:
-		fprintf(stderr, "[mavlink] ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud);
+		warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600", baud);
 		return -EINVAL;
 	}
 
 	/* open uart */
-	printf("[mavlink] UART is %s, baudrate is %d\n", uart_name, baud);
+	warnx("UART is %s, baudrate is %d", uart_name, baud);
 	uart = open(uart_name, O_RDWR | O_NOCTTY);
 
 	/* Try to set baud rate */
@@ -184,7 +183,7 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf
 	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) {
-			fprintf(stderr, "[mavlink] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
+			warnx("ERROR getting baudrate / termios config for %s: %d", uart_name, termios_state);
 			close(uart);
 			return -1;
 		}
@@ -197,14 +196,14 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf
 
 		/* Set baud rate */
 		if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-			fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
+			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) {
-			fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
+			warnx("ERROR setting baudrate / termios config for %s (tcsetattr)", uart_name);
 			close(uart);
 			return -1;
 		}
@@ -274,18 +273,18 @@ void mavlink_update_system(void)
 }
 
 void
-get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed,
+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 (v_status->flag_control_manual_enabled) {
+	if (control_mode->flag_control_manual_enabled) {
 		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
 	}
 
-	if (v_status->flag_hil_enabled) {
+	if (control_mode->flag_system_hil_enabled) {
 		*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
 	}
 
@@ -296,61 +295,67 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct
 		*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
 	}
 
-	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;
+	if (control_mode->flag_control_velocity_enabled) {
 		*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;
+	} 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;
+//	}
+
 }
 
 /**
@@ -362,7 +367,9 @@ int mavlink_thread_main(int argc, char *argv[])
 	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 */
@@ -431,19 +438,19 @@ int mavlink_thread_main(int argc, char *argv[])
 			/* 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(&v_status, &armed, &mavlink_state, &mavlink_mode);
+			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.state_machine, mavlink_state);
+			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,
-						    v_status.voltage_battery * 1000.0f,
-						    v_status.current_battery * 1000.0f,
+						    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,
@@ -474,9 +481,9 @@ int mavlink_thread_main(int argc, char *argv[])
 static void
 usage()
 {
-	fprintf(stderr, "usage: mavlink start [-d <devicename>] [-b <baud rate>]\n"
-			"       mavlink stop\n"
-			"       mavlink status\n");
+	fprintf(stderr, "usage: mavlink_onboard start [-d <devicename>] [-b <baud rate>]\n"
+			"       mavlink_onboard stop\n"
+			"       mavlink_onboard status\n");
 	exit(1);
 }
 
@@ -492,7 +499,7 @@ int mavlink_onboard_main(int argc, char *argv[])
 
 		/* this is not an error */
 		if (thread_running)
-			errx(0, "mavlink already running\n");
+			errx(0, "already running");
 
 		thread_should_exit = false;
 		mavlink_task = task_spawn_cmd("mavlink_onboard",
@@ -509,7 +516,7 @@ int mavlink_onboard_main(int argc, char *argv[])
 		while (thread_running) {
 			usleep(200000);
 		}
-		warnx("terminated.");
+		warnx("terminated");
 		exit(0);
 	}
 
diff --git a/src/modules/mavlink_onboard/mavlink_bridge_header.h b/src/modules/mavlink_onboard/mavlink_bridge_header.h
index 3ad3bb617..b72bbb2b1 100644
--- a/src/modules/mavlink_onboard/mavlink_bridge_header.h
+++ b/src/modules/mavlink_onboard/mavlink_bridge_header.h
@@ -78,4 +78,6 @@ extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int len
 mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
 mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan);
 
+#include <v1.0/common/mavlink.h>
+
 #endif /* MAVLINK_BRIDGE_HEADER_H */
diff --git a/src/modules/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c
index 0acbea675..4658bcc1d 100644
--- a/src/modules/mavlink_onboard/mavlink_receiver.c
+++ b/src/modules/mavlink_onboard/mavlink_receiver.c
@@ -50,7 +50,6 @@
 #include <mqueue.h>
 #include <string.h>
 #include "mavlink_bridge_header.h"
-#include <v1.0/common/mavlink.h>
 #include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
@@ -101,11 +100,11 @@ handle_message(mavlink_message_t *msg)
 		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))) {
+				|| (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");
+				warnx("terminating...");
 				fflush(stdout);
 				usleep(50000);
 
@@ -133,6 +132,7 @@ handle_message(mavlink_message_t *msg)
 				if (cmd_pub <= 0) {
 					cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
 				}
+
 				/* publish */
 				orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
 			}
@@ -157,6 +157,7 @@ handle_message(mavlink_message_t *msg)
 		/* 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);
@@ -187,6 +188,7 @@ handle_message(mavlink_message_t *msg)
 		/* 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);
@@ -204,6 +206,7 @@ handle_message(mavlink_message_t *msg)
 
 		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);
 		}
@@ -220,7 +223,7 @@ handle_message(mavlink_message_t *msg)
 			/* switch to a receiving link mode */
 			gcs_link = false;
 
-			/* 
+			/*
 			 * rate control mode - defined by MAVLink
 			 */
 
@@ -228,33 +231,37 @@ handle_message(mavlink_message_t *msg)
 			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;
+			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;
+			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) {
+			if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) {
 				ml_armed = false;
 			}
 
@@ -266,6 +273,7 @@ handle_message(mavlink_message_t *msg)
 			/* 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);
@@ -282,31 +290,36 @@ handle_message(mavlink_message_t *msg)
 static void *
 receive_thread(void *arg)
 {
-	int uart_fd = *((int*)arg);
+	int uart_fd = *((int *)arg);
 
 	const int timeout = 1000;
-	uint8_t ch;
+	uint8_t buf[32];
 
 	mavlink_message_t msg;
 
-	prctl(PR_SET_NAME, "mavlink offb rcv", getpid());
+	prctl(PR_SET_NAME, "mavlink_onboard_rcv", getpid());
 
-	while (!thread_should_exit) {
+	struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } };
 
-		struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } };
+	ssize_t nread = 0;
 
+	while (!thread_should_exit) {
 		if (poll(fds, 1, timeout) > 0) {
-			/* non-blocking read until buffer is empty */
-			int nread = 0;
+			if (nread < sizeof(buf)) {
+				/* to avoid reading very small chunks wait for data before reading */
+				usleep(1000);
+			}
 
-			do {
-				nread = read(uart_fd, &ch, 1);
+			/* non-blocking read. read may return negative values */
+			nread = read(uart_fd, buf, sizeof(buf));
 
-				if (mavlink_parse_char(chan, ch, &msg, &status)) { //parse the char
+			/* 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);
 				}
-			} while (nread > 0);
+			}
 		}
 	}
 
@@ -320,12 +333,12 @@ receive_start(int uart)
 	pthread_attr_init(&receiveloop_attr);
 
 	struct sched_param param;
-  	param.sched_priority = SCHED_PRIORITY_MAX - 40;
-  	(void)pthread_attr_setschedparam(&receiveloop_attr, &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;
-}
\ No newline at end of file
+}
diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h
index f18f56243..1b49c9ce4 100644
--- a/src/modules/mavlink_onboard/orb_topics.h
+++ b/src/modules/mavlink_onboard/orb_topics.h
@@ -52,9 +52,11 @@
 #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>
@@ -69,7 +71,7 @@ struct mavlink_subscriptions {
 	int act_3_sub;
 	int gps_sub;
 	int man_control_sp_sub;
-	int armed_sub;
+	int safety_sub;
 	int actuators_sub;
 	int local_pos_sub;
 	int spa_sub;
diff --git a/src/modules/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h
index 38a4db372..c84b6fd26 100644
--- a/src/modules/mavlink_onboard/util.h
+++ b/src/modules/mavlink_onboard/util.h
@@ -50,5 +50,6 @@ extern volatile bool thread_should_exit;
 /**
  * Translate the custom state into standard mavlink modes and state.
  */
-extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed,
+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);
diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c
index 99f25cfe9..60a211817 100644
--- a/src/modules/multirotor_att_control/multirotor_att_control_main.c
+++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c
@@ -2,6 +2,7 @@
  *
  *   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
@@ -38,6 +39,7 @@
  * 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>
@@ -57,12 +59,13 @@
 #include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <drivers/drv_gyro.h>
-#include <uORB/topics/vehicle_status.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>
@@ -74,333 +77,311 @@
 #include "multirotor_attitude_control.h"
 #include "multirotor_rate_control.h"
 
-PARAM_DEFINE_FLOAT(MC_RCLOSS_THR, 0.0f); // This defines the throttle when the RC signal is lost.
-
 __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 orb_advert_t actuator_pub;
-
-static struct vehicle_status_s state;
+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 */
-	memset(&state, 0, sizeof(state));
 	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 sensor_combined_s raw;
-	memset(&raw, 0, sizeof(raw));
 	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 to attitude, motor setpoints and system state */
-	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int param_sub = orb_subscribe(ORB_ID(parameter_update));
-	int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
-	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
-	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-
-	/*
-	 * Do not rate-limit the loop to prevent aliasing
-	 * if rate-limiting would be desired later, the line below would
-	 * enable it.
-	 *
-	 * rate-limit the attitude subscription to 200Hz to pace our loop
-	 * orb_set_interval(att_sub, 5);
-	 */
-	struct pollfd fds[2] = {
-		{ .fd = att_sub, .events = POLLIN },
-		{ .fd = param_sub, .events = POLLIN }
-	};
+	/* 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;
 	}
 
-	actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
+	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);
-	int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
 
 	/* 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");
 
-	/* welcome user */
-	printf("[multirotor_att_control] starting\n");
+	warnx("starting");
 
 	/* store last control mode to detect mode switches */
-	bool flag_control_manual_enabled = false;
-	bool flag_control_attitude_enabled = false;
-	bool flag_system_armed = false;
-
-	/* store if yaw position or yaw speed has been changed */
 	bool control_yaw_position = true;
+	bool reset_yaw_sp = true;
 
-	/* store if we stopped a yaw movement */
-	bool first_time_after_yaw_speed_control = true;
-
-	/* prepare the handle for the failsafe throttle */
-	param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR");
-	float failsafe_throttle = 0.0f;
-
+	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, 2, 500);
+		int ret = poll(fds, 1, 500);
 
 		if (ret < 0) {
 			/* poll error, count it in perf */
 			perf_count(mc_err_perf);
 
-		} else if (ret == 0) {
-			/* no return value, ignore */
-		} else {
+		} else if (ret > 0) {
+			/* only run controller if attitude changed */
+			perf_begin(mc_loop_perf);
 
-			/* only update parameters if they changed */
-			if (fds[1].revents & POLLIN) {
-				/* read from param to clear updated flag */
-				struct parameter_update_s update;
-				orb_copy(ORB_ID(parameter_update), param_sub, &update);
+			/* 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 */
-				// XXX no params here yet
 			}
 
-			/* only run controller if attitude changed */
-			if (fds[0].revents & POLLIN) {
+			/* control mode */
+			orb_check(vehicle_control_mode_sub, &updated);
 
-				perf_begin(mc_loop_perf);
+			if (updated) {
+				orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub, &control_mode);
+			}
 
-				/* get a local copy of system state */
-				bool updated;
-				orb_check(state_sub, &updated);
+			/* manual control setpoint */
+			orb_check(manual_control_setpoint_sub, &updated);
 
-				if (updated) {
-					orb_copy(ORB_ID(vehicle_status), state_sub, &state);
-				}
+			if (updated) {
+				orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual);
+			}
 
-				/* get a local copy of manual setpoint */
-				orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
-				/* get a local copy of attitude */
-				orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
-				/* get a local copy of attitude setpoint */
-				orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
-				/* get a local copy of rates setpoint */
-				orb_check(setpoint_sub, &updated);
-
-				if (updated) {
-					orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp);
-				}
+			/* attitude setpoint */
+			orb_check(vehicle_attitude_setpoint_sub, &updated);
 
-				/* get a local copy of the current sensor values */
-				orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
+			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);
 
-				/** STEP 1: Define which input is the dominating control input */
-				if (state.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;
-//						printf("thrust_rate=%8.4f\n",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;
-//						printf("thrust_att=%8.4f\n",offboard_sp.p4);
-						att_sp.timestamp = hrt_absolute_time();
-						/* STEP 2: publish the result to the vehicle actuators */
-						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-					}
+			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);
 
-				} else if (state.flag_control_manual_enabled) {
+			if (updated) {
+				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
+			}
+
+			/* set flag to safe value */
+			control_yaw_position = true;
 
-					if (state.flag_control_attitude_enabled) {
+			/* 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);
 
-						/* initialize to current yaw if switching to manual or att control */
-						if (state.flag_control_attitude_enabled != flag_control_attitude_enabled ||
-						    state.flag_control_manual_enabled != flag_control_manual_enabled ||
-						    state.flag_system_armed != flag_system_armed) {
-							att_sp.yaw_body = att.yaw;
+				} 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;
 						}
 
-						static bool rc_loss_first_time = true;
-
-						/* if the RC signal is lost, try to stay level and go slowly back down to ground */
-						if (state.rc_signal_lost) {
-							/* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */
-							param_get(failsafe_throttle_handle, &failsafe_throttle);
-							att_sp.roll_body = 0.0f;
-							att_sp.pitch_body = 0.0f;
-
-							/*
-							 * Only go to failsafe throttle if last known throttle was
-							 * high enough to create some lift to make hovering state likely.
-							 *
-							 * This is to prevent that someone landing, but not disarming his
-							 * multicopter (throttle = 0) does not make it jump up in the air
-							 * if shutting down his remote.
-							 */
-							if (isfinite(manual.throttle) && manual.throttle > 0.2f) {
-								att_sp.thrust = failsafe_throttle;
-
-							} else {
-								att_sp.thrust = 0.0f;
-							}
-
-							/* keep current yaw, do not attempt to go to north orientation,
-							 * since if the pilot regains RC control, he will be lost regarding
-							 * the current orientation.
-							 */
-							if (rc_loss_first_time)
-								att_sp.yaw_body = att.yaw;
-
-							rc_loss_first_time = false;
+					} 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 {
-							rc_loss_first_time = true;
-
-							att_sp.roll_body = manual.roll;
-							att_sp.pitch_body = manual.pitch;
-
-							/* set attitude if arming */
-							if (!flag_control_attitude_enabled && state.flag_system_armed) {
-								att_sp.yaw_body = att.yaw;
-							}
-
-							/* act if stabilization is active or if the (nonsense) direct pass through mode is set */
-							if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS ||
-							    state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
-
-								if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) {
-									rates_sp.yaw = manual.yaw;
-									control_yaw_position = false;
-
-								} else {
-									/*
-									 * This mode SHOULD be the default mode, which is:
-									 * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS
-									 *
-									 * However, we fall back to this setting for all other (nonsense)
-									 * settings as well.
-									 */
-
-									/* only move setpoint if manual input is != 0 */
-									if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
-										rates_sp.yaw = manual.yaw;
-										control_yaw_position = false;
-										first_time_after_yaw_speed_control = true;
-
-									} else {
-										if (first_time_after_yaw_speed_control) {
-											att_sp.yaw_body = att.yaw;
-											first_time_after_yaw_speed_control = false;
-										}
-
-										control_yaw_position = true;
-									}
-								}
-							}
+							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;
-							att_sp.timestamp = hrt_absolute_time();
 						}
+					}
 
-						/* STEP 2: publish the controller output */
-						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+					/* 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();
-							/* STEP 2: publish the result to the vehicle actuators */
-							orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-						}
+					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;
+					}
 
-					} else {
-						/* manual rate inputs, from RC control or joystick */
-						if (state.flag_control_rates_enabled &&
-						    state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) {
-							rates_sp.roll = manual.roll;
+					att_sp.timestamp = hrt_absolute_time();
 
-							rates_sp.pitch = manual.pitch;
-							rates_sp.yaw = manual.yaw;
-							rates_sp.thrust = manual.throttle;
-							rates_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;
 				}
 
-				/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
-				if (state.flag_control_attitude_enabled) {
-					multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position);
-
-					orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
+			} 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);
+					}
 				}
 
-				/* measure in what intervals the controller runs */
-				perf_count(mc_interval_perf);
+				/* 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);
+			}
 
-				float gyro[3];
+			/* 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_valid = false;
-				orb_check(rates_sp_sub, &rates_sp_valid);
+				bool rates_sp_updated = false;
+				orb_check(vehicle_rates_setpoint_sub, &rates_sp_updated);
 
-				if (rates_sp_valid) {
-					orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
+				if (rates_sp_updated) {
+					orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rates_setpoint_sub, &rates_sp);
 				}
 
 				/* apply controller */
-				gyro[0] = att.rollspeed;
-				gyro[1] = att.pitchspeed;
-				gyro[2] = att.yawspeed;
+				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;
+			}
 
-				multirotor_control_rates(&rates_sp, gyro, &actuators);
-				orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+			/* 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;
 
-				/* update state */
-				flag_control_attitude_enabled = state.flag_control_attitude_enabled;
-				flag_control_manual_enabled = state.flag_control_manual_enabled;
-				flag_system_armed = state.flag_system_armed;
+			actuators.timestamp = hrt_absolute_time();
+			orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
 
-				perf_end(mc_loop_perf);
-			} /* end of poll call for attitude updates */
-		} /* end of poll return value check */
+			perf_end(mc_loop_perf);
+		}
 	}
 
-	printf("[multirotor att control] stopping, disarming motors.\n");
+	warnx("stopping, disarming motors");
 
 	/* kill all outputs */
 	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
@@ -408,10 +389,9 @@ mc_thread_main(int argc, char *argv[])
 
 	orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
 
-
-	close(att_sub);
-	close(state_sub);
-	close(manual_sub);
+	close(vehicle_attitude_sub);
+	close(vehicle_control_mode_sub);
+	close(manual_control_setpoint_sub);
 	close(actuator_pub);
 	close(att_sp_pub);
 
@@ -467,11 +447,11 @@ int multirotor_att_control_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 		mc_task = task_spawn_cmd("multirotor_att_control",
-				     SCHED_DEFAULT,
-				     SCHED_PRIORITY_MAX - 15,
-				     2048,
-				     mc_thread_main,
-				     NULL);
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 15,
+					 2048,
+					 mc_thread_main,
+					 NULL);
 		exit(0);
 	}
 
diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c
index 76dbb36d3..8245aa560 100644
--- a/src/modules/multirotor_att_control/multirotor_attitude_control.c
+++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c
@@ -1,12 +1,12 @@
 /****************************************************************************
  *
  *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @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>
+ *           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
@@ -39,7 +39,15 @@
 
 /*
  * @file multirotor_attitude_control.c
- * Implementation of attitude controller
+ *
+ * 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"
@@ -54,15 +62,15 @@
 #include <systemlib/param/param.h>
 #include <drivers/drv_hrt.h>
 
-PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f);
+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, 0.2f);
+PARAM_DEFINE_FLOAT(MC_ATT_P, 6.8f);
 PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f);
-PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f);
+PARAM_DEFINE_FLOAT(MC_ATT_D, 0.0f);
 //PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f);
 //PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f);
 
@@ -158,21 +166,17 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
 }
 
 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)
+				 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;
-	float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f;
 	last_run = hrt_absolute_time();
 
 	if (last_input != att_sp->timestamp) {
 		last_input = att_sp->timestamp;
 	}
 
-	static int sensor_delay;
-	sensor_delay = hrt_absolute_time() - att->timestamp;
-
 	static int motor_skip_counter = 0;
 
 	static PID_t pitch_controller;
@@ -190,10 +194,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 		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);
-		pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f,
-			 1000.0f, PID_MODE_DERIVATIV_SET);
+		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;
 	}
@@ -208,13 +210,13 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 		pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
 	}
 
-	/* reset integral if on ground */
-	if (att_sp->thrust < 0.1f) {
+	/* 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 */
@@ -227,6 +229,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 
 	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);
@@ -244,6 +247,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 	}
 
 	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_attitude_control.h b/src/modules/multirotor_att_control/multirotor_attitude_control.h
index 2cf83e443..431a435f7 100644
--- a/src/modules/multirotor_att_control/multirotor_attitude_control.h
+++ b/src/modules/multirotor_att_control/multirotor_attitude_control.h
@@ -1,12 +1,12 @@
 /****************************************************************************
  *
  *   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 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>
+ *           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
@@ -39,7 +39,15 @@
 
 /*
  * @file multirotor_attitude_control.h
- * Attitude control for multi rotors.
+ *
+ * Definition 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>
  */
 
 #ifndef MULTIROTOR_ATTITUDE_CONTROL_H_
@@ -52,6 +60,6 @@
 #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);
+				 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_ */
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c
index deba1ac03..86ac0e4ff 100644
--- a/src/modules/multirotor_att_control/multirotor_rate_control.c
+++ b/src/modules/multirotor_att_control/multirotor_rate_control.c
@@ -1,8 +1,10 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   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
@@ -36,10 +38,12 @@
 /**
  * @file multirotor_rate_control.c
  *
- * Implementation of rate controller
+ * 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"
@@ -55,31 +59,23 @@
 #include <systemlib/err.h>
 #include <drivers/drv_hrt.h>
 
-PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */
-PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
-PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
-//PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f);
-//PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f);
+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.0f); /* 0.15 F405 Flamewheel */
-PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.0f);
+PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f);
+PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f);
 PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
-//PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f);
-//PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f);	/**< roughly < 500 deg/s limit */
 
 struct mc_rate_control_params {
 
 	float yawrate_p;
 	float yawrate_d;
 	float yawrate_i;
-	//float yawrate_awu;
-	//float yawrate_lim;
 
 	float attrate_p;
 	float attrate_d;
 	float attrate_i;
-	//float attrate_awu;
-	//float attrate_lim;
 
 	float rate_lim;
 };
@@ -89,14 +85,10 @@ struct mc_rate_control_param_handles {
 	param_t yawrate_p;
 	param_t yawrate_i;
 	param_t yawrate_d;
-	//param_t yawrate_awu;
-	//param_t yawrate_lim;
 
 	param_t attrate_p;
 	param_t attrate_i;
 	param_t attrate_d;
-	//param_t attrate_awu;
-	//param_t attrate_lim;
 };
 
 /**
@@ -118,14 +110,10 @@ static int parameters_init(struct mc_rate_control_param_handles *h)
 	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->yawrate_awu 	=	param_find("MC_YAWRATE_AWU");
-	//h->yawrate_lim 	=	param_find("MC_YAWRATE_LIM");
 
 	h->attrate_p 	= 	param_find("MC_ATTRATE_P");
 	h->attrate_i 	= 	param_find("MC_ATTRATE_I");
 	h->attrate_d 	= 	param_find("MC_ATTRATE_D");
-	//h->attrate_awu 	= 	param_find("MC_ATTRATE_AWU");
-	//h->attrate_lim 	= 	param_find("MC_ATTRATE_LIM");
 
 	return OK;
 }
@@ -135,29 +123,21 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
 	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->yawrate_awu, &(p->yawrate_awu));
-	//param_get(h->yawrate_lim, &(p->yawrate_lim));
 
 	param_get(h->attrate_p, &(p->attrate_p));
 	param_get(h->attrate_i, &(p->attrate_i));
 	param_get(h->attrate_d, &(p->attrate_d));
-	//param_get(h->attrate_awu, &(p->attrate_awu));
-	//param_get(h->attrate_lim, &(p->attrate_lim));
 
 	return OK;
 }
 
 void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
-			      const float rates[], struct actuator_controls_s *actuators)
+			      const float rates[], struct actuator_controls_s *actuators, bool reset_integral)
 {
-	static float roll_control_last  = 0;
-	static float pitch_control_last = 0;
 	static uint64_t last_run = 0;
 	const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
 	static uint64_t last_input = 0;
 
-	float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f;
-
 	if (last_input != rate_sp->timestamp) {
 		last_input = rate_sp->timestamp;
 	}
@@ -166,6 +146,10 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 
 	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;
 
@@ -176,54 +160,36 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 		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);
-		// warnx("rate ctrl: p.yawrate_p: %8.4f, loop: %d Hz, input: %d Hz",
-		// 	(double)p.yawrate_p, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
-	}
-
-	/* calculate current control outputs */
-
-	/* control pitch (forward) output */
-	float pitch_control = p.attrate_p * (rate_sp->pitch - rates[1]) - (p.attrate_d * pitch_control_last);
-
-	/* increase resilience to faulty control inputs */
-	if (isfinite(pitch_control)) {
-		pitch_control_last = pitch_control;
-
-	} else {
-		pitch_control = 0.0f;
-		warnx("rej. NaN ctrl pitch");
+		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);
 	}
 
-	/* control roll (left/right) output */
-	float roll_control = p.attrate_p * (rate_sp->roll - rates[0]) - (p.attrate_d * roll_control_last);
-
-	/* increase resilience to faulty control inputs */
-	if (isfinite(roll_control)) {
-		roll_control_last = roll_control;
-
-	} else {
-		roll_control = 0.0f;
-		warnx("rej. NaN ctrl roll");
+	/* 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);
 	}
 
-	/* control yaw rate */
-	float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]);
-
-	/* increase resilience to faulty control inputs */
-	if (!isfinite(yaw_rate_control)) {
-		yaw_rate_control = 0.0f;
-		warnx("rej. NaN ctrl yaw");
-	}
+	/* 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_rate_control;
+	actuators->control[2] = yaw_control;
 	actuators->control[3] = rate_sp->thrust;
 
 	motor_skip_counter++;
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h
index 03dec317a..ca7794c59 100644
--- a/src/modules/multirotor_att_control/multirotor_rate_control.h
+++ b/src/modules/multirotor_att_control/multirotor_rate_control.h
@@ -1,12 +1,12 @@
 /****************************************************************************
  *
- *   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 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>
+ *   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>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -39,7 +39,15 @@
 
 /*
  * @file multirotor_attitude_control.h
- * Attitude control for multi rotors.
+ *
+ * Definition of rate 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>
  */
 
 #ifndef MULTIROTOR_RATE_CONTROL_H_
@@ -51,6 +59,6 @@
 #include <uORB/topics/actuator_controls.h>
 
 void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
-			      const float rates[], struct actuator_controls_s *actuators);
+			      const float rates[], struct actuator_controls_s *actuators, bool reset_integral);
 
 #endif /* MULTIROTOR_RATE_CONTROL_H_ */
diff --git a/src/modules/multirotor_pos_control/module.mk b/src/modules/multirotor_pos_control/module.mk
index d04847745..bc4b48fb4 100644
--- a/src/modules/multirotor_pos_control/module.mk
+++ b/src/modules/multirotor_pos_control/module.mk
@@ -38,4 +38,5 @@
 MODULE_COMMAND	= multirotor_pos_control
 
 SRCS		= multirotor_pos_control.c \
-		  multirotor_pos_control_params.c
+		  multirotor_pos_control_params.c \
+		  thrust_pid.c
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index f39d11438..3d23d0c09 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -1,7 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   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
@@ -35,13 +35,14 @@
 /**
  * @file multirotor_pos_control.c
  *
- * Skeleton for multirotor position controller
+ * 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>
@@ -52,15 +53,22 @@
 #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_vicon_position.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 */
@@ -79,12 +87,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]);
  */
 static void usage(const char *reason);
 
-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: deamon {start|stop|status} [-p <additional params>]\n\n");
+
+	fprintf(stderr, "usage: multirotor_pos_control {start|stop|status}\n\n");
 	exit(1);
 }
 
@@ -92,9 +104,9 @@ usage(const char *reason)
  * 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().
+ * to task_spawn().
  */
 int multirotor_pos_control_main(int argc, char *argv[])
 {
@@ -104,32 +116,36 @@ int multirotor_pos_control_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			printf("multirotor pos control already running\n");
+			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);
+		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) {
-			printf("\tmultirotor pos control app is running\n");
+			warnx("app is running");
+
 		} else {
-			printf("\tmultirotor pos control app not started\n");
+			warnx("app not started");
 		}
+
 		exit(0);
 	}
 
@@ -137,98 +153,534 @@ int multirotor_pos_control_main(int argc, char *argv[])
 	exit(1);
 }
 
-static int
-multirotor_pos_control_thread_main(int argc, char *argv[])
+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 */
-	printf("[multirotor pos control] Control started, taking over position control\n");
+	warnx("started");
+	static int mavlink_fd;
+	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	mavlink_log_info(mavlink_fd, "[mpc] started");
 
 	/* structures */
-	struct vehicle_status_s state;
+	struct vehicle_control_mode_s control_mode;
+	memset(&control_mode, 0, sizeof(control_mode));
 	struct vehicle_attitude_s att;
-	//struct vehicle_global_position_setpoint_s global_pos_sp;
-	struct vehicle_local_position_setpoint_s local_pos_sp;
-	struct vehicle_vicon_position_s local_pos;
-	struct manual_control_setpoint_s manual;
+	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 state_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	int local_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
-	//int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_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 attitude 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;
 
-	int loopcounter = 0;
-
-	struct multirotor_position_control_params p;
-	struct multirotor_position_control_param_handles h;
-	parameters_init(&h);
-	parameters_update(&h, &p);
-
-
-	while (1) {
-		/* get a local copy of the vehicle state */
-		orb_copy(ORB_ID(vehicle_status), state_sub, &state);
-		/* get a local copy of manual setpoint */
-		orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
-		/* get a local copy of attitude */
-		orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
-		/* get a local copy of local position */
-		orb_copy(ORB_ID(vehicle_vicon_position), local_pos_sub, &local_pos);
-		/* get a local copy of local position setpoint */
-		orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
-
-		if (loopcounter == 500) {
-			parameters_update(&h, &p);
-			loopcounter = 0;
+	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;
 		}
 
-		// if (state.state_machine == SYSTEM_STATE_AUTO) {
-			
-			// XXX IMPLEMENT POSITION CONTROL HERE
-
-			float dT = 1.0f / 50.0f;
-
-			float x_setpoint = 0.0f;
-
-			// XXX enable switching between Vicon and local position estimate
-			/* local pos is the Vicon position */
-
-			// XXX just an example, lacks rotation around world-body transformation
-			att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p;
-			att_sp.roll_body = 0.0f;
-			att_sp.yaw_body = 0.0f;
-			att_sp.thrust = 0.3f;
-			att_sp.timestamp = hrt_absolute_time();
-
-			/* publish new attitude setpoint */
-			orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-		// } else if (state.state_machine == SYSTEM_STATE_STABILIZED) {
-			/* set setpoint to current position */
-			// XXX select pos reset channel on remote
-			/* reset setpoint to current position  (position hold) */
-			// if (1 == 2) {
-			// 	local_pos_sp.x = local_pos.x;
-			// 	local_pos_sp.y = local_pos.y;
-			// 	local_pos_sp.z = local_pos.z;
-			// 	local_pos_sp.yaw = att.yaw;
-			// }
-		// }
+		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);
-		loopcounter++;
-
 	}
 
-	printf("[multirotor pos control] ending now...\n");
+	warnx("stopped");
+	mavlink_log_info(mavlink_fd, "[mpc] stopped");
 
 	thread_running = false;
 
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
index 6b73dc405..b7041e4d5 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
@@ -1,8 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
+ *   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
@@ -34,29 +33,80 @@
  ****************************************************************************/
 
 /*
- * @file multirotor_position_control_params.c
- * 
- * Parameters for EKF filter
+ * @file multirotor_pos_control_params.c
+ *
+ * Parameters for multirotor_pos_control
  */
 
 #include "multirotor_pos_control_params.h"
 
-/* Extended Kalman Filter covariances */
-
 /* controller parameters */
-PARAM_DEFINE_FLOAT(MC_POS_P, 0.2f);
+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)
 {
-	/* PID parameters */
-	h->p 	=	param_find("MC_POS_P");
+	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->p, &(p->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/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
index 274f6c22a..fc658dadb 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
@@ -1,8 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
+ *   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
@@ -34,23 +33,59 @@
  ****************************************************************************/
 
 /*
- * @file multirotor_position_control_params.h
- * 
- * Parameters for position controller
+ * @file multirotor_pos_control_params.h
+ *
+ * Parameters for multirotor_pos_control
  */
 
 #include <systemlib/param/param.h>
 
 struct multirotor_position_control_params {
-	float p;
-	float i;
-	float d;
+	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 p;
-	param_t i;
-	param_t d;
+	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;
 };
 
 /**
diff --git a/src/modules/multirotor_pos_control/position_control.c b/src/modules/multirotor_pos_control/position_control.c
deleted file mode 100644
index 9c53a5bf6..000000000
--- a/src/modules/multirotor_pos_control/position_control.c
+++ /dev/null
@@ -1,235 +0,0 @@
-// /****************************************************************************
-//  *
-//  *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
-//  *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
-//  *           @author Laurens Mackay <mackayl@student.ethz.ch>
-//  *           @author Tobias Naegeli <naegelit@student.ethz.ch>
-//  *           @author Martin Rutschmann <rutmarti@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_position_control.c
-//  * Implementation of the position control for a multirotor VTOL
-//  */
-
-// #include <stdio.h>
-// #include <stdlib.h>
-// #include <stdio.h>
-// #include <stdint.h>
-// #include <math.h>
-// #include <stdbool.h>
-// #include <float.h>
-// #include <systemlib/pid/pid.h>
-
-// #include "multirotor_position_control.h"
-
-// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
-//  const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
-//  const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp)
-// {
-// 	static PID_t distance_controller;
-
-// 	static int read_ret;
-// 	static global_data_position_t position_estimated;
-
-// 	static uint16_t counter;
-
-// 	static bool initialized;
-// 	static uint16_t pm_counter;
-
-// 	static float lat_next;
-// 	static float lon_next;
-
-// 	static float pitch_current;
-
-// 	static float thrust_total;
-
-
-// 	if (initialized == false) {
-
-// 		pid_init(&distance_controller,
-// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
-// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
-// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
-// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU],
-// 			 PID_MODE_DERIVATIV_CALC, 150);//150
-
-// //		pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
-// //		pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
-
-// 		thrust_total = 0.0f;
-
-// 		/* Position initialization */
-// 		/* Wait for new position estimate */
-// 		do {
-// 			read_ret = read_lock_position(&position_estimated);
-// 		} while (read_ret != 0);
-
-// 		lat_next = position_estimated.lat;
-// 		lon_next = position_estimated.lon;
-
-// 		/* attitude initialization */
-// 		global_data_lock(&global_data_attitude->access_conf);
-// 		pitch_current = global_data_attitude->pitch;
-// 		global_data_unlock(&global_data_attitude->access_conf);
-
-// 		initialized = true;
-// 	}
-
-// 	/* load new parameters with 10Hz */
-// 	if (counter % 50 == 0) {
-// 		if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) {
-// 			/* check whether new parameters are available */
-// 			if (global_data_parameter_storage->counter > pm_counter) {
-// 				pid_set_parameters(&distance_controller,
-// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
-// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
-// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
-// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]);
-
-// //
-// //				pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
-// //				pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
-
-// 				pm_counter = global_data_parameter_storage->counter;
-// 				printf("Position controller changed pid parameters\n");
-// 			}
-// 		}
-
-// 		global_data_unlock(&global_data_parameter_storage->access_conf);
-// 	}
-
-
-// 	/* Wait for new position estimate */
-// 	do {
-// 		read_ret = read_lock_position(&position_estimated);
-// 	} while (read_ret != 0);
-
-// 	/* Get next waypoint */ //TODO: add local copy
-
-// 	if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) {
-// 		lat_next = global_data_position_setpoint->x;
-// 		lon_next = global_data_position_setpoint->y;
-// 		global_data_unlock(&global_data_position_setpoint->access_conf);
-// 	}
-
-// 	/* Get distance to waypoint */
-// 	float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next);
-// //	if(counter % 5 == 0)
-// //		printf("distance_to_waypoint: %.4f\n", distance_to_waypoint);
-
-// 	/* Get bearing to waypoint (direction on earth surface to next waypoint) */
-// 	float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next);
-
-// 	if (counter % 5 == 0)
-// 		printf("bearing: %.4f\n", bearing);
-
-// 	/* Calculate speed in direction of bearing (needed for controller) */
-// 	float speed_norm = sqrtf(position_estimated.vx  * position_estimated.vx + position_estimated.vy * position_estimated.vy);
-// //	if(counter % 5 == 0)
-// //		printf("speed_norm: %.4f\n", speed_norm);
-// 	float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller
-
-// 	/* Control Thrust in bearing direction  */
-// 	float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint,
-// 				  CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else
-
-// //	if(counter % 5 == 0)
-// //		printf("horizontal thrust: %.4f\n", horizontal_thrust);
-
-// 	/* Get total thrust (from remote for now) */
-// 	if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
-// 		thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; 												//TODO: how should we use the RC_CHANNELS_FUNCTION enum?
-// 		global_data_unlock(&global_data_rc_channels->access_conf);
-// 	}
-
-// 	const float max_gas = 500.0f;
-// 	thrust_total *= max_gas / 20000.0f; //TODO: check this
-// 	thrust_total += max_gas / 2.0f;
-
-
-// 	if (horizontal_thrust > thrust_total) {
-// 		horizontal_thrust = thrust_total;
-
-// 	} else if (horizontal_thrust < -thrust_total) {
-// 		horizontal_thrust = -thrust_total;
-// 	}
-
-
-
-// 	//TODO: maybe we want to add a speed controller later...
-
-// 	/* Calclulate thrust in east and north direction */
-// 	float thrust_north = cosf(bearing) * horizontal_thrust;
-// 	float thrust_east = sinf(bearing) * horizontal_thrust;
-
-// 	if (counter % 10 == 0) {
-// 		printf("thrust north: %.4f\n", thrust_north);
-// 		printf("thrust east: %.4f\n", thrust_east);
-// 		fflush(stdout);
-// 	}
-
-// 	/* Get current attitude */
-// 	if (0 == global_data_trylock(&global_data_attitude->access_conf)) {
-// 		pitch_current = global_data_attitude->pitch;
-// 		global_data_unlock(&global_data_attitude->access_conf);
-// 	}
-
-// 	/* Get desired pitch & roll */
-// 	float pitch_desired = 0.0f;
-// 	float roll_desired = 0.0f;
-
-// 	if (thrust_total != 0) {
-// 		float pitch_fraction = -thrust_north / thrust_total;
-// 		float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total);
-
-// 		if (roll_fraction < -1) {
-// 			roll_fraction = -1;
-
-// 		} else if (roll_fraction > 1) {
-// 			roll_fraction = 1;
-// 		}
-
-// //		if(counter % 5 == 0)
-// //		{
-// //			printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction);
-// //			fflush(stdout);
-// //		}
-
-// 		pitch_desired = asinf(pitch_fraction);
-// 		roll_desired = asinf(roll_fraction);
-// 	}
-
-// 	att_sp.roll = roll_desired;
-// 	att_sp.pitch = pitch_desired;
-
-// 	counter++;
-// }
diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c
new file mode 100644
index 000000000..b985630ae
--- /dev/null
+++ b/src/modules/multirotor_pos_control/thrust_pid.c
@@ -0,0 +1,189 @@
+/****************************************************************************
+ *
+ *   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/multirotor_pos_control/thrust_pid.h b/src/modules/multirotor_pos_control/thrust_pid.h
new file mode 100644
index 000000000..5e169c1ba
--- /dev/null
+++ b/src/modules/multirotor_pos_control/thrust_pid.h
@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ *   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.h
+ *
+ * Definition of thrust control PID interface.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef THRUST_PID_H_
+#define THRUST_PID_H_
+
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+/* 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
+
+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;
+
+__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);
+
+__END_DECLS
+
+#endif /* THRUST_PID_H_ */
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
new file mode 100644
index 000000000..0404b06c7
--- /dev/null
+++ b/src/modules/navigator/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.
+#
+############################################################################
+
+#
+# Main Navigation Controller
+#
+
+MODULE_COMMAND	= navigator
+
+SRCS		= navigator_main.cpp \
+		  navigator_params.c
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
new file mode 100644
index 000000000..f6c44444a
--- /dev/null
+++ b/src/modules/navigator/navigator_main.cpp
@@ -0,0 +1,604 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	Lorenz Meier
+ *
+ * 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_main.c
+ * Implementation of the main navigation state machine.
+ *
+ * Handles missions, geo fencing and failsafe navigation behavior.
+ */
+
+#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/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/vehicle_status.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/mission.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>
+
+/**
+ * navigator app start / stop handling function
+ *
+ * @ingroup apps
+ */
+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() :
+
+	_task_should_exit(false),
+	_navigator_task(-1),
+
+/* subscriptions */
+	_global_pos_sub(-1),
+	_att_sub(-1),
+	_airspeed_sub(-1),
+	_vstatus_sub(-1),
+	_params_sub(-1),
+	_manual_control_sub(-1),
+
+/* publications */
+	_triplet_pub(-1),
+
+/* performance counters */
+	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
+/* states */
+	_mission_items_maxcount(20),
+	_mission_valid(false),
+	_loiter_hold(false)
+{
+	_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");
+
+	/* fetch initial parameter values */
+	parameters_update();
+}
+
+Navigator::~Navigator()
+{
+	if (_navigator_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(_navigator_task);
+				break;
+			}
+		} while (_navigator_task != -1);
+	}
+
+	navigator::g_navigator = nullptr;
+}
+
+int
+Navigator::parameters_update()
+{
+
+	//param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise));
+
+	return OK;
+}
+
+void
+Navigator::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);
+	}
+}
+
+void
+Navigator::vehicle_attitude_poll()
+{
+	/* 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);
+	}
+}
+
+void
+Navigator::mission_poll()
+{
+	/* 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");
+		}
+	}
+}
+
+void
+Navigator::task_main_trampoline(int argc, char *argv[])
+{
+	navigator::g_navigator->task_main();
+}
+
+void
+Navigator::task_main()
+{
+
+	/* inform about start */
+	warnx("Initializing..");
+	fflush(stdout);
+
+	/*
+	 * 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));
+	_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));
+
+	/* 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();
+
+	/* wakeup source(s) */
+	struct pollfd fds[2];
+
+	/* Setup of loop */
+	fds[0].fd = _params_sub;
+	fds[0].events = POLLIN;
+	fds[1].fd = _global_pos_sub;
+	fds[1].events = POLLIN;
+
+	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 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();
+		}
+
+		/* 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
+
+					if (_global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) {
+						/* waypoint is a plain navigation waypoint */
+						
+
+					} 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) {
+
+						/* waypoint is a loiter waypoint */
+						
+					}
+
+					// XXX at this point we always want no loiter hold if a
+					// mission is active
+					_loiter_hold = false;
+
+				} else {
+
+					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;
+					}
+
+					//_parameters.loiter_hold_radius
+				}
+
+			} else if (0/* seatbelt mode enabled */) {
+
+				/** SEATBELT FLIGHT **/
+				continue;
+
+			} else {
+
+				/** MANUAL FLIGHT **/
+
+				/* no flight mode applies, do not publish an attitude setpoint */
+				continue;
+			}
+
+			/********   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);
+				}
+			}
+
+			/* 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);
+
+			} else {
+				/* advertise and publish */
+				_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &_global_triplet);
+			}
+
+		}
+
+		perf_end(_loop_perf);
+	}
+
+	warnx("exiting.\n");
+
+	_navigator_task = -1;
+	_exit(0);
+}
+
+int
+Navigator::start()
+{
+	ASSERT(_navigator_task == -1);
+
+	/* start the task */
+	_navigator_task = task_spawn_cmd("navigator",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2048,
+				       (main_t)&Navigator::task_main_trampoline,
+				       nullptr);
+
+	if (_navigator_task < 0) {
+		warn("task start failed");
+		return -errno;
+	}
+
+	return OK;
+}
+
+int navigator_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		errx(1, "usage: navigator {start|stop|status}");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (navigator::g_navigator != nullptr)
+			errx(1, "already running");
+
+		navigator::g_navigator = new Navigator;
+
+		if (navigator::g_navigator == nullptr)
+			errx(1, "alloc failed");
+
+		if (OK != navigator::g_navigator->start()) {
+			delete navigator::g_navigator;
+			navigator::g_navigator = nullptr;
+			err(1, "start failed");
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		if (navigator::g_navigator == nullptr)
+			errx(1, "not running");
+
+		delete navigator::g_navigator;
+		navigator::g_navigator = nullptr;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (navigator::g_navigator) {
+			errx(0, "running");
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
new file mode 100644
index 000000000..06df9a452
--- /dev/null
+++ b/src/modules/navigator/navigator_params.c
@@ -0,0 +1,53 @@
+/****************************************************************************
+ *
+ *   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 navigator_params.c
+ *
+ * Parameters defined by the navigator task.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * Navigator parameters, accessible via MAVLink
+ *
+ */
+
+PARAM_DEFINE_FLOAT(NAV_DUMMY, 0.0f);
+
diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c
new file mode 100644
index 000000000..13328edb4
--- /dev/null
+++ b/src/modules/position_estimator_inav/inertial_filter.c
@@ -0,0 +1,31 @@
+/*
+ * inertial_filter.c
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ */
+
+#include "inertial_filter.h"
+
+void inertial_filter_predict(float dt, float x[3])
+{
+	x[0] += x[1] * dt + x[2] * dt * dt / 2.0f;
+	x[1] += x[2] * dt;
+}
+
+void inertial_filter_correct(float e, float dt, float x[3], 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;
+
+	} else if (i == 1) {
+		x[2] += w * ewdt;
+	}
+}
diff --git a/src/modules/position_estimator_inav/inertial_filter.h b/src/modules/position_estimator_inav/inertial_filter.h
new file mode 100644
index 000000000..761c17097
--- /dev/null
+++ b/src/modules/position_estimator_inav/inertial_filter.h
@@ -0,0 +1,13 @@
+/*
+ * inertial_filter.h
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ */
+
+#include <stdbool.h>
+#include <drivers/drv_hrt.h>
+
+void inertial_filter_predict(float dt, float x[3]);
+
+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
new file mode 100644
index 000000000..939d76849
--- /dev/null
+++ b/src/modules/position_estimator_inav/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.
+#
+############################################################################
+
+#
+# Makefile to build position_estimator_inav
+#
+
+MODULE_COMMAND	 	= position_estimator_inav
+SRCS		 	= position_estimator_inav_main.c \
+			position_estimator_inav_params.c \
+			inertial_filter.c
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
new file mode 100644
index 000000000..3084b6d92
--- /dev/null
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -0,0 +1,652 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@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 position_estimator_inav_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/parameter_update.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_attitude.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/optical_flow.h>
+#include <mavlink/mavlink_log.h>
+#include <poll.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
+#include <systemlib/systemlib.h>
+#include <drivers/drv_hrt.h>
+
+#include "position_estimator_inav_params.h"
+#include "inertial_filter.h"
+
+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 uint32_t updates_counter_len = 1000000;
+static const uint32_t pub_interval = 4000;	// limit publish rate to 250 Hz
+
+__EXPORT int position_estimator_inav_main(int argc, char *argv[]);
+
+int position_estimator_inav_thread_main(int argc, char *argv[]);
+
+static void usage(const char *reason);
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+
+	fprintf(stderr,
+		"usage: position_estimator_inav {start|stop|status} [-v]\n\n");
+	exit(1);
+}
+
+/**
+ * The position_estimator_inav_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_inav_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+		if (thread_running) {
+			printf("position_estimator_inav already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		verbose_mode = false;
+
+		if (argc > 1)
+			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,
+					       position_estimator_inav_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("\tposition_estimator_inav is running\n");
+
+		} else {
+			printf("\tposition_estimator_inav not started\n");
+		}
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+/****************************************************************************
+ * main
+ ****************************************************************************/
+int position_estimator_inav_thread_main(int argc, char *argv[])
+{
+	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 };
+
+	int baro_init_cnt = 0;
+	int baro_init_num = 200;
+	float baro_alt0 = 0.0f; /* to determine while start up */
+	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;
+
+	/* declare and safely initialize all structs */
+	struct actuator_controls_s actuator;
+	memset(&actuator, 0, sizeof(actuator));
+	struct actuator_armed_s armed;
+	memset(&armed, 0, sizeof(armed));
+	struct sensor_combined_s sensor;
+	memset(&sensor, 0, sizeof(sensor));
+	struct vehicle_gps_position_s gps;
+	memset(&gps, 0, sizeof(gps));
+	struct vehicle_attitude_s att;
+	memset(&att, 0, sizeof(att));
+	struct vehicle_local_position_s local_pos;
+	memset(&local_pos, 0, sizeof(local_pos));
+	struct optical_flow_s flow;
+	memset(&flow, 0, sizeof(flow));
+	struct vehicle_global_position_s global_pos;
+	memset(&global_pos, 0, sizeof(global_pos));
+
+	/* subscribe */
+	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
+	int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
+	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
+	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));
+
+	/* 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);
+
+	struct position_estimator_inav_params params;
+	struct position_estimator_inav_param_handles pos_inav_param_handles;
+	/* initialize parameter handles */
+	parameters_init(&pos_inav_param_handles);
+
+	/* first parameters read at start up */
+	struct parameter_update_s param_update;
+	orb_copy(ORB_ID(parameter_update), parameter_update_sub, &param_update); /* read from param topic to clear updated flag */
+	/* first parameters update */
+	parameters_update(&pos_inav_param_handles, &params);
+
+	struct pollfd fds_init[1] = {
+		{ .fd = sensor_combined_sub, .events = POLLIN },
+	};
+
+	/* wait for initial baro value */
+	bool wait_baro = true;
+
+	thread_running = true;
+
+	while (wait_baro && !thread_should_exit) {
+		int ret = poll(fds_init, 1, 1000);
+
+		if (ret < 0) {
+			/* poll error */
+			mavlink_log_info(mavlink_fd, "[inav] poll error on init");
+
+		} else if (ret > 0) {
+			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;
+
+					/* mean calculation over several measurements */
+					if (baro_init_cnt < baro_init_num) {
+						baro_alt0 += 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();
+						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 },
+		{ .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
+		hrt_abstime t = hrt_absolute_time();
+
+		if (ret < 0) {
+			/* poll error */
+			mavlink_log_info(mavlink_fd, "[inav] poll error on init");
+			continue;
+
+		} else if (ret > 0) {
+			/* parameter update */
+			if (fds[0].revents & POLLIN) {
+				/* read from param to clear updated flag */
+				struct parameter_update_s update;
+				orb_copy(ORB_ID(parameter_update), parameter_update_sub,
+					 &update);
+				/* update parameters */
+				parameters_update(&pos_inav_param_handles, &params);
+			}
+
+			/* actuator */
+			if (fds[1].revents & POLLIN) {
+				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);
+			}
+
+			/* vehicle attitude */
+			if (fds[3].revents & POLLIN) {
+				orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
+				attitude_updates++;
+			}
+
+			/* sensor combined */
+			if (fds[4].revents & POLLIN) {
+				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
+
+				if (sensor.accelerometer_counter != accel_counter) {
+					if (att.R_valid) {
+						/* correct accel bias, now only for Z */
+						sensor.accelerometer_m_s2[2] -= accel_bias[2];
+
+						/* transform acceleration vector from body frame to NED frame */
+						for (int i = 0; i < 3; i++) {
+							accel_NED[i] = 0.0f;
+
+							for (int j = 0; j < 3; j++) {
+								accel_NED[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];
+
+					} else {
+						memset(accel_corr, 0, sizeof(accel_corr));
+					}
+
+					accel_counter = sensor.accelerometer_counter;
+					accel_updates++;
+				}
+
+				if (sensor.baro_counter != baro_counter) {
+					baro_corr = - sensor.baro_alt_meter - z_est[0];
+					baro_counter = sensor.baro_counter;
+					baro_updates++;
+				}
+			}
+
+			/* optical flow */
+			if (fds[5].revents & POLLIN) {
+				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;
+							}
+						}
+					}
+
+				} else {
+					sonar_corr = 0.0f;
+				}
+
+				flow_updates++;
+			}
+
+			/* vehicle GPS position */
+			if (fds[6].revents & POLLIN) {
+				orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
+
+				if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) {
+					/* 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_xy_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]));
+						/* calculate correction for position */
+						gps_corr[0][0] = gps_proj[0] - x_est[0];
+						gps_corr[1][0] = gps_proj[1] - y_est[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];
+
+						} else {
+							gps_corr[0][1] = 0.0f;
+							gps_corr[1][1] = 0.0f;
+						}
+					}
+
+				} else {
+					/* no GPS lock */
+					memset(gps_corr, 0, sizeof(gps_corr));
+					ref_xy_init_start = 0;
+				}
+
+				gps_updates++;
+			}
+		}
+
+		/* end of poll return value check */
+
+		float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
+		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);
+		}
+
+		/* 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;
+
+		/* inertial filter prediction for altitude */
+		inertial_filter_predict(dt, 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);
+
+		bool gps_valid = ref_xy_inited && gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout;
+		bool flow_valid = false;	// TODO implement opt flow
+
+		/* 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 (can_estimate_xy) {
+			/* inertial filter prediction for position */
+			inertial_filter_predict(dt, x_est);
+			inertial_filter_predict(dt, 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 (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);
+
+				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);
+				}
+			}
+		}
+
+		/* 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;
+		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) {
+				landed = false;
+				landed_time = 0;
+			}
+
+		} else {
+			if (alt_disp < land_disp2 && thrust < params.land_thr) {
+				if (landed_time == 0) {
+					landed_time = t;    // land detected first time
+
+				} else {
+					if (t > landed_time + params.land_t * 1000000.0f) {
+						landed = true;
+						landed_time = 0;
+					}
+				}
+
+			} else {
+				landed_time = 0;
+			}
+		}
+
+		if (verbose_mode) {
+			/* print updates rate */
+			if (t > updates_counter_start + updates_counter_len) {
+				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);
+				updates_counter_start = t;
+				accel_updates = 0;
+				baro_updates = 0;
+				gps_updates = 0;
+				attitude_updates = 0;
+				flow_updates = 0;
+			}
+		}
+
+		if (t > pub_last + pub_interval) {
+			pub_last = t;
+			/* publish local position */
+			local_pos.timestamp = t;
+			local_pos.xy_valid = can_estimate_xy && gps_valid;
+			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.x = x_est[0];
+			local_pos.vx = x_est[1];
+			local_pos.y = y_est[0];
+			local_pos.vy = y_est[1];
+			local_pos.z = z_est[0];
+			local_pos.vz = z_est[1];
+			local_pos.landed = landed;
+			local_pos.yaw = att.yaw;
+
+			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) {
+				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;
+			}
+
+			/* 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;
+			}
+
+			if (local_pos.z_valid) {
+				global_pos.relative_alt = -local_pos.z;
+			}
+
+			if (local_pos.z_global) {
+				global_pos.alt = local_pos.ref_alt - local_pos.z;
+			}
+
+			if (local_pos.v_z_valid) {
+				global_pos.vz = local_pos.vz;
+			}
+			global_pos.yaw = local_pos.yaw;
+
+			global_pos.timestamp = t;
+
+			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");
+	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
new file mode 100644
index 000000000..4f9ddd009
--- /dev/null
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@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 position_estimator_inav_params.c
+ *
+ * Parameters for position_estimator_inav
+ */
+
+#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_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);
+
+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_acc_bias = param_find("INAV_W_ACC_BIAS");
+	h->flow_k = param_find("INAV_FLOW_K");
+	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");
+
+	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_acc_bias, &(p->w_acc_bias));
+	param_get(h->flow_k, &(p->flow_k));
+	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));
+
+	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
new file mode 100644
index 000000000..61570aea7
--- /dev/null
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h
@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@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 position_estimator_inav_params.h
+ *
+ * Parameters for Position Estimator
+ */
+
+#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_acc_bias;
+	float flow_k;
+	float sonar_filt;
+	float sonar_err;
+	float land_t;
+	float land_disp;
+	float land_thr;
+};
+
+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_acc_bias;
+	param_t flow_k;
+	param_t sonar_filt;
+	param_t sonar_err;
+	param_t land_t;
+	param_t land_disp;
+	param_t land_thr;
+};
+
+/**
+ * Initialize all parameter handles and values
+ *
+ */
+int parameters_init(struct position_estimator_inav_param_handles *h);
+
+/**
+ * Update all parameters
+ *
+ */
+int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p);
diff --git a/src/modules/position_estimator_mc/position_estimator_mc_main.c b/src/modules/position_estimator_mc/position_estimator_mc_main.c
index 984bd1329..363961819 100755
--- a/src/modules/position_estimator_mc/position_estimator_mc_main.c
+++ b/src/modules/position_estimator_mc/position_estimator_mc_main.c
@@ -497,7 +497,12 @@ int position_estimator_mc_thread_main(int argc, char *argv[])
 				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]))){
-					orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est);
+					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 */
diff --git a/src/modules/px4iofirmware/adc.c b/src/modules/px4iofirmware/adc.c
index 81566eb2a..2f5908ac5 100644
--- a/src/modules/px4iofirmware/adc.c
+++ b/src/modules/px4iofirmware/adc.c
@@ -83,6 +83,14 @@ adc_init(void)
 {
 	adc_perf = perf_alloc(PC_ELAPSED, "adc");
 
+	/* put the ADC into power-down mode */
+	rCR2 &= ~ADC_CR2_ADON;
+	up_udelay(10);
+
+	/* bring the ADC out of power-down mode */
+	rCR2 |= ADC_CR2_ADON;
+	up_udelay(10);
+
 	/* do calibration if supported */
 #ifdef ADC_CR2_CAL
 	rCR2 |= ADC_CR2_RSTCAL;
@@ -96,41 +104,25 @@ adc_init(void)
 
 	if (rCR2 & ADC_CR2_CAL)
 		return -1;
-
 #endif
 
-	/* arbitrarily configure all channels for 55 cycle sample time */
-	rSMPR1 = 0b00000011011011011011011011011011;
+	/*
+	 * Configure sampling time.
+	 * 
+	 * For electrical protection reasons, we want to be able to have
+	 * 10K in series with ADC inputs that leave the board. At 12MHz this
+	 * means we need 28.5 cycles of sampling time (per table 43 in the
+	 * datasheet).
+	 */
+	rSMPR1 = 0b00000000011011011011011011011011;
 	rSMPR2 = 0b00011011011011011011011011011011;
 
-	/* XXX for F2/4, might want to select 12-bit mode? */
-	rCR1 = 0;
-
-	/* enable the temperature sensor / Vrefint channel if supported*/
-	rCR2 =
-#ifdef ADC_CR2_TSVREFE
-		/* enable the temperature sensor in CR2 */
-		ADC_CR2_TSVREFE |
-#endif
-		0;
-
-#ifdef ADC_CCR_TSVREFE
-	/* enable temperature sensor in CCR */
-	rCCR = ADC_CCR_TSVREFE;
-#endif
+	rCR2 |=	ADC_CR2_TSVREFE;		/* enable the temperature sensor / Vrefint channel */
 
 	/* configure for a single-channel sequence */
 	rSQR1 = 0;
 	rSQR2 = 0;
-	rSQR3 = 0;	/* will be updated with the channel each tick */
-
-	/* power-cycle the ADC and turn it on */
-	rCR2 &= ~ADC_CR2_ADON;
-	up_udelay(10);
-	rCR2 |= ADC_CR2_ADON;
-	up_udelay(10);
-	rCR2 |= ADC_CR2_ADON;
-	up_udelay(10);
+	rSQR3 = 0;	/* will be updated with the channel at conversion time */
 
 	return 0;
 }
@@ -141,11 +133,12 @@ adc_init(void)
 uint16_t
 adc_measure(unsigned channel)
 {
+
 	perf_begin(adc_perf);
 
 	/* clear any previous EOC */
-	if (rSR & ADC_SR_EOC)
-		rSR &= ~ADC_SR_EOC;
+	rSR = 0;
+	(void)rDR;
 
 	/* run a single conversion right now - should take about 60 cycles (a few microseconds) max */
 	rSQR3 = channel;
@@ -158,7 +151,6 @@ adc_measure(unsigned channel)
 
 		/* never spin forever - this will give a bogus result though */
 		if (hrt_elapsed_time(&now) > 100) {
-			debug("adc timeout");
 			perf_end(adc_perf);
 			return 0xffff;
 		}
@@ -166,6 +158,7 @@ adc_measure(unsigned channel)
 
 	/* read the result and clear EOC */
 	uint16_t result = rDR;
+	rSR = 0;
 
 	perf_end(adc_perf);
 	return result;
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 3cf9ca149..941500f0d 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -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
@@ -50,7 +50,7 @@
 #define RC_CHANNEL_HIGH_THRESH		5000
 #define RC_CHANNEL_LOW_THRESH		-5000
 
-static bool	ppm_input(uint16_t *values, uint16_t *num_values);
+static bool	ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
 
 static perf_counter_t c_gather_dsm;
 static perf_counter_t c_gather_sbus;
@@ -59,14 +59,19 @@ static perf_counter_t c_gather_ppm;
 void
 controls_init(void)
 {
-	/* DSM input */
+	/* no channels */
+	r_raw_rc_count = 0;
+	system_state.rc_channels_timestamp_received = 0;
+	system_state.rc_channels_timestamp_valid = 0;
+
+	/* DSM input (USART1) */
 	dsm_init("/dev/ttyS0");
 
-	/* S.bus input */
+	/* S.bus input (USART3) */
 	sbus_init("/dev/ttyS2");
 
 	/* default to a 1:1 input map, all enabled */
-	for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
+	for (unsigned i = 0; i < PX4IO_RC_INPUT_CHANNELS; i++) {
 		unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
 
 		r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS]    = 0;
@@ -94,16 +99,67 @@ controls_tick() {
 	 * other.  Don't do that.
 	 */
 
+	/* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */
+	uint16_t rssi = 0;
+
+#ifdef ADC_RSSI
+	if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) {
+		unsigned counts = adc_measure(ADC_RSSI);
+		if (counts != 0xffff) {
+			/* use 1:1 scaling on 3.3V ADC input */
+			unsigned mV = counts * 3300 / 4096;
+
+			/* scale to 0..253 */
+			rssi = mV / 13;
+		}
+	}
+#endif
+
 	perf_begin(c_gather_dsm);
-	bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count);
-	if (dsm_updated)
-		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
+	uint16_t temp_count = r_raw_rc_count;
+	bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
+	if (dsm_updated) {
+		r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
+		r_raw_rc_count = temp_count & 0x7fff;
+		if (temp_count & 0x8000)
+			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
+		else
+			r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
+
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+
+	}
 	perf_end(c_gather_dsm);
 
 	perf_begin(c_gather_sbus);
-	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count);
-	if (sbus_updated)
+
+	bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS);
+
+	bool sbus_failsafe, sbus_frame_drop;
+	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS);
+
+	if (sbus_updated) {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
+
+		rssi = 255;
+
+		if (sbus_frame_drop) {
+			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
+			rssi = 100;
+		} else {
+			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
+		}
+
+		if (sbus_failsafe) {
+			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
+			rssi = 0;
+		} else {
+			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+		}
+
+	}
+
 	perf_end(c_gather_sbus);
 
 	/*
@@ -112,12 +168,21 @@ controls_tick() {
 	 * disable the PPM decoder completely if we have S.bus signal.
 	 */
 	perf_begin(c_gather_ppm);
-	bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count);
-	if (ppm_updated)
+	bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);
+	if (ppm_updated) {
+
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+	}
 	perf_end(c_gather_ppm);
 
-	ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS);
+	/* limit number of channels to allowable data size */
+	if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
+		r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
+
+	/* store RSSI */
+	r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;
 
 	/*
 	 * In some cases we may have received a frame, but input has still
@@ -130,95 +195,100 @@ controls_tick() {
 	 */
 	if (dsm_updated || sbus_updated || ppm_updated) {
 
-		/* update RC-received timestamp */
-		system_state.rc_channels_timestamp = hrt_absolute_time();
-
 		/* record a bitmask of channels assigned */
 		unsigned assigned_channels = 0;
 
-		/* 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;
+		/* 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;
+
+					/* and update the scaled/mapped version */
+					unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
+					if (mapped < PX4IO_CONTROL_CHANNELS) {
+
+						/* 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;
+
+						r_rc_values[mapped] = SIGNED_TO_REG(scaled);
+						assigned_channels |= (1 << mapped);
+
+					}
 				}
-
-				/* invert channel if requested */
-				if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
-					scaled = -scaled;
-
-				/* and update the scaled/mapped version */
-				unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
-				ASSERT(mapped < MAX_CONTROL_CHANNELS);
-
-				/* 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;
-
-				r_rc_values[mapped] = SIGNED_TO_REG(scaled);
-				assigned_channels |= (1 << mapped);
 			}
-		}
 
-		/* set un-assigned controls to zero */
-		for (unsigned i = 0; i < MAX_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;
+			}
 
-		/*
-		 * If we got an update with zero channels, treat it as 
-		 * a loss of input.
-		 *
-		 * This might happen if a protocol-based receiver returns an update
-		 * that contains no channels that we have mapped.
-		 */
-		if (assigned_channels == 0) {
-			rc_input_lost = true;
-		} else {
-			/* set RC OK flag */
+			/* set RC OK flag, as we got an update */
 			r_status_flags |= PX4IO_P_STATUS_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);
+			}
 		}
 
 		/*
@@ -231,7 +301,7 @@ controls_tick() {
 	 * If we haven't seen any new control data in 200ms, assume we
 	 * have lost input.
 	 */
-	if (hrt_elapsed_time(&system_state.rc_channels_timestamp) > 200000) {
+	if (hrt_elapsed_time(&system_state.rc_channels_timestamp_received) > 200000) {
 		rc_input_lost = true;
 
 		/* clear the input-kind flags here */
@@ -239,24 +309,32 @@ controls_tick() {
 			PX4IO_P_STATUS_FLAGS_RC_PPM |
 			PX4IO_P_STATUS_FLAGS_RC_DSM |
 			PX4IO_P_STATUS_FLAGS_RC_SBUS);
+
 	}
 
 	/*
 	 * Handle losing RC input
 	 */
-	if (rc_input_lost) {
 
+	/* 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)) {
 		/* Clear the RC input status flag, clear manual override flag */
 		r_status_flags &= ~(
 			PX4IO_P_STATUS_FLAGS_OVERRIDE |
 			PX4IO_P_STATUS_FLAGS_RC_OK);
 
+		/* Mark all channels as invalid, as we just lost the RX */
+		r_rc_valid = 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) {
 
-		/* Mark the arrays as empty */
+		/* Set channel count to zero */
 		r_raw_rc_count = 0;
-		r_rc_valid = 0;
 	}
 
 	/*
@@ -279,7 +357,7 @@ controls_tick() {
 		 * requested override.
 		 *
 		 */
-		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) > RC_CHANNEL_HIGH_THRESH))
+		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) < RC_CHANNEL_LOW_THRESH))
 			override = true;
 
 		if (override) {
@@ -293,11 +371,13 @@ controls_tick() {
 		} else {
 			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
 		}
+	} else {
+		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
 	}
 }
 
 static bool
-ppm_input(uint16_t *values, uint16_t *num_values)
+ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
 {
 	bool result = false;
 
@@ -312,8 +392,8 @@ ppm_input(uint16_t *values, uint16_t *num_values)
 
 		/* PPM data exists, copy it */
 		*num_values = ppm_decoded_channels;
-		if (*num_values > MAX_CONTROL_CHANNELS)
-			*num_values = MAX_CONTROL_CHANNELS;
+		if (*num_values > PX4IO_RC_INPUT_CHANNELS)
+			*num_values = PX4IO_RC_INPUT_CHANNELS;
 
 		for (unsigned i = 0; i < *num_values; i++)
 			values[i] = ppm_buffer[i];
@@ -321,6 +401,10 @@ ppm_input(uint16_t *values, uint16_t *num_values)
 		/* clear validity */
 		ppm_last_valid_decode = 0;
 
+		/* store PPM frame length */
+		if (num_values)
+			*frame_len = ppm_frame_length;
+
 		/* good if we got any channels */
 		result = (*num_values > 0);
 	}
diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c
index ea35e5513..d3f365822 100644
--- a/src/modules/px4iofirmware/dsm.c
+++ b/src/modules/px4iofirmware/dsm.c
@@ -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
@@ -40,6 +40,7 @@
  */
 
 #include <nuttx/config.h>
+#include <nuttx/arch.h>
 
 #include <fcntl.h>
 #include <unistd.h>
@@ -47,121 +48,44 @@
 
 #include <drivers/drv_hrt.h>
 
-#define DEBUG
-
 #include "px4io.h"
 
-#define DSM_FRAME_SIZE		16
-#define DSM_FRAME_CHANNELS	7
-
-static int dsm_fd = -1;
-
-static hrt_abstime last_rx_time;
-static hrt_abstime last_frame_time;
-
-static uint8_t	frame[DSM_FRAME_SIZE];
-
-static unsigned partial_frame_count;
-static unsigned	channel_shift;
-
-unsigned dsm_frame_drops;
-
-static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value);
-static void dsm_guess_format(bool reset);
-static bool dsm_decode(hrt_abstime now, uint16_t *values, uint16_t *num_values);
-
-int
-dsm_init(const char *device)
-{
-	if (dsm_fd < 0)
-		dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
-
-	if (dsm_fd >= 0) {
-		struct termios t;
+#define DSM_FRAME_SIZE		16		/**<DSM frame size in bytes*/
+#define DSM_FRAME_CHANNELS	7		/**<Max supported DSM channels*/
 
-		/* 115200bps, no parity, one stop bit */
-		tcgetattr(dsm_fd, &t);
-		cfsetspeed(&t, 115200);
-		t.c_cflag &= ~(CSTOPB | PARENB);
-		tcsetattr(dsm_fd, TCSANOW, &t);
-
-		/* initialise the decoder */
-		partial_frame_count = 0;
-		last_rx_time = hrt_absolute_time();
-
-		/* reset the format detector */
-		dsm_guess_format(true);
-
-		debug("DSM: ready");
-
-	} else {
-		debug("DSM: open failed");
-	}
-
-	return dsm_fd;
-}
-
-bool
-dsm_input(uint16_t *values, uint16_t *num_values)
-{
-	ssize_t		ret;
-	hrt_abstime	now;
-
-	/*
-	 * The DSM* protocol doesn't provide any explicit framing,
-	 * so we detect frame boundaries by the inter-frame delay.
-	 *
-	 * The minimum frame spacing is 11ms; with 16 bytes at 115200bps
-	 * frame transmission time is ~1.4ms.
-	 *
-	 * We expect to only be called when bytes arrive for processing,
-	 * and if an interval of more than 5ms passes between calls,
-	 * the first byte we read will be the first byte of a frame.
-	 *
-	 * In the case where byte(s) are dropped from a frame, this also
-	 * provides a degree of protection. Of course, it would be better
-	 * if we didn't drop bytes...
-	 */
-	now = hrt_absolute_time();
-
-	if ((now - last_rx_time) > 5000) {
-		if (partial_frame_count > 0) {
-			dsm_frame_drops++;
-			partial_frame_count = 0;
-		}
-	}
-
-	/*
-	 * Fetch bytes, but no more than we would need to complete
-	 * the current frame.
-	 */
-	ret = read(dsm_fd, &frame[partial_frame_count], DSM_FRAME_SIZE - partial_frame_count);
-
-	/* if the read failed for any reason, just give up here */
-	if (ret < 1)
-		return false;
-
-	last_rx_time = now;
-
-	/*
-	 * Add bytes to the current frame
-	 */
-	partial_frame_count += ret;
-
-	/*
-	 * If we don't have a full frame, return
-	 */
-	if (partial_frame_count < DSM_FRAME_SIZE)
-		return false;
-
-	/*
-	 * Great, it looks like we might have a frame.  Go ahead and
-	 * decode it.
-	 */
-	partial_frame_count = 0;
-	return dsm_decode(now, values, num_values);
-}
+static int dsm_fd = -1;						/**< File handle to the DSM UART */
+static hrt_abstime dsm_last_rx_time;		/**< Timestamp when we last received */
+static hrt_abstime dsm_last_frame_time;		/**< Timestamp for start of last dsm frame */
+static uint8_t dsm_frame[DSM_FRAME_SIZE];	/**< DSM dsm frame receive buffer */
+static unsigned dsm_partial_frame_count;	/**< Count of bytes received for current dsm frame */
+static unsigned dsm_channel_shift;			/**< Channel resolution, 0=unknown, 1=10 bit, 2=11 bit */
+static unsigned dsm_frame_drops;			/**< Count of incomplete DSM frames */
 
+/**
+ * Attempt to decode a single channel raw channel datum
+ *
+ * The DSM* protocol doesn't provide any explicit framing,
+ * so we detect dsm frame boundaries by the inter-dsm frame delay.
+ *
+ * The minimum dsm frame spacing is 11ms; with 16 bytes at 115200bps
+ * dsm frame transmission time is ~1.4ms.
+ *
+ * We expect to only be called when bytes arrive for processing,
+ * and if an interval of more than 5ms passes between calls,
+ * the first byte we read will be the first byte of a dsm frame.
+ *
+ * In the case where byte(s) are dropped from a dsm frame, this also
+ * provides a degree of protection. Of course, it would be better
+ * if we didn't drop bytes...
+ *
+ * Upon receiving a full dsm frame we attempt to decode it
+ *
+ * @param[in] raw 16 bit raw channel value from dsm frame
+ * @param[in] shift position of channel number in raw data
+ * @param[out] channel pointer to returned channel number
+ * @param[out] value pointer to returned channel value
+ * @return true=raw value successfully decoded
+ */
 static bool
 dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value)
 {
@@ -179,6 +103,11 @@ dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *va
 	return true;
 }
 
+/**
+ * Attempt to guess if receiving 10 or 11 bit channel values
+ *
+ * @param[in] reset true=reset the 10/11 bit state to unknown
+ */
 static void
 dsm_guess_format(bool reset)
 {
@@ -191,14 +120,14 @@ dsm_guess_format(bool reset)
 		cs10 = 0;
 		cs11 = 0;
 		samples = 0;
-		channel_shift = 0;
+		dsm_channel_shift = 0;
 		return;
 	}
 
-	/* scan the channels in the current frame in both 10- and 11-bit mode */
+	/* scan the channels in the current dsm_frame in both 10- and 11-bit mode */
 	for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
 
-		uint8_t *dp = &frame[2 + (2 * i)];
+		uint8_t *dp = &dsm_frame[2 + (2 * i)];
 		uint16_t raw = (dp[0] << 8) | dp[1];
 		unsigned channel, value;
 
@@ -209,16 +138,16 @@ dsm_guess_format(bool reset)
 		if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31))
 			cs11 |= (1 << channel);
 
-		/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-frame format */
+		/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */
 	}
 
-	/* wait until we have seen plenty of frames - 2 should normally be enough */
+	/* wait until we have seen plenty of frames - 5 should normally be enough */
 	if (samples++ < 5)
 		return;
 
 	/*
 	 * Iterate the set of sensible sniffed channel sets and see whether
-	 * decoding in 10 or 11-bit mode has yielded anything we recognise.
+	 * decoding in 10 or 11-bit mode has yielded anything we recognize.
 	 *
 	 * XXX Note that due to what seem to be bugs in the DSM2 high-resolution
 	 *     stream, we may want to sniff for longer in some cases when we think we
@@ -233,6 +162,7 @@ dsm_guess_format(bool reset)
 		0xff,	/* 8 channels (DX8) */
 		0x1ff,	/* 9 channels (DX9, etc.) */
 		0x3ff,	/* 10 channels (DX10) */
+		0x1fff,	/* 13 channels (DX10t) */
 		0x3fff	/* 18 channels (DX10) */
 	};
 	unsigned votes10 = 0;
@@ -248,13 +178,13 @@ dsm_guess_format(bool reset)
 	}
 
 	if ((votes11 == 1) && (votes10 == 0)) {
-		channel_shift = 11;
+		dsm_channel_shift = 11;
 		debug("DSM: 11-bit format");
 		return;
 	}
 
 	if ((votes10 == 1) && (votes11 == 0)) {
-		channel_shift = 10;
+		dsm_channel_shift = 10;
 		debug("DSM: 10-bit format");
 		return;
 	}
@@ -264,27 +194,149 @@ dsm_guess_format(bool reset)
 	dsm_guess_format(true);
 }
 
+/**
+ * Initialize the DSM receive functionality
+ *
+ * Open the UART for receiving DSM frames and configure it appropriately
+ *
+ * @param[in] device Device name of DSM UART
+ */
+int
+dsm_init(const char *device)
+{
+
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+	// enable power on DSM connector
+	POWER_SPEKTRUM(true);
+#endif
+
+	if (dsm_fd < 0)
+		dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
+
+	if (dsm_fd >= 0) {
+
+		struct termios t;
+
+		/* 115200bps, no parity, one stop bit */
+		tcgetattr(dsm_fd, &t);
+		cfsetspeed(&t, 115200);
+		t.c_cflag &= ~(CSTOPB | PARENB);
+		tcsetattr(dsm_fd, TCSANOW, &t);
+
+		/* initialise the decoder */
+		dsm_partial_frame_count = 0;
+		dsm_last_rx_time = hrt_absolute_time();
+
+		/* reset the format detector */
+		dsm_guess_format(true);
+
+		debug("DSM: ready");
+
+	} else {
+
+		debug("DSM: open failed");
+
+	}
+
+	return dsm_fd;
+}
+
+/**
+ * Handle DSM satellite receiver bind mode handler
+ *
+ * @param[in] cmd commands - dsm_bind_power_down, dsm_bind_power_up, dsm_bind_set_rx_out, dsm_bind_send_pulses, dsm_bind_reinit_uart
+ * @param[in] pulses Number of pulses for dsm_bind_send_pulses command
+ */
+void
+dsm_bind(uint16_t cmd, int pulses)
+{
+#if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2)
+	#warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM
+#else
+	const uint32_t usart1RxAsOutp =
+		GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10;
+
+	if (dsm_fd < 0)
+		return;
+
+	switch (cmd) {
+
+	case dsm_bind_power_down:
+
+		/*power down DSM satellite*/
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
+		POWER_RELAY1(0);
+#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */
+		POWER_SPEKTRUM(0);
+#endif
+		break;
+
+	case dsm_bind_power_up:
+
+		/*power up DSM satellite*/
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
+		POWER_RELAY1(1);
+#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */
+		POWER_SPEKTRUM(1);
+#endif
+		dsm_guess_format(true);
+		break;
+
+	case dsm_bind_set_rx_out:
+
+		/*Set UART RX pin to active output mode*/
+		stm32_configgpio(usart1RxAsOutp);
+		break;
+
+	case dsm_bind_send_pulses:
+
+		/*Pulse RX pin a number of times*/
+		for (int i = 0; i < pulses; i++) {
+			up_udelay(25);
+			stm32_gpiowrite(usart1RxAsOutp, false);
+			up_udelay(25);
+			stm32_gpiowrite(usart1RxAsOutp, true);
+		}
+		break;
+
+	case dsm_bind_reinit_uart:
+
+		/*Restore USART RX pin to RS232 receive mode*/
+		stm32_configgpio(GPIO_USART1_RX);
+		break;
+
+	}
+#endif
+}
+
+/**
+ * Decode the entire dsm frame (all contained channels)
+ *
+ * @param[in] frame_time timestamp when this dsm frame was received. Used to detect RX loss in order to reset 10/11 bit guess.
+ * @param[out] values pointer to per channel array of decoded values
+ * @param[out] num_values pointer to number of raw channel values returned
+ * @return true=DSM frame successfully decoded, false=no update
+ */
 static bool
 dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 {
-
 	/*
-		debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
-		      frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7],
-		      frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]);
+	debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
+		dsm_frame[0], dsm_frame[1], dsm_frame[2], dsm_frame[3], dsm_frame[4], dsm_frame[5], dsm_frame[6], dsm_frame[7],
+		dsm_frame[8], dsm_frame[9], dsm_frame[10], dsm_frame[11], dsm_frame[12], dsm_frame[13], dsm_frame[14], dsm_frame[15]);
 	*/
 	/*
 	 * If we have lost signal for at least a second, reset the
 	 * format guessing heuristic.
 	 */
-	if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0))
+	if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0))
 		dsm_guess_format(true);
 
-	/* we have received something we think is a frame */
-	last_frame_time = frame_time;
+	/* we have received something we think is a dsm_frame */
+	dsm_last_frame_time = frame_time;
 
-	/* if we don't know the frame format, update the guessing state machine */
-	if (channel_shift == 0) {
+	/* if we don't know the dsm_frame format, update the guessing state machine */
+	if (dsm_channel_shift == 0) {
 		dsm_guess_format(false);
 		return false;
 	}
@@ -296,32 +348,46 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 	 * Each channel is a 16-bit unsigned value containing either a 10-
 	 * or 11-bit channel value and a 4-bit channel number, shifted
 	 * either 10 or 11 bits. The MSB may also be set to indicate the
-	 * second frame in variants of the protocol where more than
+	 * second dsm_frame in variants of the protocol where more than
 	 * seven channels are being transmitted.
 	 */
 
 	for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
 
-		uint8_t *dp = &frame[2 + (2 * i)];
+		uint8_t *dp = &dsm_frame[2 + (2 * i)];
 		uint16_t raw = (dp[0] << 8) | dp[1];
 		unsigned channel, value;
 
-		if (!dsm_decode_channel(raw, channel_shift, &channel, &value))
+		if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value))
 			continue;
 
 		/* ignore channels out of range */
-		if (channel >= PX4IO_INPUT_CHANNELS)
+		if (channel >= PX4IO_RC_INPUT_CHANNELS)
 			continue;
 
 		/* update the decoded channel count */
 		if (channel >= *num_values)
 			*num_values = channel + 1;
 
-		/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
-		if (channel_shift == 11)
-			value /= 2;
+		/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */
+		if (dsm_channel_shift == 10)
+			value *= 2;
 
-		value += 998;
+		/*
+		 * Spektrum scaling is special. There are these basic considerations
+		 *
+		 *   * Midpoint is 1520 us
+		 *   * 100% travel channels are +- 400 us
+		 *
+		 * We obey the original Spektrum scaling (so a default setup will scale from
+		 * 1100 - 1900 us), but we do not obey the weird 1520 us center point
+		 * and instead (correctly) center the center around 1500 us. This is in order
+		 * to get something useful without requiring the user to calibrate on a digital
+		 * link for no reason.
+		 */
+
+		/* scaled integer for decent accuracy while staying efficient */
+		value = ((((int)value - 1024) * 1000) / 1700) + 1500;
 
 		/*
 		 * Store the decoded channel into the R/C input buffer, taking into
@@ -350,7 +416,88 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 	}
 
 	/*
+	 * Spektrum likes to send junk in higher channel numbers to fill
+	 * their packets. We don't know about a 13 channel model in their TX
+	 * lines, so if we get a channel count of 13, we'll return 12 (the last
+	 * data index that is stable).
+	 */
+	if (*num_values == 13)
+		*num_values = 12;
+
+	if (dsm_channel_shift == 11) {
+		/* Set the 11-bit data indicator */
+		*num_values |= 0x8000;
+	}
+
+	/*
 	 * XXX Note that we may be in failsafe here; we need to work out how to detect that.
 	 */
 	return true;
 }
+
+/**
+ * Called periodically to check for input data from the DSM UART
+ *
+ * The DSM* protocol doesn't provide any explicit framing,
+ * so we detect dsm frame boundaries by the inter-dsm frame delay.
+ * The minimum dsm frame spacing is 11ms; with 16 bytes at 115200bps
+ * dsm frame transmission time is ~1.4ms.
+ * We expect to only be called when bytes arrive for processing,
+ * and if an interval of more than 5ms passes between calls,
+ * the first byte we read will be the first byte of a dsm frame.
+ * In the case where byte(s) are dropped from a dsm frame, this also
+ * provides a degree of protection. Of course, it would be better
+ * if we didn't drop bytes...
+ * Upon receiving a full dsm frame we attempt to decode it.
+ *
+ * @param[out] values pointer to per channel array of decoded values
+ * @param[out] num_values pointer to number of raw channel values returned, high order bit 0:10 bit data, 1:11 bit data
+ * @return true=decoded raw channel values updated, false=no update
+ */
+bool
+dsm_input(uint16_t *values, uint16_t *num_values)
+{
+	ssize_t		ret;
+	hrt_abstime	now;
+
+	/*
+	 */
+	now = hrt_absolute_time();
+
+	if ((now - dsm_last_rx_time) > 5000) {
+		if (dsm_partial_frame_count > 0) {
+			dsm_frame_drops++;
+			dsm_partial_frame_count = 0;
+		}
+	}
+
+	/*
+	 * Fetch bytes, but no more than we would need to complete
+	 * the current dsm frame.
+	 */
+	ret = read(dsm_fd, &dsm_frame[dsm_partial_frame_count], DSM_FRAME_SIZE - dsm_partial_frame_count);
+
+	/* if the read failed for any reason, just give up here */
+	if (ret < 1)
+		return false;
+
+	dsm_last_rx_time = now;
+
+	/*
+	 * Add bytes to the current dsm frame
+	 */
+	dsm_partial_frame_count += ret;
+
+	/*
+	 * If we don't have a full dsm frame, return
+	 */
+	if (dsm_partial_frame_count < DSM_FRAME_SIZE)
+		return false;
+
+	/*
+	 * Great, it looks like we might have a dsm frame.  Go ahead and
+	 * decode it.
+	 */
+	dsm_partial_frame_count = 0;
+	return dsm_decode(now, values, num_values);
+}
diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c
index 4485daa5b..79b6546b3 100644
--- a/src/modules/px4iofirmware/i2c.c
+++ b/src/modules/px4iofirmware/i2c.c
@@ -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
@@ -69,6 +69,7 @@ static void		i2c_rx_setup(void);
 static void		i2c_tx_setup(void);
 static void		i2c_rx_complete(void);
 static void		i2c_tx_complete(void);
+static void		i2c_dump(void);
 
 static DMA_HANDLE	rx_dma;
 static DMA_HANDLE	tx_dma;
@@ -92,7 +93,7 @@ enum {
 } direction;
 
 void
-i2c_init(void)
+interface_init(void)
 {
 	debug("i2c init");
 
@@ -148,12 +149,12 @@ i2c_init(void)
 #endif
 }
 
-
 /*
   reset the I2C bus
   used to recover from lockups
  */
-void i2c_reset(void)
+void
+i2c_reset(void)
 {
 	rCR1 |= I2C_CR1_SWRST;
 	rCR1 = 0;
@@ -330,7 +331,7 @@ i2c_tx_complete(void)
 	i2c_tx_setup();
 }
 
-void
+static void
 i2c_dump(void)
 {
 	debug("CR1   0x%08x  CR2   0x%08x", rCR1,  rCR2);
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 1234e2eea..6a4429461 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -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
@@ -47,6 +47,7 @@
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_hrt.h>
 
+#include <systemlib/pwm_limit/pwm_limit.h>
 #include <systemlib/mixer/mixer.h>
 
 extern "C" {
@@ -68,13 +69,17 @@ extern "C" {
 
 /* current servo arm/disarm state */
 static bool mixer_servos_armed = false;
+static bool should_arm = false;
+static bool should_always_enable_pwm = false;
+static volatile bool in_mixer = false;
 
 /* selected control values and count for mixing */
 enum mixer_source {
 	MIX_NONE,
 	MIX_FMU,
 	MIX_OVERRIDE,
-	MIX_FAILSAFE
+	MIX_FAILSAFE,
+	MIX_OVERRIDE_FMU_OK
 };
 static mixer_source source;
 
@@ -85,9 +90,13 @@ static int	mixer_callback(uintptr_t handle,
 
 static MixerGroup mixer_group(mixer_callback, 0);
 
+/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */
+static void mixer_set_failsafe();
+
 void
 mixer_tick(void)
 {
+
 	/* check that we are receiving fresh data from the FMU */
 	if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
 
@@ -95,28 +104,31 @@ mixer_tick(void)
 		if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
 			isr_debug(1, "AP RX timeout");
 		}
-		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM);
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK);
 		r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
 
 	} else {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
 	}
 
+	/* default to failsafe mixing */
 	source = MIX_FAILSAFE;
 
 	/*
 	 * Decide which set of controls we're using.
 	 */
-	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ||
-		!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
 
-		/* don't actually mix anything - we already have raw PWM values or
-		 not a valid mixer. */
+	/* do not mix if RAW_PWM mode is on and FMU is good */
+	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
+	        (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
+
+		/* don't actually mix anything - we already have raw PWM values */
 		source = MIX_NONE;
 
 	} else {
 
 		if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
+		     (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
 		     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
 
 			/* mix from FMU controls */
@@ -125,77 +137,127 @@ mixer_tick(void)
 
 		if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
 		     (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
-		     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
+		     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
+		     !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
+		     !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
 
 		 	/* if allowed, mix from RC inputs directly */
 			source = MIX_OVERRIDE;
+		} else 	if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
+		     (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
+		     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
+		     !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
+		     (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
+
+			/* if allowed, mix from RC inputs directly up to available rc channels */
+			source = MIX_OVERRIDE_FMU_OK;
 		}
 	}
 
 	/*
+	 * Set failsafe status flag depending on mixing source
+	 */
+	if (source == MIX_FAILSAFE) {
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE;
+	} else {
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE);
+	}
+
+	/*
+	 * Decide whether the servos should be armed right now.
+	 *
+	 * We must be armed, and we must have a PWM source; either raw from
+	 * FMU or from the mixer.
+	 *
+	 * XXX correct behaviour for failsafe may require an additional case
+	 * here.
+	 */
+	should_arm = (
+		/* IO initialised without error */   (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
+		/* and IO is armed */ 		  && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
+		/* and FMU is armed */ 		  && (
+							    ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
+		/* and there is valid input via or mixer */         &&   (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) )
+		/* or direct PWM is set */               || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
+		/* or failsafe was set manually */	 || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK))
+						     )
+	);
+
+	should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
+						&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
+						&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
+
+	/*
 	 * Run the mixers.
 	 */
 	if (source == MIX_FAILSAFE) {
 
 		/* copy failsafe values to the servo outputs */
-		for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
+		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
 			r_page_servos[i] = r_page_servo_failsafe[i];
 
-	} else if (source != MIX_NONE) {
+			/* safe actuators for FMU feedback */
+			r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f);
+		}
+
+
+	} else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
 
-		float	outputs[IO_SERVO_COUNT];
+		float	outputs[PX4IO_SERVO_COUNT];
 		unsigned mixed;
 
 		/* mix */
-		mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
 
-		/* scale to PWM and update the servo outputs as required */
-		for (unsigned i = 0; i < mixed; i++) {
+		/* poor mans mutex */
+		in_mixer = true;
+		mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
+		in_mixer = false;
 
-			/* save actuator values for FMU readback */
-			r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
+		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);
 
-			/* scale to servo output */
-			r_page_servos[i] = (outputs[i] * 500.0f) + 1500;
+		for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
+			r_page_servos[i] = 0;
 
+		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
+			r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
 		}
-		for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
-			r_page_servos[i] = 0;
 	}
 
-	/*
-	 * Decide whether the servos should be armed right now.
-	 *
-	 * We must be armed, and we must have a PWM source; either raw from
-	 * FMU or from the mixer.
-	 *
-	 * XXX correct behaviour for failsafe may require an additional case
-	 * here.
-	 */
-	bool should_arm = (
-	    /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
-	 	/* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
-		/* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
-		/* IO initialised without error */  (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
-		/* FMU is available or FMU is not available but override is an option */
-		((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ))
-	);
+	/* set arming */
+	bool needs_to_arm = (should_arm || should_always_enable_pwm);
+
+	/* check any conditions that prevent arming */
+	if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) {
+		needs_to_arm = false;
+	}
+	if (!should_arm && !should_always_enable_pwm) {
+		needs_to_arm = false;
+	}
 
-	if (should_arm && !mixer_servos_armed) {
+	if (needs_to_arm && !mixer_servos_armed) {
 		/* need to arm, but not armed */
 		up_pwm_servo_arm(true);
 		mixer_servos_armed = true;
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
+		isr_debug(5, "> PWM enabled");
 
-	} else if (!should_arm && mixer_servos_armed) {
+	} else if (!needs_to_arm && mixer_servos_armed) {
 		/* armed but need to disarm */
 		up_pwm_servo_arm(false);
 		mixer_servos_armed = false;
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
+		isr_debug(5, "> PWM disabled");
 	}
 
-	if (mixer_servos_armed) {
+	if (mixer_servos_armed && should_arm) {
 		/* update the servo outputs. */
-		for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
+		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
 			up_pwm_servo_set(i, r_page_servos[i]);
+
+	} else if (mixer_servos_armed && should_always_enable_pwm) {
+		/* set the disarmed servo outputs. */
+		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
+			up_pwm_servo_set(i, r_page_servo_disarmed[i]);
 	}
 }
 
@@ -205,27 +267,38 @@ mixer_callback(uintptr_t handle,
 	       uint8_t control_index,
 	       float &control)
 {
-	if (control_group != 0)
+	if (control_group > 3)
 		return -1;
 
 	switch (source) {
 	case MIX_FMU:
-		if (control_index < PX4IO_CONTROL_CHANNELS) {
-			control = REG_TO_FLOAT(r_page_controls[control_index]);
+		if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS ) {
+			control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
 			break;
 		}
 		return -1;
 
 	case MIX_OVERRIDE:
-		if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) {
+		if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
 			control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
 			break;
 		}
 		return -1;
 
+	case MIX_OVERRIDE_FMU_OK:
+		/* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */
+		if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
+			control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
+			break;
+		} else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) {
+			control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
+			break;
+		}
+		return -1;
+
 	case MIX_FAILSAFE:
 	case MIX_NONE:
-		/* XXX we could allow for configuration of per-output failsafe values */
+		control = 0.0f;
 		return -1;
 	}
 
@@ -241,13 +314,17 @@ mixer_callback(uintptr_t handle,
 static char mixer_text[256];		/* large enough for one mixer */
 static unsigned mixer_text_length = 0;
 
-void
+int
 mixer_handle_text(const void *buffer, size_t length)
 {
-	/* do not allow a mixer change while fully armed */
-	if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
-	    /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
-		return;
+	/* do not allow a mixer change while safety off */
+	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
+		return 1;
+	}
+
+	/* abort if we're in the mixer */
+	if (in_mixer) {
+		return 1;
 	}
 
 	px4io_mixdata	*msg = (px4io_mixdata *)buffer;
@@ -255,7 +332,7 @@ mixer_handle_text(const void *buffer, size_t length)
 	isr_debug(2, "mix txt %u", length);
 
 	if (length < sizeof(px4io_mixdata))
-		return;
+		return 0;
 
 	unsigned	text_length = length - sizeof(px4io_mixdata);
 
@@ -273,14 +350,16 @@ mixer_handle_text(const void *buffer, size_t length)
 	case F2I_MIXER_ACTION_APPEND:
 		isr_debug(2, "append %d", length);
 
-		/* check for overflow - this is really fatal */
-		/* XXX could add just what will fit & try to parse, then repeat... */
+		/* disable mixing during the update */
+		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
+
+		/* check for overflow - this would be really fatal */
 		if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
 			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
-			return;
+			return 0;
 		}
 
-		/* append mixer text and nul-terminate */
+		/* append mixer text and nul-terminate, guard against overflow */
 		memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
 		mixer_text_length += text_length;
 		mixer_text[mixer_text_length] = '\0';
@@ -293,8 +372,13 @@ mixer_handle_text(const void *buffer, size_t length)
 		/* if anything was parsed */
 		if (resid != mixer_text_length) {
 
-			/* ideally, this should test resid == 0 ? */
-			r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
+			/* only set mixer ok if no residual is left over */
+			if (resid == 0) {
+				r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
+			} else {
+				/* not yet reached the end of the mixer, set as not ok */
+				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
+			}
 
 			isr_debug(2, "used %u", mixer_text_length - resid);
 
@@ -303,8 +387,46 @@ mixer_handle_text(const void *buffer, size_t length)
 				memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
 
 			mixer_text_length = resid;
+
+			/* update failsafe values */
+			mixer_set_failsafe();
 		}
 
 		break;
 	}
+
+	return 0;
+}
+
+static void
+mixer_set_failsafe()
+{
+	/* 
+	 * Check if a custom failsafe value has been written,
+	 * or if the mixer is not ok and bail out.
+	 */
+
+	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ||
+		!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
+		return;
+
+	/* set failsafe defaults to the values for all inputs = 0 */
+	float	outputs[PX4IO_SERVO_COUNT];
+	unsigned mixed;
+
+	/* mix */
+	mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
+
+	/* scale to PWM and update the servo outputs as required */
+	for (unsigned i = 0; i < mixed; i++) {
+
+		/* scale to servo output */
+		r_page_servo_failsafe[i] = (outputs[i] * 600.0f) + 1500;
+
+	}
+
+	/* disable the rest of the outputs */
+	for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
+		r_page_servo_failsafe[i] = 0;
+
 }
diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk
index 6379366f4..01869569f 100644
--- a/src/modules/px4iofirmware/module.mk
+++ b/src/modules/px4iofirmware/module.mk
@@ -3,17 +3,23 @@
 SRCS		= adc.c \
 		  controls.c \
 		  dsm.c \
-		  i2c.c \
 		  px4io.c \
 		  registers.c \
 		  safety.c \
 		  sbus.c \
 		  ../systemlib/up_cxxinitialize.c \
-		  ../systemlib/hx_stream.c \
 		  ../systemlib/perf_counter.c \
 		  mixer.cpp \
 		  ../systemlib/mixer/mixer.cpp \
 		  ../systemlib/mixer/mixer_group.cpp \
 		  ../systemlib/mixer/mixer_multirotor.cpp \
 		  ../systemlib/mixer/mixer_simple.cpp \
-		  
\ No newline at end of file
+		  ../systemlib/pwm_limit/pwm_limit.c
+
+ifeq ($(BOARD),px4io-v1)
+SRCS		+= i2c.c
+endif
+ifeq ($(BOARD),px4io-v2)
+SRCS		+= serial.c \
+		   ../systemlib/hx_stream.c
+endif
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index e575bd841..d48c6c529 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.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
@@ -36,7 +36,7 @@
 /**
  * @file protocol.h
  *
- * PX4IO I2C interface protocol.
+ * PX4IO interface protocol.
  *
  * Communication is performed via writes to and reads from 16-bit virtual
  * registers organised into pages of 255 registers each.
@@ -45,7 +45,7 @@
  * respectively. Subsequent reads and writes increment the offset within
  * the page. 
  *
- * Most pages are readable or writable but not both.
+ * Some pages are read- or write-only.
  *
  * Note that some pages may permit offset values greater than 255, which
  * can only be achieved by long writes. The offset does not wrap.
@@ -62,12 +62,11 @@
  * Note that the implementation of readable pages prefers registers within
  * readable pages to be densely packed. Page numbers do not need to be
  * packed.
+ *
+ * Definitions marked [1] are only valid on PX4IOv1 boards. Likewise, 
+ * [2] denotes definitions specific to the PX4IOv2 board.
  */
 
-#define PX4IO_CONTROL_CHANNELS			8
-#define PX4IO_INPUT_CHANNELS			12
-#define PX4IO_RELAY_CHANNELS			4
-
 /* Per C, this is safe for all 2's complement systems */
 #define REG_TO_SIGNED(_reg)	((int16_t)(_reg))
 #define SIGNED_TO_REG(_signed)	((uint16_t)(_signed))
@@ -75,17 +74,23 @@
 #define REG_TO_FLOAT(_reg)	((float)REG_TO_SIGNED(_reg) / 10000.0f)
 #define FLOAT_TO_REG(_float)	SIGNED_TO_REG((int16_t)((_float) * 10000.0f))
 
+#define PX4IO_PROTOCOL_VERSION		4
+
+/* maximum allowable sizes on this protocol version */
+#define PX4IO_PROTOCOL_MAX_CONTROL_COUNT	8	/**< The protocol does not support more than set here, individual units might support less - see PX4IO_P_CONFIG_CONTROL_COUNT */
+
 /* static configuration page */
 #define PX4IO_PAGE_CONFIG		0
-#define PX4IO_P_CONFIG_PROTOCOL_VERSION		0	/* magic numbers TBD */
-#define PX4IO_P_CONFIG_SOFTWARE_VERSION		1	/* magic numbers TBD */
+#define PX4IO_P_CONFIG_PROTOCOL_VERSION		0	/* PX4IO_PROTOCOL_VERSION */
+#define PX4IO_P_CONFIG_HARDWARE_VERSION		1	/* magic numbers TBD */
 #define PX4IO_P_CONFIG_BOOTLOADER_VERSION	2	/* get this how? */
 #define PX4IO_P_CONFIG_MAX_TRANSFER		3	/* maximum I2C transfer size */
 #define PX4IO_P_CONFIG_CONTROL_COUNT		4	/* hardcoded max control count supported */
 #define PX4IO_P_CONFIG_ACTUATOR_COUNT		5	/* hardcoded max actuator output count */
 #define PX4IO_P_CONFIG_RC_INPUT_COUNT		6	/* hardcoded max R/C input count supported */
 #define PX4IO_P_CONFIG_ADC_INPUT_COUNT		7	/* hardcoded max ADC inputs */
-#define PX4IO_P_CONFIG_RELAY_COUNT		8	/* harcoded # of relay outputs */
+#define PX4IO_P_CONFIG_RELAY_COUNT		8	/* hardcoded # of relay outputs */
+#define PX4IO_P_CONFIG_CONTROL_GROUP_COUNT	8	/**< hardcoded # of control groups*/
 
 /* dynamic status page */
 #define PX4IO_PAGE_STATUS		1
@@ -93,7 +98,7 @@
 #define PX4IO_P_STATUS_CPULOAD			1
 
 #define PX4IO_P_STATUS_FLAGS			2	 /* monitoring flags */
-#define PX4IO_P_STATUS_FLAGS_ARMED		(1 << 0) /* arm-ok and locally armed */
+#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED	(1 << 0) /* arm-ok and locally armed */
 #define PX4IO_P_STATUS_FLAGS_OVERRIDE		(1 << 1) /* in manual override */
 #define PX4IO_P_STATUS_FLAGS_RC_OK		(1 << 2) /* RC input is valid */
 #define PX4IO_P_STATUS_FLAGS_RC_PPM		(1 << 3) /* PPM input is valid */
@@ -104,18 +109,24 @@
 #define PX4IO_P_STATUS_FLAGS_MIXER_OK		(1 << 8) /* mixer is OK */
 #define PX4IO_P_STATUS_FLAGS_ARM_SYNC		(1 << 9) /* the arming state between IO and FMU is in sync */
 #define PX4IO_P_STATUS_FLAGS_INIT_OK		(1 << 10) /* initialisation of the IO completed without error */
+#define PX4IO_P_STATUS_FLAGS_FAILSAFE		(1 << 11) /* failsafe is active */
+#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF		(1 << 12) /* safety is off */
 
 #define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
-#define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* VBatt is very close to regulator dropout */
+#define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* [1] VBatt is very close to regulator dropout */
 #define PX4IO_P_STATUS_ALARMS_TEMPERATURE	(1 << 1) /* board temperature is high */
-#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT	(1 << 2) /* servo current limit was exceeded */
-#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT	(1 << 3) /* accessory current limit was exceeded */
+#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT	(1 << 2) /* [1] servo current limit was exceeded */
+#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT	(1 << 3) /* [1] accessory current limit was exceeded */
 #define PX4IO_P_STATUS_ALARMS_FMU_LOST		(1 << 4) /* timed out waiting for controls from FMU */
 #define PX4IO_P_STATUS_ALARMS_RC_LOST		(1 << 5) /* timed out waiting for RC input */
 #define PX4IO_P_STATUS_ALARMS_PWM_ERROR		(1 << 6) /* PWM configuration or output was bad */
+#define PX4IO_P_STATUS_ALARMS_VSERVO_FAULT	(1 << 7) /* [2] VServo was out of the valid range (2.5 - 5.5 V) */
 
-#define PX4IO_P_STATUS_VBATT			4	/* battery voltage in mV */
-#define PX4IO_P_STATUS_IBATT			5	/* battery current (raw ADC) */
+#define PX4IO_P_STATUS_VBATT			4	/* [1] battery voltage in mV */
+#define PX4IO_P_STATUS_IBATT			5	/* [1] battery current (raw ADC) */
+#define PX4IO_P_STATUS_VSERVO			6	/* [2] servo rail voltage in mV */
+#define PX4IO_P_STATUS_VRSSI			7	/* [2] RSSI voltage */
+#define PX4IO_P_STATUS_PRSSI			8	/* [2] RSSI PWM value */
 
 /* array of post-mix actuator outputs, -10000..10000 */
 #define PX4IO_PAGE_ACTUATORS		2		/* 0..CONFIG_ACTUATOR_COUNT-1 */
@@ -126,7 +137,17 @@
 /* array of raw RC input values, microseconds */
 #define PX4IO_PAGE_RAW_RC_INPUT		4
 #define PX4IO_P_RAW_RC_COUNT			0	/* number of valid channels */
-#define PX4IO_P_RAW_RC_BASE			1	/* CONFIG_RC_INPUT_COUNT channels from here */
+#define PX4IO_P_RAW_RC_FLAGS			1	/* RC detail status flags */
+#define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP		(1 << 0) /* single frame drop */
+#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_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) */
+#define PX4IO_P_RAW_FRAME_COUNT			4	/* Number of total received frames (wrapping counter) */
+#define PX4IO_P_RAW_LOST_FRAME_COUNT		5	/* Number of total dropped frames (wrapping counter) */
+#define PX4IO_P_RAW_RC_BASE			6	/* CONFIG_RC_INPUT_COUNT channels from here */
 
 /* array of scaled RC input values, -10000..10000 */
 #define PX4IO_PAGE_RC_INPUT		5
@@ -141,45 +162,99 @@
 #define PX4IO_RATE_MAP_BASE			0	/* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */
 
 /* setup page */
-#define PX4IO_PAGE_SETUP			100
+#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_ARMING			1	 /* arming controls */
 #define PX4IO_P_SETUP_ARMING_IO_ARM_OK		(1 << 0) /* OK to arm the IO side */
 #define PX4IO_P_SETUP_ARMING_FMU_ARMED		(1 << 1) /* FMU is already armed */
 #define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK	(1 << 2) /* OK to switch to manual override via override RC channel */
+#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM	(1 << 3) /* use custom failsafe values, not 0 values of mixer */
 #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK	(1 << 4) /* OK to try in-air restart */
+#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE	(1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */
+#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED	(1 << 6) /* Disable the IO-internal evaluation of the RC */
+#define PX4IO_P_SETUP_ARMING_LOCKDOWN		(1 << 7) /* If set, the system operates normally, but won't actuate any servos */
 
 #define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
 #define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */
 #define PX4IO_P_SETUP_PWM_ALTRATE		4	/* 'high' PWM frame output rate in Hz */
+
+#if defined(CONFIG_ARCH_BOARD_PX4IO_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 #define PX4IO_P_SETUP_RELAYS			5	/* bitmask of relay/switch outputs, 0 = off, 1 = on */
-#define PX4IO_P_SETUP_VBATT_SCALE		6	/* battery voltage correction factor (float) */
-#define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */
+#define PX4IO_P_SETUP_RELAYS_POWER1		(1<<0)	/* hardware rev [1] power relay 1 */
+#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 */
+#endif
+
+#define PX4IO_P_SETUP_VBATT_SCALE		6	/* hardware rev [1] battery voltage correction factor (float) */
+#define PX4IO_P_SETUP_VSERVO_SCALE		6	/* hardware rev [2] servo voltage correction factor (float) */
+#define PX4IO_P_SETUP_DSM			7	/* DSM bind state */
+enum {							/* DSM bind states */
+	dsm_bind_power_down = 0,
+	dsm_bind_power_up,
+	dsm_bind_set_rx_out,
+	dsm_bind_send_pulses,
+	dsm_bind_reinit_uart
+};
+ 					     /*	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_CRC		       11	/* get CRC of IO firmware */
 
 /* autopilot control values, -10000..10000 */
-#define PX4IO_PAGE_CONTROLS			101	/* 0..CONFIG_CONTROL_COUNT */
+#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 */
 
 /* raw text load to the mixer parser - ignores offset */
-#define PX4IO_PAGE_MIXERLOAD			102
+#define PX4IO_PAGE_MIXERLOAD			52
 
 /* R/C channel config */
-#define PX4IO_PAGE_RC_CONFIG			103	/* R/C input configuration */
-#define PX4IO_P_RC_CONFIG_MIN			0	/* lowest input value */
-#define PX4IO_P_RC_CONFIG_CENTER		1	/* center input value */
-#define PX4IO_P_RC_CONFIG_MAX			2	/* highest input value */
-#define PX4IO_P_RC_CONFIG_DEADZONE		3	/* band around center that is ignored */
-#define PX4IO_P_RC_CONFIG_ASSIGNMENT		4	/* mapped input value */
-#define PX4IO_P_RC_CONFIG_OPTIONS		5	/* channel options bitmask */
+#define PX4IO_PAGE_RC_CONFIG			53		/**< R/C input configuration */
+#define PX4IO_P_RC_CONFIG_MIN			0		/**< lowest input value */
+#define PX4IO_P_RC_CONFIG_CENTER		1		/**< center input value */
+#define PX4IO_P_RC_CONFIG_MAX			2		/**< highest input value */
+#define PX4IO_P_RC_CONFIG_DEADZONE		3		/**< band around center that is ignored */
+#define PX4IO_P_RC_CONFIG_ASSIGNMENT		4		/**< mapped input value */
+#define PX4IO_P_RC_CONFIG_OPTIONS		5		/**< channel options bitmask */
 #define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED	(1 << 0)
 #define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE	(1 << 1)
-#define PX4IO_P_RC_CONFIG_STRIDE		6	/* spacing between channel config data */
+#define PX4IO_P_RC_CONFIG_STRIDE		6		/**< spacing between channel config data */
 
 /* PWM output - overrides mixer */
-#define PX4IO_PAGE_DIRECT_PWM			104	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_DIRECT_PWM			54		/**< 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /* PWM failsafe values - zero disables the output */
-#define PX4IO_PAGE_FAILSAFE_PWM			105	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_FAILSAFE_PWM			55		/**< 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* Debug and test page - not used in normal operation */
+#define PX4IO_PAGE_TEST				127
+#define PX4IO_P_TEST_LED			0		/**< set the amber LED on/off */
+
+/* PWM minimum values for certain ESCs */
+#define PX4IO_PAGE_CONTROL_MIN_PWM		106		/**< 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* PWM maximum values for certain ESCs */
+#define PX4IO_PAGE_CONTROL_MAX_PWM		107		/**< 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* PWM disarmed values that are active, even when SAFETY_SAFE */
+#define PX4IO_PAGE_DISARMED_PWM		108			/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /**
  * As-needed mixer data upload.
@@ -200,3 +275,81 @@ struct px4io_mixdata {
 };
 #pragma pack(pop)
 
+/**
+ * Serial protocol encapsulation.
+ */
+
+#define PKT_MAX_REGS	32 // by agreement w/FMU
+
+#pragma pack(push, 1)
+struct IOPacket {
+	uint8_t 	count_code;
+	uint8_t 	crc;
+	uint8_t 	page;
+	uint8_t 	offset;
+	uint16_t	regs[PKT_MAX_REGS];
+};
+#pragma pack(pop)
+
+#define PKT_CODE_READ		0x00	/* FMU->IO read transaction */
+#define PKT_CODE_WRITE		0x40	/* FMU->IO write transaction */
+#define PKT_CODE_SUCCESS	0x00	/* IO->FMU success reply */
+#define PKT_CODE_CORRUPT	0x40	/* IO->FMU bad packet reply */
+#define PKT_CODE_ERROR		0x80	/* IO->FMU register op error reply */
+
+#define PKT_CODE_MASK		0xc0
+#define PKT_COUNT_MASK		0x3f
+
+#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)))
+
+static const uint8_t crc8_tab[256] __attribute__((unused)) =
+{
+	0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15,
+	0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D,
+	0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65,
+	0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D,
+	0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5,
+	0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD,
+	0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85,
+	0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD,
+	0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2,
+	0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA,
+	0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2,
+	0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A,
+	0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32,
+	0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A,
+	0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42,
+	0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A,
+	0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C,
+	0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4,
+	0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC,
+	0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4,
+	0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C,
+	0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44,
+	0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C,
+	0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34,
+	0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B,
+	0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63,
+	0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B,
+	0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13,
+	0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB,
+	0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83,
+	0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB,
+	0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3
+};
+
+static uint8_t crc_packet(struct IOPacket *pkt) __attribute__((unused));
+static uint8_t
+crc_packet(struct IOPacket *pkt)
+{
+	uint8_t *end = (uint8_t *)(&pkt->regs[PKT_COUNT(*pkt)]);
+	uint8_t *p = (uint8_t *)pkt;
+	uint8_t c = 0;
+
+	while (p < end)
+		c = crc8_tab[c ^ *(p++)];
+
+	return c;
+}
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index bc8dfc116..d4c25911e 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -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
@@ -45,11 +45,13 @@
 #include <string.h>
 #include <poll.h>
 #include <signal.h>
+#include <crc32.h>
 
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_hrt.h>
 
 #include <systemlib/perf_counter.h>
+#include <systemlib/pwm_limit/pwm_limit.h>
 
 #include <stm32_uart.h>
 
@@ -64,10 +66,7 @@ struct sys_state_s 	system_state;
 
 static struct hrt_call serial_dma_call;
 
-#ifdef CONFIG_STM32_I2C1
-/* store i2c reset count XXX this should be a register, together with other error counters */
-volatile uint32_t i2c_loop_resets = 0;
-#endif
+pwm_limit_t pwm_limit;
 
 /*
  * a set of debug buffers to allow us to send debug information from ISRs
@@ -119,6 +118,48 @@ show_debug_messages(void)
 	}
 }
 
+static void
+heartbeat_blink(void)
+{
+	static bool heartbeat = false;
+	LED_BLUE(heartbeat = !heartbeat);
+}
+
+static uint64_t reboot_time;
+
+/**
+   schedule a reboot in time_delta_usec microseconds
+ */
+void schedule_reboot(uint32_t time_delta_usec)
+{
+    reboot_time = hrt_absolute_time() + time_delta_usec;
+}
+
+/**
+   check for a scheduled reboot
+ */
+static void check_reboot(void)
+{
+    if (reboot_time != 0 && hrt_absolute_time() > reboot_time) {
+        up_systemreset();
+    }
+}
+
+static void
+calculate_fw_crc(void)
+{
+#define APP_SIZE_MAX 0xf000
+#define APP_LOAD_ADDRESS 0x08001000
+	// compute CRC of the current firmware
+	uint32_t sum = 0;
+	for (unsigned p = 0; p < APP_SIZE_MAX; p += 4) {
+		uint32_t bytes = *(uint32_t *)(p + APP_LOAD_ADDRESS);
+		sum = crc32part((uint8_t *)&bytes, sizeof(bytes), sum);
+	}
+	r_page_setup[PX4IO_P_SETUP_CRC]   = sum & 0xFFFF;
+	r_page_setup[PX4IO_P_SETUP_CRC+1] = sum >> 16;
+}
+
 int
 user_start(int argc, char *argv[])
 {
@@ -131,6 +172,9 @@ user_start(int argc, char *argv[])
 	/* configure the high-resolution time/callout interface */
 	hrt_init();
 
+	/* calculate our fw CRC so FMU can decide if we need to update */
+	calculate_fw_crc();
+
 	/*
 	 * Poll at 1ms intervals for received bytes that have not triggered
 	 * a DMA event.
@@ -147,8 +191,15 @@ user_start(int argc, char *argv[])
 	LED_BLUE(false);
 	LED_SAFETY(false);
 
-	/* turn on servo power */
+	/* turn on servo power (if supported) */
+#ifdef POWER_SERVO
 	POWER_SERVO(true);
+#endif
+
+	/* turn off S.Bus out (if supported) */
+#ifdef ENABLE_SBUS_OUT
+	ENABLE_SBUS_OUT(false);
+#endif
 
 	/* start the safety switch handler */
 	safety_init();
@@ -159,10 +210,11 @@ user_start(int argc, char *argv[])
 	/* initialise the control inputs */
 	controls_init();
 
-#ifdef CONFIG_STM32_I2C1
-	/* start the i2c handler */
-	i2c_init();
-#endif
+	/* set up the ADC */
+	adc_init();
+
+	/* start the FMU interface */
+	interface_init();
 
 	/* add a performance counter for mixing */
 	perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
@@ -176,30 +228,51 @@ user_start(int argc, char *argv[])
 	struct mallinfo minfo = mallinfo();
 	lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);
 
-#if 0
-	/* not enough memory, lock down */
-	if (minfo.mxordblk < 500) {
+	/* initialize PWM limit lib */
+	pwm_limit_init(&pwm_limit);
+
+	/*
+	 *    P O L I C E    L I G H T S
+	 *
+	 * Not enough memory, lock down.
+	 *
+	 * We might need to allocate mixers later, and this will
+	 * ensure that a developer doing a change will notice
+	 * that he just burned the remaining RAM with static
+	 * allocations. We don't want him to be able to
+	 * get past that point. This needs to be clearly
+	 * documented in the dev guide.
+         *
+	 */
+	if (minfo.mxordblk < 600) {
+
 		lowsyslog("ERR: not enough MEM");
 		bool phase = false;
 
-		if (phase) {
-			LED_AMBER(true);
-			LED_BLUE(false);
-		} else {
-			LED_AMBER(false);
-			LED_BLUE(true);
-		}
+		while (true) {
 
-		phase = !phase;
-		usleep(300000);
+			if (phase) {
+				LED_AMBER(true);
+				LED_BLUE(false);
+			} else {
+				LED_AMBER(false);
+				LED_BLUE(true);
+			}
+			up_udelay(250000);
+
+			phase = !phase;
+		}
 	}
-#endif
+
+	/* Start the failsafe led init */
+	failsafe_led_init();
 
 	/*
 	 * Run everything in a tight loop.
 	 */
 
 	uint64_t last_debug_time = 0;
+        uint64_t last_heartbeat_time = 0;
 	for (;;) {
 
 		/* track the rate at which the loop is running */
@@ -215,20 +288,28 @@ user_start(int argc, char *argv[])
 		controls_tick();
 		perf_end(controls_perf);
 
-		/* check for debug activity */
+                if ((hrt_absolute_time() - last_heartbeat_time) > 250*1000) {
+                    last_heartbeat_time = hrt_absolute_time();
+                    heartbeat_blink();
+                }
+
+                check_reboot();
+
+		/* check for debug activity (default: none) */
 		show_debug_messages();
 
-		/* post debug state at ~1Hz */
+		/* post debug state at ~1Hz - this is via an auxiliary serial port
+		 * DEFAULTS TO OFF!
+		 */
 		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 r=%u m=%u", 
+			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)i2c_loop_resets,
 				  (unsigned)minfo.mxordblk);
 			last_debug_time = hrt_absolute_time();
 		}
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 272cdb7bf..bb224f388 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -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
@@ -42,15 +42,20 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-#include <drivers/boards/px4io/px4io_internal.h>
+#include <board_config.h>
 
 #include "protocol.h"
 
+#include <systemlib/pwm_limit/pwm_limit.h>
+
 /*
  * Constants and limits.
  */
-#define MAX_CONTROL_CHANNELS	12
-#define IO_SERVO_COUNT		8
+#define PX4IO_SERVO_COUNT		8
+#define PX4IO_CONTROL_CHANNELS		8
+#define PX4IO_CONTROL_GROUPS		2
+#define PX4IO_RC_INPUT_CHANNELS		18
+#define PX4IO_RC_MAPPED_CONTROL_CHANNELS		8 /**< This is the maximum number of channels mapped/used */
 
 /*
  * Debug logging
@@ -77,6 +82,9 @@ extern volatile uint16_t	r_page_setup[];		/* PX4IO_PAGE_SETUP */
 extern volatile uint16_t	r_page_controls[];	/* PX4IO_PAGE_CONTROLS */
 extern uint16_t			r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */
 extern uint16_t			r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
+extern uint16_t			r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */
+extern uint16_t			r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */
+extern uint16_t			r_page_servo_disarmed[];	/* PX4IO_PAGE_DISARMED_PWM */
 
 /*
  * Register aliases.
@@ -88,15 +96,18 @@ extern uint16_t			r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
 
 #define r_raw_rc_count		r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT]
 #define r_raw_rc_values		(&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE])
+#define r_raw_rc_flags		r_page_raw_rc_input[PX4IO_P_RAW_RC_FLAGS]
 #define r_rc_valid		r_page_rc_input[PX4IO_P_RC_VALID]
-#define r_rc_values		(&r_page_rc_input[PX4IO_P_RAW_RC_BASE])
+#define r_rc_values		(&r_page_rc_input[PX4IO_P_RC_BASE])
 
 #define r_setup_features	r_page_setup[PX4IO_P_SETUP_FEATURES]
 #define r_setup_arming		r_page_setup[PX4IO_P_SETUP_ARMING]
 #define r_setup_pwm_rates	r_page_setup[PX4IO_P_SETUP_PWM_RATES]
 #define r_setup_pwm_defaultrate	r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE]
 #define r_setup_pwm_altrate	r_page_setup[PX4IO_P_SETUP_PWM_ALTRATE]
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 #define r_setup_relays		r_page_setup[PX4IO_P_SETUP_RELAYS]
+#endif
 
 #define r_control_values	(&r_page_controls[0])
 
@@ -105,7 +116,8 @@ extern uint16_t			r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
  */
 struct sys_state_s {
 
-	volatile uint64_t	rc_channels_timestamp;
+	volatile uint64_t	rc_channels_timestamp_received;
+	volatile uint64_t	rc_channels_timestamp_valid;
 
 	/**
 	 * Last FMU receive time, in microseconds since system boot
@@ -117,56 +129,75 @@ struct sys_state_s {
 extern struct sys_state_s system_state;
 
 /*
+ * PWM limit structure
+ */
+extern pwm_limit_t pwm_limit;
+
+/*
  * GPIO handling.
  */
-#define LED_BLUE(_s)		stm32_gpiowrite(GPIO_LED1, !(_s))
-#define LED_AMBER(_s)		stm32_gpiowrite(GPIO_LED2, !(_s))
-#define LED_SAFETY(_s)		stm32_gpiowrite(GPIO_LED3, !(_s))
+#define LED_BLUE(_s)			stm32_gpiowrite(GPIO_LED1, !(_s))
+#define LED_AMBER(_s)			stm32_gpiowrite(GPIO_LED2, !(_s))
+#define LED_SAFETY(_s)			stm32_gpiowrite(GPIO_LED3, !(_s))
+
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
+
+# define PX4IO_RELAY_CHANNELS		4
+# define POWER_SERVO(_s)		stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s))
+# define POWER_ACC1(_s)			stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
+# define POWER_ACC2(_s)			stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
+# define POWER_RELAY1(_s)		stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
+# define POWER_RELAY2(_s)		stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
+
+# define OVERCURRENT_ACC		(!stm32_gpioread(GPIO_ACC_OC_DETECT))
+# define OVERCURRENT_SERVO		(!stm32_gpioread(GPIO_SERVO_OC_DETECT))
+
+# define PX4IO_ADC_CHANNEL_COUNT	2
+# define ADC_VBATT			4
+# define ADC_IN5			5
 
-#define POWER_SERVO(_s)		stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s))
-#ifdef GPIO_ACC1_PWR_EN
-    #define POWER_ACC1(_s)		stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
-#endif
-#ifdef GPIO_ACC2_PWR_EN
-    #define POWER_ACC2(_s)		stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
-#endif
-#ifdef GPIO_RELAY1_EN
-    #define POWER_RELAY1(_s)	stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
 #endif
-#ifdef GPIO_RELAY2_EN
-    #define POWER_RELAY2(_s)	stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
+
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+
+# define PX4IO_RELAY_CHANNELS		0
+# define POWER_SPEKTRUM(_s)		stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s))
+# define ENABLE_SBUS_OUT(_s)		stm32_gpiowrite(GPIO_SBUS_OENABLE, !(_s))
+
+# define VDD_SERVO_FAULT		(!stm32_gpioread(GPIO_SERVO_FAULT_DETECT))
+
+# define PX4IO_ADC_CHANNEL_COUNT	2
+# define ADC_VSERVO			4
+# define ADC_RSSI			5
+
 #endif
 
-#define OVERCURRENT_ACC		(!stm32_gpioread(GPIO_ACC_OC_DETECT))
-#define OVERCURRENT_SERVO	(!stm32_gpioread(GPIO_SERVO_OC_DETECT))
 #define BUTTON_SAFETY		stm32_gpioread(GPIO_BTN_SAFETY)
 
-#define ADC_VBATT		4
-#define ADC_IN5			5
-#define ADC_CHANNEL_COUNT	2
+#define CONTROL_PAGE_INDEX(_group, _channel) (_group * PX4IO_CONTROL_CHANNELS + _channel)
 
 /*
  * Mixer
  */
 extern void	mixer_tick(void);
-extern void	mixer_handle_text(const void *buffer, size_t length);
+extern int	mixer_handle_text(const void *buffer, size_t length);
 
 /**
  * Safety switch/LED.
  */
 extern void	safety_init(void);
+extern void	failsafe_led_init(void);
 
-#ifdef CONFIG_STM32_I2C1
 /**
  * FMU communications
  */
-extern void	i2c_init(void);
-#endif
+extern void	interface_init(void);
+extern void	interface_tick(void);
 
 /**
  * Register space
  */
-extern void	registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
+extern int	registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
 extern int	registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values);
 
 /**
@@ -184,16 +215,16 @@ extern void	controls_init(void);
 extern void	controls_tick(void);
 extern int	dsm_init(const char *device);
 extern bool	dsm_input(uint16_t *values, uint16_t *num_values);
+extern void	dsm_bind(uint16_t cmd, int pulses);
 extern int	sbus_init(const char *device);
-extern bool	sbus_input(uint16_t *values, uint16_t *num_values);
+extern bool	sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
 
 /** global debug level for isr_debug() */
 extern volatile uint8_t debug_level;
 
-/* send a debug message to the console */
-extern void isr_debug(uint8_t level, const char *fmt, ...);
+/** send a debug message to the console */
+extern void	isr_debug(uint8_t level, const char *fmt, ...);
+
+/** schedule a reboot */
+extern void schedule_reboot(uint32_t time_delta_usec);
 
-#ifdef CONFIG_STM32_I2C1
-void i2c_dump(void);
-void i2c_reset(void);
-#endif
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 9698a0f2f..97d25bbfa 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -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
@@ -41,8 +41,12 @@
 
 #include <stdbool.h>
 #include <stdlib.h>
+#include <string.h>
 
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_pwm_output.h>
+#include <systemlib/systemlib.h>
+#include <stm32_pwr.h>
 
 #include "px4io.h"
 #include "protocol.h"
@@ -56,14 +60,18 @@ static void	pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t alt
  * Static configuration parameters.
  */
 static const uint16_t	r_page_config[] = {
-	[PX4IO_P_CONFIG_PROTOCOL_VERSION]	= 1,	/* XXX hardcoded magic number */
-	[PX4IO_P_CONFIG_SOFTWARE_VERSION]	= 1,	/* XXX hardcoded magic number */
+	[PX4IO_P_CONFIG_PROTOCOL_VERSION]	= PX4IO_PROTOCOL_VERSION,
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+	[PX4IO_P_CONFIG_HARDWARE_VERSION]	= 2,
+#else
+	[PX4IO_P_CONFIG_HARDWARE_VERSION]	= 1,
+#endif
 	[PX4IO_P_CONFIG_BOOTLOADER_VERSION]	= 3,	/* XXX hardcoded magic number */
 	[PX4IO_P_CONFIG_MAX_TRANSFER]		= 64,	/* XXX hardcoded magic number */
 	[PX4IO_P_CONFIG_CONTROL_COUNT]		= PX4IO_CONTROL_CHANNELS,
-	[PX4IO_P_CONFIG_ACTUATOR_COUNT]		= IO_SERVO_COUNT,
-	[PX4IO_P_CONFIG_RC_INPUT_COUNT]		= MAX_CONTROL_CHANNELS,
-	[PX4IO_P_CONFIG_ADC_INPUT_COUNT]	= ADC_CHANNEL_COUNT,
+	[PX4IO_P_CONFIG_ACTUATOR_COUNT]		= PX4IO_SERVO_COUNT,
+	[PX4IO_P_CONFIG_RC_INPUT_COUNT]		= PX4IO_RC_INPUT_CHANNELS,
+	[PX4IO_P_CONFIG_ADC_INPUT_COUNT]	= PX4IO_ADC_CHANNEL_COUNT,
 	[PX4IO_P_CONFIG_RELAY_COUNT]		= PX4IO_RELAY_CHANNELS,
 };
 
@@ -78,7 +86,10 @@ uint16_t		r_page_status[] = {
 	[PX4IO_P_STATUS_FLAGS]			= 0,
 	[PX4IO_P_STATUS_ALARMS]			= 0,
 	[PX4IO_P_STATUS_VBATT]			= 0,
-	[PX4IO_P_STATUS_IBATT]			= 0
+	[PX4IO_P_STATUS_IBATT]			= 0,
+	[PX4IO_P_STATUS_VSERVO]			= 0,
+	[PX4IO_P_STATUS_VRSSI]			= 0,
+	[PX4IO_P_STATUS_PRSSI]			= 0,
 };
 
 /**
@@ -86,14 +97,14 @@ uint16_t		r_page_status[] = {
  *
  * Post-mixed actuator values.
  */
-uint16_t 		r_page_actuators[IO_SERVO_COUNT];
+uint16_t 		r_page_actuators[PX4IO_SERVO_COUNT];
 
 /**
  * PAGE 3
  *
  * Servo PWM values
  */
-uint16_t		r_page_servos[IO_SERVO_COUNT];
+uint16_t		r_page_servos[PX4IO_SERVO_COUNT];
 
 /**
  * PAGE 4
@@ -103,7 +114,13 @@ uint16_t		r_page_servos[IO_SERVO_COUNT];
 uint16_t		r_page_raw_rc_input[] =
 {
 	[PX4IO_P_RAW_RC_COUNT]			= 0,
-	[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + MAX_CONTROL_CHANNELS)] = 0
+	[PX4IO_P_RAW_RC_FLAGS]			= 0,
+	[PX4IO_P_RAW_RC_NRSSI]			= 0,
+	[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
 };
 
 /**
@@ -113,7 +130,7 @@ uint16_t		r_page_raw_rc_input[] =
  */
 uint16_t		r_page_rc_input[] = {
 	[PX4IO_P_RC_VALID]			= 0,
-	[PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + MAX_CONTROL_CHANNELS)] = 0
+	[PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_RC_MAPPED_CONTROL_CHANNELS)] = 0
 };
 
 /**
@@ -131,22 +148,46 @@ uint16_t		r_page_scratch[32];
  */
 volatile uint16_t	r_page_setup[] =
 {
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+	/* default to RSSI ADC functionality */
+	[PX4IO_P_SETUP_FEATURES]		= PX4IO_P_SETUP_FEATURES_ADC_RSSI,
+#else
 	[PX4IO_P_SETUP_FEATURES]		= 0,
+#endif
 	[PX4IO_P_SETUP_ARMING]			= 0,
 	[PX4IO_P_SETUP_PWM_RATES]		= 0,
 	[PX4IO_P_SETUP_PWM_DEFAULTRATE]		= 50,
 	[PX4IO_P_SETUP_PWM_ALTRATE]		= 200,
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 	[PX4IO_P_SETUP_RELAYS]			= 0,
+#endif
+#ifdef ADC_VSERVO
+	[PX4IO_P_SETUP_VSERVO_SCALE]		= 10000,
+#else
 	[PX4IO_P_SETUP_VBATT_SCALE]		= 10000,
+#endif
 	[PX4IO_P_SETUP_SET_DEBUG]		= 0,
+	[PX4IO_P_SETUP_REBOOT_BL]		= 0,
+	[PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
 };
 
-#define PX4IO_P_SETUP_FEATURES_VALID	(0)
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+#define PX4IO_P_SETUP_FEATURES_VALID	(PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \
+					 PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \
+					 PX4IO_P_SETUP_FEATURES_ADC_RSSI | \
+					 PX4IO_P_SETUP_FEATURES_PWM_RSSI)
+#else
+#define PX4IO_P_SETUP_FEATURES_VALID	0
+#endif
 #define PX4IO_P_SETUP_ARMING_VALID	(PX4IO_P_SETUP_ARMING_FMU_ARMED | \
 					 PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
 					 PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
-					 PX4IO_P_SETUP_ARMING_IO_ARM_OK)
-#define PX4IO_P_SETUP_RATES_VALID	((1 << IO_SERVO_COUNT) - 1)
+					 PX4IO_P_SETUP_ARMING_IO_ARM_OK | \
+					 PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
+					 PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
+					 PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
+					 PX4IO_P_SETUP_ARMING_LOCKDOWN)
+#define PX4IO_P_SETUP_RATES_VALID	((1 << PX4IO_SERVO_COUNT) - 1)
 #define PX4IO_P_SETUP_RELAYS_VALID	((1 << PX4IO_RELAY_CHANNELS) - 1)
 
 /**
@@ -154,7 +195,7 @@ volatile uint16_t	r_page_setup[] =
  *
  * Control values from the FMU.
  */
-volatile uint16_t	r_page_controls[PX4IO_CONTROL_CHANNELS];
+volatile uint16_t	r_page_controls[PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS];
 
 /*
  * PAGE 102 does not have a buffer.
@@ -165,7 +206,7 @@ volatile uint16_t	r_page_controls[PX4IO_CONTROL_CHANNELS];
  *
  * R/C channel input configuration.
  */
-uint16_t		r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
+uint16_t		r_page_rc_input_config[PX4IO_RC_INPUT_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
 
 /* valid options */
 #define PX4IO_P_RC_CONFIG_OPTIONS_VALID	(PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED)
@@ -178,10 +219,36 @@ uint16_t		r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE
  * PAGE 105
  *
  * Failsafe servo PWM values
+ * 
+ * Disable pulses as default.
  */
-uint16_t		r_page_servo_failsafe[IO_SERVO_COUNT];
+uint16_t		r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 };
 
-void
+/**
+ * PAGE 106
+ *
+ * minimum PWM values when armed
+ *
+ */
+uint16_t		r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN };
+
+/**
+ * PAGE 107
+ *
+ * maximum PWM values when armed
+ *
+ */
+uint16_t		r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX };
+
+/**
+ * PAGE 108
+ *
+ * disarmed PWM values for difficult ESCs
+ *
+ */
+uint16_t		r_page_servo_disarmed[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 };
+
+int
 registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
 {
 
@@ -191,7 +258,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 	case PX4IO_PAGE_CONTROLS:
 
 		/* copy channel data */
-		while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
+		while ((offset < PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
 
 			/* XXX range-check value? */
 			r_page_controls[offset] = *values;
@@ -230,20 +297,112 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 	case PX4IO_PAGE_FAILSAFE_PWM:
 
 		/* copy channel data */
-		while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
+		while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+			if (*values == 0) {
+				/* ignore 0 */
+			} else if (*values < PWM_LOWEST_MIN) {
+				r_page_servo_failsafe[offset] = PWM_LOWEST_MIN;
+			} else if (*values > PWM_HIGHEST_MAX) {
+				r_page_servo_failsafe[offset] = PWM_HIGHEST_MAX;
+			} else {
+				r_page_servo_failsafe[offset] = *values;
+			}
 
-			/* XXX range-check value? */
-			r_page_servo_failsafe[offset] = *values;
+			/* flag the failsafe values as custom */
+			r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM;
+
+			offset++;
+			num_values--;
+			values++;
+		}
+		break;
+
+	case PX4IO_PAGE_CONTROL_MIN_PWM:
+
+		/* copy channel data */
+		while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+			if (*values == 0) {
+				/* ignore 0 */
+			} else if (*values > PWM_HIGHEST_MIN) {
+				r_page_servo_control_min[offset] = PWM_HIGHEST_MIN;
+			} else if (*values < PWM_LOWEST_MIN) {
+				r_page_servo_control_min[offset] = PWM_LOWEST_MIN;
+			} else {
+				r_page_servo_control_min[offset] = *values;
+			}
 
 			offset++;
 			num_values--;
 			values++;
 		}
 		break;
+	
+	case PX4IO_PAGE_CONTROL_MAX_PWM:
+
+		/* copy channel data */
+		while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+			if (*values == 0) {
+				/* ignore 0 */
+			} else if (*values > PWM_HIGHEST_MAX) {
+				r_page_servo_control_max[offset] = PWM_HIGHEST_MAX;
+			} else if (*values < PWM_LOWEST_MAX) {
+				r_page_servo_control_max[offset] = PWM_LOWEST_MAX;
+			} else {
+				r_page_servo_control_max[offset] = *values;
+			}
+
+			offset++;
+			num_values--;
+			values++;
+		}
+		break;
+
+	case PX4IO_PAGE_DISARMED_PWM:
+		{
+			/* flag for all outputs */
+			bool all_disarmed_off = true;
+
+			/* copy channel data */
+			while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+				if (*values == 0) {
+					/* 0 means disabling always PWM */
+					r_page_servo_disarmed[offset] = 0;
+				} else if (*values < PWM_LOWEST_MIN) {
+					r_page_servo_disarmed[offset] = PWM_LOWEST_MIN;
+					all_disarmed_off = false;
+				} else if (*values > PWM_HIGHEST_MAX) {
+					r_page_servo_disarmed[offset] = PWM_HIGHEST_MAX;
+					all_disarmed_off = false;
+				} else {
+					r_page_servo_disarmed[offset] = *values;
+					all_disarmed_off = false;
+				}
+
+				offset++;
+				num_values--;
+				values++;
+			}
+
+			if (all_disarmed_off) {
+				/* disable PWM output if disarmed */
+				r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE);
+			} else {
+				/* enable PWM output always */
+				r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE;
+			}
+		}
+		break;
 
 		/* handle text going to the mixer parser */
 	case PX4IO_PAGE_MIXERLOAD:
-		mixer_handle_text(values, num_values * sizeof(*values));
+		if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
+				    (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
+			return mixer_handle_text(values, num_values * sizeof(*values));
+		}
 		break;
 
 	default:
@@ -254,11 +413,13 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 		/* iterate individual registers, set each in turn */
 		while (num_values--) {
 			if (registers_set_one(page, offset, *values))
-				break;
+				return -1;
 			offset++;
 			values++;
 		}
+		break;
 	}
+	return 0;
 }
 
 static int
@@ -294,9 +455,35 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		case PX4IO_P_SETUP_FEATURES:
 
 			value &= PX4IO_P_SETUP_FEATURES_VALID;
-			r_setup_features = value;
 
-			/* no implemented feature selection at this point */
+			/* some of the options conflict - give S.BUS out precedence, then ADC RSSI, then PWM RSSI */
+
+			/* switch S.Bus output pin as needed */
+			#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);
+			}
+			#endif
+
+			/* disable the conflicting options with ADC RSSI */
+			if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
+				value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
+					PX4IO_P_SETUP_FEATURES_SBUS1_OUT |
+					PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
+			}
+
+			/* disable the conflicting options with PWM RSSI (without effect here, but for completeness) */
+			if (value & (PX4IO_P_SETUP_FEATURES_PWM_RSSI)) {
+				value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI |
+					PX4IO_P_SETUP_FEATURES_SBUS1_OUT |
+					PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
+			}
+
+			/* apply changes */
+			r_setup_features = value;
 
 			break;
 
@@ -311,8 +498,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			 * so that an in-air reset of FMU can not lead to a
 			 * lockup of the IO arming state.
 			 */
-			if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
-				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+
+			if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) {
+				r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
 			}
 
 			r_setup_arming = value;
@@ -340,13 +528,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value);
 			break;
 
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 		case PX4IO_P_SETUP_RELAYS:
 			value &= PX4IO_P_SETUP_RELAYS_VALID;
 			r_setup_relays = value;
-			POWER_RELAY1(value & (1 << 0) ? 1 : 0);
-			POWER_RELAY2(value & (1 << 1) ? 1 : 0);
-			POWER_ACC1(value & (1 << 2) ? 1 : 0);
-			POWER_ACC2(value & (1 << 3) ? 1 : 0);
+			POWER_RELAY1((value & PX4IO_P_SETUP_RELAYS_POWER1) ? 1 : 0);
+			POWER_RELAY2((value & PX4IO_P_SETUP_RELAYS_POWER2) ? 1 : 0);
+			POWER_ACC1((value & PX4IO_P_SETUP_RELAYS_ACC1) ? 1 : 0);
+			POWER_ACC2((value & PX4IO_P_SETUP_RELAYS_ACC2) ? 1 : 0);
+			break;
+#endif
+
+		case PX4IO_P_SETUP_VBATT_SCALE:
+			r_page_setup[PX4IO_P_SETUP_VBATT_SCALE] = value;
 			break;
 
 		case PX4IO_P_SETUP_SET_DEBUG:
@@ -354,6 +548,27 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]);
 			break;
 
+		case PX4IO_P_SETUP_REBOOT_BL:
+			if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
+			    (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
+				// don't allow reboot while armed
+				break;
+			}
+
+			// check the magic value
+			if (value != PX4IO_REBOOT_BL_MAGIC)
+				break;
+
+                        // we schedule a reboot rather than rebooting
+                        // immediately to allow the IO board to ACK
+                        // the reboot command
+                        schedule_reboot(100000);
+			break;
+
+		case PX4IO_P_SETUP_DSM:
+			dsm_bind(value & 0x0f, (value >> 4) & 0xF);
+			break;
+
 		default:
 			return -1;
 		}
@@ -361,9 +576,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 
 	case PX4IO_PAGE_RC_CONFIG: {
 
-		/* do not allow a RC config change while fully armed */
-		if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
-		    /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+		/**
+		 * do not allow a RC config change while outputs armed
+		 */
+		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
+			    (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
 			break;
 		}
 
@@ -371,7 +588,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE;
 		uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE];
 
-		if (channel >= MAX_CONTROL_CHANNELS)
+		if (channel >= PX4IO_RC_INPUT_CHANNELS)
 			return -1;
 
 		/* disable the channel until we have a chance to sanity-check it */
@@ -391,6 +608,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID;
 			r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
 
+			/* clear any existing RC disabled flag */
+			r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED);
+
 			/* set all options except the enabled option */
 			conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
 
@@ -398,6 +618,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			/* this option is normally set last */
 			if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
 				uint8_t count = 0;
+				bool disabled = false;
 
 				/* assert min..center..max ordering */
 				if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) {
@@ -416,7 +637,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 				if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
 					count++;
 				}
-				if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) {
+
+				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) {
 					count++;
 				}
 
@@ -424,7 +648,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 				if (count) {
 					isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1));
 					r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK;
-				} else {
+				} else if (!disabled) {
 					conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
 				}
 			}
@@ -436,6 +660,14 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		/* case PX4IO_RC_PAGE_CONFIG */
 	}
 
+	case PX4IO_PAGE_TEST:
+		switch (offset) {
+		case PX4IO_P_TEST_LED:
+			LED_AMBER(value & 1);
+			break;
+		}
+		break;
+
 	default:
 		return -1;
 	}
@@ -472,6 +704,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 
 		/* PX4IO_P_STATUS_ALARMS maintained externally */
 
+#ifdef ADC_VBATT
 		/* PX4IO_P_STATUS_VBATT */
 		{
 			/*
@@ -505,7 +738,8 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 				r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
 			}
 		}
-
+#endif
+#ifdef ADC_IBATT
 		/* PX4IO_P_STATUS_IBATT */
 		{
 			/*
@@ -515,26 +749,60 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 			  FMU sort it out, with user selectable
 			  configuration for their sensor
 			 */
-			unsigned counts = adc_measure(ADC_IN5);
+			unsigned counts = adc_measure(ADC_IBATT);
 			if (counts != 0xffff) {
 				r_page_status[PX4IO_P_STATUS_IBATT] = counts;
 			}
 		}
+#endif
+#ifdef ADC_VSERVO
+		/* PX4IO_P_STATUS_VSERVO */
+		{
+			unsigned counts = adc_measure(ADC_VSERVO);
+			if (counts != 0xffff) {
+				// use 3:1 scaling on 3.3V ADC input
+				unsigned mV = counts * 9900 / 4096;
+				r_page_status[PX4IO_P_STATUS_VSERVO] = mV;
+			}
+		}
+#endif
+#ifdef ADC_RSSI
+		/* PX4IO_P_STATUS_VRSSI */
+		{
+			unsigned counts = adc_measure(ADC_RSSI);
+			if (counts != 0xffff) {
+				// use 1:1 scaling on 3.3V ADC input
+				unsigned mV = counts * 3300 / 4096;
+				r_page_status[PX4IO_P_STATUS_VRSSI] = mV;
+			}
+		}
+#endif
+		/* XXX PX4IO_P_STATUS_PRSSI */
 
 		SELECT_PAGE(r_page_status);
 		break;
 
 	case PX4IO_PAGE_RAW_ADC_INPUT:
 		memset(r_page_scratch, 0, sizeof(r_page_scratch));
+#ifdef ADC_VBATT
 		r_page_scratch[0] = adc_measure(ADC_VBATT);
-		r_page_scratch[1] = adc_measure(ADC_IN5);
-
+#endif
+#ifdef ADC_IBATT
+		r_page_scratch[1] = adc_measure(ADC_IBATT);
+#endif
+
+#ifdef ADC_VSERVO
+		r_page_scratch[0] = adc_measure(ADC_VSERVO);
+#endif
+#ifdef ADC_RSSI
+		r_page_scratch[1] = adc_measure(ADC_RSSI);
+#endif
 		SELECT_PAGE(r_page_scratch);
 		break;
 
 	case PX4IO_PAGE_PWM_INFO:
 		memset(r_page_scratch, 0, sizeof(r_page_scratch));
-		for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
+		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
 			r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i);
 
 		SELECT_PAGE(r_page_scratch);
@@ -577,6 +845,15 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 	case PX4IO_PAGE_FAILSAFE_PWM:
 		SELECT_PAGE(r_page_servo_failsafe);
 		break;
+	case PX4IO_PAGE_CONTROL_MIN_PWM:
+		SELECT_PAGE(r_page_servo_control_min);
+		break;
+	case PX4IO_PAGE_CONTROL_MAX_PWM:
+		SELECT_PAGE(r_page_servo_control_max);
+		break;
+	case PX4IO_PAGE_DISARMED_PWM:
+		SELECT_PAGE(r_page_servo_disarmed);
+		break;
 
 	default:
 		return -1;
@@ -606,7 +883,7 @@ static void
 pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate)
 {
 	for (unsigned pass = 0; pass < 2; pass++) {
-		for (unsigned group = 0; group < IO_SERVO_COUNT; group++) {
+		for (unsigned group = 0; group < PX4IO_SERVO_COUNT; group++) {
 
 			/* get the channel mask for this rate group */
 			uint32_t mask = up_pwm_servo_get_rate_group(group);
diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c
index 4dbecc274..ff2e4af6e 100644
--- a/src/modules/px4iofirmware/safety.c
+++ b/src/modules/px4iofirmware/safety.c
@@ -45,7 +45,6 @@
 #include "px4io.h"
 
 static struct hrt_call arming_call;
-static struct hrt_call heartbeat_call;
 static struct hrt_call failsafe_call;
 
 /*
@@ -77,7 +76,6 @@ static unsigned blink_counter = 0;
 static bool safety_button_pressed;
 
 static void safety_check_button(void *arg);
-static void heartbeat_blink(void *arg);
 static void failsafe_blink(void *arg);
 
 void
@@ -85,10 +83,11 @@ safety_init(void)
 {
 	/* arrange for the button handler to be called at 10Hz */
 	hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL);
+}
 
-	/* arrange for the heartbeat handler to be called at 4Hz */
-	hrt_call_every(&heartbeat_call, 1000, 250000, heartbeat_blink, NULL);
-
+void
+failsafe_led_init(void)
+{
 	/* arrange for the failsafe blinker to be called at 8Hz */
 	hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL);
 }
@@ -110,7 +109,7 @@ safety_check_button(void *arg)
 	 * state machine, keep ARM_COUNTER_THRESHOLD the same
 	 * length in all cases of the if/else struct below.
 	 */
-	if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
+	if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
 		(r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) {
 
 		if (counter < ARM_COUNTER_THRESHOLD) {
@@ -118,18 +117,18 @@ safety_check_button(void *arg)
 
 		} else if (counter == ARM_COUNTER_THRESHOLD) {
 			/* switch to armed state */
-			r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED;
+			r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
 			counter++;
 		}
 
-	} else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+	} else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
 
 		if (counter < ARM_COUNTER_THRESHOLD) {
 			counter++;
 
 		} else if (counter == ARM_COUNTER_THRESHOLD) {
 			/* change to disarmed state and notify the FMU */
-			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
 			counter++;
 		}
 
@@ -140,7 +139,7 @@ safety_check_button(void *arg)
 	/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
 	uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM;
 
-	if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) {
+	if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
 		if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
 			pattern = LED_PATTERN_IO_FMU_ARMED;
 
@@ -164,23 +163,13 @@ safety_check_button(void *arg)
 }
 
 static void
-heartbeat_blink(void *arg)
-{
-	static bool heartbeat = false;
-
-	/* XXX add flags here that need to be frobbed by various loops */
-
-	LED_BLUE(heartbeat = !heartbeat);
-}
-
-static void
 failsafe_blink(void *arg)
 {
 	/* indicate that a serious initialisation error occured */
 	if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) {
 		LED_AMBER(true);
-	    return;
-    }
+		return;
+	}
 
 	static bool failsafe = false;
 
@@ -192,4 +181,4 @@ failsafe_blink(void *arg)
 	}
 
 	LED_AMBER(failsafe);
-}
\ No newline at end of file
+}
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 073ddeaba..f6ec542eb 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -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
@@ -54,6 +54,27 @@
 
 #define SBUS_FRAME_SIZE		25
 #define SBUS_INPUT_CHANNELS	16
+#define SBUS_FLAGS_BYTE		23
+#define SBUS_FAILSAFE_BIT	3
+#define SBUS_FRAMELOST_BIT	2
+
+/*
+  Measured values with Futaba FX-30/R6108SB:
+    -+100% on TX:  PCM 1.100/1.520/1.950ms -> SBus raw values: 350/1024/1700  (100% ATV)
+    -+140% on TX:  PCM 0.930/1.520/2.112ms -> SBus raw values:  78/1024/1964  (140% ATV)  
+    -+152% on TX:  PCM 0.884/1.520/2.160ms -> SBus raw values:   1/1024/2047  (140% ATV plus dirty tricks)
+*/
+
+/* define range mapping here, -+100% -> 1000..2000 */
+#define SBUS_RANGE_MIN 200.0f
+#define SBUS_RANGE_MAX 1800.0f
+
+#define SBUS_TARGET_MIN 1000.0f
+#define SBUS_TARGET_MAX 2000.0f
+
+/* pre-calculate the floating point stuff as far as possible at compile time */
+#define SBUS_SCALE_FACTOR ((SBUS_TARGET_MAX - SBUS_TARGET_MIN) / (SBUS_RANGE_MAX - SBUS_RANGE_MIN))
+#define SBUS_SCALE_OFFSET (int)(SBUS_TARGET_MIN - (SBUS_SCALE_FACTOR * SBUS_RANGE_MIN + 0.5f))
 
 static int sbus_fd = -1;
 
@@ -66,7 +87,7 @@ static unsigned partial_frame_count;
 
 unsigned sbus_frame_drops;
 
-static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values);
+static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
 
 int
 sbus_init(const char *device)
@@ -97,7 +118,7 @@ sbus_init(const char *device)
 }
 
 bool
-sbus_input(uint16_t *values, uint16_t *num_values)
+sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels)
 {
 	ssize_t		ret;
 	hrt_abstime	now;
@@ -154,7 +175,7 @@ sbus_input(uint16_t *values, uint16_t *num_values)
 	 * decode it.
 	 */
 	partial_frame_count = 0;
-	return sbus_decode(now, values, num_values);
+	return sbus_decode(now, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels);
 }
 
 /*
@@ -194,28 +215,41 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
 };
 
 static bool
-sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
+sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values)
 {
 	/* check frame boundary markers to avoid out-of-sync cases */
-	if ((frame[0] != 0x0f) || (frame[24] != 0x00)) {
+	if ((frame[0] != 0x0f)) {
 		sbus_frame_drops++;
 		return false;
 	}
 
-	/* if the failsafe or connection lost bit is set, we consider the frame invalid */
-	if ((frame[23] & (1 << 2)) && /* signal lost */
-	    (frame[23] & (1 << 3))) { /* failsafe */
-
-		/* actively announce signal loss */
-		*values = 0;
-		return false;
+	switch (frame[24]) {
+		case 0x00:
+		/* this is S.BUS 1 */
+		break;
+		case 0x03:
+		/* S.BUS 2 SLOT0: RX battery and external voltage */
+		break;
+		case 0x83:
+		/* S.BUS 2 SLOT1 */
+		break;
+		case 0x43:
+		case 0xC3:
+		case 0x23:
+		case 0xA3:
+		case 0x63:
+		case 0xE3:
+		break;
+		default:
+		/* we expect one of the bits above, but there are some we don't know yet */
+		break;
 	}
 
 	/* we have received something we think is a frame */
 	last_frame_time = frame_time;
 
-	unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ?
-			     SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS;
+	unsigned chancount = (max_values > SBUS_INPUT_CHANNELS) ?
+			     SBUS_INPUT_CHANNELS : max_values;
 
 	/* use the decoder matrix to extract channel data */
 	for (unsigned channel = 0; channel < chancount; channel++) {
@@ -234,22 +268,43 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 			}
 		}
 
-		/* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
-		values[channel] = (value / 2) + 998;
+
+		/* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */
+		values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET;
 	}
 
 	/* decode switch channels if data fields are wide enough */
-	if (PX4IO_INPUT_CHANNELS > 17 && chancount > 15) {
+	if (PX4IO_RC_INPUT_CHANNELS > 17 && chancount > 15) {
 		chancount = 18;
 
 		/* channel 17 (index 16) */
-		values[16] = (frame[23] & (1 << 0)) * 1000 + 998;
+		values[16] = (frame[SBUS_FLAGS_BYTE] & (1 << 0)) * 1000 + 998;
 		/* channel 18 (index 17) */
-		values[17] = (frame[23] & (1 << 1)) * 1000 + 998;
+		values[17] = (frame[SBUS_FLAGS_BYTE] & (1 << 1)) * 1000 + 998;
 	}
 
 	/* note the number of channels decoded */
 	*num_values = chancount;
 
+	/* decode and handle failsafe and frame-lost flags */
+	if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */
+		/* report that we failed to read anything valid off the receiver */
+		*sbus_failsafe = true;
+		*sbus_frame_drop = true;
+	}
+	else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
+		/* set a special warning flag
+		 * 
+		 * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this 
+		 * condition as fail-safe greatly reduces the reliability and range of the radio link, 
+		 * e.g. by prematurely issueing return-to-launch!!! */
+
+		*sbus_failsafe = false;
+		*sbus_frame_drop = true;
+	} else {
+		*sbus_failsafe = false;
+		*sbus_frame_drop = false;
+	}
+
 	return true;
 }
diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c
new file mode 100644
index 000000000..e9adc8cd6
--- /dev/null
+++ b/src/modules/px4iofirmware/serial.c
@@ -0,0 +1,338 @@
+/****************************************************************************
+ *
+ *   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 serial.c
+ *
+ * Serial communication for the PX4IO module.
+ */
+
+#include <stdint.h>
+#include <unistd.h>
+#include <termios.h>
+#include <fcntl.h>
+#include <string.h>
+
+#include <nuttx/arch.h>
+#include <arch/board/board.h>
+
+/* XXX might be able to prune these */
+#include <chip.h>
+#include <up_internal.h>
+#include <up_arch.h>
+#include <stm32.h>
+#include <systemlib/perf_counter.h>
+
+//#define DEBUG
+#include "px4io.h"
+
+static perf_counter_t	pc_txns;
+static perf_counter_t	pc_errors;
+static perf_counter_t	pc_ore;
+static perf_counter_t	pc_fe;
+static perf_counter_t	pc_ne;
+static perf_counter_t	pc_idle;
+static perf_counter_t	pc_badidle;
+static perf_counter_t	pc_regerr;
+static perf_counter_t	pc_crcerr;
+
+static void		rx_handle_packet(void);
+static void		rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg);
+static DMA_HANDLE	tx_dma;
+static DMA_HANDLE	rx_dma;
+
+static int		serial_interrupt(int irq, void *context);
+static void		dma_reset(void);
+
+static struct IOPacket	dma_packet;
+
+/* serial register accessors */
+#define REG(_x)		(*(volatile uint32_t *)(PX4FMU_SERIAL_BASE + _x))
+#define rSR		REG(STM32_USART_SR_OFFSET)
+#define rDR		REG(STM32_USART_DR_OFFSET)
+#define rBRR		REG(STM32_USART_BRR_OFFSET)
+#define rCR1		REG(STM32_USART_CR1_OFFSET)
+#define rCR2		REG(STM32_USART_CR2_OFFSET)
+#define rCR3		REG(STM32_USART_CR3_OFFSET)
+#define rGTPR		REG(STM32_USART_GTPR_OFFSET)
+
+void
+interface_init(void)
+{
+	pc_txns = perf_alloc(PC_ELAPSED, "txns");
+	pc_errors = perf_alloc(PC_COUNT, "errors");
+	pc_ore = perf_alloc(PC_COUNT, "overrun");
+	pc_fe = perf_alloc(PC_COUNT, "framing");
+	pc_ne = perf_alloc(PC_COUNT, "noise");
+	pc_idle = perf_alloc(PC_COUNT, "idle");
+	pc_badidle = perf_alloc(PC_COUNT, "badidle");
+	pc_regerr = perf_alloc(PC_COUNT, "regerr");
+	pc_crcerr = perf_alloc(PC_COUNT, "crcerr");
+
+	/* allocate DMA */
+	tx_dma = stm32_dmachannel(PX4FMU_SERIAL_TX_DMA);
+	rx_dma = stm32_dmachannel(PX4FMU_SERIAL_RX_DMA);
+
+	/* configure pins for serial use */
+	stm32_configgpio(PX4FMU_SERIAL_TX_GPIO);
+	stm32_configgpio(PX4FMU_SERIAL_RX_GPIO);
+
+	/* reset and configure the UART */
+	rCR1 = 0;
+	rCR2 = 0;
+	rCR3 = 0;
+
+	/* clear status/errors */
+	(void)rSR;
+	(void)rDR;
+
+	/* configure line speed */
+	uint32_t usartdiv32 = PX4FMU_SERIAL_CLOCK / (PX4FMU_SERIAL_BITRATE / 2);
+	uint32_t mantissa = usartdiv32 >> 5;
+	uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1;
+	rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT);
+
+	/* connect our interrupt */
+	irq_attach(PX4FMU_SERIAL_VECTOR, serial_interrupt);
+	up_enable_irq(PX4FMU_SERIAL_VECTOR);
+
+	/* enable UART and error/idle interrupts */
+	rCR3 = USART_CR3_EIE;
+	rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE;
+
+#if 0	/* keep this for signal integrity testing */
+	for (;;) {
+		while (!(rSR & USART_SR_TXE))
+			;
+		rDR = 0xfa;
+		while (!(rSR & USART_SR_TXE))
+			;
+		rDR = 0xa0;
+	}
+#endif
+
+	/* configure RX DMA and return to listening state */
+	dma_reset();
+
+	debug("serial init");
+}
+
+static void
+rx_handle_packet(void)
+{
+	/* check packet CRC */
+	uint8_t crc = dma_packet.crc;
+	dma_packet.crc = 0;
+	if (crc != crc_packet(&dma_packet)) {
+		perf_count(pc_crcerr);
+
+		/* send a CRC error reply */
+		dma_packet.count_code = PKT_CODE_CORRUPT;
+		dma_packet.page = 0xff;
+		dma_packet.offset = 0xff;
+
+		return;
+	}
+
+	if (PKT_CODE(dma_packet) == PKT_CODE_WRITE) {
+
+		/* it's a blind write - pass it on */
+		if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) {
+			perf_count(pc_regerr);
+			dma_packet.count_code = PKT_CODE_ERROR;
+		} else {
+			dma_packet.count_code = PKT_CODE_SUCCESS;
+		}
+		return;
+	} 
+
+	if (PKT_CODE(dma_packet) == PKT_CODE_READ) {
+
+		/* it's a read - get register pointer for reply */
+		unsigned count;
+		uint16_t *registers;
+
+		if (registers_get(dma_packet.page, dma_packet.offset, &registers, &count) < 0) {
+			perf_count(pc_regerr);
+			dma_packet.count_code = PKT_CODE_ERROR;
+		} else {
+			/* constrain reply to requested size */
+			if (count > PKT_MAX_REGS)
+				count = PKT_MAX_REGS;
+			if (count > PKT_COUNT(dma_packet))
+				count = PKT_COUNT(dma_packet);
+
+			/* copy reply registers into DMA buffer */
+			memcpy((void *)&dma_packet.regs[0], registers, count * 2);
+			dma_packet.count_code = count | PKT_CODE_SUCCESS;
+		}
+		return;
+	}
+
+	/* send a bad-packet error reply */
+	dma_packet.count_code = PKT_CODE_CORRUPT;
+	dma_packet.page = 0xff;
+	dma_packet.offset = 0xfe;
+}
+
+static void
+rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg)
+{
+	/*
+	 * We are here because DMA completed, or UART reception stopped and
+	 * we think we have a packet in the buffer.
+	 */
+	perf_begin(pc_txns);
+
+	/* disable UART DMA */
+	rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
+
+	/* handle the received packet */
+	rx_handle_packet();
+
+	/* re-set DMA for reception first, so we are ready to receive before we start sending */
+	dma_reset();
+
+	/* send the reply to the just-processed request */
+	dma_packet.crc = 0;
+	dma_packet.crc = crc_packet(&dma_packet);
+	stm32_dmasetup(
+		tx_dma,
+		(uint32_t)&rDR,
+		(uint32_t)&dma_packet,
+		PKT_SIZE(dma_packet),
+		DMA_CCR_DIR		|
+		DMA_CCR_MINC		|
+		DMA_CCR_PSIZE_8BITS	|
+		DMA_CCR_MSIZE_8BITS);
+	stm32_dmastart(tx_dma, NULL, NULL, false);
+	rCR3 |= USART_CR3_DMAT;
+
+	perf_end(pc_txns);
+}
+
+static int
+serial_interrupt(int irq, void *context)
+{
+	static bool abort_on_idle = false;
+
+	uint32_t sr = rSR;	/* get UART status register */
+	(void)rDR;		/* required to clear any of the interrupt status that brought us here */
+
+	if (sr & (USART_SR_ORE |	/* overrun error - packet was too big for DMA or DMA was too slow */
+		USART_SR_NE |		/* noise error - we have lost a byte due to noise */
+		USART_SR_FE)) {		/* framing error - start/stop bit lost or line break */
+
+		perf_count(pc_errors);
+		if (sr & USART_SR_ORE)
+			perf_count(pc_ore);
+		if (sr & USART_SR_NE)
+			perf_count(pc_ne);
+		if (sr & USART_SR_FE)
+			perf_count(pc_fe);
+
+		/* send a line break - this will abort transmission/reception on the other end */
+		rCR1 |= USART_CR1_SBK;
+
+		/* when the line goes idle, abort rather than look at the packet */
+		abort_on_idle = true;
+	}
+
+	if (sr & USART_SR_IDLE) {
+
+		/* 
+		 * If we saw an error, don't bother looking at the packet - it should have
+		 * been aborted by the sender and will definitely be bad. Get the DMA reconfigured
+		 * ready for their retry.
+		 */
+		if (abort_on_idle) {
+
+			abort_on_idle = false;
+			dma_reset();
+			return 0;
+		}
+
+		/*
+		 * The sender has stopped sending - this is probably the end of a packet.
+		 * Check the received length against the length in the header to see if 
+		 * we have something that looks like a packet.
+		 */
+		unsigned length = sizeof(dma_packet) - stm32_dmaresidual(rx_dma);
+		if ((length < 1) || (length < PKT_SIZE(dma_packet))) {
+
+			/* it was too short - possibly truncated */
+			perf_count(pc_badidle);
+			dma_reset();
+			return 0;
+		}
+
+		/*
+		 * Looks like we received a packet. Stop the DMA and go process the
+		 * packet.
+		 */
+		perf_count(pc_idle);
+		stm32_dmastop(rx_dma);
+		rx_dma_callback(rx_dma, DMA_STATUS_TCIF, NULL);
+	}
+
+	return 0;
+}
+
+static void
+dma_reset(void)
+{
+	rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
+	(void)rSR;
+	(void)rDR;
+	(void)rDR;
+
+	/* kill any pending DMA */
+	stm32_dmastop(tx_dma);
+	stm32_dmastop(rx_dma);
+
+	/* reset the RX side */
+	stm32_dmasetup(
+		rx_dma,
+		(uint32_t)&rDR,
+		(uint32_t)&dma_packet,
+		sizeof(dma_packet),
+		DMA_CCR_MINC		|
+		DMA_CCR_PSIZE_8BITS	|
+		DMA_CCR_MSIZE_8BITS     |
+		DMA_CCR_PRIVERYHI);
+
+	/* start receive DMA ready for the next packet */
+	stm32_dmastart(rx_dma, rx_dma_callback, NULL, false);
+	rCR3 |= USART_CR3_DMAR;
+}
+
diff --git a/src/modules/sdlog2/logbuffer.c b/src/modules/sdlog2/logbuffer.c
new file mode 100644
index 000000000..6a29d7e5c
--- /dev/null
+++ b/src/modules/sdlog2/logbuffer.c
@@ -0,0 +1,133 @@
+/****************************************************************************
+ *
+ *   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 logbuffer.c
+ *
+ * Ring FIFO buffer for binary log data.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <string.h>
+#include <stdlib.h>
+
+#include "logbuffer.h"
+
+int logbuffer_init(struct logbuffer_s *lb, int size)
+{
+	lb->size  = size;
+	lb->write_ptr = 0;
+	lb->read_ptr = 0;
+	lb->data = malloc(lb->size);
+	return (lb->data == 0) ? ERROR : OK;
+}
+
+int logbuffer_count(struct logbuffer_s *lb)
+{
+	int n = lb->write_ptr - lb->read_ptr;
+
+	if (n < 0) {
+		n += lb->size;
+	}
+
+	return n;
+}
+
+int logbuffer_is_empty(struct logbuffer_s *lb)
+{
+	return lb->read_ptr == lb->write_ptr;
+}
+
+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)
+		available += lb->size;
+
+	if (size > available) {
+		// buffer overflow
+		return false;
+	}
+
+	char *c = (char *) ptr;
+	int n = lb->size - lb->write_ptr;	// bytes to end of the buffer
+
+	if (n < size) {
+		// message goes over end of the buffer
+		memcpy(&(lb->data[lb->write_ptr]), c, n);
+		lb->write_ptr = 0;
+
+	} else {
+		n = 0;
+	}
+
+	// now: n = bytes already written
+	int p = size - n;	// number of bytes to write
+	memcpy(&(lb->data[lb->write_ptr]), &(c[n]), p);
+	lb->write_ptr = (lb->write_ptr + p) % lb->size;
+	return true;
+}
+
+int logbuffer_get_ptr(struct logbuffer_s *lb, void **ptr, bool *is_part)
+{
+	// bytes available to read
+	int available = lb->write_ptr - lb->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 = lb->size - lb->read_ptr;
+		*is_part = lb->write_ptr > 0;
+	}
+
+	*ptr = &(lb->data[lb->read_ptr]);
+	return n;
+}
+
+void logbuffer_mark_read(struct logbuffer_s *lb, int n)
+{
+	lb->read_ptr = (lb->read_ptr + n) % lb->size;
+}
diff --git a/src/modules/sdlog2/logbuffer.h b/src/modules/sdlog2/logbuffer.h
new file mode 100644
index 000000000..3a5e3a29f
--- /dev/null
+++ b/src/modules/sdlog2/logbuffer.h
@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   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 logbuffer.h
+ *
+ * Ring FIFO buffer for binary log data.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef SDLOG2_RINGBUFFER_H_
+#define SDLOG2_RINGBUFFER_H_
+
+#include <stdbool.h>
+
+struct logbuffer_s {
+	// pointers and size are in bytes
+	int write_ptr;
+	int read_ptr;
+	int size;
+	char *data;
+};
+
+int logbuffer_init(struct logbuffer_s *lb, int size);
+
+int logbuffer_count(struct logbuffer_s *lb);
+
+int logbuffer_is_empty(struct logbuffer_s *lb);
+
+bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size);
+
+int logbuffer_get_ptr(struct logbuffer_s *lb, void **ptr, bool *is_part);
+
+void logbuffer_mark_read(struct logbuffer_s *lb, int n);
+
+#endif
diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk
new file mode 100644
index 000000000..f53129688
--- /dev/null
+++ b/src/modules/sdlog2/module.mk
@@ -0,0 +1,43 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# sdlog2 Application
+#
+
+MODULE_COMMAND  = sdlog2
+# The main thread only buffers to RAM, needs a high priority
+MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30"
+
+SRCS = sdlog2.c \
+       logbuffer.c
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
new file mode 100644
index 000000000..41e2248bb
--- /dev/null
+++ b/src/modules/sdlog2/sdlog2.c
@@ -0,0 +1,1463 @@
+/****************************************************************************
+ *
+ *   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 sdlog2.c
+ *
+ * Simple SD logger for flight data. Buffers new sensor values and
+ * does the heavy SD I/O in a low-priority worker thread.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#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 <math.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/sensor_combined.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_outputs.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_command.h>
+#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/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_vicon_position.h>
+#include <uORB/topics/vehicle_global_velocity_setpoint.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 <uORB/topics/rc_channels.h>
+#include <uORB/topics/esc_status.h>
+
+#include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
+#include <version/version.h>
+
+#include <mavlink/mavlink_log.h>
+
+#include "logbuffer.h"
+#include "sdlog2_format.h"
+#include "sdlog2_messages.h"
+
+#define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \
+		log_msgs_written++; \
+	} else { \
+		log_msgs_skipped++; \
+	}
+
+#define LOG_ORB_SUBSCRIBE(_var, _topic) subs.##_var##_sub = orb_subscribe(ORB_ID(##_topic##)); \
+	fds[fdsc_count].fd = subs.##_var##_sub; \
+	fds[fdsc_count].events = POLLIN; \
+	fdsc_count++;
+
+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 int LOG_BUFFER_SIZE_DEFAULT = 8192;
+static const int MAX_WRITE_CHUNK = 512;
+static const int MIN_BYTES_TO_WRITE = 512;
+
+static const char *log_root = "/fs/microsd/log";
+static int mavlink_fd = -1;
+struct logbuffer_s lb;
+
+/* mutex / condition to synchronize threads */
+static pthread_mutex_t logbuffer_mutex;
+static pthread_cond_t logbuffer_cond;
+
+static char log_dir[32];
+
+/* statistics counters */
+static uint64_t start_time = 0;
+static unsigned long log_bytes_written = 0;
+static unsigned long log_msgs_written = 0;
+static unsigned long log_msgs_skipped = 0;
+
+/* GPS time, used for log files naming */
+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;
+
+/* helper flag to track system state changes */
+static bool flag_system_armed = false;
+
+static pthread_t logwriter_pthread = 0;
+static pthread_attr_t logwriter_attr;
+
+/**
+ * Log buffer writing thread. Open and close file here.
+ */
+static void *logwriter_thread(void *arg);
+
+/**
+ * SD log management function.
+ */
+__EXPORT int sdlog2_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of sd log deamon.
+ */
+int sdlog2_thread_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+static void sdlog2_usage(const char *reason);
+
+/**
+ * Print the current status.
+ */
+static void sdlog2_status(void);
+
+/**
+ * Start logging: create new file and start log writer thread.
+ */
+static void sdlog2_start_log(void);
+
+/**
+ * Stop logging: stop log writer thread and close log file.
+ */
+static void sdlog2_stop_log(void);
+
+/**
+ * Write a header to log file: list of message formats.
+ */
+static int write_formats(int fd);
+
+/**
+ * Write version message to log file.
+ */
+static int write_version(int fd);
+
+/**
+ * Write parameters to log file.
+ */
+static int write_parameters(int fd);
+
+static bool 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);
+
+static void handle_status(struct vehicle_status_s *cmd);
+
+/**
+ * Create dir for current logging session. Store dir name in 'log_dir'.
+ */
+static int create_log_dir(void);
+
+/**
+ * Select first free log file name and open it.
+ */
+static int open_log_file(void);
+
+static void
+sdlog2_usage(const char *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");
+}
+
+/**
+ * The logger 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 sdlog2_main(int argc, char *argv[])
+{
+	if (argc < 2)
+		sdlog2_usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("already running");
+			/* this is not an error */
+			exit(0);
+		}
+
+		main_thread_should_exit = false;
+		deamon_task = task_spawn_cmd("sdlog2",
+					     SCHED_DEFAULT,
+					     SCHED_PRIORITY_DEFAULT - 30,
+					     3000,
+					     sdlog2_thread_main,
+					     (const char **)argv);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		if (!thread_running) {
+			warnx("not started");
+		}
+
+		main_thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			sdlog2_status();
+
+		} else {
+			warnx("not started\n");
+		}
+
+		exit(0);
+	}
+
+	sdlog2_usage("unrecognized command");
+	exit(1);
+}
+
+int create_log_dir()
+{
+	/* create dir on sdcard if needed */
+	uint16_t dir_number = 1; // start with dir sess001
+	int mkdir_ret;
+
+	if (log_name_timestamp && gps_time != 0) {
+		/* use GPS date for log dir naming: e.g. /fs/microsd/2014-01-19 */
+		time_t gps_time_sec = gps_time / 1000000;
+		struct tm t;
+		gmtime_r(&gps_time_sec, &t);
+		int n = snprintf(log_dir, sizeof(log_dir), "%s/", log_root);
+		strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &t);
+		mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO);
+
+		if (mkdir_ret == OK) {
+			warnx("log dir created: %s", log_dir);
+
+		} else if (errno != EEXIST) {
+			warn("failed creating new dir: %s", log_dir);
+			return -1;
+		}
+
+	} else {
+		/* look for the next dir that does not exist */
+		while (dir_number <= MAX_NO_LOGFOLDER) {
+			/* format log dir: e.g. /fs/microsd/sess001 */
+			sprintf(log_dir, "%s/sess%03u", log_root, dir_number);
+			mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO);
+
+			if (mkdir_ret == 0) {
+				warnx("log dir created: %s", log_dir);
+				break;
+
+			} else if (errno != EEXIST) {
+				warn("failed creating new dir: %s", log_dir);
+				return -1;
+			}
+
+			/* dir exists already */
+			dir_number++;
+			continue;
+		}
+
+		if (dir_number >= MAX_NO_LOGFOLDER) {
+			/* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */
+			warnx("all %d possible dirs exist already", MAX_NO_LOGFOLDER);
+			return -1;
+		}
+	}
+
+	/* print logging path, important to find log file later */
+	warnx("log dir: %s", log_dir);
+	mavlink_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir);
+	return 0;
+}
+
+int open_log_file()
+{
+	/* string to hold the path to the log */
+	char log_file_name[16] = "";
+	char log_file_path[48] = "";
+
+	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), "%H_%M_%S.bin", &t);
+		snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name);
+
+	} else {
+		uint16_t 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), "log%03u.bin", file_number);
+			snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, 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 */
+			warnx("all %d possible files exist already", 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_info(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;
+}
+
+static void *logwriter_thread(void *arg)
+{
+	/* set name */
+	prctl(PR_SET_NAME, "sdlog2_writer", 0);
+
+	int log_fd = open_log_file();
+
+	if (log_fd < 0)
+		return;
+
+	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) {
+		/* make sure threads are synchronized */
+		pthread_mutex_lock(&logbuffer_mutex);
+
+		/* update read pointer if needed */
+		if (n > 0) {
+			logbuffer_mark_read(&lb, n);
+		}
+
+		/* only wait if no data is available to process */
+		if (should_wait && !logwriter_should_exit) {
+			/* blocking wait for new data at this line */
+			pthread_cond_wait(&logbuffer_cond, &logbuffer_mutex);
+		}
+
+		/* only get pointer to thread-safe data, do heavy I/O a few lines down */
+		int available = logbuffer_get_ptr(logbuf, &read_ptr, &is_part);
+
+		/* continue */
+		pthread_mutex_unlock(&logbuffer_mutex);
+
+		if (available > 0) {
+			/* do heavy IO here */
+			if (available > MAX_WRITE_CHUNK) {
+				n = MAX_WRITE_CHUNK;
+
+			} else {
+				n = available;
+			}
+
+			lseek(log_fd, 0, SEEK_CUR);
+			n = write(log_fd, read_ptr, n);
+
+			should_wait = (n == available) && !is_part;
+
+			if (n < 0) {
+				main_thread_should_exit = true;
+				err(1, "error writing log file");
+			}
+
+			if (n > 0) {
+				log_bytes_written += n;
+			}
+
+		} else {
+			n = 0;
+
+			/* exit only with empty buffer */
+			if (main_thread_should_exit || logwriter_should_exit) {
+				break;
+			}
+
+			should_wait = true;
+		}
+
+		if (++poll_count == 10) {
+			fsync(log_fd);
+			poll_count = 0;
+		}
+	}
+
+	fsync(log_fd);
+	close(log_fd);
+
+	return;
+}
+
+void sdlog2_start_log()
+{
+	warnx("start logging");
+	mavlink_log_info(mavlink_fd, "[sdlog2] start logging");
+
+	/* create log dir if needed */
+	if (create_log_dir() != 0) {
+		mavlink_log_critical(mavlink_fd, "[sdlog2] error creating log dir");
+		errx(1, "error creating log dir");
+	}
+
+	/* initialize statistics counter */
+	log_bytes_written = 0;
+	start_time = hrt_absolute_time();
+	log_msgs_written = 0;
+	log_msgs_skipped = 0;
+
+	/* initialize log buffer emptying thread */
+	pthread_attr_init(&logwriter_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(&logwriter_attr, &param);
+
+	pthread_attr_setstacksize(&logwriter_attr, 2048);
+
+	logwriter_should_exit = false;
+
+	/* start log buffer emptying thread */
+	if (0 != pthread_create(&logwriter_pthread, &logwriter_attr, logwriter_thread, &lb)) {
+		errx(1, "error creating logwriter thread");
+	}
+
+	logging_enabled = true;
+}
+
+void sdlog2_stop_log()
+{
+	warnx("stop logging");
+	mavlink_log_info(mavlink_fd, "[sdlog2] stop logging");
+
+	logging_enabled = false;
+
+	/* wake up write thread one last time */
+	pthread_mutex_lock(&logbuffer_mutex);
+	logwriter_should_exit = true;
+	pthread_cond_signal(&logbuffer_cond);
+	/* unlock, now the writer thread may return */
+	pthread_mutex_unlock(&logbuffer_mutex);
+
+	/* wait for write thread to return */
+	int ret;
+
+	if ((ret = pthread_join(logwriter_pthread, NULL)) != 0) {
+		warnx("error joining logwriter thread: %i", ret);
+	}
+
+	logwriter_pthread = 0;
+	pthread_attr_destroy(&logwriter_attr);
+
+	sdlog2_status();
+}
+
+int write_formats(int fd)
+{
+	/* construct message format packet */
+	struct {
+		LOG_PACKET_HEADER;
+		struct log_format_s body;
+	} log_msg_format = {
+		LOG_PACKET_HEADER_INIT(LOG_FORMAT_MSG),
+	};
+
+	int written = 0;
+
+	/* fill message format packet for each format and write it */
+	for (int i = 0; i < log_formats_num; i++) {
+		log_msg_format.body = log_formats[i];
+		written += write(fd, &log_msg_format, sizeof(log_msg_format));
+	}
+
+	return written;
+}
+
+int write_version(int fd)
+{
+	/* construct version message */
+	struct {
+		LOG_PACKET_HEADER;
+		struct log_VER_s body;
+	} log_msg_VER = {
+		LOG_PACKET_HEADER_INIT(LOG_VER_MSG),
+	};
+
+	/* fill version message and write it */
+	strncpy(log_msg_VER.body.fw_git, FW_GIT, sizeof(log_msg_VER.body.fw_git));
+	strncpy(log_msg_VER.body.arch, HW_ARCH, sizeof(log_msg_VER.body.arch));
+	return write(fd, &log_msg_VER, sizeof(log_msg_VER));
+}
+
+int write_parameters(int fd)
+{
+	/* construct parameter message */
+	struct {
+		LOG_PACKET_HEADER;
+		struct log_PARM_s body;
+	} log_msg_PARM = {
+		LOG_PACKET_HEADER_INIT(LOG_PARM_MSG),
+	};
+
+	int written = 0;
+	param_t params_cnt = param_count();
+
+	for (param_t param = 0; param < params_cnt; param++) {
+		/* fill parameter message and write it */
+		strncpy(log_msg_PARM.body.name, param_name(param), sizeof(log_msg_PARM.body.name));
+		float value = NAN;
+
+		switch (param_type(param)) {
+		case PARAM_TYPE_INT32: {
+				int32_t i;
+				param_get(param, &i);
+				value = i;	// cast integer to float
+				break;
+			}
+
+		case PARAM_TYPE_FLOAT:
+			param_get(param, &value);
+			break;
+
+		default:
+			break;
+		}
+
+		log_msg_PARM.body.value = value;
+		written += write(fd, &log_msg_PARM, sizeof(log_msg_PARM));
+	}
+
+	return written;
+}
+
+int sdlog2_thread_main(int argc, char *argv[])
+{
+	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+
+	if (mavlink_fd < 0) {
+		warnx("failed to open MAVLink log stream, start mavlink app first");
+	}
+
+	/* log buffer size */
+	int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT;
+
+	logging_enabled = false;
+	log_on_start = false;
+	log_when_armed = false;
+	log_name_timestamp = false;
+
+	flag_system_armed = false;
+
+	/* work around some stupidity in task_create's argv handling */
+	argc -= 2;
+	argv += 2;
+	int ch;
+
+	while ((ch = getopt(argc, argv, "r:b:eat")) != EOF) {
+		switch (ch) {
+		case 'r': {
+				unsigned long r = strtoul(optarg, NULL, 10);
+
+				if (r == 0) {
+					sleep_delay = 0;
+
+				} else {
+					sleep_delay = 1000000 / r;
+				}
+			}
+			break;
+
+		case 'b': {
+				unsigned long s = strtoul(optarg, NULL, 10);
+
+				if (s < 1) {
+					s = 1;
+				}
+
+				log_buffer_size = 1024 * s;
+			}
+			break;
+
+		case 'e':
+			log_on_start = true;
+			break;
+
+		case 'a':
+			log_when_armed = true;
+			break;
+
+		case 't':
+			log_name_timestamp = true;
+			break;
+
+		case '?':
+			if (optopt == 'c') {
+				warnx("option -%c requires an argument", optopt);
+
+			} else if (isprint(optopt)) {
+				warnx("unknown option `-%c'", optopt);
+
+			} else {
+				warnx("unknown option character `\\x%x'", optopt);
+			}
+
+		default:
+			sdlog2_usage("unrecognized flag");
+			errx(1, "exiting");
+		}
+	}
+
+	gps_time = 0;
+
+	/* 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);
+	}
+
+	/* copy conversion scripts */
+	const char *converter_in = "/etc/logging/conv.zip";
+	char *converter_out = malloc(64);
+	snprintf(converter_out, 64, "%s/conv.zip", log_root);
+
+	if (file_copy(converter_in, converter_out) != OK) {
+		warn("unable to copy conversion scripts");
+	}
+
+	free(converter_out);
+
+	/* initialize log buffer with specified size */
+	warnx("log buffer size: %i bytes", log_buffer_size);
+
+	if (OK != logbuffer_init(&lb, log_buffer_size)) {
+		errx(1, "can't allocate log buffer, exiting");
+	}
+
+	struct vehicle_status_s buf_status;
+
+	memset(&buf_status, 0, sizeof(buf_status));
+
+	/* warning! using union here to save memory, elements should be used separately! */
+	union {
+		struct vehicle_command_s cmd;
+		struct sensor_combined_s sensor;
+		struct vehicle_attitude_s att;
+		struct vehicle_attitude_setpoint_s att_sp;
+		struct vehicle_rates_setpoint_s rates_sp;
+		struct actuator_outputs_s act_outputs;
+		struct actuator_controls_s act_controls;
+		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 vehicle_vicon_position_s vicon_pos;
+		struct optical_flow_s flow;
+		struct rc_channels_s rc;
+		struct differential_pressure_s diff_pres;
+		struct airspeed_s airspeed;
+		struct esc_status_s esc;
+		struct vehicle_global_velocity_setpoint_s global_vel_sp;
+		struct battery_status_s battery;
+	} 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 {
+		LOG_PACKET_HEADER;
+		union {
+			struct log_TIME_s log_TIME;
+			struct log_ATT_s log_ATT;
+			struct log_ATSP_s log_ATSP;
+			struct log_IMU_s log_IMU;
+			struct log_SENS_s log_SENS;
+			struct log_LPOS_s log_LPOS;
+			struct log_LPSP_s log_LPSP;
+			struct log_GPS_s log_GPS;
+			struct log_ATTC_s log_ATTC;
+			struct log_STAT_s log_STAT;
+			struct log_RC_s log_RC;
+			struct log_OUT0_s log_OUT0;
+			struct log_AIRS_s log_AIRS;
+			struct log_ARSP_s log_ARSP;
+			struct log_FLOW_s log_FLOW;
+			struct log_GPOS_s log_GPOS;
+			struct log_GPSP_s log_GPSP;
+			struct log_ESC_s log_ESC;
+			struct log_GVSP_s log_GVSP;
+			struct log_BATT_s log_BATT;
+		} body;
+	} log_msg = {
+		LOG_PACKET_HEADER_INIT(0)
+	};
+#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];
+
+	/* --- 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.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.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;
+	}
+
+	/*
+	 * set up poll to block for new data,
+	 * wait for a maximum of 1000 ms
+	 */
+	const int poll_timeout = 1000;
+
+	thread_running = true;
+
+	/* initialize thread synchronization */
+	pthread_mutex_init(&logbuffer_mutex, NULL);
+	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;
+
+	/* 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;
+			}
+		}
+
+		sdlog2_start_log();
+	}
+
+	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++;
+			}
+
+			/* --- VEHICLE STATUS - LOG MANAGEMENT --- */
+			if (fds[ifds++].revents & POLLIN) {
+				orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status);
+
+				if (log_when_armed) {
+					handle_status(&buf_status);
+				}
+
+				handled_topics++;
+			}
+
+			/* --- GPS POSITION - LOG MANAGEMENT --- */
+			if (fds[ifds++].revents & POLLIN) {
+				orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
+
+				if (log_name_timestamp) {
+					gps_time = buf.gps_pos.time_gps_usec;
+				}
+
+				handled_topics++;
+			}
+
+			if (!logging_enabled || !check_data || handled_topics >= poll_ret) {
+				continue;
+			}
+
+			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);
+			}
+
+			/* --- 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);
+			}
+
+			/* --- 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;
+
+				if (buf.sensor.gyro_counter != gyro_counter) {
+					gyro_counter = buf.sensor.gyro_counter;
+					write_IMU = true;
+				}
+
+				if (buf.sensor.accelerometer_counter != accelerometer_counter) {
+					accelerometer_counter = buf.sensor.accelerometer_counter;
+					write_IMU = true;
+				}
+
+				if (buf.sensor.magnetometer_counter != magnetometer_counter) {
+					magnetometer_counter = buf.sensor.magnetometer_counter;
+					write_IMU = true;
+				}
+
+				if (buf.sensor.baro_counter != baro_counter) {
+					baro_counter = buf.sensor.baro_counter;
+					write_SENS = true;
+				}
+
+				if (buf.sensor.differential_pressure_counter != differential_pressure_counter) {
+					differential_pressure_counter = buf.sensor.differential_pressure_counter;
+					write_SENS = true;
+				}
+
+				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);
+				}
+
+				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);
+				}
+			}
+
+			/* --- 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);
+			}
+
+			/* --- 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);
+			}
+
+			/* --- 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);
+			}
+
+			/* --- 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);
+			}
+
+			/* --- 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);
+			}
+
+			/* --- 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);
+			}
+
+			/* --- 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);
+			}
+
+			/* --- 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);
+			}
+
+			/* --- 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
+			}
+
+			/* --- 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);
+			}
+
+			/* --- 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);
+			}
+
+			/* --- 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);
+			}
+
+			/* --- 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);
+				}
+			}
+
+			/* --- 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);
+			}
+
+			/* --- 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);
+			}
+
+			/* 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 (use_sleep) {
+			usleep(sleep_delay);
+		}
+	}
+
+	if (logging_enabled)
+		sdlog2_stop_log();
+
+	pthread_mutex_destroy(&logbuffer_mutex);
+	pthread_cond_destroy(&logbuffer_cond);
+
+	free(lb.data);
+
+	warnx("exiting");
+
+	thread_running = false;
+
+	return 0;
+}
+
+void sdlog2_status()
+{
+	float kibibytes = log_bytes_written / 1024.0f;
+	float mebibytes = kibibytes / 1024.0f;
+	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);
+	mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped);
+}
+
+/**
+ * @return 0 if file exists
+ */
+bool file_exist(const char *filename)
+{
+	struct stat buffer;
+	return stat(filename, &buffer) == 0;
+}
+
+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) {
+		ret = fwrite(buf, 1, nread, target);
+
+		if (ret <= 0) {
+			warnx("error writing file");
+			ret = 1;
+			break;
+		}
+	}
+
+	fsync(fileno(target));
+
+	fclose(source);
+	fclose(target);
+
+	return OK;
+}
+
+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 */
+	switch (cmd->command) {
+
+	case VEHICLE_CMD_PREFLIGHT_STORAGE:
+		param = (int)(cmd->param3);
+
+		if (param == 1)	{
+			sdlog2_start_log();
+
+		} else if (param == 0)	{
+			sdlog2_stop_log();
+		}
+
+		break;
+
+	default:
+		/* silently ignore */
+		break;
+	}
+}
+
+void handle_status(struct vehicle_status_s *status)
+{
+	// TODO use flag from actuator_armed here?
+	bool armed = status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR;
+
+	if (armed != flag_system_armed) {
+		flag_system_armed = armed;
+
+		if (flag_system_armed) {
+			sdlog2_start_log();
+
+		} else {
+			sdlog2_stop_log();
+		}
+	}
+}
diff --git a/src/modules/sdlog2/sdlog2_format.h b/src/modules/sdlog2/sdlog2_format.h
new file mode 100644
index 000000000..dc5e6c8bd
--- /dev/null
+++ b/src/modules/sdlog2/sdlog2_format.h
@@ -0,0 +1,98 @@
+/****************************************************************************
+ *
+ *   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 sdlog2_format.h
+ *
+ * General log format structures and macro.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+/*
+Format characters in the format string for binary log messages
+  b   : int8_t
+  B   : uint8_t
+  h   : int16_t
+  H   : uint16_t
+  i   : int32_t
+  I   : uint32_t
+  f   : float
+  n   : char[4]
+  N   : char[16]
+  Z   : char[64]
+  c   : int16_t * 100
+  C   : uint16_t * 100
+  e   : int32_t * 100
+  E   : uint32_t * 100
+  L   : int32_t latitude/longitude
+  M   : uint8_t flight mode
+
+  q   : int64_t
+  Q   : uint64_t
+ */
+
+#ifndef SDLOG2_FORMAT_H_
+#define SDLOG2_FORMAT_H_
+
+#define LOG_PACKET_HEADER_LEN	   3
+#define LOG_PACKET_HEADER	       uint8_t head1, head2, msg_type;
+#define LOG_PACKET_HEADER_INIT(id) .head1 = HEAD_BYTE1, .head2 = HEAD_BYTE2, .msg_type = id
+
+// once the logging code is all converted we will remove these from
+// this header
+#define HEAD_BYTE1  0xA3    // Decimal 163
+#define HEAD_BYTE2  0x95    // Decimal 149
+
+struct log_format_s {
+	uint8_t type;
+	uint8_t length;		// full packet length including header
+	char name[4];
+	char format[16];
+	char labels[64];
+};
+
+#define LOG_FORMAT(_name, _format, _labels) { \
+		.type = LOG_##_name##_MSG, \
+			.length = sizeof(struct log_##_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)
+
+#endif /* SDLOG2_FORMAT_H_ */
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
new file mode 100644
index 000000000..a784a1f30
--- /dev/null
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -0,0 +1,311 @@
+/****************************************************************************
+ *
+ *   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 sdlog2_messages.h
+ *
+ * Log messages and structures definition.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef SDLOG2_MESSAGES_H_
+#define SDLOG2_MESSAGES_H_
+
+#include "sdlog2_format.h"
+
+/* define message formats */
+
+#pragma pack(push, 1)
+/* --- ATT - ATTITUDE --- */
+#define LOG_ATT_MSG 2
+struct log_ATT_s {
+	float roll;
+	float pitch;
+	float yaw;
+	float roll_rate;
+	float pitch_rate;
+	float yaw_rate;
+};
+
+/* --- ATSP - ATTITUDE SET POINT --- */
+#define LOG_ATSP_MSG 3
+struct log_ATSP_s {
+	float roll_sp;
+	float pitch_sp;
+	float yaw_sp;
+	float thrust_sp;
+};
+
+/* --- IMU - IMU SENSORS --- */
+#define LOG_IMU_MSG 4
+struct log_IMU_s {
+	float acc_x;
+	float acc_y;
+	float acc_z;
+	float gyro_x;
+	float gyro_y;
+	float gyro_z;
+	float mag_x;
+	float mag_y;
+	float mag_z;
+};
+
+/* --- SENS - OTHER SENSORS --- */
+#define LOG_SENS_MSG 5
+struct log_SENS_s {
+	float baro_pres;
+	float baro_alt;
+	float baro_temp;
+	float diff_pres;
+};
+
+/* --- LPOS - LOCAL POSITION --- */
+#define LOG_LPOS_MSG 6
+struct log_LPOS_s {
+	float x;
+	float y;
+	float z;
+	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 landed;
+};
+
+/* --- LPSP - LOCAL POSITION SETPOINT --- */
+#define LOG_LPSP_MSG 7
+struct log_LPSP_s {
+	float x;
+	float y;
+	float z;
+	float yaw;
+};
+
+/* --- GPS - GPS POSITION --- */
+#define LOG_GPS_MSG 8
+struct log_GPS_s {
+	uint64_t gps_time;
+	uint8_t fix_type;
+	float eph;
+	float epv;
+	int32_t lat;
+	int32_t lon;
+	float alt;
+	float vel_n;
+	float vel_e;
+	float vel_d;
+	float cog;
+};
+
+/* --- ATTC - ATTITUDE CONTROLS (ACTUATOR_0 CONTROLS)--- */
+#define LOG_ATTC_MSG 9
+struct log_ATTC_s {
+	float roll;
+	float pitch;
+	float yaw;
+	float thrust;
+};
+
+/* --- STAT - VEHICLE STATE --- */
+#define LOG_STAT_MSG 10
+struct log_STAT_s {
+	uint8_t main_state;
+	uint8_t navigation_state;
+	uint8_t arming_state;
+	float battery_remaining;
+	uint8_t battery_warning;
+	uint8_t landed;
+};
+
+/* --- RC - RC INPUT CHANNELS --- */
+#define LOG_RC_MSG 11
+struct log_RC_s {
+	float channel[8];
+	uint8_t channel_count;
+};
+
+/* --- OUT0 - ACTUATOR_0 OUTPUT --- */
+#define LOG_OUT0_MSG 12
+struct log_OUT0_s {
+	float output[8];
+};
+
+/* --- AIRS - AIRSPEED --- */
+#define LOG_AIRS_MSG 13
+struct log_AIRS_s {
+	float indicated_airspeed;
+	float true_airspeed;
+};
+
+/* --- ARSP - ATTITUDE RATE SET POINT --- */
+#define LOG_ARSP_MSG 14
+struct log_ARSP_s {
+	float roll_rate_sp;
+	float pitch_rate_sp;
+	float yaw_rate_sp;
+};
+
+/* --- FLOW - OPTICAL FLOW --- */
+#define LOG_FLOW_MSG 15
+struct log_FLOW_s {
+	int16_t flow_raw_x;
+	int16_t flow_raw_y;
+	float flow_comp_x;
+	float flow_comp_y;
+	float distance;
+	uint8_t	quality;
+	uint8_t sensor_id;
+};
+
+/* --- GPOS - GLOBAL POSITION ESTIMATE --- */
+#define LOG_GPOS_MSG 16
+struct log_GPOS_s {
+	int32_t lat;
+	int32_t lon;
+	float alt;
+	float vel_n;
+	float vel_e;
+	float vel_d;
+};
+
+/* --- GPSP - GLOBAL POSITION SETPOINT --- */
+#define LOG_GPSP_MSG 17
+struct log_GPSP_s {
+	uint8_t altitude_is_relative;
+	int32_t lat;
+	int32_t lon;
+	float altitude;
+	float yaw;
+	float loiter_radius;
+	int8_t loiter_direction;
+	uint8_t nav_cmd;
+	float param1;
+	float param2;
+	float param3;
+	float param4;
+};
+
+/* --- ESC - ESC STATE --- */
+#define LOG_ESC_MSG 18
+struct log_ESC_s {
+	uint16_t counter;
+	uint8_t esc_count;
+	uint8_t esc_connectiontype;
+	uint8_t esc_num;
+	uint16_t esc_address;
+	uint16_t esc_version;
+	uint16_t esc_voltage;
+	uint16_t esc_current;
+	uint16_t esc_rpm;
+	uint16_t esc_temperature;
+	float    esc_setpoint;
+	uint16_t esc_setpoint_raw;
+};
+
+/* --- GVSP - GLOBAL VELOCITY SETPOINT --- */
+#define LOG_GVSP_MSG 19
+struct log_GVSP_s {
+	float vx;
+	float vy;
+	float vz;
+};
+
+/* --- BATT - BATTERY --- */
+#define LOG_BATT_MSG 20
+struct log_BATT_s {
+	float voltage;
+	float voltage_filtered;
+	float current;
+	float discharged;
+};
+
+/* --- TIME - TIME STAMP --- */
+#define LOG_TIME_MSG 129
+struct log_TIME_s {
+	uint64_t t;
+};
+
+/* --- VER - VERSION --- */
+#define LOG_VER_MSG 130
+struct log_VER_s {
+	char arch[16];
+	char fw_git[64];
+};
+
+/* --- PARM - PARAMETER --- */
+#define LOG_PARM_MSG 131
+struct log_PARM_s {
+	char name[16];
+	float value;
+};
+
+#pragma pack(pop)
+
+/* 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"),
+	/* system-level messages, ID >= 0x80 */
+	// 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"),
+};
+
+static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
+
+#endif /* SDLOG2_MESSAGES_H_ */
diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp
new file mode 100644
index 000000000..96a443c6e
--- /dev/null
+++ b/src/modules/segway/BlockSegwayController.cpp
@@ -0,0 +1,58 @@
+#include "BlockSegwayController.hpp"
+
+void BlockSegwayController::update() {
+	// wait for a sensor update, check for exit condition every 100 ms
+	if (poll(&_attPoll, 1, 100) < 0) return; // poll error
+
+	uint64_t newTimeStamp = hrt_absolute_time();
+	float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
+	_timeStamp = newTimeStamp;
+
+	// check for sane values of dt
+	// to prevent large control responses
+	if (dt > 1.0f || dt < 0) return;
+
+	// set dt for all child blocks
+	setDt(dt);
+
+	// check for new updates
+	if (_param_update.updated()) updateParams();
+
+	// get new information from subscriptions
+	updateSubscriptions();
+
+	// default all output to zero unless handled by mode
+	for (unsigned i = 2; i < NUM_ACTUATOR_CONTROLS; i++)
+		_actuators.control[i] = 0.0f;
+
+	// only update guidance in auto mode
+	if (_status.main_state == MAIN_STATE_AUTO) {
+		// update guidance
+	}
+
+	// compute speed command
+	float spdCmd = -th2v.update(_att.pitch) - q2v.update(_att.pitchspeed);
+
+	// handle autopilot modes
+	if (_status.main_state == MAIN_STATE_AUTO ||
+	    _status.main_state == MAIN_STATE_SEATBELT ||
+	    _status.main_state == MAIN_STATE_EASY) {
+		_actuators.control[0] = spdCmd;
+		_actuators.control[1] = spdCmd;
+
+	} else if (_status.main_state == MAIN_STATE_MANUAL) {
+		if (_status.navigation_state == NAVIGATION_STATE_DIRECT) {
+			_actuators.control[CH_LEFT] = _manual.throttle;
+			_actuators.control[CH_RIGHT] = _manual.pitch;
+
+		} else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) {
+			_actuators.control[0] = spdCmd;
+			_actuators.control[1] = spdCmd;
+		}
+	}
+
+	// update all publications
+	updatePublications();
+
+}
+
diff --git a/src/modules/segway/BlockSegwayController.hpp b/src/modules/segway/BlockSegwayController.hpp
new file mode 100644
index 000000000..4a01f785c
--- /dev/null
+++ b/src/modules/segway/BlockSegwayController.hpp
@@ -0,0 +1,27 @@
+#pragma once
+
+#include <controllib/uorb/blocks.hpp>
+
+using namespace control;
+
+class BlockSegwayController : public control::BlockUorbEnabledAutopilot {
+public:
+	BlockSegwayController() :
+		BlockUorbEnabledAutopilot(NULL,"SEG"),
+		th2v(this, "TH2V"),
+		q2v(this, "Q2V"),
+		_attPoll(),
+		_timeStamp(0)
+	{
+		_attPoll.fd = _att.getHandle();
+		_attPoll.events = POLLIN;
+	}
+	void update();
+private:
+	enum {CH_LEFT, CH_RIGHT};
+	BlockPI th2v;
+	BlockP q2v;
+	struct pollfd _attPoll;
+	uint64_t _timeStamp;
+};
+
diff --git a/src/modules/segway/module.mk b/src/modules/segway/module.mk
new file mode 100644
index 000000000..d5da85601
--- /dev/null
+++ b/src/modules/segway/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# segway controller
+#
+
+MODULE_COMMAND	= segway
+
+SRCS		= segway_main.cpp \
+		  BlockSegwayController.cpp \
+		  params.c
diff --git a/src/modules/segway/params.c b/src/modules/segway/params.c
new file mode 100644
index 000000000..d72923717
--- /dev/null
+++ b/src/modules/segway/params.c
@@ -0,0 +1,8 @@
+#include <systemlib/param/param.h>
+
+// 16 is max name length
+PARAM_DEFINE_FLOAT(SEG_TH2V_P, 10.0f); // pitch to voltage
+PARAM_DEFINE_FLOAT(SEG_TH2V_I, 0.0f); // pitch integral to voltage
+PARAM_DEFINE_FLOAT(SEG_TH2V_I_MAX, 0.0f); // integral limiter
+PARAM_DEFINE_FLOAT(SEG_Q2V, 1.0f); // pitch rate to voltage
+
diff --git a/src/modules/segway/segway_main.cpp b/src/modules/segway/segway_main.cpp
new file mode 100644
index 000000000..061fbf9b9
--- /dev/null
+++ b/src/modules/segway/segway_main.cpp
@@ -0,0 +1,157 @@
+/****************************************************************************
+ *
+ *   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 segway_main.cpp
+ * @author James Goppert
+ *
+ * Segway controller using control library
+ */
+
+#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 "BlockSegwayController.hpp"
+
+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 */
+
+/**
+ * Deamon management function.
+ */
+extern "C" __EXPORT int segway_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of deamon.
+ */
+int segway_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: segway {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_create().
+ */
+int segway_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;
+
+		deamon_task = task_spawn_cmd("segway",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 10,
+					 5120,
+					 segway_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");
+
+		} else {
+			warnx("not started");
+		}
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+int segway_thread_main(int argc, char *argv[])
+{
+
+	warnx("starting");
+
+	using namespace control;
+
+	BlockSegwayController autopilot;
+
+	thread_running = true;
+
+	while (!thread_should_exit) {
+		autopilot.update();
+	}
+
+	warnx("exiting.");
+
+	thread_running = false;
+
+	return 0;
+}
diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk
index ebbc580e1..aa538fd6b 100644
--- a/src/modules/sensors/module.mk
+++ b/src/modules/sensors/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
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 230060148..30659fd3a 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -1,9 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 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>
+ *   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,18 +35,109 @@
  * @file sensor_params.c
  *
  * Parameters defined by the sensors task.
+ *
+ * @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>
 
 #include <systemlib/param/param.h>
 
+/**
+ * Gyro X offset
+ *
+ * This is an X-axis offset for the gyro. Adjust it according to the calibration data.
+ *
+ * @min -10.0
+ * @max 10.0
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
+
+/**
+ * Gyro Y offset
+ *
+ * @min -10.0
+ * @max 10.0
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
+
+/**
+ * Gyro Z offset
+ *
+ * @min -5.0
+ * @max 5.0
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
 
+/**
+ * Gyro X scaling
+ *
+ * X-axis scaling.
+ *
+ * @min -1.5
+ * @max 1.5
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f);
+
+/**
+ * Gyro Y scaling
+ *
+ * Y-axis scaling.
+ *
+ * @min -1.5
+ * @max 1.5
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f);
+
+/**
+ * Gyro Z scaling
+ *
+ * Z-axis scaling.
+ *
+ * @min -1.5
+ * @max 1.5
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f);
+
+/**
+ * Magnetometer X offset
+ *
+ * This is an X-axis offset for the magnetometer.
+ *
+ * @min -500.0
+ * @max 500.0
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f);
+
+/**
+ * Magnetometer Y offset
+ *
+ * This is an Y-axis offset for the magnetometer.
+ *
+ * @min -500.0
+ * @max 500.0
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f);
+
+/**
+ * Magnetometer Z offset
+ *
+ * This is an Z-axis offset for the magnetometer.
+ *
+ * @min -500.0
+ * @max 500.0
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f);
 
 PARAM_DEFINE_FLOAT(SENS_MAG_XSCALE, 1.0f);
@@ -64,55 +152,157 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f);
 PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f);
 PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
 
-PARAM_DEFINE_INT32(SENS_DPRES_OFF, 1667);
+PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f);
+PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0);
 
+PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
+PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0);
+
+/**
+ * RC Channel 1 Minimum
+ *
+ * Minimum value for RC channel 1
+ *
+ * @min 800.0
+ * @max 1500.0
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
+
+/**
+ * RC Channel 1 Trim
+ *
+ * Mid point value (same as min for throttle)
+ *
+ * @min 800.0
+ * @max 2200.0
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
+
+/**
+ * RC Channel 1 Maximum
+ *
+ * Maximum value for RC channel 1
+ *
+ * @min 1500.0
+ * @max 2200.0
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f);
+
+/**
+ * RC Channel 1 Reverse
+ *
+ * Set to -1 to reverse channel.
+ *
+ * @min -1.0
+ * @max 1.0
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_FLOAT(RC1_REV, 1.0f);
-PARAM_DEFINE_FLOAT(RC1_DZ, 0.0f);
 
-PARAM_DEFINE_FLOAT(RC2_MIN, 1000);
-PARAM_DEFINE_FLOAT(RC2_TRIM, 1500);
-PARAM_DEFINE_FLOAT(RC2_MAX, 2000);
+/**
+ * RC Channel 1 dead zone
+ *
+ * The +- range of this value around the trim value will be considered as zero.
+ *
+ * @min 0.0
+ * @max 100.0
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_FLOAT(RC1_DZ, 10.0f);
+
+/**
+ * RC Channel 2 Minimum
+ *
+ * Minimum value for RC channel 2
+ *
+ * @min 800.0
+ * @max 1500.0
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_FLOAT(RC2_MIN, 1000.0f);
+
+/**
+ * RC Channel 2 Trim
+ *
+ * Mid point value (same as min for throttle)
+ *
+ * @min 800.0
+ * @max 2200.0
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_FLOAT(RC2_TRIM, 1500.0f);
+
+/**
+ * RC Channel 2 Maximum
+ *
+ * Maximum value for RC channel 2
+ *
+ * @min 1500.0
+ * @max 2200.0
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_FLOAT(RC2_MAX, 2000.0f);
+
+/**
+ * RC Channel 2 Reverse
+ *
+ * Set to -1 to reverse channel.
+ *
+ * @min -1.0
+ * @max 1.0
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_FLOAT(RC2_REV, 1.0f);
-PARAM_DEFINE_FLOAT(RC2_DZ, 0.0f);
+
+/**
+ * RC Channel 2 dead zone
+ *
+ * The +- range of this value around the trim value will be considered as zero.
+ *
+ * @min 0.0
+ * @max 100.0
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_FLOAT(RC2_DZ, 10.0f);
 
 PARAM_DEFINE_FLOAT(RC3_MIN, 1000);
 PARAM_DEFINE_FLOAT(RC3_TRIM, 1500);
 PARAM_DEFINE_FLOAT(RC3_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC3_REV, 1.0f);
-PARAM_DEFINE_FLOAT(RC3_DZ, 0.0f);
+PARAM_DEFINE_FLOAT(RC3_DZ, 10.0f);
 
 PARAM_DEFINE_FLOAT(RC4_MIN, 1000);
 PARAM_DEFINE_FLOAT(RC4_TRIM, 1500);
 PARAM_DEFINE_FLOAT(RC4_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC4_REV, 1.0f);
-PARAM_DEFINE_FLOAT(RC4_DZ, 30.0f);
+PARAM_DEFINE_FLOAT(RC4_DZ, 10.0f);
 
 PARAM_DEFINE_FLOAT(RC5_MIN, 1000);
 PARAM_DEFINE_FLOAT(RC5_TRIM, 1500);
 PARAM_DEFINE_FLOAT(RC5_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC5_REV, 1.0f);
-PARAM_DEFINE_FLOAT(RC5_DZ,  0.0f);
+PARAM_DEFINE_FLOAT(RC5_DZ,  10.0f);
 
 PARAM_DEFINE_FLOAT(RC6_MIN, 1000);
 PARAM_DEFINE_FLOAT(RC6_TRIM, 1500);
 PARAM_DEFINE_FLOAT(RC6_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC6_REV, 1.0f);
-PARAM_DEFINE_FLOAT(RC6_DZ, 0.0f);
+PARAM_DEFINE_FLOAT(RC6_DZ, 10.0f);
 
 PARAM_DEFINE_FLOAT(RC7_MIN, 1000);
 PARAM_DEFINE_FLOAT(RC7_TRIM, 1500);
 PARAM_DEFINE_FLOAT(RC7_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC7_REV, 1.0f);
-PARAM_DEFINE_FLOAT(RC7_DZ, 0.0f);
+PARAM_DEFINE_FLOAT(RC7_DZ, 10.0f);
 
 PARAM_DEFINE_FLOAT(RC8_MIN, 1000);
 PARAM_DEFINE_FLOAT(RC8_TRIM, 1500);
 PARAM_DEFINE_FLOAT(RC8_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC8_REV, 1.0f);
-PARAM_DEFINE_FLOAT(RC8_DZ, 0.0f);
+PARAM_DEFINE_FLOAT(RC8_DZ, 10.0f);
 
 PARAM_DEFINE_FLOAT(RC9_MIN, 1000);
 PARAM_DEFINE_FLOAT(RC9_TRIM, 1500);
@@ -150,32 +340,128 @@ PARAM_DEFINE_FLOAT(RC14_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC14_REV, 1.0f);
 PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f);
 
-PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */
+PARAM_DEFINE_FLOAT(RC15_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC15_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC15_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC15_REV, 1.0f);
+PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f);
+
+PARAM_DEFINE_FLOAT(RC16_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC16_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC16_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC16_REV, 1.0f);
+PARAM_DEFINE_FLOAT(RC16_DZ, 0.0f);
+
+PARAM_DEFINE_FLOAT(RC17_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC17_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC17_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC17_REV, 1.0f);
+PARAM_DEFINE_FLOAT(RC17_DZ, 0.0f);
+
+PARAM_DEFINE_FLOAT(RC18_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC18_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC18_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC18_REV, 1.0f);
+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 */
+
+PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000);
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
+#else
 /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
-PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f));
+/* 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) */
+PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f);
+#endif
+PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124);	/* scaling for 3DR power brick */
 
+/**
+ * Roll control channel mapping.
+ *
+ * The channel index (starting from 1 for channel 1) indicates
+ * which channel should be used for reading roll inputs from.
+ * A value of zero indicates the switch is not assigned.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
+
+/**
+ * Pitch control channel mapping.
+ *
+ * The channel index (starting from 1 for channel 1) indicates
+ * which channel should be used for reading pitch inputs from.
+ * A value of zero indicates the switch is not assigned.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
+
+/**
+ * Throttle control channel mapping.
+ *
+ * The channel index (starting from 1 for channel 1) indicates
+ * which channel should be used for reading throttle inputs from.
+ * A value of zero indicates the switch is not assigned.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
+
+/**
+ * Yaw control channel mapping.
+ *
+ * The channel index (starting from 1 for channel 1) indicates
+ * which channel should be used for reading yaw inputs from.
+ * A value of zero indicates the switch is not assigned.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
 
-PARAM_DEFINE_INT32(RC_MAP_OVER_SW, 5);
-PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 6);
+/**
+ * Mode switch channel mapping.
+ *
+ * This is the main flight mode selector.
+ * The channel index (starting from 1 for channel 1) indicates
+ * which channel should be used for deciding about the main mode.
+ * A value of zero indicates the switch is not assigned.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0);
+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_MAN_SW, 0);
-PARAM_DEFINE_INT32(RC_MAP_SAS_SW, 0);
-PARAM_DEFINE_INT32(RC_MAP_RTL_SW, 0);
-PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
+//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
 
 PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0);
 
-PARAM_DEFINE_INT32(RC_MAP_AUX1, 0);	/**< default function: camera yaw / azimuth */
-PARAM_DEFINE_INT32(RC_MAP_AUX2, 0);	/**< default function: camera pitch / tilt */
-PARAM_DEFINE_INT32(RC_MAP_AUX3, 0);	/**< default function: camera trigger */
-PARAM_DEFINE_INT32(RC_MAP_AUX4, 0);	/**< default function: camera roll */
-PARAM_DEFINE_INT32(RC_MAP_AUX5, 0);	/**< default function: payload drop */
+PARAM_DEFINE_INT32(RC_MAP_AUX1, 0);	/**< default function: camera pitch */
+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);
 
-PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.4f);
-PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.4f);
-PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 1.0f);
+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 */
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 8bf5fdbd0..b50a694eb 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -50,6 +50,7 @@
 #include <stdio.h>
 #include <errno.h>
 #include <math.h>
+#include <mathlib/mathlib.h>
 
 #include <nuttx/analog/adc.h>
 
@@ -60,20 +61,22 @@
 #include <drivers/drv_baro.h>
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_adc.h>
+#include <drivers/drv_airspeed.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/perf_counter.h>
+#include <conversion/rotation.h>
 
-#include <systemlib/ppm_decode.h>
 #include <systemlib/airspeed.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/differential_pressure.h>
@@ -91,13 +94,39 @@
 #define BARO_HEALTH_COUNTER_LIMIT_OK 5
 #define ADC_HEALTH_COUNTER_LIMIT_OK  5
 
+/**
+ * Analog layout:
+ * FMU:
+ * IN2 - battery voltage
+ * IN3 - battery current
+ * IN4 - 5V sense
+ * IN10 - spare (we could actually trim these from the set)
+ * IN11 - spare (we could actually trim these from the set)
+ * IN12 - spare (we could actually trim these from the set)
+ * IN13 - aux1
+ * IN14 - aux2
+ * IN15 - pressure sensor
+ *
+ * IO:
+ * IN4 - servo supply rail
+ * IN5 - analog RSSI
+ */
+
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 #define ADC_BATTERY_VOLTAGE_CHANNEL	10
+#define ADC_BATTERY_CURRENT_CHANNEL	-1
 #define ADC_AIRSPEED_VOLTAGE_CHANNEL	11
+#endif
 
-#define BAT_VOL_INITIAL 0.f
-#define BAT_VOL_LOWPASS_1 0.99f
-#define BAT_VOL_LOWPASS_2 0.01f
-#define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+#define ADC_BATTERY_VOLTAGE_CHANNEL	2
+#define ADC_BATTERY_CURRENT_CHANNEL	3
+#define ADC_5V_RAIL_SENSE		4
+#define ADC_AIRSPEED_VOLTAGE_CHANNEL	15
+#endif
+
+#define BATT_V_LOWPASS 0.001f
+#define BATT_V_IGNORE_THRESHOLD 3.5f
 
 /**
  * HACK - true temperature is much less than indicated temperature in baro,
@@ -105,8 +134,6 @@
  */
 #define PCB_TEMP_ESTIMATE_DEG 5.0f
 
-#define PPM_INPUT_TIMEOUT_INTERVAL	50000 /**< 50 ms timeout / 20 Hz */
-
 #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
 
 /**
@@ -137,16 +164,14 @@ public:
 	int		start();
 
 private:
-	static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX;	/**< maximum number of r/c channels we handle */
+	static const unsigned _rc_max_chan_count = RC_INPUT_MAX_CHANNELS;	/**< maximum number of r/c channels we handle */
 
-#if CONFIG_HRT_PPM
-	hrt_abstime	_ppm_last_valid;		/**< last time we got a valid ppm signal */
+	hrt_abstime	_rc_last_valid;		/**< last time we got a valid RC signal */
 
 	/**
-	 * Gather and publish PPM input data.
+	 * Gather and publish RC input data.
 	 */
-	void		ppm_poll();
-#endif
+	void		rc_poll();
 
 	/* XXX should not be here - should be own driver */
 	int 		_fd_adc;			/**< ADC driver handle */
@@ -163,14 +188,15 @@ private:
 	int		_mag_sub;			/**< raw mag data subscription */
 	int 		_rc_sub;			/**< raw rc channels data subscription */
 	int		_baro_sub;			/**< raw baro data subscription */
-	int     _airspeed_sub;		/**< airspeed subscription */
-	int		_diff_pres_sub;		/**< raw differential pressure subscription */
-	int		_vstatus_sub;			/**< vehicle status subscription */
+	int		_airspeed_sub;			/**< airspeed subscription */
+	int		_diff_pres_sub;			/**< raw differential pressure subscription */
+	int		_vcontrol_mode_sub;			/**< vehicle control mode subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
 	int 		_manual_control_sub;			/**< notification of manual control updates */
 
 	orb_advert_t	_sensor_pub;			/**< combined sensor data topic */
 	orb_advert_t	_manual_control_pub;		/**< manual control signal topic */
+	orb_advert_t	_actuator_group_3_pub;		/**< manual control as actuator topic */
 	orb_advert_t	_rc_pub;			/**< raw r/c control topic */
 	orb_advert_t	_battery_pub;			/**< battery status */
 	orb_advert_t	_airspeed_pub;			/**< airspeed */
@@ -184,36 +210,44 @@ 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 */
+	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 */
+
 	struct {
 		float min[_rc_max_chan_count];
 		float trim[_rc_max_chan_count];
 		float max[_rc_max_chan_count];
 		float rev[_rc_max_chan_count];
 		float dz[_rc_max_chan_count];
-		// float ex[_rc_max_chan_count];
 		float scaling_factor[_rc_max_chan_count];
 
 		float gyro_offset[3];
+		float gyro_scale[3];
 		float mag_offset[3];
 		float mag_scale[3];
 		float accel_offset[3];
 		float accel_scale[3];
-		int diff_pres_offset_pa;
+		float diff_pres_offset_pa;
+		float diff_pres_analog_enabled;
 
-		int rc_type;
+		int board_rotation;
+		int external_mag_rotation;
 
 		int rc_map_roll;
 		int rc_map_pitch;
 		int rc_map_yaw;
 		int rc_map_throttle;
 
-		int rc_map_manual_override_sw;
-		int rc_map_auto_mode_sw;
+		int rc_map_mode_sw;
+		int rc_map_return_sw;
+		int rc_map_assisted_sw;
+		int rc_map_mission_sw;
 
-		int rc_map_manual_mode_sw;
-		int rc_map_sas_mode_sw;
-		int rc_map_rtl_sw;
-		int rc_map_offboard_ctrl_mode_sw;
+//		int rc_map_offboard_ctrl_mode_sw;
 
 		int rc_map_flaps;
 
@@ -228,7 +262,13 @@ private:
 		float rc_scale_yaw;
 		float rc_scale_flaps;
 
+		int rc_fs_ch;
+		int rc_fs_mode;
+		float rc_fs_thr;
+
 		float battery_voltage_scaling;
+		float battery_current_scaling;
+
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -237,30 +277,27 @@ private:
 		param_t max[_rc_max_chan_count];
 		param_t rev[_rc_max_chan_count];
 		param_t dz[_rc_max_chan_count];
-		// param_t ex[_rc_max_chan_count];
-		param_t rc_type;
-
-		param_t rc_demix;
 
 		param_t gyro_offset[3];
+		param_t gyro_scale[3];
 		param_t accel_offset[3];
 		param_t accel_scale[3];
 		param_t mag_offset[3];
 		param_t mag_scale[3];
 		param_t diff_pres_offset_pa;
+		param_t diff_pres_analog_enabled;
 
 		param_t rc_map_roll;
 		param_t rc_map_pitch;
 		param_t rc_map_yaw;
 		param_t rc_map_throttle;
 
-		param_t rc_map_manual_override_sw;
-		param_t rc_map_auto_mode_sw;
+		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_manual_mode_sw;
-		param_t rc_map_sas_mode_sw;
-		param_t rc_map_rtl_sw;
-		param_t rc_map_offboard_ctrl_mode_sw;
+//		param_t rc_map_offboard_ctrl_mode_sw;
 
 		param_t rc_map_flaps;
 
@@ -275,7 +312,16 @@ private:
 		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 battery_voltage_scaling;
+		param_t battery_current_scaling;
+
+		param_t board_rotation;
+		param_t external_mag_rotation;
+
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -350,9 +396,9 @@ private:
 	void		diff_pres_poll(struct sensor_combined_s &raw);
 
 	/**
-	 * Check for changes in vehicle status.
+	 * Check for changes in vehicle control mode.
 	 */
-	void		vehicle_status_poll();
+	void		vehicle_control_mode_poll();
 
 	/**
 	 * Check for changes in parameters.
@@ -387,13 +433,11 @@ namespace sensors
 #endif
 static const int ERROR = -1;
 
-Sensors	*g_sensors;
+Sensors	*g_sensors = nullptr;
 }
 
 Sensors::Sensors() :
-#ifdef CONFIG_HRT_PPM
-	_ppm_last_valid(0),
-#endif
+	_rc_last_valid(0),
 
 	_fd_adc(-1),
 	_last_adc(0),
@@ -409,20 +453,27 @@ Sensors::Sensors() :
 	_mag_sub(-1),
 	_rc_sub(-1),
 	_baro_sub(-1),
-	_vstatus_sub(-1),
+	_vcontrol_mode_sub(-1),
 	_params_sub(-1),
 	_manual_control_sub(-1),
 
 /* publications */
 	_sensor_pub(-1),
 	_manual_control_pub(-1),
+	_actuator_group_3_pub(-1),
 	_rc_pub(-1),
 	_battery_pub(-1),
 	_airspeed_pub(-1),
 	_diff_pres_pub(-1),
 
 /* performance counters */
-	_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update"))
+	_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)
 {
 
 	/* basic r/c parameters */
@@ -451,8 +502,6 @@ Sensors::Sensors() :
 
 	}
 
-	_parameter_handles.rc_type = param_find("RC_TYPE");
-
 	/* mandatory input switched, mapped to channels 1-4 per default */
 	_parameter_handles.rc_map_roll 	= param_find("RC_MAP_ROLL");
 	_parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH");
@@ -460,16 +509,16 @@ Sensors::Sensors() :
 	_parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE");
 
 	/* mandatory mode switches, mapped to channel 5 and 6 per default */
-	_parameter_handles.rc_map_manual_override_sw = param_find("RC_MAP_OVER_SW");
-	_parameter_handles.rc_map_auto_mode_sw = param_find("RC_MAP_MODE_SW");
+	_parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW");
+	_parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW");
 
 	_parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS");
 
 	/* optional mode switches, not mapped per default */
-	_parameter_handles.rc_map_manual_mode_sw = param_find("RC_MAP_MAN_SW");
-	_parameter_handles.rc_map_sas_mode_sw = param_find("RC_MAP_SAS_SW");
-	_parameter_handles.rc_map_rtl_sw = param_find("RC_MAP_RTL_SW");
-	_parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW");
+	_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_aux1 = param_find("RC_MAP_AUX1");
 	_parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2");
@@ -482,10 +531,18 @@ Sensors::Sensors() :
 	_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");
+
 	/* gyro offsets */
 	_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
 	_parameter_handles.gyro_offset[1] = param_find("SENS_GYRO_YOFF");
 	_parameter_handles.gyro_offset[2] = param_find("SENS_GYRO_ZOFF");
+	_parameter_handles.gyro_scale[0] = param_find("SENS_GYRO_XSCALE");
+	_parameter_handles.gyro_scale[1] = param_find("SENS_GYRO_YSCALE");
+	_parameter_handles.gyro_scale[2] = param_find("SENS_GYRO_ZSCALE");
 
 	/* accel offsets */
 	_parameter_handles.accel_offset[0] = param_find("SENS_ACC_XOFF");
@@ -506,8 +563,14 @@ 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.battery_voltage_scaling = param_find("BAT_V_SCALING");
+	_parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING");
+
+	/* rotations */
+	_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
+	_parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -542,53 +605,39 @@ int
 Sensors::parameters_update()
 {
 	bool rc_valid = true;
+	float tmpScaleFactor = 0.0f;
+	float tmpRevFactor = 0.0f;
 
 	/* rc values */
-	for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) {
+	for (unsigned int i = 0; i < _rc_max_chan_count; i++) {
 
-		if (param_get(_parameter_handles.min[i], &(_parameters.min[i])) != OK) {
-			warnx("Failed getting min for chan %d", i);
-		}
-
-		if (param_get(_parameter_handles.trim[i], &(_parameters.trim[i])) != OK) {
-			warnx("Failed getting trim for chan %d", i);
-		}
+		param_get(_parameter_handles.min[i], &(_parameters.min[i]));
+		param_get(_parameter_handles.trim[i], &(_parameters.trim[i]));
+		param_get(_parameter_handles.max[i], &(_parameters.max[i]));
+		param_get(_parameter_handles.rev[i], &(_parameters.rev[i]));
+		param_get(_parameter_handles.dz[i], &(_parameters.dz[i]));
 
-		if (param_get(_parameter_handles.max[i], &(_parameters.max[i])) != OK) {
-			warnx("Failed getting max for chan %d", i);
-		}
-
-		if (param_get(_parameter_handles.rev[i], &(_parameters.rev[i])) != OK) {
-			warnx("Failed getting rev for chan %d", i);
-		}
-
-		if (param_get(_parameter_handles.dz[i], &(_parameters.dz[i])) != OK) {
-			warnx("Failed getting dead zone for chan %d", i);
-		}
-
-		_parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
+		tmpScaleFactor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
+		tmpRevFactor = tmpScaleFactor * _parameters.rev[i];
 
 		/* handle blowup in the scaling factor calculation */
-		if (!isfinite(_parameters.scaling_factor[i]) ||
-		    _parameters.scaling_factor[i] * _parameters.rev[i] < 0.000001f ||
-		    _parameters.scaling_factor[i] * _parameters.rev[i] > 0.2f) {
-
+		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]));
 			/* scaling factors do not make sense, lock them down */
-			_parameters.scaling_factor[i] = 0;
+			_parameters.scaling_factor[i] = 0.0f;
 			rc_valid = false;
-		}
 
+		} else {
+			_parameters.scaling_factor[i] = tmpScaleFactor;
+		}
 	}
 
 	/* handle wrong values */
 	if (!rc_valid)
 		warnx("WARNING     WARNING     WARNING\n\nRC CALIBRATION NOT SANE!\n\n");
 
-	/* remote control type */
-	if (param_get(_parameter_handles.rc_type, &(_parameters.rc_type)) != OK) {
-		warnx("Failed getting remote control type");
-	}
-
 	/* channel mapping */
 	if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) {
 		warnx("Failed getting roll chan index");
@@ -606,69 +655,42 @@ Sensors::parameters_update()
 		warnx("Failed getting throttle chan index");
 	}
 
-	if (param_get(_parameter_handles.rc_map_manual_override_sw, &(_parameters.rc_map_manual_override_sw)) != OK) {
-		warnx("Failed getting override sw chan index");
-	}
-
-	if (param_get(_parameter_handles.rc_map_auto_mode_sw, &(_parameters.rc_map_auto_mode_sw)) != OK) {
-		warnx("Failed getting auto mode sw chan index");
-	}
-
-	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_manual_mode_sw, &(_parameters.rc_map_manual_mode_sw)) != OK) {
-		warnx("Failed getting manual mode sw chan index");
-	}
-
-	if (param_get(_parameter_handles.rc_map_rtl_sw, &(_parameters.rc_map_rtl_sw)) != OK) {
-		warnx("Failed getting rtl sw chan index");
-	}
-
-	if (param_get(_parameter_handles.rc_map_sas_mode_sw, &(_parameters.rc_map_sas_mode_sw)) != OK) {
-		warnx("Failed getting sas mode sw chan index");
-	}
-
-	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_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) {
+		warnx("Failed getting mode sw chan index");
 	}
 
-	if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) {
-		warnx("Failed getting mode aux 1 index");
+	if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) {
+		warnx("Failed getting return sw chan index");
 	}
 
-	if (param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)) != OK) {
-		warnx("Failed getting mode aux 2 index");
+	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_aux3, &(_parameters.rc_map_aux3)) != OK) {
-		warnx("Failed getting mode aux 3 index");
+	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_aux4, &(_parameters.rc_map_aux4)) != OK) {
-		warnx("Failed getting mode aux 4 index");
-	}
-
-	if (param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)) != OK) {
-		warnx("Failed getting mode aux 5 index");
-	}
-
-	if (param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)) != OK) {
-		warnx("Failed getting rc scaling for roll");
-	}
-
-	if (param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)) != OK) {
-		warnx("Failed getting rc scaling for pitch");
-	}
-
-	if (param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)) != OK) {
-		warnx("Failed getting rc scaling for yaw");
+	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_scale_flaps, &(_parameters.rc_scale_flaps)) != OK) {
-		warnx("Failed getting rc scaling for flaps");
-	}
+//	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");
+//	}
+
+	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));
 
 	/* update RC function mappings */
 	_rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
@@ -676,15 +698,14 @@ Sensors::parameters_update()
 	_rc.function[PITCH] = _parameters.rc_map_pitch - 1;
 	_rc.function[YAW] = _parameters.rc_map_yaw - 1;
 
-	_rc.function[OVERRIDE] = _parameters.rc_map_manual_override_sw - 1;
-	_rc.function[AUTO_MODE] = _parameters.rc_map_auto_mode_sw - 1;
+	_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[FLAPS] = _parameters.rc_map_flaps - 1;
 
-	_rc.function[MANUAL_MODE] = _parameters.rc_map_manual_mode_sw - 1;
-	_rc.function[RTL] = _parameters.rc_map_rtl_sw - 1;
-	_rc.function[SAS_MODE] = _parameters.rc_map_sas_mode_sw - 1;
-	_rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 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;
@@ -696,6 +717,9 @@ Sensors::parameters_update()
 	param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0]));
 	param_get(_parameter_handles.gyro_offset[1], &(_parameters.gyro_offset[1]));
 	param_get(_parameter_handles.gyro_offset[2], &(_parameters.gyro_offset[2]));
+	param_get(_parameter_handles.gyro_scale[0], &(_parameters.gyro_scale[0]));
+	param_get(_parameter_handles.gyro_scale[1], &(_parameters.gyro_scale[1]));
+	param_get(_parameter_handles.gyro_scale[2], &(_parameters.gyro_scale[2]));
 
 	/* accel offsets */
 	param_get(_parameter_handles.accel_offset[0], &(_parameters.accel_offset[0]));
@@ -716,12 +740,24 @@ 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));
 
 	/* 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");
 	}
 
+	/* 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");
+	}
+
+	param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
+	param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation));
+
+	get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
+	get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
+
 	return OK;
 }
 
@@ -737,13 +773,30 @@ Sensors::accel_init()
 		errx(1, "FATAL: no accelerometer found");
 
 	} else {
-		/* set the accel internal sampling rate up to at leat 500Hz */
-		ioctl(fd, ACCELIOCSSAMPLERATE, 500);
 
-		/* set the driver to poll at 500Hz */
-		ioctl(fd, SENSORIOCSPOLLRATE, 500);
+		// XXX do the check more elegantly
+
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+
+		/* set the accel internal sampling rate up to at leat 1000Hz */
+		ioctl(fd, ACCELIOCSSAMPLERATE, 1000);
+
+		/* set the driver to poll at 1000Hz */
+		ioctl(fd, SENSORIOCSPOLLRATE, 1000);
+
+#elif CONFIG_ARCH_BOARD_PX4FMU_V2
+
+		/* set the accel internal sampling rate up to at leat 800Hz */
+		ioctl(fd, ACCELIOCSSAMPLERATE, 800);
+
+		/* set the driver to poll at 800Hz */
+		ioctl(fd, SENSORIOCSPOLLRATE, 800);
+
+#else
+#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
+
+#endif
 
-		warnx("using system accel");
 		close(fd);
 	}
 }
@@ -760,13 +813,29 @@ Sensors::gyro_init()
 		errx(1, "FATAL: no gyro found");
 
 	} else {
-		/* set the gyro internal sampling rate up to at leat 500Hz */
-		ioctl(fd, GYROIOCSSAMPLERATE, 500);
 
-		/* set the driver to poll at 500Hz */
-		ioctl(fd, SENSORIOCSPOLLRATE, 500);
+		// XXX do the check more elegantly
+
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+
+		/* set the gyro internal sampling rate up to at least 1000Hz */
+		if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK)
+			ioctl(fd, GYROIOCSSAMPLERATE, 800);
+
+		/* set the driver to poll at 1000Hz */
+		if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK)
+			ioctl(fd, SENSORIOCSPOLLRATE, 800);
+
+#else
+
+		/* set the gyro internal sampling rate up to at least 760Hz */
+		ioctl(fd, GYROIOCSSAMPLERATE, 760);
+
+		/* set the driver to poll at 760Hz */
+		ioctl(fd, SENSORIOCSPOLLRATE, 760);
+
+#endif
 
-		warnx("using system gyro");
 		close(fd);
 	}
 }
@@ -775,6 +844,7 @@ void
 Sensors::mag_init()
 {
 	int	fd;
+	int	ret;
 
 	fd = open(MAG_DEVICE_PATH, 0);
 
@@ -783,11 +853,38 @@ Sensors::mag_init()
 		errx(1, "FATAL: no magnetometer found");
 	}
 
-	/* set the mag internal poll rate to at least 150Hz */
-	ioctl(fd, MAGIOCSSAMPLERATE, 150);
+	/* try different mag sampling rates */
 
-	/* set the driver to poll at 150Hz */
-	ioctl(fd, SENSORIOCSPOLLRATE, 150);
+
+	ret = ioctl(fd, MAGIOCSSAMPLERATE, 150);
+
+	if (ret == OK) {
+		/* set the pollrate accordingly */
+		ioctl(fd, SENSORIOCSPOLLRATE, 150);
+
+	} else {
+		ret = ioctl(fd, MAGIOCSSAMPLERATE, 100);
+
+		/* if the slower sampling rate still fails, something is wrong */
+		if (ret == OK) {
+			/* set the driver to poll also at the slower rate */
+			ioctl(fd, SENSORIOCSPOLLRATE, 100);
+
+		} else {
+			errx(1, "FATAL: mag sampling rate could not be set");
+		}
+	}
+
+
+
+	ret = ioctl(fd, MAGIOCGEXTERNAL, 0);
+
+	if (ret < 0)
+		errx(1, "FATAL: unknown if magnetometer is external or onboard");
+	else if (ret == 1)
+		_mag_is_external = true;
+	else
+		_mag_is_external = false;
 
 	close(fd);
 }
@@ -801,7 +898,7 @@ Sensors::baro_init()
 
 	if (fd < 0) {
 		warn("%s", BARO_DEVICE_PATH);
-		warnx("No barometer found, ignoring");
+		errx(1, "FATAL: No barometer found");
 	}
 
 	/* set the driver to poll at 150Hz */
@@ -833,9 +930,12 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
 
-		raw.accelerometer_m_s2[0] = accel_report.x;
-		raw.accelerometer_m_s2[1] = accel_report.y;
-		raw.accelerometer_m_s2[2] = accel_report.z;
+		math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z};
+		vect = _board_rotation * vect;
+
+		raw.accelerometer_m_s2[0] = vect(0);
+		raw.accelerometer_m_s2[1] = vect(1);
+		raw.accelerometer_m_s2[2] = vect(2);
 
 		raw.accelerometer_raw[0] = accel_report.x_raw;
 		raw.accelerometer_raw[1] = accel_report.y_raw;
@@ -856,9 +956,12 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
 
-		raw.gyro_rad_s[0] = gyro_report.x;
-		raw.gyro_rad_s[1] = gyro_report.y;
-		raw.gyro_rad_s[2] = gyro_report.z;
+		math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z};
+		vect = _board_rotation * vect;
+
+		raw.gyro_rad_s[0] = vect(0);
+		raw.gyro_rad_s[1] = vect(1);
+		raw.gyro_rad_s[2] = vect(2);
 
 		raw.gyro_raw[0] = gyro_report.x_raw;
 		raw.gyro_raw[1] = gyro_report.y_raw;
@@ -879,9 +982,16 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
 
-		raw.magnetometer_ga[0] = mag_report.x;
-		raw.magnetometer_ga[1] = mag_report.y;
-		raw.magnetometer_ga[2] = mag_report.z;
+		math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z};
+
+		if (_mag_is_external)
+			vect = _external_mag_rotation * vect;
+		else
+			vect = _board_rotation * vect;
+
+		raw.magnetometer_ga[0] = vect(0);
+		raw.magnetometer_ga[1] = vect(1);
+		raw.magnetometer_ga[2] = vect(2);
 
 		raw.magnetometer_raw[0] = mag_report.x_raw;
 		raw.magnetometer_raw[1] = mag_report.y_raw;
@@ -922,8 +1032,8 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 		raw.differential_pressure_counter++;
 
 		_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);
+		_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);
 
 		/* announce the airspeed if needed, just publish else */
 		if (_airspeed_pub > 0) {
@@ -936,21 +1046,21 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 }
 
 void
-Sensors::vehicle_status_poll()
+Sensors::vehicle_control_mode_poll()
 {
-	struct vehicle_status_s vstatus;
-	bool vstatus_updated;
+	struct vehicle_control_mode_s vcontrol_mode;
+	bool vcontrol_mode_updated;
 
-	/* Check HIL state if vehicle status has changed */
-	orb_check(_vstatus_sub, &vstatus_updated);
+	/* Check HIL state if vehicle control mode has changed */
+	orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated);
 
-	if (vstatus_updated) {
+	if (vcontrol_mode_updated) {
 
-		orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &vstatus);
+		orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode);
 
 		/* switching from non-HIL to HIL mode */
 		//printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled);
-		if (vstatus.flag_hil_enabled && !_hil_enabled) {
+		if (vcontrol_mode.flag_system_hil_enabled && !_hil_enabled) {
 			_hil_enabled = true;
 			_publishing = false;
 
@@ -983,11 +1093,11 @@ Sensors::parameter_update_poll(bool forced)
 		int fd = open(GYRO_DEVICE_PATH, 0);
 		struct gyro_scale gscale = {
 			_parameters.gyro_offset[0],
-			1.0f,
+			_parameters.gyro_scale[0],
 			_parameters.gyro_offset[1],
-			1.0f,
+			_parameters.gyro_scale[1],
 			_parameters.gyro_offset[2],
-			1.0f,
+			_parameters.gyro_scale[2],
 		};
 
 		if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
@@ -1025,6 +1135,21 @@ Sensors::parameter_update_poll(bool forced)
 
 		close(fd);
 
+		fd = open(AIRSPEED_DEVICE_PATH, 0);
+
+		/* this sensor is optional, abort without error */
+
+		if (fd > 0) {
+			struct airspeed_scale airscale = {
+				_parameters.diff_pres_offset_pa,
+				1.0f,
+			};
+
+			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]);
@@ -1038,21 +1163,23 @@ Sensors::parameter_update_poll(bool forced)
 void
 Sensors::adc_poll(struct sensor_combined_s &raw)
 {
+	/* only read if publishing */
+	if (!_publishing)
+		return;
 
+	hrt_abstime t = hrt_absolute_time();
 	/* rate limit to 100 Hz */
-	if (hrt_absolute_time() - _last_adc >= 10000) {
+	if (t - _last_adc >= 10000) {
 		/* make space for a maximum of eight channels */
 		struct adc_msg_s buf_adc[8];
 		/* read all channels available */
 		int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc));
 
-		for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
-			
-			if (ret >= (int)sizeof(buf_adc[0])) {
-
+		if (ret >= (int)sizeof(buf_adc[0])) {
+			for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
 				/* Save raw voltage values */
 				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_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f);
 				}
 
 				/* look for specific channels and process the raw voltage to measurement data */
@@ -1060,42 +1187,56 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 					/* Voltage in volts */
 					float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling);
 
-					if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
-
+					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_v < 3.0f) {
-							_battery_status.voltage_v = voltage;
+						if (_battery_status.voltage_filtered_v < BATT_V_IGNORE_THRESHOLD) {
+							_battery_status.voltage_filtered_v = voltage;
 						}
 
-						_battery_status.timestamp = hrt_absolute_time();
-						_battery_status.voltage_v = (BAT_VOL_LOWPASS_1 * (_battery_status.voltage_v + BAT_VOL_LOWPASS_2 * voltage));;
-						/* current and discharge are unknown */
-						_battery_status.current_a = -1.0f;
-						_battery_status.discharged_mah = -1.0f;
+						_battery_status.timestamp = t;
+						_battery_status.voltage_filtered_v += (voltage - _battery_status.voltage_filtered_v) * BATT_V_LOWPASS;
 
-						/* announce the battery voltage if needed, just publish else */
-						if (_battery_pub > 0) {
-							orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
+					} else {
+						/* mark status as invalid */
+						_battery_status.voltage_v = -1.0f;
+						_battery_status.voltage_filtered_v = -1.0f;
+					}
 
-						} else {
-							_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
+				} else if (ADC_BATTERY_CURRENT_CHANNEL == buf_adc[i].am_channel) {
+					/* 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)
+									_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
+					 * a valid voltage from a connected sensor. Also assume a non-
+					 * zero offset from the sensor if its connected.
 					 */
-					if (voltage > 0.4f) {
+					if (voltage > 0.4f && _parameters.diff_pres_analog_enabled) {
 
 						float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor
 
-						_diff_pres.timestamp = hrt_absolute_time();
+						_diff_pres.timestamp = t;
 						_diff_pres.differential_pressure_pa = diff_pres_pa;
 						_diff_pres.voltage = voltage;
 
@@ -1108,28 +1249,38 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 						}
 					}
 				}
-
-				_last_adc = hrt_absolute_time();
+			}
+			_last_adc = t;
+			if (_battery_status.voltage_v > 0.0f) {
+				/* announce the battery status if needed, just publish else */
+				if (_battery_pub > 0) {
+					orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
+
+				} else {
+					_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
+				}
 			}
 		}
 	}
 }
 
-#if CONFIG_HRT_PPM
 void
-Sensors::ppm_poll()
+Sensors::rc_poll()
 {
-
-	/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
 	bool rc_updated;
 	orb_check(_rc_sub, &rc_updated);
 
 	if (rc_updated) {
+		/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
 		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;
@@ -1137,10 +1288,11 @@ Sensors::ppm_poll()
 		manual_control.yaw = NAN;
 		manual_control.throttle = NAN;
 
-		manual_control.manual_mode_switch = NAN;
-		manual_control.manual_sas_switch = NAN;
-		manual_control.return_to_launch_switch = NAN;
-		manual_control.auto_offboard_input_switch = 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;
@@ -1153,13 +1305,25 @@ Sensors::ppm_poll()
 		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;
+			}
+		}
+
 		unsigned channel_limit = rc_input.channel_count;
 
 		if (channel_limit > _rc_max_chan_count)
 			channel_limit = _rc_max_chan_count;
 
 		/* we are accepting this message */
-		_ppm_last_valid = rc_input.timestamp;
+		_rc_last_valid = rc_input.timestamp_last_signal;
 
 		/* Read out values from raw message */
 		for (unsigned int i = 0; i < channel_limit; i++) {
@@ -1169,6 +1333,7 @@ Sensors::ppm_poll()
 			 */
 			if (rc_input.values[i] < _parameters.min[i])
 				rc_input.values[i] = _parameters.min[i];
+
 			if (rc_input.values[i] > _parameters.max[i])
 				rc_input.values[i] = _parameters.max[i];
 
@@ -1207,9 +1372,9 @@ Sensors::ppm_poll()
 		}
 
 		_rc.chan_count = rc_input.channel_count;
-		_rc.timestamp = rc_input.timestamp;
+		_rc.timestamp = rc_input.timestamp_last_signal;
 
-		manual_control.timestamp = rc_input.timestamp;
+		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);
@@ -1240,12 +1405,6 @@ Sensors::ppm_poll()
 			manual_control.yaw *= _parameters.rc_scale_yaw;
 		}
 
-		/* override switch input */
-		manual_control.manual_override_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OVERRIDE]].scaled);
-
-		/* mode switch input */
-		manual_control.auto_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[AUTO_MODE]].scaled);
-
 		/* flaps */
 		if (_rc.function[FLAPS] >= 0) {
 
@@ -1256,22 +1415,28 @@ Sensors::ppm_poll()
 			}
 		}
 
-		if (_rc.function[MANUAL_MODE] >= 0) {
-			manual_control.manual_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MANUAL_MODE]].scaled);
+		if (_rc.function[MODE] >= 0) {
+			manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
 		}
 
-		if (_rc.function[SAS_MODE] >= 0) {
-			manual_control.manual_sas_switch = limit_minus_one_to_one(_rc.chan[_rc.function[SAS_MODE]].scaled);
+		if (_rc.function[MISSION] >= 0) {
+			manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
 		}
 
-		if (_rc.function[RTL] >= 0) {
-			manual_control.return_to_launch_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RTL]].scaled);
+		/* land switch input */
+		if (_rc.function[RETURN] >= 0) {
+			manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].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);
+		/* 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);
@@ -1293,6 +1458,16 @@ Sensors::ppm_poll()
 			manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled);
 		}
 
+		/* 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 */
 		if (_rc_pub > 0) {
 			orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
@@ -1309,10 +1484,17 @@ Sensors::ppm_poll()
 		} else {
 			_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
 		}
+
+		/* 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);
+
+		} else {
+			_actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3);
+		}
 	}
 
 }
-#endif
 
 void
 Sensors::task_main_trampoline(int argc, char *argv[])
@@ -1324,10 +1506,6 @@ void
 Sensors::task_main()
 {
 
-	/* inform about start */
-	printf("[sensors] Initializing..\n");
-	fflush(stdout);
-
 	/* start individual sensors */
 	accel_init();
 	gyro_init();
@@ -1344,12 +1522,15 @@ Sensors::task_main()
 	_rc_sub = orb_subscribe(ORB_ID(input_rc));
 	_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
 	_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
-	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 
 	/* rate limit vehicle status updates to 5Hz */
-	orb_set_interval(_vstatus_sub, 200);
+	orb_set_interval(_vcontrol_mode_sub, 200);
+
+	/* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */
+	orb_set_interval(_gyro_sub, 4);
 
 	/*
 	 * do advertisements
@@ -1363,7 +1544,10 @@ Sensors::task_main()
 	raw.adc_voltage_v[3] = 0.0f;
 
 	memset(&_battery_status, 0, sizeof(_battery_status));
-	_battery_status.voltage_v = BAT_VOL_INITIAL;
+	_battery_status.voltage_v = -1.0f;
+	_battery_status.voltage_filtered_v = -1.0f;
+	_battery_status.current_a = -1.0f;
+	_battery_status.discharged_mah = -1.0f;
 
 	/* get a set of initial values */
 	accel_poll(raw);
@@ -1386,7 +1570,7 @@ Sensors::task_main()
 
 	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. */
@@ -1402,7 +1586,7 @@ Sensors::task_main()
 		perf_begin(_loop_perf);
 
 		/* check vehicle status for changes to publication state */
-		vehicle_status_poll();
+		vehicle_control_mode_poll();
 
 		/* check parameters for updates */
 		parameter_update_poll();
@@ -1425,10 +1609,8 @@ Sensors::task_main()
 		if (_publishing)
 			orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
 
-#ifdef CONFIG_HRT_PPM
 		/* Look for new r/c input data */
-		ppm_poll();
-#endif
+		rc_poll();
 
 		perf_end(_loop_perf);
 	}
@@ -1446,11 +1628,11 @@ Sensors::start()
 
 	/* start the task */
 	_sensors_task = task_spawn_cmd("sensors_task",
-				   SCHED_DEFAULT,
-				   SCHED_PRIORITY_MAX - 5,
-				   2048,
-				   (main_t)&Sensors::task_main_trampoline,
-				   nullptr);
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2048,
+				       (main_t)&Sensors::task_main_trampoline,
+				       nullptr);
 
 	if (_sensors_task < 0) {
 		warn("task start failed");
@@ -1468,7 +1650,7 @@ int sensors_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (sensors::g_sensors != nullptr)
-			errx(1, "sensors task already running");
+			errx(0, "sensors task already running");
 
 		sensors::g_sensors = new Sensors;
 
@@ -1502,6 +1684,7 @@ int sensors_main(int argc, char *argv[])
 		}
 	}
 
-	errx(1, "unrecognized command");
+	warnx("unrecognized command");
+	return 1;
 }
 
diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c
index 15bb833a9..1a479c205 100644
--- a/src/modules/systemlib/airspeed.c
+++ b/src/modules/systemlib/airspeed.c
@@ -42,7 +42,7 @@
 
 #include <stdio.h>
 #include <math.h>
-#include "conversions.h"
+#include <geo/geo.h>
 #include "airspeed.h"
 
 
@@ -59,10 +59,10 @@
 float calc_indicated_airspeed(float differential_pressure)
 {
 
-	if (differential_pressure > 0) {
+	if (differential_pressure > 0.0f) {
 		return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
 	} else {
-		return -sqrtf((2.0f*fabs(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
+		return -sqrtf((2.0f*fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
 	}
 
 }
@@ -95,17 +95,21 @@ float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_am
 float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius)
 {
 	float density = get_air_density(static_pressure, temperature_celsius);
+
 	if (density < 0.0001f || !isfinite(density)) {
-	 density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
-//	 printf("[airspeed] Invalid air density, using density at sea level\n");
+		density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
 	}
 
 	float pressure_difference = total_pressure - static_pressure;
 
-	if(pressure_difference > 0) {
+	if (pressure_difference > 0) {
 		return sqrtf((2.0f*(pressure_difference)) / density);
-	} else
-	{
-		return -sqrtf((2.0f*fabs(pressure_difference)) / density);
+	} else {
+		return -sqrtf((2.0f*fabsf(pressure_difference)) / density);
 	}
 }
+
+float get_air_density(float static_pressure, float temperature_celsius)
+{
+	return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS));
+}
diff --git a/src/modules/systemlib/airspeed.h b/src/modules/systemlib/airspeed.h
index def53f0c1..8dccaab9c 100644
--- a/src/modules/systemlib/airspeed.h
+++ b/src/modules/systemlib/airspeed.h
@@ -85,6 +85,14 @@
   */
  __EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius);
 
+ /**
+ * Calculates air density.
+ *
+ * @param static_pressure ambient pressure in millibar
+ * @param temperature_celcius air / ambient temperature in celcius
+ */
+__EXPORT float get_air_density(float static_pressure, float temperature_celsius);
+
 __END_DECLS
 
 #endif
diff --git a/src/modules/systemlib/board_serial.c b/src/modules/systemlib/board_serial.c
new file mode 100644
index 000000000..ad8c2bf83
--- /dev/null
+++ b/src/modules/systemlib/board_serial.c
@@ -0,0 +1,60 @@
+/****************************************************************************
+ *
+ *   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 board_serial.h
+ * Read off the board serial
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author David "Buzz" Bussenschutt <davidbuzz@gmail.com>
+ *
+ */
+
+#include "otp.h"
+#include "board_config.h"
+#include "board_serial.h"
+
+int get_board_serial(char *serialid)
+{
+	const volatile unsigned *udid_ptr = (const unsigned *)UDID_START;
+	union udid id;
+	val_read((unsigned *)&id, udid_ptr, sizeof(id));
+
+
+	/* Copy the serial from the chips non-write memory and swap endianess */
+	serialid[0] = id.data[3];   serialid[1] = id.data[2];  serialid[2] = id.data[1];  serialid[3] = id.data[0];
+	serialid[4] = id.data[7];   serialid[5] = id.data[6];  serialid[6] = id.data[5];  serialid[7] = id.data[4];
+	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
new file mode 100644
index 000000000..b14bb4376
--- /dev/null
+++ b/src/modules/systemlib/board_serial.h
@@ -0,0 +1,49 @@
+/****************************************************************************
+ *
+ *   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 board_serial.h
+ * Read off the board serial
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author David "Buzz" Bussenschutt <davidbuzz@gmail.com>
+ *
+ */
+
+#pragma once
+
+__BEGIN_DECLS
+
+__EXPORT int get_board_serial(char *serialid);
+
+__END_DECLS
diff --git a/src/modules/systemlib/bson/tinybson.c b/src/modules/systemlib/bson/tinybson.c
index 8aca6a25d..49403c98b 100644
--- a/src/modules/systemlib/bson/tinybson.c
+++ b/src/modules/systemlib/bson/tinybson.c
@@ -407,6 +407,9 @@ bson_encoder_fini(bson_encoder_t encoder)
 		memcpy(encoder->buf, &len, sizeof(len));
 	}
 
+	/* sync file */
+	fsync(encoder->fd);
+
 	return 0;
 }
 
diff --git a/src/modules/systemlib/conversions.c b/src/modules/systemlib/conversions.c
index ac94252c5..9105d83cb 100644
--- a/src/modules/systemlib/conversions.c
+++ b/src/modules/systemlib/conversions.c
@@ -55,100 +55,3 @@ int16_t_from_bytes(uint8_t bytes[])
 
 	return u.w;
 }
-
-void rot2quat(const float R[9], float Q[4])
-{
-	float q0_2;
-	float q1_2;
-	float q2_2;
-	float q3_2;
-	int32_t idx;
-
-	/* conversion of rotation matrix to quaternion
-	 * choose the largest component to begin with */
-	q0_2 = (((1.0F + R[0]) + R[4]) + R[8]) / 4.0F;
-	q1_2 = (((1.0F + R[0]) - R[4]) - R[8]) / 4.0F;
-	q2_2 = (((1.0F - R[0]) + R[4]) - R[8]) / 4.0F;
-	q3_2 = (((1.0F - R[0]) - R[4]) + R[8]) / 4.0F;
-
-	idx = 0;
-
-	if (q0_2 < q1_2) {
-		q0_2 = q1_2;
-
-		idx = 1;
-	}
-
-	if (q0_2 < q2_2) {
-		q0_2 = q2_2;
-		idx = 2;
-	}
-
-	if (q0_2 < q3_2) {
-		q0_2 = q3_2;
-		idx = 3;
-	}
-
-	q0_2 = sqrtf(q0_2);
-
-	/* solve for the remaining three components */
-	if (idx == 0) {
-		q1_2 = q0_2;
-		q2_2 = (R[5] - R[7]) / 4.0F / q0_2;
-		q3_2 = (R[6] - R[2]) / 4.0F / q0_2;
-		q0_2 = (R[1] - R[3]) / 4.0F / q0_2;
-
-	} else if (idx == 1) {
-		q2_2 = q0_2;
-		q1_2 = (R[5] - R[7]) / 4.0F / q0_2;
-		q3_2 = (R[3] + R[1]) / 4.0F / q0_2;
-		q0_2 = (R[6] + R[2]) / 4.0F / q0_2;
-
-	} else if (idx == 2) {
-		q3_2 = q0_2;
-		q1_2 = (R[6] - R[2]) / 4.0F / q0_2;
-		q2_2 = (R[3] + R[1]) / 4.0F / q0_2;
-		q0_2 = (R[7] + R[5]) / 4.0F / q0_2;
-
-	} else {
-		q1_2 = (R[1] - R[3]) / 4.0F / q0_2;
-		q2_2 = (R[6] + R[2]) / 4.0F / q0_2;
-		q3_2 = (R[7] + R[5]) / 4.0F / q0_2;
-	}
-
-	/* return values */
-	Q[0] = q1_2;
-	Q[1] = q2_2;
-	Q[2] = q3_2;
-	Q[3] = q0_2;
-}
-
-void quat2rot(const float Q[4], float R[9])
-{
-	float q0_2;
-	float q1_2;
-	float q2_2;
-	float q3_2;
-
-	memset(&R[0], 0, 9U * sizeof(float));
-
-	q0_2 = Q[0] * Q[0];
-	q1_2 = Q[1] * Q[1];
-	q2_2 = Q[2] * Q[2];
-	q3_2 = Q[3] * Q[3];
-
-	R[0] = ((q0_2 + q1_2) - q2_2) - q3_2;
-	R[3] = 2.0F * (Q[1] * Q[2] - Q[0] * Q[3]);
-	R[6] = 2.0F * (Q[1] * Q[3] + Q[0] * Q[2]);
-	R[1] = 2.0F * (Q[1] * Q[2] + Q[0] * Q[3]);
-	R[4] = ((q0_2 + q2_2) - q1_2) - q3_2;
-	R[7] = 2.0F * (Q[2] * Q[3] - Q[0] * Q[1]);
-	R[2] = 2.0F * (Q[1] * Q[3] - Q[0] * Q[2]);
-	R[5] = 2.0F * (Q[2] * Q[3] + Q[0] * Q[1]);
-	R[8] = ((q0_2 + q3_2) - q1_2) - q2_2;
-}
-
-float get_air_density(float static_pressure, float temperature_celsius)
-{
-	return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS));
-}
diff --git a/src/modules/systemlib/conversions.h b/src/modules/systemlib/conversions.h
index 5d485b01f..dc383e770 100644
--- a/src/modules/systemlib/conversions.h
+++ b/src/modules/systemlib/conversions.h
@@ -44,11 +44,6 @@
 #include <float.h>
 #include <stdint.h>
 
-#define CONSTANTS_ONE_G						9.80665f		// m/s^2
-#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C	1.225f			// kg/m^3
-#define CONSTANTS_AIR_GAS_CONST				287.1f 			// J/(kg * K)
-#define CONSTANTS_ABSOLUTE_NULL_CELSIUS		-273.15f		// °C
-
 __BEGIN_DECLS
 
 /**
@@ -61,34 +56,6 @@ __BEGIN_DECLS
  */
 __EXPORT int16_t int16_t_from_bytes(uint8_t bytes[]);
 
-/**
- * Converts a 3 x 3 rotation matrix to an unit quaternion.
- *
- * All orientations are expressed in NED frame.
- *
- * @param R rotation matrix to convert
- * @param Q quaternion to write back to
- */
-__EXPORT void rot2quat(const float R[9], float Q[4]);
-
-/**
- * Converts an unit quaternion to a 3 x 3 rotation matrix.
- *
- * All orientations are expressed in NED frame.
- *
- * @param Q quaternion to convert
- * @param R rotation matrix to write back to
- */
-__EXPORT void quat2rot(const float Q[4], float R[9]);
-
-/**
- * Calculates air density.
- *
- * @param static_pressure ambient pressure in millibar
- * @param temperature_celcius air / ambient temperature in celcius
- */
-__EXPORT float get_air_density(float static_pressure, float temperature_celsius);
-
 __END_DECLS
 
 #endif /* CONVERSIONS_H_ */
diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c
index 8fdff8ac0..ccc516f39 100644
--- a/src/modules/systemlib/cpuload.c
+++ b/src/modules/systemlib/cpuload.c
@@ -71,8 +71,6 @@ extern FAR struct _TCB *sched_gettcb(pid_t pid);
 
 void cpuload_initialize_once()
 {
-//	if (!system_load.initialized)
-//	{
 	system_load.start_time = hrt_absolute_time();
 	int i;
 
@@ -80,27 +78,28 @@ void cpuload_initialize_once()
 		system_load.tasks[i].valid = false;
 	}
 
-	system_load.total_count = 0;
-
 	uint64_t now = hrt_absolute_time();
 
-	/* initialize idle thread statically */
-	system_load.tasks[0].start_time = now;
-	system_load.tasks[0].total_runtime = 0;
-	system_load.tasks[0].curr_start_time = 0;
-	system_load.tasks[0].tcb = sched_gettcb(0);
-	system_load.tasks[0].valid = true;
-	system_load.total_count++;
-
-	/* initialize init thread statically */
-	system_load.tasks[1].start_time = now;
-	system_load.tasks[1].total_runtime = 0;
-	system_load.tasks[1].curr_start_time = 0;
-	system_load.tasks[1].tcb = sched_gettcb(1);
-	system_load.tasks[1].valid = true;
-	/* count init thread */
-	system_load.total_count++;
-	//	}
+	int static_tasks_count = 2;	// there are at least 2 threads that should be initialized statically - "idle" and "init"
+
+#ifdef CONFIG_PAGING
+	static_tasks_count++;	// include paging thread in initialization
+#endif /* CONFIG_PAGING */
+#if CONFIG_SCHED_WORKQUEUE
+	static_tasks_count++;	// include high priority work0 thread in initialization
+#endif /* CONFIG_SCHED_WORKQUEUE */
+#if CONFIG_SCHED_LPWORK
+	static_tasks_count++;	// include low priority work1 thread in initialization
+#endif /* CONFIG_SCHED_WORKQUEUE */
+
+	// perform static initialization of "system" threads
+	for (system_load.total_count = 0; system_load.total_count < static_tasks_count; system_load.total_count++) {
+		system_load.tasks[system_load.total_count].start_time = now;
+		system_load.tasks[system_load.total_count].total_runtime = 0;
+		system_load.tasks[system_load.total_count].curr_start_time = 0;
+		system_load.tasks[system_load.total_count].tcb = sched_gettcb(system_load.total_count);	// it is assumed that these static threads have consecutive PIDs
+		system_load.tasks[system_load.total_count].valid = true;
+	}
 }
 
 void sched_note_start(FAR struct tcb_s *tcb)
diff --git a/src/modules/systemlib/cpuload.h b/src/modules/systemlib/cpuload.h
index c7aa18d3c..16d132fdb 100644
--- a/src/modules/systemlib/cpuload.h
+++ b/src/modules/systemlib/cpuload.h
@@ -40,15 +40,15 @@ __BEGIN_DECLS
 #include <nuttx/sched.h>
 
 struct system_load_taskinfo_s {
-	uint64_t total_runtime;      ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load
-	uint64_t curr_start_time;     ///< Start time of the current scheduling slot
-	uint64_t start_time;         ///< FIRST start time of task
-	FAR struct tcb_s *tcb;               ///<
-	bool valid;                  ///< Task is currently active / valid
+	uint64_t total_runtime;			///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load
+	uint64_t curr_start_time;		///< Start time of the current scheduling slot
+	uint64_t start_time;			///< FIRST start time of task
+	FAR struct tcb_s *tcb;			///<
+	bool valid;						///< Task is currently active / valid
 };
 
 struct system_load_s {
-	uint64_t start_time;         ///< Global start time of measurements
+	uint64_t start_time;			///< Global start time of measurements
 	struct system_load_taskinfo_s tasks[CONFIG_MAX_TASKS];
 	uint8_t initialized;
 	int total_count;
diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c
index 8d77f14a8..8e9c2bfcf 100644
--- a/src/modules/systemlib/hx_stream.c
+++ b/src/modules/systemlib/hx_stream.c
@@ -53,14 +53,26 @@
 
 
 struct hx_stream {
-	uint8_t			buf[HX_STREAM_MAX_FRAME + 4];
-	unsigned		frame_bytes;
-	bool			escaped;
-	bool			txerror;
-
+	/* RX state */
+	uint8_t			rx_buf[HX_STREAM_MAX_FRAME + 4];
+	unsigned		rx_frame_bytes;
+	bool			rx_escaped;
+	hx_stream_rx_callback	rx_callback;
+	void			*rx_callback_arg;
+
+	/* TX state */
 	int			fd;
-	hx_stream_rx_callback	callback;
-	void			*callback_arg;
+	bool			tx_error;
+	uint8_t			*tx_buf;
+	unsigned		tx_resid;
+	uint32_t		tx_crc;
+	enum {
+		TX_IDLE = 0,
+		TX_SEND_START,
+		TX_SEND_DATA,
+		TX_SENT_ESCAPE,
+		TX_SEND_END
+	}			tx_state;
 
 	perf_counter_t		pc_tx_frames;
 	perf_counter_t		pc_rx_frames;
@@ -81,21 +93,7 @@ static void
 hx_tx_raw(hx_stream_t stream, uint8_t c)
 {
 	if (write(stream->fd, &c, 1) != 1)
-		stream->txerror = true;
-}
-
-static void
-hx_tx_byte(hx_stream_t stream, uint8_t c)
-{
-	switch (c) {
-	case FBO:
-	case CEO:
-		hx_tx_raw(stream, CEO);
-		c ^= 0x20;
-		break;
-	}
-
-	hx_tx_raw(stream, c);
+		stream->tx_error = true;
 }
 
 static int
@@ -105,11 +103,11 @@ hx_rx_frame(hx_stream_t stream)
 		uint8_t	b[4];
 		uint32_t w;
 	} u;
-	unsigned length = stream->frame_bytes;
+	unsigned length = stream->rx_frame_bytes;
 
 	/* reset the stream */
-	stream->frame_bytes = 0;
-	stream->escaped = false;
+	stream->rx_frame_bytes = 0;
+	stream->rx_escaped = false;
 
 	/* not a real frame - too short */
 	if (length < 4) {
@@ -122,11 +120,11 @@ hx_rx_frame(hx_stream_t stream)
 	length -= 4;
 
 	/* compute expected CRC */
-	u.w = crc32(&stream->buf[0], length);
+	u.w = crc32(&stream->rx_buf[0], length);
 
 	/* compare computed and actual CRC */
 	for (unsigned i = 0; i < 4; i++) {
-		if (u.b[i] != stream->buf[length + i]) {
+		if (u.b[i] != stream->rx_buf[length + i]) {
 			perf_count(stream->pc_rx_errors);
 			return 0;
 		}
@@ -134,7 +132,7 @@ hx_rx_frame(hx_stream_t stream)
 
 	/* frame is good */
 	perf_count(stream->pc_rx_frames);
-	stream->callback(stream->callback_arg, &stream->buf[0], length);
+	stream->rx_callback(stream->rx_callback_arg, &stream->rx_buf[0], length);
 	return 1;
 }
 
@@ -150,8 +148,8 @@ hx_stream_init(int fd,
 	if (stream != NULL) {
 		memset(stream, 0, sizeof(struct hx_stream));
 		stream->fd = fd;
-		stream->callback = callback;
-		stream->callback_arg = arg;
+		stream->rx_callback = callback;
+		stream->rx_callback_arg = arg;
 	}
 
 	return stream;
@@ -179,49 +177,112 @@ hx_stream_set_counters(hx_stream_t stream,
 	stream->pc_rx_errors = rx_errors;
 }
 
+void
+hx_stream_reset(hx_stream_t stream)
+{
+	stream->rx_frame_bytes = 0;
+	stream->rx_escaped = false;
+
+	stream->tx_buf = NULL;
+	stream->tx_resid = 0;
+	stream->tx_state = TX_IDLE;
+}
+
 int
-hx_stream_send(hx_stream_t stream,
+hx_stream_start(hx_stream_t stream,
 	       const void *data,
 	       size_t count)
 {
-	union {
-		uint8_t	b[4];
-		uint32_t w;
-	} u;
-	const uint8_t *p = (const uint8_t *)data;
-	unsigned resid = count;
-
-	if (resid > HX_STREAM_MAX_FRAME)
+	if (count > HX_STREAM_MAX_FRAME)
 		return -EINVAL;
 
-	/* start the frame */
-	hx_tx_raw(stream, FBO);
+	stream->tx_buf = data;
+	stream->tx_resid = count;
+	stream->tx_state = TX_SEND_START;
+	stream->tx_crc = crc32(data, count);
+	return OK;
+}
+
+int
+hx_stream_send_next(hx_stream_t stream)
+{
+	int c;
+
+	/* sort out what we're going to send */
+	switch (stream->tx_state) {
 
-	/* transmit the data */
-	while (resid--)
-		hx_tx_byte(stream, *p++);
+	case TX_SEND_START:
+		stream->tx_state = TX_SEND_DATA;
+		return FBO;
 
-	/* compute the CRC */
-	u.w = crc32(data, count);
+	case TX_SEND_DATA:
+		c = *stream->tx_buf;
 
-	/* send the CRC */
-	p = &u.b[0];
-	resid = 4;
+		switch (c) {
+		case FBO:
+		case CEO:
+			stream->tx_state = TX_SENT_ESCAPE;
+			return CEO;
+		}
+		break;
+
+	case TX_SENT_ESCAPE:
+		c = *stream->tx_buf ^ 0x20;
+		stream->tx_state = TX_SEND_DATA;
+		break;
+
+	case TX_SEND_END:
+		stream->tx_state = TX_IDLE;
+		return FBO;
+
+	case TX_IDLE:
+	default:
+		return -1;
+	}
+
+	/* if we are here, we have consumed a byte from the buffer */
+	stream->tx_resid--;
+	stream->tx_buf++;
+
+	/* buffer exhausted */
+	if (stream->tx_resid == 0) {
+		uint8_t *pcrc = (uint8_t *)&stream->tx_crc;
+
+		/* was the buffer the frame CRC? */
+		if (stream->tx_buf == (pcrc + sizeof(stream->tx_crc))) {
+			stream->tx_state = TX_SEND_END;
+		} else {
+			/* no, it was the payload - switch to sending the CRC */
+			stream->tx_buf = pcrc;
+			stream->tx_resid = sizeof(stream->tx_crc);
+		}
+	}
+	return c;
+}
+
+int
+hx_stream_send(hx_stream_t stream,
+	       const void *data,
+	       size_t count)
+{
+	int result;
 
-	while (resid--)
-		hx_tx_byte(stream, *p++);
+	result = hx_stream_start(stream, data, count);
+	if (result != OK)
+		return result;
 
-	/* and the trailing frame separator */
-	hx_tx_raw(stream, FBO);
+	int c;
+	while ((c = hx_stream_send_next(stream)) >= 0)
+		hx_tx_raw(stream, c);
 
 	/* check for transmit error */
-	if (stream->txerror) {
-		stream->txerror = false;
+	if (stream->tx_error) {
+		stream->tx_error = false;
 		return -EIO;
 	}
 
 	perf_count(stream->pc_tx_frames);
-	return 0;
+	return OK;
 }
 
 void
@@ -234,17 +295,17 @@ hx_stream_rx(hx_stream_t stream, uint8_t c)
 	}
 
 	/* escaped? */
-	if (stream->escaped) {
-		stream->escaped = false;
+	if (stream->rx_escaped) {
+		stream->rx_escaped = false;
 		c ^= 0x20;
 
 	} else if (c == CEO) {
-		/* now escaped, ignore the byte */
-		stream->escaped = true;
+		/* now rx_escaped, ignore the byte */
+		stream->rx_escaped = true;
 		return;
 	}
 
 	/* save for later */
-	if (stream->frame_bytes < sizeof(stream->buf))
-		stream->buf[stream->frame_bytes++] = c;
+	if (stream->rx_frame_bytes < sizeof(stream->rx_buf))
+		stream->rx_buf[stream->rx_frame_bytes++] = c;
 }
diff --git a/src/modules/systemlib/hx_stream.h b/src/modules/systemlib/hx_stream.h
index 128689953..1f3927222 100644
--- a/src/modules/systemlib/hx_stream.h
+++ b/src/modules/systemlib/hx_stream.h
@@ -58,7 +58,8 @@ __BEGIN_DECLS
  * Allocate a new hx_stream object.
  *
  * @param fd		The file handle over which the protocol will
- *			communicate.
+ *			communicate, or -1 if the protocol will use
+ *			hx_stream_start/hx_stream_send_next.
  * @param callback	Called when a frame is received.
  * @param callback_arg	Passed to the callback.
  * @return		A handle to the stream, or NULL if memory could
@@ -80,6 +81,7 @@ __EXPORT extern void		hx_stream_free(hx_stream_t stream);
  *
  * Any counter may be set to NULL to disable counting that datum.
  *
+ * @param stream	A handle returned from hx_stream_init.
  * @param tx_frames	Counter for transmitted frames.
  * @param rx_frames	Counter for received frames.
  * @param rx_errors	Counter for short and corrupt received frames.
@@ -90,6 +92,44 @@ __EXPORT extern void		hx_stream_set_counters(hx_stream_t stream,
 		perf_counter_t rx_errors);
 
 /**
+ * Reset a stream.
+ *
+ * Forces the local stream state to idle.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ */
+__EXPORT extern void		hx_stream_reset(hx_stream_t stream);
+
+/**
+ * Prepare to send a frame.
+ *
+ * Use this in conjunction with hx_stream_send_next to 
+ * set the frame to be transmitted.
+ *
+ * Use hx_stream_send() to write to the stream fd directly.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ * @param data		Pointer to the data to send.
+ * @param count		The number of bytes to send.
+ * @return		Zero on success, -errno on error.
+ */
+__EXPORT extern int		hx_stream_start(hx_stream_t stream,
+		const void *data,
+		size_t count);
+
+/**
+ * Get the next byte to send for a stream.
+ *
+ * This requires that the stream be prepared for sending by
+ * calling hx_stream_start first.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ * @return		The byte to send, or -1 if there is 
+ *			nothing left to send.
+ */
+__EXPORT extern int		hx_stream_send_next(hx_stream_t stream);
+
+/**
  * Send a frame.
  *
  * This function will block until all frame bytes are sent if
diff --git a/src/modules/mavlink/mavlink_log.c b/src/modules/systemlib/mavlink_log.c
index d9416a08b..f321f9ceb 100644
--- a/src/modules/mavlink/mavlink_log.c
+++ b/src/modules/systemlib/mavlink_log.c
@@ -41,29 +41,38 @@
 
 #include <string.h>
 #include <stdlib.h>
+#include <stdio.h>
 #include <stdarg.h>
 
 #include <mavlink/mavlink_log.h>
 
-void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size)
+__EXPORT void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size)
 {
 	lb->size  = size;
 	lb->start = 0;
 	lb->count = 0;
-	lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage));
+	lb->elems = calloc(lb->size, sizeof(struct mavlink_logmessage));
 }
 
-int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb)
+__EXPORT void mavlink_logbuffer_destroy(struct mavlink_logbuffer *lb)
+{
+	lb->size  = 0;
+	lb->start = 0;
+	lb->count = 0;
+	free(lb->elems);
+}
+
+__EXPORT int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb)
 {
 	return lb->count == (int)lb->size;
 }
 
-int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb)
+__EXPORT int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb)
 {
 	return lb->count == 0;
 }
 
-void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem)
+__EXPORT void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem)
 {
 	int end = (lb->start + lb->count) % lb->size;
 	memcpy(&(lb->elems[end]), elem, sizeof(struct mavlink_logmessage));
@@ -76,12 +85,13 @@ void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_
 	}
 }
 
-int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem)
+__EXPORT int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem)
 {
 	if (!mavlink_logbuffer_is_empty(lb)) {
 		memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage));
 		lb->start = (lb->start + 1) % lb->size;
 		--lb->count;
+
 		return 0;
 
 	} else {
@@ -89,7 +99,7 @@ int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessa
 	}
 }
 
-void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...)
+__EXPORT void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...)
 {
 	va_list ap;
 	va_start(ap, fmt);
diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp
index df0dfe838..cce46bf5f 100644
--- a/src/modules/systemlib/mixer/mixer.cpp
+++ b/src/modules/systemlib/mixer/mixer.cpp
@@ -50,6 +50,8 @@
 #include <stdio.h>
 #include <math.h>
 #include <unistd.h>
+#include <ctype.h>
+#include <systemlib/err.h>
 
 #include "mixer.h"
 
@@ -114,6 +116,33 @@ Mixer::scale_check(struct mixer_scaler_s &scaler)
 	return 0;
 }
 
+const char *
+Mixer::findtag(const char *buf, unsigned &buflen, char tag)
+{
+	while (buflen >= 2) {
+		if ((buf[0] == tag) && (buf[1] == ':'))
+			return buf;
+		buf++;
+		buflen--;
+	}
+	return nullptr;
+}
+
+const char *
+Mixer::skipline(const char *buf, unsigned &buflen)
+{
+	const char *p;
+
+	/* if we can find a CR or NL in the buffer, skip up to it */
+	if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) {
+		/* skip up to it AND one beyond - could be on the NUL symbol now */
+		buflen -= (p - buf) + 1;
+		return p + 1;
+	}
+
+	return nullptr;
+}
+
 /****************************************************************************/
 
 NullMixer::NullMixer() :
@@ -142,6 +171,22 @@ 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--) {
+		if (buf[i] == '\0')
+			continue;
+
+		/* require a space or newline at the end of the buffer, fail on printable chars */
+		if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
+			/* found a line ending or space, so no split symbols / numbers. good. */
+			break;
+		} else {
+			return nm;
+		}
+
+	}
 
 	if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) {
 		nm = new NullMixer;
diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h
index bbfa130a9..1c889a811 100644
--- a/src/modules/systemlib/mixer/mixer.h
+++ b/src/modules/systemlib/mixer/mixer.h
@@ -128,7 +128,9 @@
 #ifndef _SYSTEMLIB_MIXER_MIXER_H
 #define _SYSTEMLIB_MIXER_MIXER_H value
 
+#include <nuttx/config.h>
 #include "drivers/drv_mixer.h"
+#include "mixer_load.h"
 
 /**
  * Abstract class defining a mixer mixing zero or more inputs to
@@ -210,6 +212,24 @@ protected:
 	 */
 	static int			scale_check(struct mixer_scaler_s &scaler);
 
+	/**
+	 * Find a tag
+	 *
+	 * @param buf			The buffer to operate on.
+	 * @param buflen		length of the buffer.
+	 * @param tag			character to search for.
+	 */
+	static const char *		findtag(const char *buf, unsigned &buflen, char tag);
+
+	/**
+	 * Skip a line
+	 *
+	 * @param buf			The buffer to operate on.
+	 * @param buflen		length of the buffer.
+	 * @return			0 / OK if a line could be skipped, 1 else
+	 */
+	static const char *		skipline(const char *buf, unsigned &buflen);
+
 private:
 };
 
@@ -239,6 +259,11 @@ public:
 	void				reset();
 
 	/**
+	 * Count the mixers in the group.
+	 */
+	unsigned			count();
+
+	/**
 	 * Adds mixers to the group based on a text description in a buffer.
 	 *
 	 * Mixer definitions begin with a single capital letter and a colon.
@@ -424,6 +449,7 @@ public:
 		HEX_PLUS,	/**< hex in + configuration */
 		OCTA_X,
 		OCTA_PLUS,
+		OCTA_COX,
 
 		MAX_GEOMETRY
 	};
diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp
index 1dbc512cd..3ed99fba0 100644
--- a/src/modules/systemlib/mixer/mixer_group.cpp
+++ b/src/modules/systemlib/mixer/mixer_group.cpp
@@ -111,6 +111,20 @@ MixerGroup::mix(float *outputs, unsigned space)
 	return index;
 }
 
+unsigned
+MixerGroup::count()
+{
+	Mixer	*mixer = _first;
+	unsigned index = 0;
+
+	while ((mixer != nullptr)) {
+		mixer = mixer->_next;
+		index++;
+	}
+
+	return index;
+}
+
 void
 MixerGroup::groups_required(uint32_t &groups)
 {
@@ -170,6 +184,7 @@ MixerGroup::load_from_buf(const char *buf, unsigned &buflen)
 
 			/* only adjust buflen if parsing was successful */
 			buflen = resid;
+			debug("SUCCESS - buflen: %d", buflen);
 		} else {
 
 			/*
diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c
new file mode 100644
index 000000000..a55ddf8a3
--- /dev/null
+++ b/src/modules/systemlib/mixer/mixer_load.c
@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   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 mixer_load.c
+ *
+ * Programmable multi-channel mixer library.
+ */
+
+#include <nuttx/config.h>
+#include <string.h>
+#include <stdio.h>
+#include <ctype.h>
+
+#include "mixer_load.h"
+
+int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
+{
+	FILE		*fp;
+	char		line[120];
+
+	/* open the mixer definition file */
+	fp = fopen(fname, "r");
+	if (fp == NULL) {
+		return 1;
+	}
+
+	/* read valid lines from the file into a buffer */
+	buf[0] = '\0';
+	for (;;) {
+
+		/* get a line, bail on error/EOF */
+		line[0] = '\0';
+		if (fgets(line, sizeof(line), fp) == NULL)
+			break;
+
+		/* if the line doesn't look like a mixer definition line, skip it */
+		if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':'))
+			continue;
+
+		/* compact whitespace in the buffer */
+		char *t, *f;
+		for (f = line; *f != '\0'; f++) {
+			/* scan for space characters */
+			if (*f == ' ') {
+				/* look for additional spaces */
+				t = f + 1;
+				while (*t == ' ')
+					t++;
+				if (*t == '\0') {
+					/* strip trailing whitespace */
+					*f = '\0';
+				} else if (t > (f + 1)) {
+					memmove(f + 1, t, strlen(t) + 1);
+				}
+			}
+		}
+
+		/* if the line is too long to fit in the buffer, bail */
+		if ((strlen(line) + strlen(buf) + 1) >= maxlen) {
+			return 1;
+		}
+
+		/* add the line to the buffer */
+		strcat(buf, line);
+	}
+
+	return 0;
+}
+
diff --git a/src/modules/systemlib/mixer/mixer_load.h b/src/modules/systemlib/mixer/mixer_load.h
new file mode 100644
index 000000000..4b7091d5b
--- /dev/null
+++ b/src/modules/systemlib/mixer/mixer_load.h
@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   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 mixer_load.h
+ *
+ */
+
+
+#ifndef _SYSTEMLIB_MIXER_LOAD_H
+#define _SYSTEMLIB_MIXER_LOAD_H value
+
+#include <nuttx/config.h>
+
+__BEGIN_DECLS
+
+__EXPORT int load_mixer_file(const char *fname, char *buf, unsigned maxlen);
+
+__END_DECLS
+
+#endif
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index 8ded0b05c..bf77795d5 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -130,7 +130,17 @@ const MultirotorMixer::Rotor _config_octa_plus[] = {
 	{  1.000000,  0.000000, -1.00 },
 	{ -1.000000,  0.000000, -1.00 },
 };
-const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOMETRY] = {
+const MultirotorMixer::Rotor _config_octa_cox[] = {
+	{ -0.707107,  0.707107,  1.00 },
+	{  0.707107,  0.707107, -1.00 },
+	{  0.707107, -0.707107,  1.00 },
+	{ -0.707107, -0.707107, -1.00 },
+	{  0.707107,  0.707107,  1.00 },
+	{ -0.707107,  0.707107, -1.00 },
+	{ -0.707107, -0.707107,  1.00 },
+	{  0.707107, -0.707107, -1.00 },
+};
+const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = {
 	&_config_quad_x[0],
 	&_config_quad_plus[0],
 	&_config_quad_v[0],
@@ -139,8 +149,9 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOME
 	&_config_hex_plus[0],
 	&_config_octa_x[0],
 	&_config_octa_plus[0],
+	&_config_octa_cox[0],
 };
-const unsigned _config_rotor_count[MultirotorMixer::Geometry::MAX_GEOMETRY] = {
+const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = {
 	4, /* quad_x */
 	4, /* quad_plus */
 	4, /* quad_v */
@@ -149,6 +160,7 @@ const unsigned _config_rotor_count[MultirotorMixer::Geometry::MAX_GEOMETRY] = {
 	6, /* hex_plus */
 	8, /* octa_x */
 	8, /* octa_plus */
+	8, /* octa_cox */
 };
 
 }
@@ -181,6 +193,23 @@ 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--) {
+		if (buf[i] == '\0')
+			continue;
+
+		/* require a space or newline at the end of the buffer, fail on printable chars */
+		if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
+			/* found a line ending or space, so no split symbols / numbers. good. */
+			break;
+		} else {
+			debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]);
+			return nullptr;
+		}
+
+	}
 
 	if (sscanf(buf, "R: %s %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &used) != 5) {
 		debug("multirotor parse failed on '%s'", buf);
@@ -188,11 +217,17 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 	}
 
 	if (used > (int)buflen) {
-		debug("multirotor spec used %d of %u", used, buflen);
+		debug("OVERFLOW: multirotor spec used %d of %u", used, buflen);
+		return nullptr;
+	}
+
+	buf = skipline(buf, buflen);
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
 		return nullptr;
 	}
 
-	buflen -= used;
+	debug("remaining in buf: %d, first char: %c", buflen, buf[0]);
 
 	if (!strcmp(geomname, "4+")) {
 		geometry = MultirotorMixer::QUAD_PLUS;
@@ -217,6 +252,9 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 
 	} else if (!strcmp(geomname, "8x")) {
 		geometry = MultirotorMixer::OCTA_X;
+		
+	} else if (!strcmp(geomname, "8c")) {
+		geometry = MultirotorMixer::OCTA_COX;
 
 	} else {
 		debug("unrecognised geometry '%s'", geomname);
diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp
index 07dc5f37f..c3985b5de 100644
--- a/src/modules/systemlib/mixer/mixer_simple.cpp
+++ b/src/modules/systemlib/mixer/mixer_simple.cpp
@@ -71,44 +71,30 @@ SimpleMixer::~SimpleMixer()
 		free(_info);
 }
 
-static const char *
-findtag(const char *buf, unsigned &buflen, char tag)
-{
-	while (buflen >= 2) {
-		if ((buf[0] == tag) && (buf[1] == ':'))
-			return buf;
-		buf++;
-		buflen--;
-	}
-	return nullptr;
-}
-
-static void
-skipline(const char *buf, unsigned &buflen)
-{
-	const char *p;
-
-	/* if we can find a CR or NL in the buffer, skip up to it */
-	if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen)))
-		buflen -= (p - buf);
-}
-
 int
 SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler)
 {
 	int ret;
 	int s[5];
+	int n = -1;
 	
 	buf = findtag(buf, buflen, 'O');
-	if ((buf == nullptr) || (buflen < 12))
+	if ((buf == nullptr) || (buflen < 12)) {
+		debug("output parser failed finding tag, ret: '%s'", buf);
 		return -1;
+	}
 
-	if ((ret = sscanf(buf, "O: %d %d %d %d %d",
-			  &s[0], &s[1], &s[2], &s[3], &s[4])) != 5) {
-		debug("scaler parse failed on '%s' (got %d)", buf, ret);
+	if ((ret = sscanf(buf, "O: %d %d %d %d %d %n",
+			  &s[0], &s[1], &s[2], &s[3], &s[4], &n)) != 5) {
+		debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n);
+		return -1;
+	}
+
+	buf = skipline(buf, buflen);
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
 		return -1;
 	}
-	skipline(buf, buflen);
 
 	scaler.negative_scale	= s[0] / 10000.0f;
 	scaler.positive_scale	= s[1] / 10000.0f;
@@ -126,15 +112,22 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale
 	int s[5];
 
 	buf = findtag(buf, buflen, 'S');
-	if ((buf == nullptr) || (buflen < 16))
+	if ((buf == nullptr) || (buflen < 16)) {
+		debug("control parser failed finding tag, ret: '%s'", buf);
 		return -1;
+	}
 
 	if (sscanf(buf, "S: %u %u %d %d %d %d %d",
 		   &u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4]) != 7) {
 		debug("control parse failed on '%s'", buf);
 		return -1;
 	}
-	skipline(buf, buflen);
+
+	buf = skipline(buf, buflen);
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
+		return -1;
+	}
 
 	control_group		= u[0];
 	control_index		= u[1];
@@ -162,7 +155,11 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
 		goto out;
 	}
 
-	buflen -= used;
+	buf = skipline(buf, buflen);
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
+		goto out;
+	}
 
 	mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs));
 
@@ -173,22 +170,27 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
 
 	mixinfo->control_count = inputs;
 
-	if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler))
+	if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler)) {
+		debug("simple mixer parser failed parsing out scaler tag, ret: '%s'", buf);
 		goto out;
+	}
 
 	for (unsigned i = 0; i < inputs; i++) {
 		if (parse_control_scaler(end - buflen, buflen,
 					 mixinfo->controls[i].scaler,
 					 mixinfo->controls[i].control_group,
-					 mixinfo->controls[i].control_index))
+					 mixinfo->controls[i].control_index)) {
+			debug("simple mixer parser failed parsing ctrl scaler tag, ret: '%s'", buf);
 			goto out;
+		}
+
 	}
 
 	sm = new SimpleMixer(control_cb, cb_handle, mixinfo);
 
 	if (sm != nullptr) {
 		mixinfo = nullptr;
-		debug("loaded mixer with %d inputs", inputs);
+		debug("loaded mixer with %d input(s)", inputs);
 
 	} else {
 		debug("could not allocate memory for mixer");
diff --git a/src/modules/systemlib/mixer/module.mk b/src/modules/systemlib/mixer/module.mk
index 4d45e1c50..fc7485e20 100644
--- a/src/modules/systemlib/mixer/module.mk
+++ b/src/modules/systemlib/mixer/module.mk
@@ -39,4 +39,5 @@ LIBNAME	= mixerlib
 SRCS		= mixer.cpp \
 		  mixer_group.cpp \
 		  mixer_multirotor.cpp \
-		  mixer_simple.cpp
+		  mixer_simple.cpp \
+		  mixer_load.c
diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables
index 683c63040..050bf2f47 100755
--- a/src/modules/systemlib/mixer/multi_tables
+++ b/src/modules/systemlib/mixer/multi_tables
@@ -74,7 +74,18 @@ set octa_plus {
 	  90 CW
 }
 
-set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus octa_x octa_plus}
+set octa_cox {
+	  45 CCW
+	 -45 CW
+	-135 CCW
+	 135 CW
+	 -45 CCW
+	  45 CW
+	 135 CCW
+	-135 CW
+}
+
+set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus 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]]}
 
diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk
index fd0289c9a..3953b757d 100644
--- a/src/modules/systemlib/module.mk
+++ b/src/modules/systemlib/module.mk
@@ -45,6 +45,12 @@ SRCS		 = err.c \
 		   getopt_long.c \
 		   up_cxxinitialize.c \
 		   pid/pid.c \
-		   geo/geo.c \
 		   systemlib.c \
-		   airspeed.c
+		   airspeed.c \
+		   system_params.c \
+		   mavlink_log.c \
+		   rc_check.c \
+		   otp.c \
+		   board_serial.c \
+		   pwm_limit/pwm_limit.c
+
diff --git a/src/modules/systemlib/otp.c b/src/modules/systemlib/otp.c
new file mode 100644
index 000000000..695574fdc
--- /dev/null
+++ b/src/modules/systemlib/otp.c
@@ -0,0 +1,224 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+ *   Authors:
+ *      Lorenz Meier <lm@inf.ethz.ch>
+ *      David "Buzz" Bussenschutt <davidbuzz@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 otp.c
+ * otp estimation
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author David "Buzz" Bussenschutt <davidbuzz@gmail.com>
+ *
+ */
+
+#include <nuttx/config.h>
+#include <board_config.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+#include <string.h>  // memset
+#include "conversions.h"
+#include "otp.h"
+#include "err.h"   // warnx 
+#include <assert.h>
+
+
+int val_read(void *dest, volatile const void *src, int bytes)
+{
+
+	int i;
+
+	for (i = 0; i < bytes / 4; i++) {
+		*(((volatile unsigned *)dest) + i) = *(((volatile unsigned *)src) + i);
+	}
+
+	return i * 4;
+}
+
+
+int write_otp(uint8_t id_type, uint32_t vid, uint32_t pid, char *signature)
+{
+
+	warnx("write_otp: PX4 / %02X / %02X / %02X  / ... etc  \n", id_type, vid, pid);
+
+	int errors = 0;
+
+	// descriptor
+	if (F_write_byte(ADDR_OTP_START, 'P'))
+		errors++;
+	  //  write the 'P' from PX4. to first byte in OTP
+	if (F_write_byte(ADDR_OTP_START + 1, 'X'))
+		errors++; //  write the 'P' from PX4. to first byte in OTP
+	if (F_write_byte(ADDR_OTP_START + 2, '4'))
+		errors++;
+	if (F_write_byte(ADDR_OTP_START + 3, '\0'))
+		errors++;
+	//id_type
+	if (F_write_byte(ADDR_OTP_START + 4, id_type))
+		errors++;
+	// vid and pid are 4 bytes each
+	if (F_write_word(ADDR_OTP_START + 5, vid))
+		errors++;
+	if (F_write_word(ADDR_OTP_START + 9, pid))
+		errors++;
+
+	// leave some 19 bytes of space, and go to the next block...
+	// then the auth sig starts
+	for (int i = 0 ; i < 128 ; i++) {
+		if (F_write_byte(ADDR_OTP_START + 32 + i, signature[i]))
+			errors++;
+	}
+
+	return errors;
+}
+
+int lock_otp(void)
+{
+	//determine the required locking size - can only write full lock bytes */
+//	int size = sizeof(struct otp) / 32;
+//
+//	struct otp_lock otp_lock_mem;
+//
+//	memset(&otp_lock_mem, OTP_LOCK_UNLOCKED, sizeof(otp_lock_mem));
+//	for (int i = 0; i < sizeof(otp_lock_mem) / sizeof(otp_lock_mem.lock_bytes[0]); i++)
+//		otp_lock_mem.lock_bytes[i] = OTP_LOCK_LOCKED;
+	//XXX add the actual call here to write the OTP_LOCK bytes only at final stage
+	// val_copy(lock_ptr, &otp_lock_mem, sizeof(otp_lock_mem));
+
+	int locksize = 5;
+
+	int errors = 0;
+
+	// or just realise it's exctly 5x 32byte blocks we need to lock.  1 block for ID,type,vid,pid, and 4 blocks for certificate, which is 128 bytes.
+	for (int i = 0 ; i < locksize ; i++) {
+		if (F_write_byte(ADDR_OTP_LOCK_START + i, OTP_LOCK_LOCKED))
+			errors++;
+	}
+
+	return errors;
+}
+
+
+
+// COMPLETE, BUSY, or other flash error?
+int F_GetStatus(void)
+{
+	int fs = F_COMPLETE;
+
+	if ((FLASH->status & F_BSY) == F_BSY) { fs = F_BUSY; } else {
+
+		if ((FLASH->status & F_WRPERR) != (uint32_t)0x00) { fs = F_ERROR_WRP; } else {
+
+			if ((FLASH->status & (uint32_t)0xEF) != (uint32_t)0x00) { fs = F_ERROR_PROGRAM; } else {
+
+				if ((FLASH->status & F_OPERR) != (uint32_t)0x00) { fs = F_ERROR_OPERATION; } else {
+					fs = F_COMPLETE;
+				}
+			}
+		}
+	}
+
+	return fs;
+}
+
+
+// enable FLASH Registers
+void F_unlock(void)
+{
+	if ((FLASH->control & F_CR_LOCK) != 0) {
+		FLASH->key = F_KEY1;
+		FLASH->key = F_KEY2;
+	}
+}
+
+//  lock the FLASH Registers
+void F_lock(void)
+{
+	FLASH->control |= F_CR_LOCK;
+}
+
+// flash write word.
+int F_write_word(uint32_t Address, uint32_t Data)
+{
+	unsigned char octet[4] = {0, 0, 0, 0};
+
+	int ret = 0;
+
+	for (int i = 0; i < 4; i++) {
+		octet[i] = (Data >> (i * 8)) & 0xFF;
+		ret = F_write_byte(Address + i, octet[i]);
+	}
+
+	return ret;
+}
+
+// flash write byte
+int F_write_byte(uint32_t Address, uint8_t Data)
+{
+	volatile int status = F_COMPLETE;
+
+	//warnx("F_write_byte: %08X %02d", Address , Data ) ;
+
+	//Check the parameters
+	assert(IS_F_ADDRESS(Address));
+
+	//Wait for FLASH operation to complete by polling on BUSY flag.
+	status = F_GetStatus();
+
+	while (status == F_BUSY) { status = F_GetStatus();}
+
+	if (status == F_COMPLETE) {
+		//if the previous operation is completed, proceed to program the new data
+		FLASH->control &= CR_PSIZE_MASK;
+		FLASH->control |= F_PSIZE_BYTE;
+		FLASH->control |= F_CR_PG;
+
+		*(volatile uint8_t *)Address = Data;
+
+		//Wait for FLASH operation to complete by polling on BUSY flag.
+		status = F_GetStatus();
+
+		while (status == F_BUSY) { status = F_GetStatus();}
+
+		//if the program operation is completed, disable the PG Bit
+		FLASH->control &= (~F_CR_PG);
+	}
+
+	//Return the Program Status
+	return !(status == F_COMPLETE);
+}
+
+
+
diff --git a/src/modules/systemlib/otp.h b/src/modules/systemlib/otp.h
new file mode 100644
index 000000000..f10e129d8
--- /dev/null
+++ b/src/modules/systemlib/otp.h
@@ -0,0 +1,151 @@
+/****************************************************************************
+ *
+ *   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 otp.h
+ * One TIme Programmable ( OTP ) Flash routine/s.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author David "Buzz" Bussenschutt <davidbuzz@gmail.com>
+ *
+ */
+
+#ifndef OTP_H_
+#define OTP_H_
+
+__BEGIN_DECLS
+
+#define ADDR_OTP_START			0x1FFF7800
+#define ADDR_OTP_LOCK_START		0x1FFF7A00
+
+#define OTP_LOCK_LOCKED			0x00
+#define OTP_LOCK_UNLOCKED		0xFF
+
+
+
+#include <unistd.h>
+#include <stdio.h>
+
+// possible flash statuses
+#define F_BUSY 1
+#define F_ERROR_WRP 2
+#define F_ERROR_PROGRAM 3
+#define F_ERROR_OPERATION 4
+#define F_COMPLETE 5
+
+typedef struct {
+	volatile uint32_t accesscontrol;      // 0x00
+	volatile uint32_t key;     //  0x04
+	volatile uint32_t optionkey;  //  0x08
+	volatile uint32_t status;       // 0x0C
+	volatile uint32_t control;       // 0x10
+	volatile uint32_t optioncontrol;    //0x14
+} flash_registers;
+
+#define PERIPH_BASE           ((uint32_t)0x40000000) //Peripheral base address
+#define AHB1PERIPH_BASE       (PERIPH_BASE + 0x00020000)
+#define F_R_BASE          (AHB1PERIPH_BASE + 0x3C00)
+#define FLASH               ((flash_registers *) F_R_BASE)
+
+#define F_BSY ((uint32_t)0x00010000) //FLASH Busy flag bit
+#define F_OPERR ((uint32_t)0x00000002) //FLASH operation Error flag bit
+#define F_WRPERR ((uint32_t)0x00000010) //FLASH Write protected error flag bit
+#define CR_PSIZE_MASK ((uint32_t)0xFFFFFCFF)
+#define F_PSIZE_WORD ((uint32_t)0x00000200)
+#define F_PSIZE_BYTE ((uint32_t)0x00000000)
+#define F_CR_PG ((uint32_t)0x00000001) // a bit in the F_CR register
+#define F_CR_LOCK ((uint32_t)0x80000000) // also another bit.
+
+#define F_KEY1 ((uint32_t)0x45670123)
+#define F_KEY2 ((uint32_t)0xCDEF89AB)
+#define IS_F_ADDRESS(ADDRESS) ((((ADDRESS) >= 0x08000000) && ((ADDRESS) < 0x080FFFFF)) || (((ADDRESS) >= 0x1FFF7800) && ((ADDRESS) < 0x1FFF7A0F)))
+
+
+
+#pragma pack(push, 1)
+
+/*
+ * The OTP area is divided into 16 OTP data blocks of 32 bytes and one lock OTP block of 16 bytes.
+ * The OTP data and lock blocks cannot be erased. The lock block contains 16 bytes LOCKBi (0 ≤ i ≤ 15)
+ * to lock the corresponding OTP data block (blocks 0 to 15). Each OTP data block can be programmed
+ * until the value 0x00 is programmed in the corresponding OTP lock byte. The lock bytes must only
+ * contain 0x00 and 0xFF values, otherwise the OTP bytes might not be taken into account correctly.
+ */
+
+struct otp {
+	// first 32 bytes =  the '0' Block
+	char		id[4];		///4 bytes < 'P' 'X' '4' '\n'
+	uint8_t		id_type;	///1 byte < 0 for USB VID, 1 for generic VID
+	uint32_t	vid;        ///4 bytes
+	uint32_t	pid;        ///4 bytes
+	char        unused[19];  ///19 bytes
+	// Cert-of-Auth is next 4 blocks ie 1-4  ( where zero is first block )
+	char        signature[128];
+	// insert extras here
+	uint32_t	lock_bytes[4];
+};
+
+struct otp_lock {
+	uint8_t		lock_bytes[16];
+};
+#pragma pack(pop)
+
+#define ADDR_F_SIZE		0x1FFF7A22
+
+#pragma pack(push, 1)
+union udid {
+	uint32_t	serial[3];
+	char  data[12];
+};
+#pragma pack(pop)
+
+
+/**
+ *   s
+ */
+//__EXPORT float calc_indicated_airspeed(float differential_pressure);
+
+__EXPORT void F_unlock(void);
+__EXPORT void F_lock(void);
+__EXPORT int val_read(void *dest, volatile const void *src, int bytes);
+__EXPORT int val_write(volatile void *dest, const void *src, int bytes);
+__EXPORT int write_otp(uint8_t id_type, uint32_t vid, uint32_t pid, char *signature);
+__EXPORT int lock_otp(void);
+
+
+__EXPORT int F_write_byte(uint32_t Address, uint8_t Data);
+__EXPORT int F_write_word(uint32_t Address, uint32_t Data);
+
+__END_DECLS
+
+#endif
diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index 69a9bdf9b..2d773fd25 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -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
@@ -48,6 +48,7 @@
 #include <unistd.h>
 #include <systemlib/err.h>
 #include <errno.h>
+#include <semaphore.h>
 
 #include <sys/stat.h>
 
@@ -60,7 +61,7 @@
 #include "uORB/uORB.h"
 #include "uORB/topics/parameter_update.h"
 
-#if 1
+#if 0
 # define debug(fmt, args...)		do { warnx(fmt, ##args); } while(0)
 #else
 # define debug(fmt, args...)		do { } while(0)
@@ -95,18 +96,20 @@ 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)
 {
-	/* XXX */
+	//do {} while (sem_wait(&param_sem) != 0);
 }
 
 /** unlock the parameter store */
 static void
 param_unlock(void)
 {
-	/* XXX */
+	//sem_post(&param_sem);
 }
 
 /** assert that the parameter store is locked */
@@ -505,27 +508,86 @@ param_get_default_file(void)
 int
 param_save_default(void)
 {
-	/* delete the file in case it exists */
-	unlink(param_get_default_file());
+	int res;
+	int fd;
 
-	/* create the file */
-	int fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL);
+	const char *filename = param_get_default_file();
+
+	/* write parameters to temp file */
+	fd = open(filename, O_WRONLY | O_CREAT);
 
 	if (fd < 0) {
-		warn("opening '%s' for writing failed", param_get_default_file());
-		return -1;
+		warn("failed to open param file: %s", filename);
+		return ERROR;
+	}
+
+	if (res == OK) {
+		res = param_export(fd, false);
+
+	        if (res != OK) {
+	            warnx("failed to write parameters to file: %s", filename);
+	        }
 	}
 
-	int result = param_export(fd, false);
 	close(fd);
 
-	if (result != 0) {
-		warn("error exporting parameters to '%s'", param_get_default_file());
-		unlink(param_get_default_file());
-		return -2;
+	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);
 	}
 
-	return 0;
+	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
 }
 
 /**
@@ -534,9 +596,9 @@ param_save_default(void)
 int
 param_load_default(void)
 {
-	int fd = open(param_get_default_file(), O_RDONLY);
+	int fd_load = open(param_get_default_file(), O_RDONLY);
 
-	if (fd < 0) {
+	if (fd_load < 0) {
 		/* no parameter file is OK, otherwise this is an error */
 		if (errno != ENOENT) {
 			warn("open '%s' for reading failed", param_get_default_file());
@@ -545,8 +607,8 @@ param_load_default(void)
 		return 1;
 	}
 
-	int result = param_load(fd);
-	close(fd);
+	int result = param_load(fd_load);
+	close(fd_load);
 
 	if (result != 0) {
 		warn("error reading parameters from '%s'", param_get_default_file());
diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c
index 879f4715a..b4ca0ed3e 100644
--- a/src/modules/systemlib/perf_counter.c
+++ b/src/modules/systemlib/perf_counter.c
@@ -201,17 +201,23 @@ perf_end(perf_counter_t handle)
 	switch (handle->type) {
 	case PC_ELAPSED: {
 			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
-			hrt_abstime elapsed = hrt_absolute_time() - pce->time_start;
 
-			pce->event_count++;
-			pce->time_total += elapsed;
+			if (pce->time_start != 0) {
+				hrt_abstime elapsed = hrt_absolute_time() - pce->time_start;
 
-			if ((pce->time_least > elapsed) || (pce->time_least == 0))
-				pce->time_least = elapsed;
+				pce->event_count++;
+				pce->time_total += elapsed;
 
-			if (pce->time_most < elapsed)
-				pce->time_most = elapsed;
+				if ((pce->time_least > elapsed) || (pce->time_least == 0))
+					pce->time_least = elapsed;
+
+				if (pce->time_most < elapsed)
+					pce->time_most = elapsed;
+
+				pce->time_start = 0;
+			}
 		}
+		break;
 
 	default:
 		break;
@@ -219,6 +225,27 @@ perf_end(perf_counter_t handle)
 }
 
 void
+perf_cancel(perf_counter_t handle)
+{
+	if (handle == NULL)
+		return;
+
+	switch (handle->type) {
+	case PC_ELAPSED: {
+			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+
+			pce->time_start = 0;
+		}
+		break;
+
+	default:
+		break;
+	}
+}
+
+
+
+void
 perf_reset(perf_counter_t handle)
 {
 	if (handle == NULL)
@@ -268,10 +295,11 @@ 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, min %lluus max %lluus\n",
+		printf("%s: %llu events, %lluus elapsed, %llu avg, min %lluus max %lluus\n",
 		       handle->name,
 		       pce->event_count,
 		       pce->time_total,
+		       pce->time_total / pce->event_count,
 		       pce->time_least,
 		       pce->time_most);
 		break;
@@ -294,6 +322,32 @@ perf_print_counter(perf_counter_t handle)
 	}
 }
 
+uint64_t
+perf_event_count(perf_counter_t handle)
+{
+	if (handle == NULL)
+		return 0;
+
+	switch (handle->type) {
+	case PC_COUNT:
+		return ((struct perf_ctr_count *)handle)->event_count;
+
+	case PC_ELAPSED: {
+		struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+		return pce->event_count;
+	}
+
+	case PC_INTERVAL: {
+		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
+		return pci->event_count;
+	}
+
+	default:
+		break;
+	}
+	return 0;
+}
+
 void
 perf_print_all(void)
 {
diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h
index 5c2cb15b2..e1e3cbe95 100644
--- a/src/modules/systemlib/perf_counter.h
+++ b/src/modules/systemlib/perf_counter.h
@@ -92,13 +92,25 @@ __EXPORT extern void		perf_begin(perf_counter_t handle);
  * End a performance event.
  *
  * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * If a call is made without a corresopnding perf_begin call, or if perf_cancel
+ * has been called subsequently, no change is made to the counter.
  *
  * @param handle		The handle returned from perf_alloc.
  */
 __EXPORT extern void		perf_end(perf_counter_t handle);
 
 /**
- * Reset a performance event.
+ * Cancel a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * It reverts the effect of a previous perf_begin.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+__EXPORT extern void		perf_cancel(perf_counter_t handle);
+
+/**
+ * Reset a performance counter.
  *
  * This call resets performance counter to initial state
  *
@@ -123,6 +135,14 @@ __EXPORT extern void		perf_print_all(void);
  */
 __EXPORT extern void		perf_reset_all(void);
 
+/**
+ * Return current event_count
+ *
+ * @param handle		The counter returned from perf_alloc.
+ * @return			event_count
+ */
+__EXPORT extern uint64_t	perf_event_count(perf_counter_t handle);
+
 __END_DECLS
 
 #endif
diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c
index 49315cdc9..77c952f52 100644
--- a/src/modules/systemlib/pid/pid.c
+++ b/src/modules/systemlib/pid/pid.c
@@ -1,9 +1,11 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Laurens Mackay <mackayl@student.ethz.ch>
- *           @author Tobias Naegeli <naegelit@student.ethz.ch>
- *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Laurens Mackay <mackayl@student.ethz.ch>
+ *           Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Martin Rutschmann <rutmarti@student.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
@@ -36,14 +38,23 @@
 
 /**
  * @file pid.c
- * Implementation of generic PID control interface
+ *
+ * Implementation of generic PID control interface.
+ *
+ * @author Laurens Mackay <mackayl@student.ethz.ch>
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
 #include "pid.h"
 #include <math.h>
 
+#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 limit, uint8_t mode, float dt_min)
 {
 	pid->kp = kp;
 	pid->ki = ki;
@@ -51,13 +62,13 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
 	pid->intmax = intmax;
 	pid->limit = limit;
 	pid->mode = mode;
-	pid->count = 0;
-	pid->saturated = 0;
-	pid->last_output = 0;
-
-	pid->sp = 0;
-	pid->error_previous = 0;
-	pid->integral = 0;
+	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->integral = 0.0f;
 }
 __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit)
 {
@@ -136,14 +147,14 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 	// Calculated current error value
 	float error = pid->sp - val;
 
-	if (isfinite(error)) {				// Why is this necessary?  DEW
-		pid->error_previous = error;
-	}
-
 	// Calculate or measured current error derivative
-
 	if (pid->mode == PID_MODE_DERIVATIV_CALC) {
-		d = (error - pid->error_previous) / dt;
+		d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
+		pid->error_previous = error;
+
+	} else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) {
+		d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
+		pid->error_previous = -val;
 
 	} else if (pid->mode == PID_MODE_DERIVATIV_SET) {
 		d = -val_dot;
@@ -152,35 +163,49 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 		d = 0.0f;
 	}
 
-	// Calculate the error integral and check for saturation
-	i = pid->integral + (error * dt);
+	if (!isfinite(d)) {
+		d = 0.0f;
+	}
 
-	if (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;
+	if (pid->ki > 0.0f) {
+		// Calculate the error integral and check for saturation
+		i = pid->integral + (error * dt);
 
-	} else {
-		if (!isfinite(i)) {
-			i = 0;
+		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;
+			}
+
+			pid->integral = i;
+			pid->saturated = 0;
 		}
 
-		pid->integral = i;
+	} else {
+		i = 0.0f;
 		pid->saturated = 0;
 	}
 
 	// Calculate the output.  Limit output magnitude to pid->limit
-	float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd);
+	float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
 
-	if (output > pid->limit) output = pid->limit;
+	if (isfinite(output)) {
+		if (pid->limit > SIGMA) {
+			if (output > pid->limit) {
+				output = pid->limit;
 
-	if (output < -pid->limit) output = -pid->limit;
+			} else if (output < -pid->limit) {
+				output = -pid->limit;
+			}
+		}
 
-	if (isfinite(output)) {
 		pid->last_output = output;
 	}
 
-
 	return pid->last_output;
 }
 
diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h
index 64d668867..eca228464 100644
--- a/src/modules/systemlib/pid/pid.h
+++ b/src/modules/systemlib/pid/pid.h
@@ -1,9 +1,11 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Laurens Mackay <mackayl@student.ethz.ch>
- *           @author Tobias Naegeli <naegelit@student.ethz.ch>
- *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Laurens Mackay <mackayl@student.ethz.ch>
+ *           Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Martin Rutschmann <rutmarti@student.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
@@ -36,7 +38,14 @@
 
 /**
  * @file pid.h
- * Definition of generic PID control interface
+ *
+ * Definition of generic PID control interface.
+ *
+ * @author Laurens Mackay <mackayl@student.ethz.ch>
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
 #ifndef PID_H_
@@ -44,11 +53,16 @@
 
 #include <stdint.h>
 
+__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	1
+#define PID_MODE_DERIVATIV_SET	2
 // Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID)
 #define PID_MODE_DERIVATIV_NONE 9
 
@@ -62,17 +76,19 @@ typedef struct {
 	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);
+__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 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
 
 #endif /* PID_H_ */
diff --git a/src/modules/systemlib/ppm_decode.h b/src/modules/systemlib/ppm_decode.h
index 6c5e15345..5a1ad84da 100644
--- a/src/modules/systemlib/ppm_decode.h
+++ b/src/modules/systemlib/ppm_decode.h
@@ -57,6 +57,7 @@ __BEGIN_DECLS
  * PPM decoder state
  */
 __EXPORT extern uint16_t	ppm_buffer[PPM_MAX_CHANNELS];	/**< decoded PPM channel values */
+__EXPORT extern uint16_t	ppm_frame_length;				/**< length of the decoded PPM frame (includes gap) */
 __EXPORT extern unsigned	ppm_decoded_channels;	/**< count of decoded channels */
 __EXPORT extern hrt_abstime	ppm_last_valid_decode;	/**< timestamp of the last valid decode */
 
diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c
new file mode 100644
index 000000000..190b315f1
--- /dev/null
+++ b/src/modules/systemlib/pwm_limit/pwm_limit.c
@@ -0,0 +1,151 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file pwm_limit.c
+ *
+ * Lib to limit PWM output
+ *
+ * @author Julian Oes <joes@student.ethz.ch>
+ */
+
+#include "pwm_limit.h"
+#include <math.h>
+#include <stdbool.h>
+#include <drivers/drv_hrt.h>
+#include <stdio.h>
+
+void pwm_limit_init(pwm_limit_t *limit)
+{
+	limit->state = PWM_LIMIT_STATE_INIT;
+	limit->time_armed = 0;
+	return;
+}
+
+void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit)
+{
+
+	/* first evaluate state changes */
+	switch (limit->state) {
+		case PWM_LIMIT_STATE_INIT:
+
+			if (armed) {
+
+				/* set arming time for the first call */
+				if (limit->time_armed == 0) {
+					limit->time_armed = hrt_absolute_time();
+				}
+
+				if (hrt_elapsed_time(&limit->time_armed) >= INIT_TIME_US) {
+					limit->state = PWM_LIMIT_STATE_OFF;
+				}
+			}
+			break;
+		case PWM_LIMIT_STATE_OFF:
+			if (armed) {
+				limit->state = PWM_LIMIT_STATE_RAMP;
+
+				/* reset arming time, used for ramp timing */
+				limit->time_armed = hrt_absolute_time();
+			}
+			break;
+		case PWM_LIMIT_STATE_RAMP:
+			if (!armed) {
+				limit->state = PWM_LIMIT_STATE_OFF;
+			} else if (hrt_elapsed_time(&limit->time_armed) >= RAMP_TIME_US) {
+				limit->state = PWM_LIMIT_STATE_ON;
+			}
+			break;
+		case PWM_LIMIT_STATE_ON:
+			if (!armed) {
+				limit->state = PWM_LIMIT_STATE_OFF;
+			}
+			break;
+		default:
+			break;
+	}
+
+	unsigned progress;
+	uint16_t temp_pwm;
+
+	/* then set effective_pwm based on state */
+	switch (limit->state) {
+		case PWM_LIMIT_STATE_OFF:
+		case PWM_LIMIT_STATE_INIT:
+			for (unsigned i=0; i<num_channels; i++) {
+				effective_pwm[i] = disarmed_pwm[i];
+			}
+			break;
+		case PWM_LIMIT_STATE_RAMP:
+			{
+				hrt_abstime diff = hrt_elapsed_time(&limit->time_armed);
+
+				progress = diff * 10000 / RAMP_TIME_US;
+
+				for (unsigned i=0; i<num_channels; i++) {
+	                
+					uint16_t ramp_min_pwm;
+	                
+					/* if a disarmed pwm value was set, blend between disarmed and min */
+					if (disarmed_pwm[i] > 0) {
+
+						/* safeguard against overflows */
+						unsigned disarmed = disarmed_pwm[i];
+						if (disarmed > min_pwm[i]) {
+							disarmed = min_pwm[i];
+						}
+
+						unsigned disarmed_min_diff = min_pwm[i] - disarmed;
+						ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000;
+
+					} else {
+	                    
+						/* no disarmed pwm value set, choose min pwm */
+						ramp_min_pwm = min_pwm[i];
+					}
+
+					effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2;
+				}
+			}
+			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;
+			}
+			break;
+		default:
+			break;
+	}
+	return;
+}
diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.h b/src/modules/systemlib/pwm_limit/pwm_limit.h
new file mode 100644
index 000000000..6a667ac6f
--- /dev/null
+++ b/src/modules/systemlib/pwm_limit/pwm_limit.h
@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file pwm_limit.h
+ *
+ * Lib to limit PWM output
+ *
+ * @author Julian Oes <joes@student.ethz.ch>
+ */
+
+#ifndef PWM_LIMIT_H_
+#define PWM_LIMIT_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+
+__BEGIN_DECLS
+
+/*
+ * time for the ESCs to initialize
+ * (this is not actually needed if PWM is sent right after boot)
+ */
+#define INIT_TIME_US 500000
+/*
+ * time to slowly ramp up the ESCs
+ */
+#define RAMP_TIME_US 2500000
+
+enum pwm_limit_state {
+	PWM_LIMIT_STATE_OFF = 0,
+	PWM_LIMIT_STATE_INIT,
+	PWM_LIMIT_STATE_RAMP,
+	PWM_LIMIT_STATE_ON
+};
+
+typedef struct {
+	enum pwm_limit_state state;
+	uint64_t time_armed;
+} pwm_limit_t;
+
+__EXPORT void pwm_limit_init(pwm_limit_t *limit);
+
+__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit);
+
+__END_DECLS
+
+#endif /* PWM_LIMIT_H_ */
diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c
new file mode 100644
index 000000000..21e15ec56
--- /dev/null
+++ b/src/modules/systemlib/rc_check.c
@@ -0,0 +1,151 @@
+/****************************************************************************
+ *
+ *   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 rc_check.c
+ *
+ * RC calibration check
+ */
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <fcntl.h>
+
+#include <systemlib/rc_check.h>
+#include <systemlib/param/param.h>
+#include <mavlink/mavlink_log.h>
+#include <drivers/drv_rc_input.h>
+
+int rc_calibration_check(int mavlink_fd) {
+
+	char nbuf[20];
+	param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max,
+	_parameter_handles_rev, _parameter_handles_dz;
+
+	float param_min, param_max, param_trim, param_rev, param_dz;
+
+	/* first check channel mappings */
+			/* check which map param applies */
+		// if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) {
+		// 	mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
+		// 	/* give system time to flush error message in case there are more */
+		// 	usleep(100000);
+		// 	count++;
+		// }
+
+	int channel_fail_count = 0;
+
+	for (int i = 0; i < RC_INPUT_MAX_CHANNELS; i++) {
+		/* should the channel be enabled? */
+		uint8_t count = 0;
+
+		/* min values */
+		sprintf(nbuf, "RC%d_MIN", i + 1);
+		_parameter_handles_min = param_find(nbuf);
+		param_get(_parameter_handles_min, &param_min);
+
+		/* trim values */
+		sprintf(nbuf, "RC%d_TRIM", i + 1);
+		_parameter_handles_trim = param_find(nbuf);
+		param_get(_parameter_handles_trim, &param_trim);
+
+		/* max values */
+		sprintf(nbuf, "RC%d_MAX", i + 1);
+		_parameter_handles_max = param_find(nbuf);
+		param_get(_parameter_handles_max, &param_max);
+
+		/* channel reverse */
+		sprintf(nbuf, "RC%d_REV", i + 1);
+		_parameter_handles_rev = param_find(nbuf);
+		param_get(_parameter_handles_rev, &param_rev);
+
+		/* channel deadzone */
+		sprintf(nbuf, "RC%d_DZ", i + 1);
+		_parameter_handles_dz = param_find(nbuf);
+		param_get(_parameter_handles_dz, &param_dz);
+
+		/* assert min..center..max ordering */
+		if (param_min < 500) {
+			count++;
+			mavlink_log_critical(mavlink_fd, "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);
+			/* 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);
+			/* 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);
+			/* 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);
+			/* give system time to flush error message in case there are more */
+			usleep(100000);
+			count++;
+		}
+
+		/* check which map param applies */
+		// if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) {
+		// 	mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
+		// 	/* give system time to flush error message in case there are more */
+		// 	usleep(100000);
+		// 	count++;
+		// }
+
+		/* 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));
+			usleep(100000);
+		}
+
+		channel_fail_count += count;
+	}
+
+	return channel_fail_count;
+}
diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h
new file mode 100644
index 000000000..e70b83cce
--- /dev/null
+++ b/src/modules/systemlib/rc_check.h
@@ -0,0 +1,52 @@
+/****************************************************************************
+ *
+ *   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 rc_check.h
+ *
+ * RC calibration check
+ */
+
+#pragma once
+
+ __BEGIN_DECLS
+
+/**
+ * Check the RC calibration
+ *
+ * @return			0 / OK if RC calibration is ok, index + 1 of the first
+ *				channel that failed else (so 1 == first channel failed)
+ */
+__EXPORT int	rc_calibration_check(int mavlink_fd);
+
+__END_DECLS
diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c
new file mode 100644
index 000000000..75be090f8
--- /dev/null
+++ b/src/modules/systemlib/system_params.c
@@ -0,0 +1,47 @@
+/****************************************************************************
+ *
+ *   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 system_params.c
+ *
+ * System wide parameters
+ */
+
+#include <nuttx/config.h>
+#include <systemlib/param/param.h>
+
+// Auto-start script with index #n
+PARAM_DEFINE_INT32(SYS_AUTOSTART, 0);
+
+// Automatically configure default values
+PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0);
diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c
index 3283aad8a..57a751e1c 100644
--- a/src/modules/systemlib/systemlib.c
+++ b/src/modules/systemlib/systemlib.c
@@ -43,13 +43,29 @@
 #include <fcntl.h>
 #include <sched.h>
 #include <signal.h>
-#include <sys/stat.h>
 #include <unistd.h>
 #include <float.h>
 #include <string.h>
 
+#include <sys/stat.h>
+#include <sys/types.h>
+
+#include <stm32_pwr.h>
+
 #include "systemlib.h"
 
+void
+systemreset(bool to_bootloader)
+{
+	if (to_bootloader) {
+		stm32_pwr_enablebkp();
+
+		/* XXX wow, this is evil - write a magic number into backup register zero */
+		*(uint32_t *)0x40002850 = 0xb007b007;
+	}
+	up_systemreset();
+}
+
 static void kill_task(FAR struct tcb_s *tcb, FAR void *arg);
 
 void killall()
diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h
index 0194b5e52..3728f2067 100644
--- a/src/modules/systemlib/systemlib.h
+++ b/src/modules/systemlib/systemlib.h
@@ -42,11 +42,11 @@
 #include <float.h>
 #include <stdint.h>
 
-/** Reboots the board */
-extern void up_systemreset(void) noreturn_function;
-
 __BEGIN_DECLS
 
+/** Reboots the board */
+__EXPORT void systemreset(bool to_bootloader) noreturn_function;
+
 /** Sends SIGUSR1 to all processes */
 __EXPORT void killall(void);
 
diff --git a/src/modules/test/foo.c b/src/modules/test/foo.c
deleted file mode 100644
index ff6af031f..000000000
--- a/src/modules/test/foo.c
+++ /dev/null
@@ -1,4 +0,0 @@
-int test_main(void)
-{
-	return 0;
-}
\ No newline at end of file
diff --git a/src/modules/test/module.mk b/src/modules/test/module.mk
deleted file mode 100644
index abf80eedf..000000000
--- a/src/modules/test/module.mk
+++ /dev/null
@@ -1,4 +0,0 @@
-
-MODULE_NAME	 = test
-SRCS		 = foo.c
-
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 4197f6fb2..c6a252b55 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -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
@@ -37,6 +37,10 @@
  * Common object definitions without a better home.
  */
 
+/**
+ * @defgroup topics List of all uORB topics.
+ */
+
 #include <nuttx/config.h>
 
 #include <drivers/drv_orb_dev.h>
@@ -77,9 +81,15 @@ ORB_DEFINE(home_position, struct home_position_s);
 #include "topics/vehicle_status.h"
 ORB_DEFINE(vehicle_status, struct vehicle_status_s);
 
+#include "topics/safety.h"
+ORB_DEFINE(safety, struct safety_s);
+
 #include "topics/battery_status.h"
 ORB_DEFINE(battery_status, struct battery_status_s);
 
+#include "topics/servorail_status.h"
+ORB_DEFINE(servorail_status, struct servorail_status_s);
+
 #include "topics/vehicle_global_position.h"
 ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s);
 
@@ -98,27 +108,45 @@ ORB_DEFINE(rc_channels, struct rc_channels_s);
 #include "topics/vehicle_command.h"
 ORB_DEFINE(vehicle_command, struct vehicle_command_s);
 
+#include "topics/vehicle_control_mode.h"
+ORB_DEFINE(vehicle_control_mode, struct vehicle_control_mode_s);
+
 #include "topics/vehicle_local_position_setpoint.h"
 ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoint_s);
 
+#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/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);
+
 #include "topics/vehicle_attitude_setpoint.h"
 ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);
 
 #include "topics/manual_control_setpoint.h"
 ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s);
 
+#include "topics/vehicle_control_debug.h"
+ORB_DEFINE(vehicle_control_debug, struct vehicle_control_debug_s);
+
 #include "topics/offboard_control_setpoint.h"
 ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s);
 
 #include "topics/optical_flow.h"
 ORB_DEFINE(optical_flow, struct optical_flow_s);
 
+#include "topics/filtered_bottom_flow.h"
+ORB_DEFINE(filtered_bottom_flow, struct filtered_bottom_flow_s);
+
 #include "topics/omnidirectional_flow.h"
 ORB_DEFINE(omnidirectional_flow, struct omnidirectional_flow_s);
 
@@ -137,14 +165,9 @@ ORB_DEFINE(actuator_controls_0, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_1, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_2, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
-ORB_DEFINE(actuator_armed, struct actuator_armed_s);
 
-/* actuator controls, as set by actuators / mixers after limiting */
-#include "topics/actuator_controls_effective.h"
-ORB_DEFINE(actuator_controls_effective_0, struct actuator_controls_effective_s);
-ORB_DEFINE(actuator_controls_effective_1, struct actuator_controls_effective_s);
-ORB_DEFINE(actuator_controls_effective_2, struct actuator_controls_effective_s);
-ORB_DEFINE(actuator_controls_effective_3, struct actuator_controls_effective_s);
+#include "topics/actuator_armed.h"
+ORB_DEFINE(actuator_armed, struct actuator_armed_s);
 
 #include "topics/actuator_outputs.h"
 ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s);
@@ -152,5 +175,14 @@ ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s);
 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);
+
 #include "topics/debug_key_value.h"
 ORB_DEFINE(debug_key_value, struct debug_key_value_s);
+
+#include "topics/navigation_capabilities.h"
+ORB_DEFINE(navigation_capabilities, struct navigation_capabilities_s);
+
+#include "topics/esc_status.h"
+ORB_DEFINE(esc_status, struct esc_status_s);
diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h
new file mode 100644
index 000000000..6e944ffee
--- /dev/null
+++ b/src/modules/uORB/topics/actuator_armed.h
@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ *   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 actuator_armed.h
+ *
+ * Actuator armed topic
+ *
+ */
+
+#ifndef TOPIC_ACTUATOR_ARMED_H
+#define TOPIC_ACTUATOR_ARMED_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/** 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) */
+};
+
+ORB_DECLARE(actuator_armed);
+
+#endif
\ No newline at end of file
diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h
index b7c4196c0..e768ab2f6 100644
--- a/src/modules/uORB/topics/actuator_controls.h
+++ b/src/modules/uORB/topics/actuator_controls.h
@@ -52,27 +52,27 @@
 #define NUM_ACTUATOR_CONTROLS		8
 #define NUM_ACTUATOR_CONTROL_GROUPS	4	/**< for sanity checking */
 
+/* control sets with pre-defined applications */
+#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS	ORB_ID(actuator_controls_0)
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 struct actuator_controls_s {
 	uint64_t timestamp;
 	float	control[NUM_ACTUATOR_CONTROLS];
 };
 
+/**
+ * @}
+ */
+
 /* actuator control sets; this list can be expanded as more controllers emerge */
 ORB_DECLARE(actuator_controls_0);
 ORB_DECLARE(actuator_controls_1);
 ORB_DECLARE(actuator_controls_2);
 ORB_DECLARE(actuator_controls_3);
 
-/* control sets with pre-defined applications */
-#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS	ORB_ID(actuator_controls_0)
-
-/** global 'actuator output is live' control. */
-struct actuator_armed_s {
-	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) */
-};
-
-ORB_DECLARE(actuator_armed);
-
-#endif
\ No newline at end of file
+#endif
diff --git a/src/modules/uORB/topics/actuator_controls_effective.h b/src/modules/uORB/topics/actuator_controls_effective.h
index 088c4fc8f..54d84231f 100644
--- a/src/modules/uORB/topics/actuator_controls_effective.h
+++ b/src/modules/uORB/topics/actuator_controls_effective.h
@@ -46,25 +46,34 @@
 #ifndef TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H
 #define TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H
 
-#include <stdint.h>
-#include "../uORB.h"
-#include "actuator_controls.h"
+//#include <stdint.h>
+//#include "../uORB.h"
+//#include "actuator_controls.h"
+//
+//#define NUM_ACTUATOR_CONTROLS_EFFECTIVE		NUM_ACTUATOR_CONTROLS
+//#define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE	NUM_ACTUATOR_CONTROL_GROUPS	/**< for sanity checking */
+//
+///**
+// * @addtogroup topics
+// * @{
+// */
+//
+//struct actuator_controls_effective_s {
+//	uint64_t timestamp;
+//	float	control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE];
+//};
+//
+///**
+// * @}
+// */
+//
+///* actuator control sets; this list can be expanded as more controllers emerge */
+//ORB_DECLARE(actuator_controls_effective_0);
+//ORB_DECLARE(actuator_controls_effective_1);
+//ORB_DECLARE(actuator_controls_effective_2);
+//ORB_DECLARE(actuator_controls_effective_3);
+//
+///* control sets with pre-defined applications */
+//#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE	ORB_ID(actuator_controls_effective_0)
 
-#define NUM_ACTUATOR_CONTROLS_EFFECTIVE		NUM_ACTUATOR_CONTROLS
-#define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE	NUM_ACTUATOR_CONTROL_GROUPS	/**< for sanity checking */
-
-struct actuator_controls_effective_s {
-	uint64_t timestamp;
-	float	control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE];
-};
-
-/* actuator control sets; this list can be expanded as more controllers emerge */
-ORB_DECLARE(actuator_controls_effective_0);
-ORB_DECLARE(actuator_controls_effective_1);
-ORB_DECLARE(actuator_controls_effective_2);
-ORB_DECLARE(actuator_controls_effective_3);
-
-/* control sets with pre-defined applications */
-#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE	ORB_ID(actuator_controls_effective_0)
-
-#endif /* TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H */
\ No newline at end of file
+#endif /* TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H */
diff --git a/src/modules/uORB/topics/actuator_outputs.h b/src/modules/uORB/topics/actuator_outputs.h
index bbe429073..446140423 100644
--- a/src/modules/uORB/topics/actuator_outputs.h
+++ b/src/modules/uORB/topics/actuator_outputs.h
@@ -52,12 +52,21 @@
 #define NUM_ACTUATOR_OUTPUTS		16
 #define NUM_ACTUATOR_OUTPUT_GROUPS	4	/**< for sanity checking */
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 struct actuator_outputs_s {
 	uint64_t timestamp;				/**< output timestamp in us since system boot */
 	float	output[NUM_ACTUATOR_OUTPUTS];		/**< output data, in natural output units */
-	int noutputs;					/**< valid outputs */
+	unsigned noutputs;					/**< valid outputs */
 };
 
+/**
+ * @}
+ */
+
 /* actuator output sets; this list can be expanded as more drivers emerge */
 ORB_DECLARE(actuator_outputs_0);
 ORB_DECLARE(actuator_outputs_1);
diff --git a/src/modules/uORB/topics/battery_status.h b/src/modules/uORB/topics/battery_status.h
index c40d0d4e5..d473dff3f 100644
--- a/src/modules/uORB/topics/battery_status.h
+++ b/src/modules/uORB/topics/battery_status.h
@@ -53,9 +53,10 @@
  */
 struct battery_status_s {
 	uint64_t	timestamp;		/**< microseconds since system boot, needed to integrate */
-	float   	voltage_v;		/**< Battery voltage in volts, filtered           	 */
-	float		current_a;		/**< Battery current in amperes, filtered, -1 if unknown */
-	float		discharged_mah;		/**< Discharged amount in mAh, filtered, -1 if unknown	 */
+	float   	voltage_v;		/**< Battery voltage in volts, 0 if unknown           	 */
+	float   	voltage_filtered_v;		/**< Battery voltage in volts, filtered, 0 if unknown  	 */
+	float		current_a;		/**< Battery current in amperes, -1 if unknown */
+	float		discharged_mah;		/**< Discharged amount in mAh, -1 if unknown	 */
 };
 
 /**
@@ -65,4 +66,4 @@ struct battery_status_s {
 /* register this as object request broker structure */
 ORB_DECLARE(battery_status);
 
-#endif
\ No newline at end of file
+#endif
diff --git a/src/modules/uORB/topics/debug_key_value.h b/src/modules/uORB/topics/debug_key_value.h
index a9d1b83fd..9253c787d 100644
--- a/src/modules/uORB/topics/debug_key_value.h
+++ b/src/modules/uORB/topics/debug_key_value.h
@@ -47,6 +47,7 @@
 
 /**
  * @addtogroup topics
+ * @{
  */
 
 /**
diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h
index 8ce85213b..5d94d4288 100644
--- a/src/modules/uORB/topics/differential_pressure.h
+++ b/src/modules/uORB/topics/differential_pressure.h
@@ -52,10 +52,12 @@
  * Differential pressure.
  */
 struct differential_pressure_s {
-	uint64_t	timestamp;						/**< microseconds since system boot, needed to integrate */
-	uint16_t	differential_pressure_pa;		/**< Differential pressure reading */
-	uint16_t	max_differential_pressure_pa;	/**< Maximum differential pressure reading */
-	float		voltage;						/**< Voltage from analog airspeed sensors (voltage divider already compensated) */
+	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 */
 
 };
 
diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h
new file mode 100644
index 000000000..11332d7a7
--- /dev/null
+++ b/src/modules/uORB/topics/esc_status.h
@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: @author Marco Bauer <marco@wtns.de>
+ *
+ * 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 esc_status.h
+ * Definition of the esc_status uORB topic.
+ *
+ * Published the state machine and the system status bitfields
+ * (see SYS_STATUS mavlink message), published only by commander app.
+ *
+ * All apps should write to subsystem_info:
+ *
+ *  (any app) --> subsystem_info (published) --> (commander app state machine)  --> esc_status --> (mavlink app)
+ */
+
+#ifndef ESC_STATUS_H_
+#define ESC_STATUS_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * The number of ESCs supported.
+ * Current (Q2/2013) we support 8 ESCs,
+ */
+#define CONNECTED_ESC_MAX   8
+
+enum ESC_VENDOR {
+	ESC_VENDOR_GENERIC = 0,					/**< generic ESC */
+	ESC_VENDOR_MIKROKOPTER,					/**< Mikrokopter */
+	ESC_VENDOR_GRAUPNER_HOTT				/**< Graupner HoTT ESC */	
+};
+
+enum ESC_CONNECTION_TYPE {
+	ESC_CONNECTION_TYPE_PPM = 0,			/**< Traditional PPM ESC */
+	ESC_CONNECTION_TYPE_SERIAL,			/**< Serial Bus connected ESC */
+	ESC_CONNECTION_TYPE_ONESHOOT,			/**< One Shoot PPM */
+	ESC_CONNECTION_TYPE_I2C,				/**< I2C */
+	ESC_CONNECTION_TYPE_CAN					/**< CAN-Bus */
+};
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Electronic speed controller status.
+ */
+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 */
+		uint16_t esc_version;			/**< Version of current ESC - if supported */
+		uint16_t esc_voltage;			/**< Voltage measured from current ESC - if supported */
+		uint16_t esc_current;			/**< Current measured from current ESC (100mA steps) - if supported */
+		uint16_t esc_rpm;				/**< RPM measured from current ESC - if supported */
+		uint16_t esc_temperature;		/**< Temperature measured from current ESC - if supported */
+		float    esc_setpoint;			/**< setpoint of current ESC */
+		uint16_t esc_setpoint_raw;		/**< setpoint of current ESC (Value sent to ESC) */
+		uint16_t esc_state;				/**< State of ESC - depend on Vendor */
+		uint16_t esc_errorcount;		/**< Number of reported errors by ESC - if supported */
+	} esc[CONNECTED_ESC_MAX];
+
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+//ORB_DECLARE(esc_status);
+ORB_DECLARE_OPTIONAL(esc_status);
+
+#endif
diff --git a/src/modules/uORB/topics/filtered_bottom_flow.h b/src/modules/uORB/topics/filtered_bottom_flow.h
new file mode 100644
index 000000000..ab6de2613
--- /dev/null
+++ b/src/modules/uORB/topics/filtered_bottom_flow.h
@@ -0,0 +1,74 @@
+/****************************************************************************
+ *
+ *   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 filtered_bottom_flow.h
+ * Definition of the filtered bottom flow uORB topic.
+ */
+
+#ifndef TOPIC_FILTERED_BOTTOM_FLOW_H_
+#define TOPIC_FILTERED_BOTTOM_FLOW_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Filtered bottom flow in bodyframe.
+ */
+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					   */
+	float sumy;				/**< Integrated bodyframe y flow in meters			   		   */
+
+	float vx; 				/**< Flow bodyframe x speed, m/s							   */
+	float vy;				/**< Flow bodyframe y Speed, m/s 							   */
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(filtered_bottom_flow);
+
+#endif
diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
index 261a8a4ad..eac6d6e98 100644
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
@@ -56,17 +56,18 @@ struct manual_control_setpoint_s {
 	float yaw;				/**< rudder / yaw rate / yaw */
 	float throttle;				/**< throttle / collective thrust / altitude */
 
-	float manual_override_switch;		/**< manual override mode (mandatory) */
-	float auto_mode_switch;			/**< auto mode switch (mandatory) */
+	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
 	 * to indicate that it does not contain valid data.
 	 */
-	float manual_mode_switch;		/**< manual mode (man, sas, alt) switch (optional) */
-	float manual_sas_switch;		/**< sas mode (rates / attitude) switch (optional) */
-	float return_to_launch_switch;		/**< return to launch switch (0 = disabled, 1 = enabled) */
-	float auto_offboard_input_switch;	/**< controller setpoint source (0 = onboard, 1 = offboard) */
+
+	// XXX needed or parameter?
+	//float auto_offboard_input_switch;	/**< controller setpoint source (0 = onboard, 1 = offboard) */
 
 	float flaps;				/**< flap position */
 
diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
new file mode 100644
index 000000000..978a3383a
--- /dev/null
+++ b/src/modules/uORB/topics/mission.h
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   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
+ * 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.h
+ * Definition of one mission item.
+ */
+
+#ifndef TOPIC_MISSION_H_
+#define TOPIC_MISSION_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+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
+};
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * 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.
+ */
+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				*/
+	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     */
+	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;
+};
+
+struct mission_s
+{
+	struct mission_item_s *items;
+	unsigned count;
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(mission);
+
+#endif
diff --git a/src/modules/uORB/topics/navigation_capabilities.h b/src/modules/uORB/topics/navigation_capabilities.h
new file mode 100644
index 000000000..6a3e811e3
--- /dev/null
+++ b/src/modules/uORB/topics/navigation_capabilities.h
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   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 navigation_capabilities.h
+ *
+ * Definition of navigation capabilities uORB topic.
+ */
+
+#ifndef TOPIC_NAVIGATION_CAPABILITIES_H_
+#define TOPIC_NAVIGATION_CAPABILITIES_H_
+
+#include "../uORB.h"
+#include <stdint.h>
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Airspeed
+ */
+struct navigation_capabilities_s {
+    float turn_distance;    /**< the optimal distance to a waypoint to switch to the next */
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(navigation_capabilities);
+
+#endif
diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h
index 2e895c59c..7901b930a 100644
--- a/src/modules/uORB/topics/offboard_control_setpoint.h
+++ b/src/modules/uORB/topics/offboard_control_setpoint.h
@@ -44,11 +44,6 @@
 #include "../uORB.h"
 
 /**
- * @addtogroup topics
- * @{
- */
-
-/**
  * Off-board control inputs.
  * 
  * Typically sent by a ground control station / joystick or by
@@ -66,6 +61,11 @@ enum OFFBOARD_CONTROL_MODE
 	OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 7, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
 };
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 struct offboard_control_setpoint_s {
 	uint64_t timestamp;
 
diff --git a/src/modules/uORB/topics/omnidirectional_flow.h b/src/modules/uORB/topics/omnidirectional_flow.h
index 8f4be3b3f..a6ad8a131 100644
--- a/src/modules/uORB/topics/omnidirectional_flow.h
+++ b/src/modules/uORB/topics/omnidirectional_flow.h
@@ -46,6 +46,7 @@
 
 /**
  * @addtogroup topics
+ * @{
  */
 
 /**
diff --git a/src/modules/uORB/topics/optical_flow.h b/src/modules/uORB/topics/optical_flow.h
index c854f0079..98f0e3fa2 100644
--- a/src/modules/uORB/topics/optical_flow.h
+++ b/src/modules/uORB/topics/optical_flow.h
@@ -57,8 +57,8 @@ struct optical_flow_s {
 
 	uint64_t timestamp;		/**< in microseconds since system start          */
 
-	uint16_t flow_raw_x;		/**< flow in pixels in X direction, not rotation-compensated */
-	uint16_t flow_raw_y;		/**< flow in pixels in Y direction, not rotation-compensated */
+	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 */
 	float flow_comp_y_m;		/**< speed over ground in meters, rotation-compensated */
 	float ground_distance_m;	/**< Altitude / distance to ground in meters */
diff --git a/src/modules/uORB/topics/parameter_update.h b/src/modules/uORB/topics/parameter_update.h
index 300e895c7..68964deb0 100644
--- a/src/modules/uORB/topics/parameter_update.h
+++ b/src/modules/uORB/topics/parameter_update.h
@@ -42,11 +42,20 @@
 #include <stdint.h>
 #include "../uORB.h"
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 struct parameter_update_s {
 	/** time at which the latest parameter was updated */
 	uint64_t	timestamp;
 };
 
+/**
+ * @}
+ */
+
 ORB_DECLARE(parameter_update);
 
 #endif
\ No newline at end of file
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index 9dd54df91..086b2ef15 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -1,9 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Nils Wenzler <wenzlern@student.ethz.ch>
- *           @author Ivan Ovinnikov <oivan@student.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
+ *   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
@@ -46,16 +43,14 @@
 #include "../uORB.h"
 
 /**
- * @addtogroup topics
- * @{
- */
-
-/**
  * The number of RC channel inputs supported.
- * Current (Q1/2013) radios support up to 18 channels,
- * leaving at a sane value of 14.
+ * 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_MAX   14
+#define RC_CHANNELS_MAPPED_MAX   15
 
 /** 
  * This defines the mapping of the RC functions.
@@ -68,28 +63,32 @@ enum RC_CHANNELS_FUNCTION
   ROLL     = 1,
   PITCH    = 2,
   YAW      = 3,
-  OVERRIDE = 4,
-  AUTO_MODE = 5,
-  MANUAL_MODE = 6,
-  SAS_MODE = 7,
-  RTL = 8,
-  OFFBOARD_MODE = 9,
-  FLAPS   = 10,
-  AUX_1   = 11,
-  AUX_2   = 12,
-  AUX_3   = 13,
-  AUX_4   = 14,
-  AUX_5   = 15,
+  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. */
 };
 
+/**
+ * @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_MAX];
+  } chan[RC_CHANNELS_MAPPED_MAX];
   uint8_t chan_count;                 /**< number of valid channels */
 
   /*String array to store the names of the functions*/
diff --git a/src/modules/uORB/topics/safety.h b/src/modules/uORB/topics/safety.h
new file mode 100644
index 000000000..a5d21cd4a
--- /dev/null
+++ b/src/modules/uORB/topics/safety.h
@@ -0,0 +1,57 @@
+/****************************************************************************
+ *
+ *   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 safety.h
+ *
+ * Safety topic to pass safety state from px4io driver to commander
+ * This concerns only the safety button of the px4io but has nothing to do
+ * with arming/disarming.
+ */
+
+#ifndef TOPIC_SAFETY_H
+#define TOPIC_SAFETY_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+struct safety_s {
+
+	uint64_t	timestamp;
+	bool	safety_switch_available;	/**< Set to true if a safety switch is connected */
+	bool	safety_off;			/**< Set to true if safety is off */
+};
+
+ORB_DECLARE(safety);
+
+#endif
\ No newline at end of file
diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h
index 9a76b5182..ad164555e 100644
--- a/src/modules/uORB/topics/sensor_combined.h
+++ b/src/modules/uORB/topics/sensor_combined.h
@@ -46,11 +46,6 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
-/**
- * @addtogroup topics
- * @{
- */
-
 enum MAGNETOMETER_MODE {
 	MAGNETOMETER_MODE_NORMAL = 0,
 	MAGNETOMETER_MODE_POSITIVE_BIAS,
@@ -58,6 +53,11 @@ enum MAGNETOMETER_MODE {
 };
 
 /**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
  * Sensor readings in raw and SI-unit form.
  *
  * These values are read from the sensors. Raw values are in sensor-specific units,
diff --git a/src/modules/uORB/topics/servorail_status.h b/src/modules/uORB/topics/servorail_status.h
new file mode 100644
index 000000000..55668790b
--- /dev/null
+++ b/src/modules/uORB/topics/servorail_status.h
@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ *   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 servorail_status.h
+ *
+ * Definition of the servorail status uORB topic.
+ */
+
+#ifndef SERVORAIL_STATUS_H_
+#define SERVORAIL_STATUS_H_
+
+#include "../uORB.h"
+#include <stdint.h>
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Servorail voltages and status
+ */
+struct servorail_status_s {
+	uint64_t	timestamp;		/**< microseconds since system boot */
+	float   	voltage_v;		/**< Servo rail voltage in volts */
+	float   	rssi_v;			/**< RSSI pin voltage in volts */
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(servorail_status);
+
+#endif
diff --git a/src/modules/uORB/topics/subsystem_info.h b/src/modules/uORB/topics/subsystem_info.h
index c415e832e..cfe0bf69e 100644
--- a/src/modules/uORB/topics/subsystem_info.h
+++ b/src/modules/uORB/topics/subsystem_info.h
@@ -50,10 +50,6 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
-/**
- * @addtogroup topics
- */
-
 enum SUBSYSTEM_TYPE
 {
 	SUBSYSTEM_TYPE_GYRO = 1,
@@ -76,6 +72,10 @@ enum SUBSYSTEM_TYPE
 };
 
 /**
+ * @addtogroup topics
+ */
+
+/**
  * State of individual sub systems
  */
 struct subsystem_info_s {
diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h
new file mode 100644
index 000000000..828fb31cc
--- /dev/null
+++ b/src/modules/uORB/topics/telemetry_status.h
@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ *   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 telemetry_status.h
+ *
+ * Telemetry status topics - radio status outputs
+ */
+
+#ifndef TOPIC_TELEMETRY_STATUS_H
+#define TOPIC_TELEMETRY_STATUS_H
+
+#include <stdint.h>
+#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
+};
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+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  */
+};
+
+/**
+ * @}
+ */
+
+ORB_DECLARE(telemetry_status);
+
+#endif /* TOPIC_TELEMETRY_STATUS_H */
\ No newline at end of file
diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h
index c31c81d0c..4380a5ee7 100755
--- a/src/modules/uORB/topics/vehicle_attitude.h
+++ b/src/modules/uORB/topics/vehicle_attitude.h
@@ -48,6 +48,7 @@
 
 /**
  * @addtogroup topics
+ * @{
  */
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
index a7cda34a8..1a245132a 100644
--- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
@@ -64,8 +64,16 @@ struct vehicle_attitude_setpoint_s
 	float R_body[9];				/**< 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
+	float q_d[4];				/** Desired quaternion for quaternion control */
+	bool q_d_valid;					/**< Set to true if quaternion vector is valid */
+	float q_e[4];				/** Attitude error in quaternion */
+	bool q_e_valid;					/**< Set to true if quaternion error vector is valid */
+
 	float thrust;					/**< Thrust in Newton the power system should generate */
 
+	bool	roll_reset_integral;			/**< Reset roll 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
new file mode 100644
index 000000000..fbfab09f3
--- /dev/null
+++ b/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h
@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ *   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 vehicle_bodyframe_speed_setpoint.h
+ * Definition of the bodyframe speed setpoint uORB topic.
+ */
+
+#ifndef TOPIC_VEHICLE_BODYFRAME_SPEED_SETPOINT_H_
+#define TOPIC_VEHICLE_BODYFRAME_SPEED_SETPOINT_H_
+
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+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				  		*/
+	float vy;		/**< in m/s				  		*/
+//	float vz;		/**< in m/s				  		*/
+	float thrust_sp;
+	float yaw_sp;	/**< in radian		-PI +PI		*/
+}; /**< Speed in bodyframe to go to */
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_bodyframe_speed_setpoint);
+
+#endif
diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h
index fac571659..e6e4743c6 100644
--- a/src/modules/uORB/topics/vehicle_command.h
+++ b/src/modules/uORB/topics/vehicle_command.h
@@ -46,11 +46,6 @@
 #include "../uORB.h"
 
 /**
- * @addtogroup topics
- * @{
- */
-
-/**
  * Commands for commander app.
  *
  * Should contain all commands from MAVLink's VEHICLE_CMD ENUM,
@@ -91,7 +86,8 @@ enum VEHICLE_CMD
 	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_ENUM_END=401, /*  | */
+	VEHICLE_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX|  */
+	VEHICLE_CMD_ENUM_END=501, /*  | */
 };
 
 /**
@@ -110,6 +106,10 @@ enum VEHICLE_CMD_RESULT
 	VEHICLE_CMD_RESULT_ENUM_END=5, /*  | */
 };
 
+/**
+ * @addtogroup topics
+ * @{
+ */
 
 struct vehicle_command_s
 {
@@ -120,7 +120,7 @@ struct vehicle_command_s
 	float param5;			/**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum.   */
 	float param6;			/**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum.   */
 	float param7;			/**< Parameter 7, as defined by MAVLink VEHICLE_CMD enum.   */
-	uint16_t command;		/**< Command ID, as defined MAVLink by VEHICLE_CMD enum.   */
+	enum VEHICLE_CMD command;		/**< Command ID, as defined MAVLink by VEHICLE_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 source_system;		/**< System sending the command  */
diff --git a/src/modules/uORB/topics/vehicle_control_debug.h b/src/modules/uORB/topics/vehicle_control_debug.h
new file mode 100644
index 000000000..6184284a4
--- /dev/null
+++ b/src/modules/uORB/topics/vehicle_control_debug.h
@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   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 vehicle_control_debug.h
+ * For debugging purposes to log PID parts of controller
+ */
+
+#ifndef TOPIC_VEHICLE_CONTROL_DEBUG_H_
+#define TOPIC_VEHICLE_CONTROL_DEBUG_H_
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+struct vehicle_control_debug_s
+{
+	uint64_t timestamp; /**< in microseconds since system start */
+
+	float roll_p;		/**< roll P control part		*/
+	float roll_i;		/**< roll I control part 		*/
+	float roll_d;		/**< roll D control part 		*/
+
+	float roll_rate_p;	/**< roll rate P control part		*/
+	float roll_rate_i;	/**< roll rate I control part 		*/
+	float roll_rate_d;	/**< roll rate D control part 		*/
+
+	float pitch_p;		/**< pitch P control part		*/
+	float pitch_i;		/**< pitch I control part 		*/
+	float pitch_d;		/**< pitch D control part 		*/
+
+	float pitch_rate_p;	/**< pitch rate P control part		*/
+	float pitch_rate_i;	/**< pitch rate I control part 		*/
+	float pitch_rate_d;	/**< pitch rate D control part 		*/
+
+	float yaw_p;		/**< yaw P control part			*/
+	float yaw_i;		/**< yaw I control part 		*/
+	float yaw_d;		/**< yaw D control part 		*/
+
+	float yaw_rate_p;	/**< yaw rate P control part		*/
+	float yaw_rate_i;	/**< yaw rate I control part 		*/
+	float yaw_rate_d;	/**< yaw rate D control part 		*/
+
+}; /**< vehicle_control_debug */
+
+ /**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_control_debug);
+
+#endif
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
new file mode 100644
index 000000000..38fb74c9b
--- /dev/null
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -0,0 +1,93 @@
+/****************************************************************************
+ *
+ *   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>
+ *
+ * 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_control_mode.h
+ * Definition of the vehicle_control_mode uORB topic.
+ * 
+ * All control apps should depend their actions based on the flags set here.
+ */
+
+#ifndef VEHICLE_CONTROL_MODE
+#define VEHICLE_CONTROL_MODE
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics @{
+ */
+
+
+/**
+ * state machine / state of vehicle.
+ *
+ * Encodes the complete system state and is set by the commander app.
+ */
+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;
+
+	bool flag_external_manual_override_ok;	/**< external override non-fatal for system. Only true for fixed wing */
+
+	// XXX needs yet to be set by state machine helper
+	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_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
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_control_mode);
+
+#endif
diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h
index f036c7223..143734e37 100644
--- a/src/modules/uORB/topics/vehicle_global_position.h
+++ b/src/modules/uORB/topics/vehicle_global_position.h
@@ -62,18 +62,17 @@
 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							LOGME */
-	int32_t lon;			/**< Longitude in 1E7 degrees							LOGME */
-	float alt;				/**< Altitude in meters									LOGME */
-	float relative_alt;		/**< Altitude above home position in meters, 			LOGME */
-	float vx; 				/**< Ground X Speed (Latitude), m/s in ENU				LOGME */
-	float vy;				/**< Ground Y Speed (Longitude), m/s in ENU				LOGME */
-	float vz;				/**< Ground Z Speed (Altitude), m/s	in ENU				LOGME */
-	float hdg; 				/**< Compass heading in radians -PI..+PI.					  */
+	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.					   */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h b/src/modules/uORB/topics/vehicle_global_position_set_triplet.h
index 318abba89..8516b263f 100644
--- a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h
+++ b/src/modules/uORB/topics/vehicle_global_position_set_triplet.h
@@ -60,8 +60,8 @@
  */
 struct vehicle_global_position_set_triplet_s
 {
-	bool previous_valid;
-	bool next_valid;
+	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;
diff --git a/src/modules/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/vehicle_global_position_setpoint.h
index eec6a8229..a56434d3b 100644
--- a/src/modules/uORB/topics/vehicle_global_position_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_global_position_setpoint.h
@@ -45,6 +45,7 @@
 #include <stdint.h>
 #include <stdbool.h>
 #include "../uORB.h"
+#include "mission.h"
 
 /**
  * @addtogroup topics
@@ -65,7 +66,14 @@ struct vehicle_global_position_setpoint_s
 	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     */
-	bool is_loiter;			/**< true if loitering is enabled			*/
+	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 */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h
new file mode 100644
index 000000000..73961cdfe
--- /dev/null
+++ b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h
@@ -0,0 +1,64 @@
+/****************************************************************************
+ *
+ *   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 vehicle_global_velocity_setpoint.h
+ * Definition of the global velocity setpoint uORB topic.
+ */
+
+#ifndef TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_
+#define TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_
+
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+struct vehicle_global_velocity_setpoint_s
+{
+	float vx;		/**< in m/s NED			  		*/
+	float vy;		/**< in m/s NED			  		*/
+	float vz;		/**< in m/s NED			  		*/
+}; /**< Velocity setpoint in NED frame */
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_global_velocity_setpoint);
+
+#endif
diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h
index 0a7fb4e9d..1639a08c2 100644
--- a/src/modules/uORB/topics/vehicle_gps_position.h
+++ b/src/modules/uORB/topics/vehicle_gps_position.h
@@ -56,9 +56,9 @@
 struct vehicle_gps_position_s
 {
 	uint64_t timestamp_position;			/**< Timestamp for position information */
-	int32_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 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 */
diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h
index 76eddeacd..427153782 100644
--- a/src/modules/uORB/topics/vehicle_local_position.h
+++ b/src/modules/uORB/topics/vehicle_local_position.h
@@ -54,27 +54,29 @@
  */
 struct vehicle_local_position_s
 {
-	uint64_t timestamp;			/**< time of this estimate, in microseconds since system start */
-	bool valid;				/**< true if position satisfies validity criteria of estimator */
-
-	float x;				/**< X positin in meters in NED earth-fixed frame */
-	float y;				/**< X positin in meters in NED earth-fixed frame */
-	float z;				/**< Z positin in meters in NED earth-fixed frame (negative altitude) */
-	float absolute_alt;			/**< Altitude as defined by pressure / GPS, 			LOGME */
-	float vx; 				/**< Ground X Speed (Latitude), m/s in NED				LOGME */
-	float vy;				/**< Ground Y Speed (Longitude), m/s in NED				LOGME */
-	float vz;				/**< Ground Z Speed (Altitude), m/s	in NED				LOGME */
-	float hdg; 				/**< Compass heading in radians -PI..+PI.					  */
-
-	// TODO Add covariances here
-
+	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 */
+	bool v_xy_valid;		/**< true if vy and vy are valid */
+	bool v_z_valid;			/**< true if vz is valid */
+	/* Position in local NED frame */
+	float x;				/**< X position in meters in NED earth-fixed frame */
+	float y;				/**< X position in meters in NED earth-fixed frame */
+	float z;				/**< Z position in meters in NED earth-fixed frame (negative altitude) */
+	/* Velocity in NED frame */
+	float vx; 				/**< Ground X Speed (Latitude), m/s in NED */
+	float vy;				/**< Ground Y Speed (Longitude), m/s in NED */
+	float vz;				/**< Ground Z Speed (Altitude), m/s	in NED */
+	/* Heading */
+	float yaw;
 	/* Reference position in GPS / WGS84 frame */
-	uint64_t home_timestamp;/**< Time when home position was set						  */
-	int32_t home_lat;		/**< Latitude in 1E7 degrees							LOGME */
-	int32_t home_lon;		/**< Longitude in 1E7 degrees							LOGME */
-	float home_alt;			/**< Altitude in meters									LOGME */
-	float home_hdg; 		/**< Compass heading in radians -PI..+PI.					  */
-
+	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 */
+	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 */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index c7c1048f6..6ea48a680 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -58,22 +58,63 @@
  * @addtogroup topics @{
  */
 
-/* State Machine */
-typedef enum
-{
-	SYSTEM_STATE_PREFLIGHT = 0,
-	SYSTEM_STATE_STANDBY = 1,
-	SYSTEM_STATE_GROUND_READY = 2,
-	SYSTEM_STATE_MANUAL = 3,
-	SYSTEM_STATE_STABILIZED = 4,
-	SYSTEM_STATE_AUTO = 5,
-	SYSTEM_STATE_MISSION_ABORT = 6,
-	SYSTEM_STATE_EMCY_LANDING = 7,
-	SYSTEM_STATE_EMCY_CUTOFF = 8,
-	SYSTEM_STATE_GROUND_ERROR = 9,
-	SYSTEM_STATE_REBOOT= 10,
-
-} commander_state_machine_t;
+/* main state machine */
+typedef enum {
+	MAIN_STATE_MANUAL = 0,
+	MAIN_STATE_SEATBELT,
+	MAIN_STATE_EASY,
+	MAIN_STATE_AUTO,
+} 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;
+
+typedef enum {
+	ARMING_STATE_INIT = 0,
+	ARMING_STATE_STANDBY,
+	ARMING_STATE_ARMED,
+	ARMING_STATE_ARMED_ERROR,
+	ARMING_STATE_STANDBY_ERROR,
+	ARMING_STATE_REBOOT,
+	ARMING_STATE_IN_AIR_RESTORE
+} arming_state_t;
+
+typedef enum {
+	HIL_STATE_OFF = 0,
+	HIL_STATE_ON
+} hil_state_t;
+
+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;
 
 enum VEHICLE_MODE_FLAG {
 	VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
@@ -86,26 +127,6 @@ enum VEHICLE_MODE_FLAG {
 	VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1
 }; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */
 
-enum VEHICLE_FLIGHT_MODE {
-	VEHICLE_FLIGHT_MODE_MANUAL = 0,		/**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */
-	VEHICLE_FLIGHT_MODE_STAB,		/**< attitude or rate stabilization plus velocity or position stabilization */
-	VEHICLE_FLIGHT_MODE_HOLD,		/**< hold current position (hover or loiter around position when switched) */
-	VEHICLE_FLIGHT_MODE_AUTO		/**< attitude or rate stabilization plus absolute position control and waypoints */
-};
-
-enum VEHICLE_MANUAL_CONTROL_MODE {
-	VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0,	/**< no attitude control, direct stick input mixing (only fixed wing) */
-	VEHICLE_MANUAL_CONTROL_MODE_RATES,	/**< body rates control mode */
-	VEHICLE_MANUAL_CONTROL_MODE_SAS		/**< stability augmented system (SAS) mode */
-};
-
-enum VEHICLE_MANUAL_SAS_MODE {
-	VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0,	/**< roll, pitch and yaw absolute */
-	VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE,	/**< roll and pitch absolute, yaw rate */
-	VEHICLE_MANUAL_SAS_MODE_SIMPLE,				/**< simple mode (includes altitude hold) */
-	VEHICLE_MANUAL_SAS_MODE_ALTITUDE			/**< altitude hold */
-};
-
 /**
  * Should match 1:1 MAVLink's MAV_TYPE ENUM
  */
@@ -132,11 +153,15 @@ enum VEHICLE_TYPE {
 };
 
 enum VEHICLE_BATTERY_WARNING {
-    VEHICLE_BATTERY_WARNING_NONE = 0,    /**< no battery low voltage warning active */
-    VEHICLE_BATTERY_WARNING_WARNING,        /**< warning of low voltage 1. stage */
-    VEHICLE_BATTERY_WARNING_ALERT            /**< aleting of low voltage 2. stage */
+    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 */
 };
 
+/**
+ * @addtogroup topics
+ * @{
+ */
 
 /**
  * state machine / state of vehicle.
@@ -149,55 +174,55 @@ struct vehicle_status_s
 
 	uint16_t counter;   /**< incremented by the writing thread everytime new data is stored */
 	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
-	uint64_t failsave_lowlevel_start_time;		/**< time when the lowlevel failsafe flag was set */
-	//uint64_t failsave_highlevel_begin; TO BE COMPLETED
 
-	commander_state_machine_t state_machine;	/**< current flight state, main state machine */
-	enum VEHICLE_FLIGHT_MODE flight_mode;		/**< current flight mode, as defined by mode switch */
-	enum VEHICLE_MANUAL_CONTROL_MODE manual_control_mode;	/**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */
-	enum VEHICLE_MANUAL_SAS_MODE	manual_sas_mode;	/**< current stabilization mode */
+	main_state_t main_state;				/**< main state machine */
+	navigation_state_t navigation_state;	/**< navigation state machine */
+	arming_state_t arming_state;			/**< current arming state */
+	hil_state_t hil_state;					/**< current hil 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 */
 	int32_t component_id;				/**< subsystem / component id, inspired by MAVLink's component ID field */
 
-	/* system flags - these represent the state predicates */
-
-	bool flag_system_armed;				/**< true is motors / actuators are armed */
-	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_hil_enabled;				/**< true if hardware in the loop simulation is enabled */
-
-	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 speed (implies direction) is controlled */
-	bool flag_control_position_enabled;		/**< true if position is controlled */
-
-	bool flag_preflight_gyro_calibration;		/**< true if gyro calibration is requested */
-	bool flag_preflight_mag_calibration;		/**< true if mag calibration is requested */
-	bool flag_preflight_accel_calibration;
-	bool flag_preflight_airspeed_calibration;
+	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;
+	bool condition_system_returned_to_home;
+	bool condition_auto_mission_available;
+	bool condition_global_position_valid;		/**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */
+	bool condition_launch_position_valid;		/**< indicates a valid launch position */
+	bool condition_home_position_valid;		/**< indicates a valid home position (a valid home position is not always a valid launch) */
+	bool condition_local_position_valid;
+	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 rc_signal_found_once;
-	bool rc_signal_lost;				/**< true if RC reception is terminally lost */
-	bool rc_signal_cutting_off;			/**< true if RC reception is weak / cutting off */
-	uint64_t rc_signal_lost_interval;		/**< interval in microseconds since when no RC signal is available */
+	bool rc_signal_lost;				/**< true if RC reception lost */
+	bool rc_input_blocked;				/**< set if RC input should be ignored */
 
 	bool offboard_control_signal_found_once;
 	bool offboard_control_signal_lost;
 	bool offboard_control_signal_weak;
 	uint64_t offboard_control_signal_lost_interval;	/**< interval in microseconds without an offboard control message */
 
-	bool failsave_lowlevel;				/**< Set to true if low-level failsafe mode is enabled */
-	//bool failsave_highlevel;
-
 	/* see SYS_STATUS mavlink message for the following */
 	uint32_t onboard_control_sensors_present;
 	uint32_t onboard_control_sensors_enabled;
 	uint32_t onboard_control_sensors_health;
-	float load;
-	float voltage_battery;
-	float current_battery;
+	
+	float load;					/**< processor load from 0 to 1 */
+	float battery_voltage;
+	float battery_current;
 	float battery_remaining;
+
 	enum VEHICLE_BATTERY_WARNING battery_warning;    /**< current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum */
 	uint16_t drop_rate_comm;
 	uint16_t errors_comm;
@@ -205,15 +230,6 @@ struct vehicle_status_s
 	uint16_t errors_count2;
 	uint16_t errors_count3;
 	uint16_t errors_count4;
-
-	bool flag_global_position_valid;		/**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */
-	bool flag_local_position_valid;
-	bool flag_vector_flight_mode_ok;		/**< position estimation, battery voltage and other critical subsystems are good for autonomous flight */
-	bool flag_auto_flight_mode_ok;			/**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */
-	bool flag_external_manual_override_ok;		/**< external override non-fatal for system. Only true for fixed wing */
-	bool flag_valid_launch_position;		/**< indicates a valid launch position */
-	bool flag_valid_home_position;			/**< indicates a valid home position (a valid home position is not always a valid launch) */
-	bool flag_airspeed_valid;			/**< set to true by the commander app if there is a valid airspeed measurement available */
 };
 
 /**
diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp
index 7abbf42ae..149b8f6d2 100644
--- a/src/modules/uORB/uORB.cpp
+++ b/src/modules/uORB/uORB.cpp
@@ -249,8 +249,10 @@ ORBDevNode::close(struct file *filp)
 	} else {
 		SubscriberData *sd = filp_to_sd(filp);
 
-		if (sd != nullptr)
+		if (sd != nullptr) {
+			hrt_cancel(&sd->update_call);
 			delete sd;
+		}
 	}
 
 	return CDev::close(filp);
diff --git a/src/modules/unit_test/module.mk b/src/modules/unit_test/module.mk
new file mode 100644
index 000000000..f00b0f592
--- /dev/null
+++ b/src/modules/unit_test/module.mk
@@ -0,0 +1,39 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Makefile to build the unit test library.
+#
+
+SRCS		 = unit_test.cpp
+
diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp
new file mode 100644
index 000000000..64ee544a2
--- /dev/null
+++ b/src/modules/unit_test/unit_test.cpp
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@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 unit_test.cpp
+ * A unit test library.
+ *
+ */
+
+#include "unit_test.h"
+
+#include <systemlib/err.h>
+
+
+UnitTest::UnitTest()
+{
+}
+
+UnitTest::~UnitTest()
+{
+}
+
+void
+UnitTest::print_results(const char* result)
+{
+	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());
+    	}
+}
diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h
new file mode 100644
index 000000000..3020734f4
--- /dev/null
+++ b/src/modules/unit_test/unit_test.h
@@ -0,0 +1,93 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@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 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_
+
+#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_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);
+};
+
+
+
+#endif /* UNIT_TEST_H_ */
diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c
new file mode 100644
index 000000000..476015f3e
--- /dev/null
+++ b/src/systemcmds/config/config.c
@@ -0,0 +1,330 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.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 config.c
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ *
+ * config tool.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/stat.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_mag.h>
+#include <drivers/drv_device.h>
+
+#include "systemlib/systemlib.h"
+#include "systemlib/err.h"
+
+__EXPORT int config_main(int argc, char *argv[]);
+
+static void	do_gyro(int argc, char *argv[]);
+static void	do_accel(int argc, char *argv[]);
+static void	do_mag(int argc, char *argv[]);
+static void	do_device(int argc, char *argv[]);
+
+int
+config_main(int argc, char *argv[])
+{
+	if (argc >= 2) {
+		if (!strcmp(argv[1], "gyro")) {
+			do_gyro(argc - 2, argv + 2);
+		} else if (!strcmp(argv[1], "accel")) {
+			do_accel(argc - 2, argv + 2);
+		} else if (!strcmp(argv[1], "mag")) {
+			do_mag(argc - 2, argv + 2);
+		} else {
+			do_device(argc - 1, argv + 1);
+		}
+	}
+	
+	errx(1, "expected a command, try 'gyro', 'accel', 'mag'");
+}
+
+static void
+do_device(int argc, char *argv[])
+{
+	if (argc < 2) {
+		errx(1, "no device path provided and command provided.");
+	}
+
+	int	fd;
+	int	ret;
+
+	fd = open(argv[0], 0);
+
+	if (fd < 0) {
+		warn("%s", argv[0]);
+		errx(1, "FATAL: no device found");
+
+	} else {
+
+		if (argc == 2 && !strcmp(argv[1], "block")) {
+
+			/* disable the device publications */
+			ret = ioctl(fd, DEVIOCSPUBBLOCK, 1);
+
+			if (ret)
+				errx(ret,"uORB publications could not be blocked");
+
+		} else if (argc == 2 && !strcmp(argv[1], "unblock")) {
+
+			/* enable the device publications */
+			ret = ioctl(fd, DEVIOCSPUBBLOCK, 0);
+
+			if (ret)
+				errx(ret,"uORB publications could not be unblocked");
+
+		} else {
+			errx("no valid command: %s", argv[1]);
+		}
+	}
+
+	exit(0);
+}
+
+static void
+do_gyro(int argc, char *argv[])
+{
+	int	fd;
+	int	ret;
+
+	fd = open(GYRO_DEVICE_PATH, 0);
+
+	if (fd < 0) {
+		warn("%s", GYRO_DEVICE_PATH);
+		errx(1, "FATAL: no gyro found");
+
+	} else {
+
+		if (argc == 2 && !strcmp(argv[0], "sampling")) {
+
+			/* set the gyro internal sampling rate up to at least i Hz */
+			ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
+
+			if (ret)
+				errx(ret,"sampling rate could not be set");
+
+		} else if (argc == 2 && !strcmp(argv[0], "rate")) {
+
+			/* set the driver to poll at i Hz */
+			ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
+
+			if (ret)
+				errx(ret,"pollrate could not be set");
+
+		} else if (argc == 2 && !strcmp(argv[0], "range")) {
+
+			/* set the range to i dps */
+			ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0));
+
+			if (ret)
+				errx(ret,"range could not be set");
+
+		} else if (argc == 1 && !strcmp(argv[0], "check")) {
+			ret = ioctl(fd, GYROIOCSELFTEST, 0);
+
+			if (ret) {
+				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);
+			} else {
+				warnx("gyro calibration and self test OK");
+			}
+
+		} else {
+			errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2000' to set measurement range to 2000 dps\n\t");
+		}
+
+		int srate = ioctl(fd, GYROIOCGSAMPLERATE, 0);
+		int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
+		int range = ioctl(fd, GYROIOCGRANGE, 0);
+
+		warnx("gyro: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", srate, prate, range);
+
+		close(fd);
+	}
+
+	exit(0);
+}
+
+static void
+do_mag(int argc, char *argv[])
+{
+	int fd;
+	int ret;
+
+	fd = open(MAG_DEVICE_PATH, 0);
+
+	if (fd < 0) {
+		warn("%s", MAG_DEVICE_PATH);
+		errx(1, "FATAL: no magnetometer found");
+
+	} else {
+
+		if (argc == 2 && !strcmp(argv[0], "sampling")) {
+
+			/* set the mag internal sampling rate up to at least i Hz */
+			ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
+
+			if (ret)
+				errx(ret,"sampling rate could not be set");
+
+		} else if (argc == 2 && !strcmp(argv[0], "rate")) {
+
+			/* set the driver to poll at i Hz */
+			ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
+
+			if (ret)
+				errx(ret,"pollrate could not be set");
+
+		} else if (argc == 2 && !strcmp(argv[0], "range")) {
+
+			/* set the range to i G */
+			ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[1], NULL, 0));
+
+			if (ret)
+				errx(ret,"range could not be set");
+
+		} else if(argc == 1 && !strcmp(argv[0], "check")) {
+			ret = ioctl(fd, MAGIOCSELFTEST, 0);
+
+			if (ret) {
+				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);
+			} else {
+				warnx("mag calibration and self test OK");
+			}
+
+		} else {
+			errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\n\t");
+		}
+
+		int srate = ioctl(fd, MAGIOCGSAMPLERATE, 0);
+		int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
+		int range = ioctl(fd, MAGIOCGRANGE, 0);
+
+		warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", srate, prate, range);
+
+		close(fd);
+	}
+
+	exit(0);
+}
+
+static void
+do_accel(int argc, char *argv[])
+{
+	int	fd;
+	int	ret;
+
+	fd = open(ACCEL_DEVICE_PATH, 0);
+
+	if (fd < 0) {
+		warn("%s", ACCEL_DEVICE_PATH);
+		errx(1, "FATAL: no accelerometer found");
+
+	} else {
+
+		if (argc == 2 && !strcmp(argv[0], "sampling")) {
+
+			/* set the accel internal sampling rate up to at least i Hz */
+			ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
+
+			if (ret)
+				errx(ret,"sampling rate could not be set");
+
+		} else if (argc == 2 && !strcmp(argv[0], "rate")) {
+
+			/* set the driver to poll at i Hz */
+			ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
+
+			if (ret)
+				errx(ret,"pollrate could not be set");
+
+		} else if (argc == 2 && !strcmp(argv[0], "range")) {
+
+			/* set the range to i G */
+			ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 0));
+
+			if (ret)
+				errx(ret,"range could not be set");
+
+		} else if(argc == 1 && !strcmp(argv[0], "check")) {
+			ret = ioctl(fd, ACCELIOCSELFTEST, 0);
+
+			if (ret) {
+				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);
+			} else {
+				warnx("accel calibration and self test OK");
+			}
+
+		} else {
+			errx(1,"no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 4' to set measurement range to 4 G\n\t");
+		}
+
+		int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0);
+		int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
+		int range = ioctl(fd, ACCELIOCGRANGE, 0);
+
+		warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", srate, prate, range);
+
+		close(fd);
+	}
+
+	exit(0);
+}
diff --git a/src/systemcmds/config/module.mk b/src/systemcmds/config/module.mk
new file mode 100644
index 000000000..0a75810b0
--- /dev/null
+++ b/src/systemcmds/config/module.mk
@@ -0,0 +1,44 @@
+############################################################################
+#
+#   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 the config tool.
+#
+
+MODULE_COMMAND	 = config
+SRCS		 = config.c
+
+MODULE_STACKSIZE = 4096
+
+MAXOPTIMIZATION	 = -Os
+
diff --git a/src/systemcmds/eeprom/eeprom.c b/src/systemcmds/eeprom/eeprom.c
deleted file mode 100644
index 49da51358..000000000
--- a/src/systemcmds/eeprom/eeprom.c
+++ /dev/null
@@ -1,265 +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 eeprom.c
- *
- * EEPROM service and utility app.
- */
-
-#include <nuttx/config.h>
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <stdbool.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <sys/mount.h>
-#include <sys/ioctl.h>
-#include <sys/stat.h>
-
-#include <nuttx/i2c.h>
-#include <nuttx/mtd.h>
-#include <nuttx/fs/nxffs.h>
-#include <nuttx/fs/ioctl.h>
-
-#include <arch/board/board.h>
-
-#include "systemlib/systemlib.h"
-#include "systemlib/param/param.h"
-#include "systemlib/err.h"
-
-#ifndef PX4_I2C_BUS_ONBOARD
-#  error PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM
-#endif
-
-__EXPORT int eeprom_main(int argc, char *argv[]);
-
-static void	eeprom_attach(void);
-static void	eeprom_start(void);
-static void	eeprom_erase(void);
-static void	eeprom_ioctl(unsigned operation);
-static void	eeprom_save(const char *name);
-static void	eeprom_load(const char *name);
-static void	eeprom_test(void);
-
-static bool attached = false;
-static bool started = false;
-static struct mtd_dev_s *eeprom_mtd;
-
-int eeprom_main(int argc, char *argv[])
-{
-	if (argc >= 2) {
-		if (!strcmp(argv[1], "start"))
-			eeprom_start();
-
-		if (!strcmp(argv[1], "save_param"))
-			eeprom_save(argv[2]);
-
-		if (!strcmp(argv[1], "load_param"))
-			eeprom_load(argv[2]);
-
-		if (!strcmp(argv[1], "erase"))
-			eeprom_erase();
-
-		if (!strcmp(argv[1], "test"))
-			eeprom_test();
-
-		if (0) {	/* these actually require a file on the filesystem... */
-
-			if (!strcmp(argv[1], "reformat"))
-				eeprom_ioctl(FIOC_REFORMAT);
-
-			if (!strcmp(argv[1], "repack"))
-				eeprom_ioctl(FIOC_OPTIMIZE);
-		}
-	}
-
-	errx(1, "expected a command, try 'start'\n\t'save_param /eeprom/parameters'\n\t'load_param /eeprom/parameters'\n\t'erase'\n");
-}
-
-
-static void
-eeprom_attach(void)
-{
-	/* find the right I2C */
-	struct i2c_dev_s *i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD);
-	/* this resets the I2C bus, set correct bus speed again */
-	I2C_SETFREQUENCY(i2c, 400000);
-
-	if (i2c == NULL)
-		errx(1, "failed to locate I2C bus");
-
-	/* start the MTD driver, attempt 5 times */
-	for (int i = 0; i < 5; i++) {
-		eeprom_mtd = at24c_initialize(i2c);
-		if (eeprom_mtd) {
-			/* abort on first valid result */
-			if (i > 0) {
-				warnx("warning: EEPROM needed %d attempts to attach", i+1);
-			}
-			break;
-		}
-	}
-
-	/* if last attempt is still unsuccessful, abort */
-	if (eeprom_mtd == NULL)
-		errx(1, "failed to initialize EEPROM driver");
-
-	attached = true;
-}
-
-static void
-eeprom_start(void)
-{
-	int ret;
-
-	if (started)
-		errx(1, "EEPROM already mounted");
-
-	if (!attached)
-		eeprom_attach();
-
-	/* start NXFFS */
-	ret = nxffs_initialize(eeprom_mtd);
-
-	if (ret < 0)
-		errx(1, "failed to initialize NXFFS - erase EEPROM to reformat");
-
-	/* mount the EEPROM */
-	ret = mount(NULL, "/eeprom", "nxffs", 0, NULL);
-
-	if (ret < 0)
-		errx(1, "failed to mount /eeprom - erase EEPROM to reformat");
-
-	started = true;
-	warnx("mounted EEPROM at /eeprom");
-	exit(0);
-}
-
-extern int at24c_nuke(void);
-
-static void
-eeprom_erase(void)
-{
-	if (!attached)
-		eeprom_attach();
-
-	if (at24c_nuke())
-		errx(1, "erase failed");
-
-	errx(0, "erase done, reboot now");
-}
-
-static void
-eeprom_ioctl(unsigned operation)
-{
-	int fd;
-
-	fd = open("/eeprom/.", 0);
-
-	if (fd < 0)
-		err(1, "open /eeprom");
-
-	if (ioctl(fd, operation, 0) < 0)
-		err(1, "ioctl");
-
-	exit(0);
-}
-
-static void
-eeprom_save(const char *name)
-{
-	if (!started)
-		errx(1, "must be started first");
-
-	if (!name)
-		err(1, "missing argument for device name, try '/eeprom/parameters'");
-
-	warnx("WARNING: 'eeprom save_param' deprecated - use 'param save' instead");
-
-	/* delete the file in case it exists */
-	unlink(name);
-
-	/* create the file */
-	int fd = open(name, O_WRONLY | O_CREAT | O_EXCL);
-
-	if (fd < 0)
-		err(1, "opening '%s' failed", name);
-
-	int result = param_export(fd, false);
-	close(fd);
-
-	if (result < 0) {
-		unlink(name);
-		errx(1, "error exporting to '%s'", name);
-	}
-
-	exit(0);
-}
-
-static void
-eeprom_load(const char *name)
-{
-	if (!started)
-		errx(1, "must be started first");
-
-	if (!name)
-		err(1, "missing argument for device name, try '/eeprom/parameters'");
-
-	warnx("WARNING: 'eeprom load_param' deprecated - use 'param load' instead");
-
-	int fd = open(name, O_RDONLY);
-
-	if (fd < 0)
-		err(1, "open '%s'", name);
-
-	int result = param_load(fd);
-	close(fd);
-
-	if (result < 0)
-		errx(1, "error importing from '%s'", name);
-
-	exit(0);
-}
-
-extern void at24c_test(void);
-
-static void
-eeprom_test(void)
-{
-	at24c_test();
-	exit(0);
-}
diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c
new file mode 100644
index 000000000..ad1996694
--- /dev/null
+++ b/src/systemcmds/esc_calib/esc_calib.c
@@ -0,0 +1,338 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file esc_calib.c
+ *
+ * Tool for ESC calibration
+ */
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <sys/mount.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+
+#include <nuttx/i2c.h>
+#include <nuttx/mtd.h>
+#include <nuttx/fs/nxffs.h>
+#include <nuttx/fs/ioctl.h>
+
+#include <arch/board/board.h>
+
+#include "systemlib/systemlib.h"
+#include "systemlib/err.h"
+#include "drivers/drv_pwm_output.h"
+
+#include <uORB/topics/actuator_controls.h>
+
+static void	usage(const char *reason);
+__EXPORT int	esc_calib_main(int argc, char *argv[]);
+
+static void
+usage(const char *reason)
+{
+	if (reason != NULL)
+		warnx("%s", reason);
+
+	errx(1,
+		"usage:\n"
+		"esc_calib\n"
+		"    [-d <device>        PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
+		"    [-l <pwm>           Low PWM value in us (default: %dus)\n"
+		"    [-h <pwm>           High PWM value in us (default: %dus)\n"
+		"    [-c <channels>]     Supply channels (e.g. 1234)\n"
+		"    [-m <chanmask> ]    Directly supply channel mask (e.g. 0xF)\n"
+		"    [-a]                Use all outputs\n"
+		, PWM_DEFAULT_MIN, PWM_DEFAULT_MAX);
+}
+
+int
+esc_calib_main(int argc, char *argv[])
+{
+	char *dev = PWM_OUTPUT_DEVICE_PATH;
+	char *ep;
+	int ch;
+	int ret;
+	char c;
+
+	unsigned max_channels = 0;
+
+	uint32_t set_mask = 0;
+	unsigned long channels;
+	unsigned single_ch = 0;
+
+	uint16_t pwm_high = PWM_DEFAULT_MAX;
+	uint16_t pwm_low = PWM_DEFAULT_MIN;
+
+	struct pollfd fds;
+	fds.fd = 0; /* stdin */
+	fds.events = POLLIN;
+
+	if (argc < 2) {
+		usage("no channels provided");
+	}
+
+	int arg_consumed = 0;
+
+	while ((ch = getopt(argc, argv, "d:c:m:al:h:")) != EOF) {
+		switch (ch) {
+
+		case 'd':
+			dev = optarg;
+			arg_consumed += 2;
+			break;
+
+		case 'c':
+			/* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */
+			channels = strtoul(optarg, &ep, 0);
+
+			while ((single_ch = channels % 10)) {
+
+				set_mask |= 1<<(single_ch-1);
+				channels /= 10;
+			}
+			break;
+
+		case 'm':
+			/* Read in mask directly */
+			set_mask = strtoul(optarg, &ep, 0);
+			if (*ep != '\0')
+				usage("bad set_mask value");
+			break;
+
+		case 'a':
+			/* Choose all channels */
+			for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) {
+				set_mask |= 1<<i;
+			}
+			break;
+
+		case 'l':
+			/* Read in custom low value */
+			pwm_low = strtoul(optarg, &ep, 0);
+			if (*ep != '\0' || pwm_low < PWM_LOWEST_MIN || pwm_low > PWM_HIGHEST_MIN)
+				usage("low PWM invalid");
+			break;
+		case 'h':
+			/* Read in custom high value */
+			pwm_high = strtoul(optarg, &ep, 0);
+			if (*ep != '\0' || pwm_high > PWM_HIGHEST_MAX || pwm_high < PWM_LOWEST_MAX)
+				usage("high PWM invalid");
+			break;
+		default:
+			usage(NULL);
+		}
+	}
+
+	if (set_mask == 0)
+		usage("no channels chosen");
+
+	/* make sure no other source is publishing control values now */
+	struct actuator_controls_s actuators;
+	int act_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
+
+	/* clear changed flag */
+	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, act_sub, &actuators);
+
+	/* wait 50 ms */
+	usleep(50000);
+
+	/* now expect nothing changed on that topic */
+	bool orb_updated;
+	orb_check(act_sub, &orb_updated);
+
+	if (orb_updated) {
+		errx(1, "ABORTING! Attitude control still active. Please ensure to shut down all controllers:\n"
+			"\tmultirotor_att_control stop\n"
+			"\tfw_att_control stop\n");
+	}
+
+	printf("\nATTENTION, please remove or fix propellers before starting calibration!\n"
+	       "\n"
+	       "Make sure\n"
+	       "\t - that the ESCs are not powered\n"
+	       "\t - that safety is off (two short blinks)\n"
+	       "\t - that the controllers are stopped\n"
+	       "\n"
+	       "Do you want to start calibration now: y or n?\n");
+
+	/* wait for user input */
+	while (1) {
+
+		ret = poll(&fds, 1, 0);
+
+		if (ret > 0) {
+
+			read(0, &c, 1);
+
+			if (c == 'y' || c == 'Y') {
+
+				break;
+
+			} else if (c == 0x03 || c == 0x63 || c == 'q') {
+				printf("ESC calibration exited\n");
+				exit(0);
+
+			} else if (c == 'n' || c == 'N') {
+				printf("ESC calibration aborted\n");
+				exit(0);
+
+			} else {
+				printf("Unknown input, ESC calibration aborted\n");
+				exit(0);
+			}
+		}
+
+		/* rate limit to ~ 20 Hz */
+		usleep(50000);
+	}
+
+	/* open for ioctl only */
+	int fd = open(dev, 0);
+
+	if (fd < 0)
+		err(1, "can't open %s", dev);
+
+	/* get number of channels available on the device */
+	ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&max_channels);
+	if (ret != OK)
+		err(1, "PWM_SERVO_GET_COUNT");
+
+	/* tell IO/FMU that its ok to disable its safety with the switch */
+	ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
+	if (ret != OK)
+		err(1, "PWM_SERVO_SET_ARM_OK");
+	/* tell IO/FMU that the system is armed (it will output values if safety is off) */
+	ret = ioctl(fd, PWM_SERVO_ARM, 0);
+	if (ret != OK)
+		err(1, "PWM_SERVO_ARM");
+
+	warnx("Outputs armed");
+
+
+	/* wait for user confirmation */
+	printf("\nHigh PWM set: %d\n"
+	       "\n"
+	       "Connect battery now and hit ENTER after the ESCs confirm the first calibration step\n"
+	       "\n", pwm_high);
+	fflush(stdout);
+
+	while (1) {
+		/* set max PWM */
+		for (unsigned i = 0; i < max_channels; i++) {
+
+			if (set_mask & 1<<i) {
+				ret = ioctl(fd, PWM_SERVO_SET(i), pwm_high);
+
+				if (ret != OK)
+					err(1, "PWM_SERVO_SET(%d), value: %d", i, pwm_high);
+			}
+		}
+
+		ret = poll(&fds, 1, 0);
+
+		if (ret > 0) {
+
+			read(0, &c, 1);
+
+			if (c == 13) {
+
+				break;
+
+			} else if (c == 0x03 || c == 0x63 || c == 'q') {
+				warnx("ESC calibration exited");
+				exit(0);
+			}
+		}
+
+		/* rate limit to ~ 20 Hz */
+		usleep(50000);
+	}
+
+	printf("Low PWM set: %d\n"
+	       "\n"
+	       "Hit ENTER when finished\n"
+	       "\n", pwm_low);
+
+	while (1) {
+
+		/* set disarmed PWM */
+		for (unsigned i = 0; i < max_channels; i++) {
+			if (set_mask & 1<<i) {
+				ret = ioctl(fd, PWM_SERVO_SET(i), pwm_low);
+
+				if (ret != OK)
+					err(1, "PWM_SERVO_SET(%d), value: %d", i, pwm_low);
+			}
+		}
+
+		ret = poll(&fds, 1, 0);
+
+		if (ret > 0) {
+
+			read(0, &c, 1);
+
+			if (c == 13) {
+
+				break;
+
+			} else if (c == 0x03 || c == 0x63 || c == 'q') {
+				printf("ESC calibration exited\n");
+				exit(0);
+			}
+		}
+
+		/* rate limit to ~ 20 Hz */
+		usleep(50000);
+	}
+
+	/* disarm */
+	ret = ioctl(fd, PWM_SERVO_DISARM, 0);
+	if (ret != OK)
+		err(1, "PWM_SERVO_DISARM");
+
+	warnx("Outputs disarmed");
+
+	printf("ESC calibration finished\n");
+
+	exit(0);
+}
diff --git a/src/systemcmds/esc_calib/module.mk b/src/systemcmds/esc_calib/module.mk
new file mode 100644
index 000000000..990c56768
--- /dev/null
+++ b/src/systemcmds/esc_calib/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   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 the esc_calib tool.
+#
+
+MODULE_COMMAND	 = esc_calib
+SRCS		 = esc_calib.c
+
+MODULE_STACKSIZE = 4096
diff --git a/src/systemcmds/hw_ver/hw_ver.c b/src/systemcmds/hw_ver/hw_ver.c
new file mode 100644
index 000000000..4b84523cc
--- /dev/null
+++ b/src/systemcmds/hw_ver/hw_ver.c
@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file hw_ver.c
+ *
+ * Show and test hardware version.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <string.h>
+#include <errno.h>
+#include <version/version.h>
+
+__EXPORT int hw_ver_main(int argc, char *argv[]);
+
+int
+hw_ver_main(int argc, char *argv[])
+{
+	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);
+
+			} else {
+				errx(1, "not enough arguments, try 'compare PX4FMU_1'");
+			}
+		}
+	}
+
+	errx(1, "expected a command, try 'show' or 'compare'");
+}
diff --git a/src/systemcmds/hw_ver/module.mk b/src/systemcmds/hw_ver/module.mk
new file mode 100644
index 000000000..3cc08b6a1
--- /dev/null
+++ b/src/systemcmds/hw_ver/module.mk
@@ -0,0 +1,43 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Show and test hardware version
+#
+
+MODULE_COMMAND	 = hw_ver
+SRCS		 = hw_ver.c
+
+MODULE_STACKSIZE = 1024
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/systemcmds/i2c/i2c.c b/src/systemcmds/i2c/i2c.c
index 4da238039..34405c496 100644
--- a/src/systemcmds/i2c/i2c.c
+++ b/src/systemcmds/i2c/i2c.c
@@ -52,7 +52,7 @@
 
 #include <nuttx/i2c.h>
 
-#include <arch/board/board.h>
+#include <board_config.h>
 
 #include "systemlib/systemlib.h"
 #include "systemlib/err.h"
diff --git a/src/systemcmds/mixer/mixer.c b/src/systemcmds/mixer/mixer.cpp
index 55c4f0836..6da39d371 100644
--- a/src/systemcmds/mixer/mixer.c
+++ b/src/systemcmds/mixer/mixer.cpp
@@ -47,10 +47,15 @@
 #include <nuttx/compiler.h>
 
 #include <systemlib/err.h>
-#include <drivers/drv_mixer.h>
+#include <systemlib/mixer/mixer.h>
 #include <uORB/topics/actuator_controls.h>
 
-__EXPORT int mixer_main(int argc, char *argv[]);
+/**
+ * Mixer utility for loading mixer files to devices
+ *
+ * @ingroup apps
+ */
+extern "C" __EXPORT int mixer_main(int argc, char *argv[]);
 
 static void	usage(const char *reason) noreturn_function;
 static void	load(const char *devname, const char *fname) noreturn_function;
@@ -87,9 +92,7 @@ static void
 load(const char *devname, const char *fname)
 {
 	int		dev;
-	FILE		*fp;
-	char		line[80];
-	char		buf[512];
+	char		buf[2048];
 
 	/* open the device */
 	if ((dev = open(devname, 0)) < 0)
@@ -99,49 +102,7 @@ load(const char *devname, const char *fname)
 	if (ioctl(dev, MIXERIOCRESET, 0))
 		err(1, "can't reset mixers on %s", devname);
 
-	/* open the mixer definition file */
-	fp = fopen(fname, "r");
-	if (fp == NULL)
-		err(1, "can't open %s", fname);
-
-	/* read valid lines from the file into a buffer */
-	buf[0] = '\0';
-	for (;;) {
-
-		/* get a line, bail on error/EOF */
-		line[0] = '\0';
-		if (fgets(line, sizeof(line), fp) == NULL)
-			break;
-
-		/* if the line doesn't look like a mixer definition line, skip it */
-		if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':'))
-			continue;
-
-		/* compact whitespace in the buffer */
-		char *t, *f;
-		for (f = buf; *f != '\0'; f++) {
-			/* scan for space characters */
-			if (*f == ' ') {
-				/* look for additional spaces */
-				t = f + 1;
-				while (*t == ' ')
-					t++;
-				if (*t == '\0') {
-					/* strip trailing whitespace */
-					*f = '\0';
-				} else if (t > (f + 1)) {
-					memmove(f + 1, t, strlen(t) + 1);
-				}
-			}
-		}
-
-		/* if the line is too long to fit in the buffer, bail */
-		if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf))
-			break;
-
-		/* add the line to the buffer */
-		strcat(buf, line);
-	}
+	load_mixer_file(fname, &buf[0], sizeof(buf));
 
 	/* XXX pass the buffer to the device */
 	int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf);
diff --git a/src/systemcmds/mixer/module.mk b/src/systemcmds/mixer/module.mk
index d5a3f9f77..cdbff75f0 100644
--- a/src/systemcmds/mixer/module.mk
+++ b/src/systemcmds/mixer/module.mk
@@ -36,7 +36,7 @@
 #
 
 MODULE_COMMAND	 = mixer
-SRCS		 = mixer.c
+SRCS		 = mixer.cpp
 
 MODULE_STACKSIZE = 4096
 
diff --git a/src/systemcmds/eeprom/24xxxx_mtd.c b/src/systemcmds/mtd/24xxxx_mtd.c
index e34be44e3..e34be44e3 100644
--- a/src/systemcmds/eeprom/24xxxx_mtd.c
+++ b/src/systemcmds/mtd/24xxxx_mtd.c
diff --git a/src/systemcmds/mtd/module.mk b/src/systemcmds/mtd/module.mk
new file mode 100644
index 000000000..b3fceceb5
--- /dev/null
+++ b/src/systemcmds/mtd/module.mk
@@ -0,0 +1,6 @@
+#
+# RAMTRON file system driver
+#
+
+MODULE_COMMAND	 = mtd
+SRCS		 = mtd.c 24xxxx_mtd.c
diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c
new file mode 100644
index 000000000..0a88d40f3
--- /dev/null
+++ b/src/systemcmds/mtd/mtd.c
@@ -0,0 +1,489 @@
+/****************************************************************************
+ *
+ *   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 mtd.c
+ *
+ * mtd service and utility app.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/mount.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+
+#include <nuttx/spi.h>
+#include <nuttx/mtd.h>
+#include <nuttx/fs/nxffs.h>
+#include <nuttx/fs/ioctl.h>
+
+#include <arch/board/board.h>
+
+#include "systemlib/systemlib.h"
+#include "systemlib/param/param.h"
+#include "systemlib/err.h"
+
+#include <board_config.h>
+
+__EXPORT int mtd_main(int argc, char *argv[]);
+
+#ifndef CONFIG_MTD
+
+/* create a fake command with decent warnx to not confuse users */
+int mtd_main(int argc, char *argv[])
+{
+	errx(1, "MTD not enabled, skipping.");
+}
+
+#else
+
+#ifdef CONFIG_MTD_RAMTRON
+static void	ramtron_attach(void);
+#else
+
+#ifndef PX4_I2C_BUS_ONBOARD
+#  error PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM
+#endif
+
+static void	at24xxx_attach(void);
+#endif
+static void	mtd_start(char *partition_names[], unsigned n_partitions);
+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 int	mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks, 
+	unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions);
+
+static bool attached = false;
+static bool started = false;
+static struct mtd_dev_s *mtd_dev;
+static unsigned n_partitions_current = 0;
+
+/* note, these will be equally sized */
+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]);
+
+int mtd_main(int argc, char *argv[])
+{
+	if (argc >= 2) {
+		if (!strcmp(argv[1], "start")) {
+
+			/* start mapping according to user request */
+			if (argc >= 3) {
+				mtd_start(argv + 2, argc - 2);
+			} else {
+				mtd_start(partition_names_default, n_partitions_default);
+			}
+		}
+
+		if (!strcmp(argv[1], "test"))
+			mtd_test();
+
+		if (!strcmp(argv[1], "readtest")) {
+			if (argc >= 3) {
+				mtd_readtest(argv + 2, argc - 2);
+			} else {
+				mtd_readtest(partition_names_default, n_partitions_default);
+			}
+                }
+
+		if (!strcmp(argv[1], "rwtest")) {
+			if (argc >= 3) {
+				mtd_rwtest(argv + 2, argc - 2);
+			} else {
+				mtd_rwtest(partition_names_default, n_partitions_default);
+			}
+                }
+
+		if (!strcmp(argv[1], "status"))
+			mtd_status();
+
+		if (!strcmp(argv[1], "erase")) {
+			if (argc >= 3) {
+				mtd_erase(argv + 2, argc - 2);
+			} else {
+				mtd_erase(partition_names_default, n_partitions_default);
+			}
+                }
+	}
+
+	errx(1, "expected a command, try 'start', 'erase', 'status', 'readtest', 'rwtest' or 'test'");
+}
+
+struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev);
+struct mtd_dev_s *mtd_partition(FAR struct mtd_dev_s *mtd,
+                                    off_t firstblock, off_t nblocks);
+
+#ifdef CONFIG_MTD_RAMTRON
+static void
+ramtron_attach(void)
+{
+	/* find the right spi */
+	struct spi_dev_s *spi = up_spiinitialize(2);
+	/* this resets the spi bus, set correct bus speed again */
+	SPI_SETFREQUENCY(spi, 10 * 1000 * 1000);
+	SPI_SETBITS(spi, 8);
+	SPI_SETMODE(spi, SPIDEV_MODE3);
+	SPI_SELECT(spi, SPIDEV_FLASH, false);
+
+	if (spi == NULL)
+		errx(1, "failed to locate spi bus");
+
+	/* start the RAMTRON driver, attempt 5 times */
+	for (int i = 0; i < 5; i++) {
+		mtd_dev = ramtron_initialize(spi);
+
+		if (mtd_dev) {
+			/* abort on first valid result */
+			if (i > 0) {
+				warnx("warning: mtd needed %d attempts to attach", i + 1);
+			}
+
+			break;
+		}
+	}
+
+	/* if last attempt is still unsuccessful, abort */
+	if (mtd_dev == NULL)
+		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");
+
+	attached = true;
+}
+#else
+
+static void
+at24xxx_attach(void)
+{
+	/* find the right I2C */
+	struct i2c_dev_s *i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD);
+	/* this resets the I2C bus, set correct bus speed again */
+	I2C_SETFREQUENCY(i2c, 400000);
+
+	if (i2c == NULL)
+		errx(1, "failed to locate I2C bus");
+
+	/* start the MTD driver, attempt 5 times */
+	for (int i = 0; i < 5; i++) {
+		mtd_dev = at24c_initialize(i2c);
+		if (mtd_dev) {
+			/* abort on first valid result */
+			if (i > 0) {
+				warnx("warning: EEPROM needed %d attempts to attach", i+1);
+			}
+			break;
+		}
+	}
+
+	/* if last attempt is still unsuccessful, abort */
+	if (mtd_dev == NULL)
+		errx(1, "failed to initialize EEPROM driver");
+
+	attached = true;
+}
+#endif
+
+static void
+mtd_start(char *partition_names[], unsigned n_partitions)
+{
+	int ret;
+
+	if (started)
+		errx(1, "mtd already mounted");
+
+	if (!attached) {
+		#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+		at24xxx_attach();
+		#else
+		ramtron_attach();
+		#endif
+	}
+
+	if (!mtd_dev) {
+		warnx("ERROR: Failed to create RAMTRON FRAM MTD instance");
+		exit(1);
+	}
+
+	unsigned long blocksize, erasesize, neraseblocks;
+	unsigned blkpererase, nblocks, partsize;
+
+	ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions);
+	if (ret)
+		exit(3);
+
+	/* Now create MTD FLASH partitions */
+
+	FAR struct mtd_dev_s *part[n_partitions];
+	char blockname[32];
+
+	unsigned offset;
+	unsigned i;
+
+	for (offset = 0, i = 0; i < n_partitions; offset += nblocks, i++) {
+
+		/* Create the partition */
+
+		part[i] = mtd_partition(mtd_dev, offset, nblocks);
+
+		if (!part[i]) {
+			warnx("ERROR: mtd_partition failed. offset=%lu nblocks=%lu",
+			      (unsigned long)offset, (unsigned long)nblocks);
+			exit(4);
+		}
+
+		/* Initialize to provide an FTL block driver on the MTD FLASH interface */
+
+		snprintf(blockname, sizeof(blockname), "/dev/mtdblock%d", i);
+
+		ret = ftl_initialize(i, part[i]);
+
+		if (ret < 0) {
+			warnx("ERROR: ftl_initialize %s failed: %d", blockname, ret);
+			exit(5);
+		}
+
+		/* Now create a character device on the block device */
+
+		ret = bchdev_register(blockname, partition_names[i], false);
+
+		if (ret < 0) {
+			warnx("ERROR: bchdev_register %s failed: %d", partition_names[i], ret);
+			exit(6);
+		}
+	}
+
+	n_partitions_current = n_partitions;
+
+	started = true;
+	exit(0);
+}
+
+int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks, 
+	unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions)
+{
+		/* Get the geometry of the FLASH device */
+
+	FAR struct mtd_geometry_s geo;
+
+	int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_GEOMETRY, (unsigned long)((uintptr_t)&geo));
+
+	if (ret < 0) {
+		warnx("ERROR: mtd->ioctl failed: %d", ret);
+		return ret;
+	}
+
+	*blocksize = geo.blocksize;
+	*erasesize = geo.blocksize;
+	*neraseblocks = geo.neraseblocks;
+
+	/* Determine the size of each partition.  Make each partition an even
+	 * multiple of the erase block size (perhaps not using some space at the
+	 * end of the FLASH).
+	 */
+
+	*blkpererase = geo.erasesize / geo.blocksize;
+	*nblocks     = (geo.neraseblocks / n_partitions) * *blkpererase;
+	*partsize    = *nblocks * geo.blocksize;
+
+	return ret;
+}
+
+/*
+  get partition size in bytes
+ */
+static ssize_t mtd_get_partition_size(void)
+{
+	unsigned long blocksize, erasesize, neraseblocks;
+	unsigned blkpererase, nblocks, partsize = 0;
+
+	int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions_current);
+	if (ret != OK) {
+		errx(1, "Failed to get geometry");
+	}
+	return partsize;
+}
+
+void mtd_print_info()
+{
+	if (!attached)
+		exit(1);
+
+	unsigned long blocksize, erasesize, neraseblocks;
+	unsigned blkpererase, nblocks, partsize;
+
+	int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions_current);
+	if (ret)
+		exit(3);
+
+	warnx("Flash Geometry:");
+
+	printf("  blocksize:      %lu\n", blocksize);
+	printf("  erasesize:      %lu\n", erasesize);
+	printf("  neraseblocks:   %lu\n", neraseblocks);
+	printf("  No. partitions: %u\n", n_partitions_current);
+	printf("  Partition size: %u Blocks (%u bytes)\n", nblocks, partsize);
+	printf("  TOTAL SIZE: %u KiB\n", neraseblocks * erasesize / 1024);
+
+}
+
+void
+mtd_test(void)
+{
+	warnx("This test routine does not test anything yet!");
+	exit(1);
+}
+
+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];
+	memset(v, 0xFF, sizeof(v));
+	for (uint8_t i = 0; i < n_partitions; i++) {
+		uint32_t count = 0;
+		printf("Erasing %s\n", partition_names[i]);
+		int fd = open(partition_names[i], O_WRONLY);
+		if (fd == -1) {
+			errx(1, "Failed to open partition");
+		}
+		while (write(fd, v, sizeof(v)) == sizeof(v)) {
+			count += sizeof(v);
+		}
+		printf("Erased %lu bytes\n", (unsigned long)count);
+		close(fd);
+	}
+	exit(0);
+}
+
+/*
+  readtest is useful during startup to validate the device is
+  responding on the bus. It relies on the driver returning an error on
+  bad reads (the ramtron driver does return an error)
+ */
+void
+mtd_readtest(char *partition_names[], unsigned n_partitions)
+{
+	ssize_t expected_size = mtd_get_partition_size();
+
+	uint8_t v[128];
+	for (uint8_t i = 0; i < n_partitions; i++) {
+		uint32_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) {
+			errx(1, "Failed to open partition");
+		}
+		while (read(fd, v, sizeof(v)) == sizeof(v)) {
+			count += sizeof(v);
+		}
+		if (count != expected_size) {
+			errx(1,"Failed to read partition - got %u/%u bytes", count, expected_size);
+		}
+		close(fd);
+	}
+	printf("readtest OK\n");
+	exit(0);
+}
+
+/*
+  rwtest is useful during startup to validate the device is
+  responding on the bus for both reads and writes. It reads data in
+  blocks and writes the data back, then reads it again, failing if the
+  data isn't the same
+ */
+void
+mtd_rwtest(char *partition_names[], unsigned n_partitions)
+{
+	ssize_t expected_size = mtd_get_partition_size();
+
+	uint8_t v[128], v2[128];
+	for (uint8_t i = 0; i < n_partitions; i++) {
+		uint32_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) {
+			errx(1, "Failed to open partition");
+		}
+		while (read(fd, v, sizeof(v)) == sizeof(v)) {
+			count += sizeof(v);
+                        if (lseek(fd, offset, SEEK_SET) != offset) {
+                            errx(1, "seek failed");                            
+                        }
+                        if (write(fd, v, sizeof(v)) != sizeof(v)) {
+                            errx(1, "write failed");
+                        }
+                        if (lseek(fd, offset, SEEK_SET) != offset) {
+                            errx(1, "seek failed");                            
+                        }
+                        if (read(fd, v2, sizeof(v2)) != sizeof(v2)) {
+                            errx(1, "read failed");
+                        }
+                        if (memcmp(v, v2, sizeof(v2)) != 0) {
+                            errx(1, "memcmp failed");
+                        }
+                        offset += sizeof(v);
+		}
+		if (count != expected_size) {
+			errx(1,"Failed to read partition - got %u/%u bytes", count, expected_size);
+		}
+		close(fd);
+	}
+	printf("rwtest OK\n");
+	exit(0);
+}
+
+#endif
diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk
new file mode 100644
index 000000000..a48588535
--- /dev/null
+++ b/src/systemcmds/nshterm/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   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 nshterm utility
+#
+
+MODULE_COMMAND	 = nshterm
+SRCS		 = nshterm.c
+
+MODULE_STACKSIZE = 3000
diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c
new file mode 100644
index 000000000..7d9484d3e
--- /dev/null
+++ b/src/systemcmds/nshterm/nshterm.c
@@ -0,0 +1,117 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Andrew Tridgell
+ *
+ * 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 nshterm.c
+ * start a nsh terminal on a given port. This can be useful for error
+ * handling in startup scripts to start a nsh shell on /dev/ttyACM0
+ * for diagnostics
+ */
+
+#include <nuttx/config.h>
+#include <termios.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdarg.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <apps/nsh.h>
+#include <fcntl.h>
+#include <systemlib/err.h>
+
+__EXPORT int nshterm_main(int argc, char *argv[]);
+
+int
+nshterm_main(int argc, char *argv[])
+{
+    if (argc < 2) {
+        printf("Usage: nshterm <device>\n");
+        exit(1);
+    }
+    uint8_t retries = 0;
+    int fd = -1;
+
+    /* try the first 30 seconds */
+    while (retries < 300) {
+        /* the retries are to cope with the behaviour of /dev/ttyACM0 */
+        /* which may not be ready immediately. */
+        fd = open(argv[1], O_RDWR);
+        if (fd != -1) {
+            break;
+        }
+        usleep(100000);
+        retries++;
+    }
+    if (fd == -1) {
+        perror(argv[1]);
+        exit(1);
+    }
+
+    /* set up the serial port with output processing */
+    
+    /* Try to set baud rate */
+    struct termios uart_config;
+    int termios_state;
+
+    /* Back up the original uart configuration to restore it after exit */
+    if ((termios_state = tcgetattr(fd, &uart_config)) < 0) {
+        warnx("ERROR get termios config %s: %d\n", argv[1], termios_state);
+        close(fd);
+        return -1;
+    }
+
+    /* Set ONLCR flag (which appends a CR for every LF) */
+    uart_config.c_oflag |= (ONLCR | OPOST/* | OCRNL*/);
+
+    if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
+        warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", argv[1]);
+        close(fd);
+        return -1;
+    }
+
+    /* setup standard file descriptors */
+    close(0);
+    close(1);
+    close(2);
+    dup2(fd, 0);
+    dup2(fd, 1);
+    dup2(fd, 2);
+
+    nsh_consolemain(0, NULL);
+
+    close(fd);
+
+    return OK;
+}
diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c
index 60e61d07b..0cbba0a37 100644
--- a/src/systemcmds/param/param.c
+++ b/src/systemcmds/param/param.c
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (c) 2012, 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
@@ -63,6 +62,7 @@ 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);
 
 int
 param_main(int argc, char *argv[])
@@ -72,7 +72,12 @@ param_main(int argc, char *argv[])
 			if (argc >= 3) {
 				do_save(argv[2]);
 			} else {
-				do_save(param_get_default_file());
+				if (param_save_default()) {
+					warnx("Param export failed.");
+					exit(1);
+				} else {
+					exit(0);
+				}
 			}
 		}
 
@@ -117,19 +122,24 @@ param_main(int argc, char *argv[])
 				errx(1, "not enough arguments.\nTry 'param set PARAM_NAME 3'");
 			}
 		}
+
+		if (!strcmp(argv[1], "compare")) {
+			if (argc >= 4) {
+				do_compare(argv[2], &argv[3], argc - 3);
+			} else {
+				errx(1, "not enough arguments.\nTry 'param compare PARAM_NAME 3'");
+			}
+		}
 	}
 	
-	errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'select' or 'save'");
+	errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'compare', 'select' or 'save'");
 }
 
 static void
 do_save(const char* param_file_name)
 {
-	/* delete the parameter file in case it exists */
-	unlink(param_file_name);
-
 	/* create the file */
-	int fd = open(param_file_name, O_WRONLY | O_CREAT | O_EXCL);
+	int fd = open(param_file_name, O_WRONLY | O_CREAT);
 
 	if (fd < 0)
 		err(1, "opening '%s' failed", param_file_name);
@@ -138,7 +148,7 @@ do_save(const char* param_file_name)
 	close(fd);
 
 	if (result < 0) {
-		unlink(param_file_name);
+		(void)unlink(param_file_name);
 		errx(1, "error exporting to '%s'", param_file_name);
 	}
 
@@ -195,11 +205,38 @@ do_show_print(void *arg, param_t param)
 	int32_t i;
 	float f;
 	const char *search_string = (const char*)arg;
+	const char *p_name = (const char*)param_name(param);
 
 	/* print nothing if search string is invalid and not matching */
-	if (!(arg == NULL || (!strcmp(search_string, param_name(param))))) {
-		/* param not found */
-		return;
+	if (!(arg == NULL)) {
+
+		/* start search */
+		char *ss = search_string;
+		char *pp = p_name;
+		bool mismatch = false;
+
+		/* XXX this comparison is only ok for trailing wildcards */
+		while (*ss != '\0' && *pp != '\0') {
+
+			if (*ss == *pp) {
+				ss++;
+				pp++;
+			} else if (*ss == '*') {
+				if (*(ss + 1) != '\0') {
+					warnx("* symbol only allowed at end of search string.");
+					exit(1);
+				}
+
+				pp++;
+			} else {
+				/* param not found */
+				return;
+			}
+		}
+
+		/* the search string must have been consumed */
+		if (!(*ss == '\0' || *ss == '*'))
+			return;
 	}
 
 	printf("%c %s: ",
@@ -283,7 +320,7 @@ do_set(const char* name, const char* val)
 			char* end;
 			f = strtod(val,&end);
 			param_set(param, &f);
-			printf(" -> new: %f\n", f);
+			printf(" -> new: %4.4f\n", (double)f);
 
 		}
 
@@ -295,3 +332,73 @@ do_set(const char* name, const char* val)
 
 	exit(0);
 }
+
+static void
+do_compare(const char* name, const char* vals[], unsigned comparisons)
+{
+	int32_t i;
+	float f;
+	param_t param = param_find(name);
+
+	/* set nothing if parameter cannot be found */
+	if (param == PARAM_INVALID) {
+		/* param not found */
+		errx(1, "Error: Parameter %s not found.", name);
+	}
+
+	/*
+	 * Set parameter if type is known and conversion from string to value turns out fine
+	 */
+
+	int ret = 1;
+
+	switch (param_type(param)) {
+	case PARAM_TYPE_INT32:
+		if (!param_get(param, &i)) {
+
+			/* convert string */
+			char* end;
+
+			for (unsigned k = 0; k < comparisons; k++) {
+
+				int j = strtol(vals[k],&end,10);
+
+				if (i == j) {
+					printf(" %d: ", i);
+					ret = 0;
+				}
+			}
+		}
+
+		break;
+
+	case PARAM_TYPE_FLOAT:
+		if (!param_get(param, &f)) {
+
+			/* convert string */
+			char* end;
+
+			for (unsigned k = 0; k < comparisons; k++) {
+
+				float g = strtod(vals[k], &end);
+				if (fabsf(f - g) < 1e-7f) {
+					printf(" %4.4f: ", (double)f);
+					ret = 0;	
+				}
+			}
+		}
+
+		break;
+
+	default:
+		errx(1, "<unknown / unsupported type %d>\n", 0 + param_type(param));
+	}
+
+	if (ret == 0) {
+		printf("%c %s: match\n",
+		param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
+		param_name(param));
+	}
+
+	exit(ret);
+}
diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c
index 7752ffe67..982b03782 100644
--- a/src/systemcmds/preflight_check/preflight_check.c
+++ b/src/systemcmds/preflight_check/preflight_check.c
@@ -57,6 +57,7 @@
 #include <drivers/drv_baro.h>
 
 #include <mavlink/mavlink_log.h>
+#include <systemlib/rc_check.h>
 
 __EXPORT int preflight_check_main(int argc, char *argv[]);
 static int led_toggle(int leds, int led);
@@ -108,7 +109,7 @@ int preflight_check_main(int argc, char *argv[])
 	/* ---- ACCEL ---- */
 
 	close(fd);
-	fd = open(ACCEL_DEVICE_PATH, 0);
+	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
 	ret = ioctl(fd, ACCELIOCSELFTEST, 0);
 	
 	if (ret != OK) {
@@ -118,6 +119,29 @@ int preflight_check_main(int argc, char *argv[])
 		goto system_eval;
 	}
 
+	/* check measurement result range */
+	struct accel_report acc;
+	ret = read(fd, &acc, sizeof(acc));
+
+	if (ret == sizeof(acc)) {
+		/* evaluate values */
+		if (sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z) > 30.0f /* m/s^2 */) {
+			warnx("accel with spurious values");
+			mavlink_log_critical(mavlink_fd, "SENSOR FAIL: |ACCEL| > 30 m/s^2");
+			/* this is frickin' fatal */
+			fail_on_error = true;
+			system_ok = false;
+			goto system_eval;
+		}
+	} else {
+		warnx("accel read failed");
+		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL READ");
+		/* this is frickin' fatal */
+		fail_on_error = true;
+		system_ok = false;
+		goto system_eval;
+	}
+
 	/* ---- GYRO ---- */
 
 	close(fd);
@@ -135,104 +159,15 @@ int preflight_check_main(int argc, char *argv[])
 
 	close(fd);
 	fd = open(BARO_DEVICE_PATH, 0);
+	close(fd);
 
 	/* ---- RC CALIBRATION ---- */
 
-	param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max,
-	_parameter_handles_rev, _parameter_handles_dz;
-
-	float param_min, param_max, param_trim, param_rev, param_dz;
-
-	bool rc_ok = true;
-	char nbuf[20];
-
-	/* first check channel mappings */
-			/* check which map param applies */
-		// if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) {
-		// 	mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
-		// 	/* give system time to flush error message in case there are more */
-		// 	usleep(100000);
-		// 	count++;
-		// }
-
-	for (int i = 0; i < 12; i++) {
-		/* should the channel be enabled? */
-		uint8_t count = 0;
-
-		/* min values */
-		sprintf(nbuf, "RC%d_MIN", i + 1);
-		_parameter_handles_min = param_find(nbuf);
-		param_get(_parameter_handles_min, &param_min);
-
-		/* trim values */
-		sprintf(nbuf, "RC%d_TRIM", i + 1);
-		_parameter_handles_trim = param_find(nbuf);
-		param_get(_parameter_handles_trim, &param_trim);
-
-		/* max values */
-		sprintf(nbuf, "RC%d_MAX", i + 1);
-		_parameter_handles_max = param_find(nbuf);
-		param_get(_parameter_handles_max, &param_max);
-
-		/* channel reverse */
-		sprintf(nbuf, "RC%d_REV", i + 1);
-		_parameter_handles_rev = param_find(nbuf);
-		param_get(_parameter_handles_rev, &param_rev);
-
-		/* channel deadzone */
-		sprintf(nbuf, "RC%d_DZ", i + 1);
-		_parameter_handles_dz = param_find(nbuf);
-		param_get(_parameter_handles_dz, &param_dz);
-
-		/* assert min..center..max ordering */
-		if (param_min < 500) {
-			count++;
-			mavlink_log_critical(mavlink_fd, "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);
-			/* 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", i+1);
-			/* 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", i+1);
-			/* 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);
-			/* give system time to flush error message in case there are more */
-			usleep(100000);
-			count++;
-		}
+	bool rc_ok = (OK == rc_calibration_check(mavlink_fd));
 
-		/* check which map param applies */
-		// if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) {
-		// 	mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
-		// 	/* give system time to flush error message in case there are more */
-		// 	usleep(100000);
-		// 	count++;
-		// }
-
-		/* sanity checks pass, enable channel */
-		if (count) {
-			mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
-			usleep(100000);
-			rc_ok = false;
-		}
-	}
+	/* warn */
+	if (!rc_ok)
+		warnx("rc calibration test failed");
 
 	/* require RC ok to keep system_ok */
 	system_ok &= rc_ok;
@@ -248,34 +183,43 @@ system_eval:
 	} else {
 		fflush(stdout);
 
-		int buzzer = open("/dev/tone_alarm", O_WRONLY);
+		warnx("PREFLIGHT CHECK ERROR! TRIGGERING ALARM");
+		fflush(stderr);
+
+		int buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY);
 		int leds = open(LED_DEVICE_PATH, 0);
 
+		if (leds < 0) {
+			close(buzzer);
+			errx(1, "failed to open leds, aborting");
+		}
+
 		/* flip blue led into alternating amber */
 		led_off(leds, LED_BLUE);
 		led_off(leds, LED_AMBER);
 		led_toggle(leds, LED_BLUE);
 
 		/* display and sound error */
-		for (int i = 0; i < 150; i++)
+		for (int i = 0; i < 14; i++)
 		{
 			led_toggle(leds, LED_BLUE);
 			led_toggle(leds, LED_AMBER);
 
 			if (i % 10 == 0) {
-				ioctl(buzzer, TONE_SET_ALARM, 4);
+				ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE);
 			} else if (i % 5 == 0) {
-				ioctl(buzzer, TONE_SET_ALARM, 2);
+				ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE);
 			}
 			usleep(100000);
 		}
 
 		/* stop alarm */
-		ioctl(buzzer, TONE_SET_ALARM, 0);
+		ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE);
 
-		/* switch on leds */
+		/* switch off leds */
 		led_on(leds, LED_BLUE);
 		led_on(leds, LED_AMBER);
+		close(leds);
 
 		if (fail_on_error) {
 			/* exit with error message */
@@ -307,4 +251,4 @@ static int led_off(int leds, int led)
 static int led_on(int leds, int led)
 {
 	return ioctl(leds, LED_ON, led);
-}
\ No newline at end of file
+}
diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index ff733df52..7c23f38c2 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -45,6 +45,7 @@
 #include <stdbool.h>
 #include <unistd.h>
 #include <fcntl.h>
+#include <poll.h>
 #include <sys/mount.h>
 #include <sys/ioctl.h>
 #include <sys/stat.h>
@@ -71,16 +72,36 @@ usage(const char *reason)
 		warnx("%s", reason);
 	errx(1, 
 		"usage:\n"
-		"pwm [-v] [-d <device>] [-u <alt_rate>] [-c <channel group>] [-m chanmask ] [arm|disarm] [<channel_value> ...]\n"
-		"  -v                 Print information about the PWM device\n"
-		"  <device>           PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
-		"  <alt_rate>         PWM update rate for channels in <alt_channel_mask>\n"
-		"  <channel_group>    Channel group that should update at the alternate rate (may be specified more than once)\n"
-		"  arm | disarm       Arm or disarm the ouptut\n"
-		"  <channel_value>... PWM output values in microseconds to assign to the PWM outputs\n"
-		"  <chanmask>         Directly supply alt rate channel mask (debug use only)\n"
+		"pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info  ...\n"
 		"\n"
-		"When -c is specified, any channel groups not listed with -c will update at the default rate.\n"
+		"  arm                      Arm output\n"
+		"  disarm                   Disarm output\n"
+		"\n"
+		"  rate ...                 Configure PWM rates\n"
+		"    [-g <channel group>]   Channel group that should update at the alternate rate\n"
+		"    [-m <chanmask> ]       Directly supply channel mask\n"
+		"    [-a]                   Configure all outputs\n"
+		"    -r <alt_rate>          PWM rate (50 to 400 Hz)\n"
+		"\n"
+		"  failsafe ...      	    Configure failsafe PWM values\n"
+		"  disarmed ...      	    Configure disarmed PWM values\n"
+		"  min ...           	    Configure minimum PWM values\n"
+		"  max ...           	    Configure maximum PWM values\n"
+		"    [-c <channels>]        Supply channels (e.g. 1234)\n"
+		"    [-m <chanmask> ]       Directly supply channel mask (e.g. 0xF)\n"
+		"    [-a]                   Configure all outputs\n"
+		"    -p <pwm value>         PWM value\n"
+		"\n"
+		"  test ...                 Directly set PWM values\n"
+		"    [-c <channels>]        Supply channels (e.g. 1234)\n"
+		"    [-m <chanmask> ]       Directly supply channel mask (e.g. 0xF)\n"
+		"    [-a]                   Configure all outputs\n"
+		"    -p <pwm value>         PWM value\n"
+		"\n"
+		"  info                     Print information about the PWM device\n"
+		"\n"
+		"    -v                     Print verbose information\n"
+		"    -d <device>            PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
 		);
 
 }
@@ -92,143 +113,408 @@ pwm_main(int argc, char *argv[])
 	unsigned alt_rate = 0;
 	uint32_t alt_channel_groups = 0;
 	bool alt_channels_set = false;
-	bool print_info = false;
+	bool print_verbose = false;
 	int ch;
 	int ret;
 	char *ep;
+	uint32_t set_mask = 0;
 	unsigned group;
-	int32_t set_mask = -1;
+	unsigned long channels;
+	unsigned single_ch = 0;
+	unsigned pwm_value = 0;
 
-	if (argc < 2)
+	if (argc < 1)
 		usage(NULL);
 
-	while ((ch = getopt(argc, argv, "c:d:u:vm:")) != EOF) {
+	while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) {
 		switch (ch) {
+
+		case 'd':
+			if (NULL == strstr(optarg, "/dev/")) {
+				warnx("device %s not valid", optarg);
+				usage(NULL);
+			}
+			dev = optarg;
+			break;
+
+		case 'v':
+			print_verbose = true;
+			break;
+
 		case 'c':
+			/* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */
+			channels = strtoul(optarg, &ep, 0);
+
+			while ((single_ch = channels % 10)) {
+
+				set_mask |= 1<<(single_ch-1);
+				channels /= 10;
+			}
+			break;
+
+		case 'g':
 			group = strtoul(optarg, &ep, 0);
 			if ((*ep != '\0') || (group >= 32))
 				usage("bad channel_group value");
 			alt_channel_groups |= (1 << group);
 			alt_channels_set = true;
+			warnx("alt channels set, group: %d", group);
 			break;
 
-		case 'd':
-			dev = optarg;
+		case 'm':
+			/* Read in mask directly */
+			set_mask = strtoul(optarg, &ep, 0);
+			if (*ep != '\0')
+				usage("bad set_mask value");
 			break;
 
-		case 'u':
-			alt_rate = strtol(optarg, &ep, 0);
+		case 'a':
+			for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) {
+				set_mask |= 1<<i;
+			}
+			break;
+		case 'p':
+			pwm_value = strtoul(optarg, &ep, 0);
 			if (*ep != '\0')
-				usage("bad alt_rate value");
+				usage("bad PWM value provided");
 			break;
-
-		case 'm':
-			set_mask = strtol(optarg, &ep, 0);
+		case 'r':
+			alt_rate = strtoul(optarg, &ep, 0);
 			if (*ep != '\0')
-				usage("bad set_mask value");
+				usage("bad alternative rate provided");
 			break;
-
-		case 'v':
-			print_info = true;
+		default:
 			break;
+		}
+	}
 
-		default:
-			usage(NULL);
+	if (print_verbose && set_mask > 0) {
+		warnx("Chose channels: ");
+		printf("    ");
+		for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) {
+			if (set_mask & 1<<i)
+				printf("%d ", i+1);
 		}
+		printf("\n");
 	}
-	argc -= optind;
-	argv += optind;
 
 	/* open for ioctl only */
 	int fd = open(dev, 0);
 	if (fd < 0)
 		err(1, "can't open %s", dev);
 
-	/* change alternate PWM rate */
-	if (alt_rate > 0) {
-		ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate);
+	/* get the number of servo channels */
+	unsigned servo_count;
+	ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
+	if (ret != OK)
+		err(1, "PWM_SERVO_GET_COUNT");
+
+	if (!strcmp(argv[1], "arm")) {
+		/* tell safety that its ok to disable it with the switch */
+		ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
 		if (ret != OK)
-			err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)");
-	}
+			err(1, "PWM_SERVO_SET_ARM_OK");
+		/* tell IO that the system is armed (it will output values if safety is off) */
+		ret = ioctl(fd, PWM_SERVO_ARM, 0);
+		if (ret != OK)
+			err(1, "PWM_SERVO_ARM");
+
+		if (print_verbose)
+			warnx("Outputs armed");
+		exit(0);
 
-	/* directly supplied channel mask */
-	if (set_mask != -1) {
-		ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, set_mask);
+	} else if (!strcmp(argv[1], "disarm")) {
+		/* disarm, but do not revoke the SET_ARM_OK flag */
+		ret = ioctl(fd, PWM_SERVO_DISARM, 0);
 		if (ret != OK)
-			err(1, "PWM_SERVO_SELECT_UPDATE_RATE");
-	}
+			err(1, "PWM_SERVO_DISARM");
+
+		if (print_verbose)
+			warnx("Outputs disarmed");
+		exit(0);
+
+	} else if (!strcmp(argv[1], "rate")) {
+
+		/* change alternate PWM rate */
+		if (alt_rate > 0) {
+			ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate);
+			if (ret != OK)
+				err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)");
+		}
+
+		/* directly supplied channel mask */
+		if (set_mask > 0) {
+			ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, set_mask);
+			if (ret != OK)
+				err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE");
+		}
+
+		/* assign alternate rate to channel groups */
+		if (alt_channels_set) {
+			uint32_t mask = 0;
 
-	/* assign alternate rate to channel groups */
-	if (alt_channels_set) {
-		uint32_t mask = 0;
+			for (group = 0; group < 32; group++) {
+				if ((1 << group) & alt_channel_groups) {
+					uint32_t group_mask;
 
-		for (unsigned group = 0; group < 32; group++) {
-			if ((1 << group) & alt_channel_groups) {
-				uint32_t group_mask;
+					ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask);
+					if (ret != OK)
+						err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group);
 
-				ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask);
-				if (ret != OK)
-					err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group);
+					mask |= group_mask;
+				}
+			}
+
+			ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, mask);
+			if (ret != OK)
+				err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE");
+		}
+		exit(0);
+
+	} else if (!strcmp(argv[1], "min")) {
+
+		if (set_mask == 0) {
+			usage("no channels set");
+		}
+		if (pwm_value == 0)
+			usage("no PWM value provided");
+
+		struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
 
-				mask |= group_mask;
+		for (unsigned i = 0; i < servo_count; i++) {
+			if (set_mask & 1<<i) {
+				pwm_values.values[i] = pwm_value;
+				if (print_verbose)
+					warnx("Channel %d: min PWM: %d", i+1, pwm_value);
 			}
+			pwm_values.channel_count++;
 		}
 
-		ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, mask);
-		if (ret != OK)
-			err(1, "PWM_SERVO_SELECT_UPDATE_RATE");
-	}
+		if (pwm_values.channel_count == 0) {
+			usage("no PWM values added");
+		} else {
+
+			ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
+			if (ret != OK)
+				errx(ret, "failed setting min values");
+		}
+		exit(0);
+
+	} else if (!strcmp(argv[1], "max")) {
+
+		if (set_mask == 0) {
+			usage("no channels set");
+		}
+		if (pwm_value == 0)
+			usage("no PWM value provided");
 
-	/* iterate remaining arguments */
-	unsigned channel = 0;
-	while (argc--) {
-		const char *arg = argv[0];
-		argv++;
-		if (!strcmp(arg, "arm")) {
-			ret = ioctl(fd, PWM_SERVO_ARM, 0);
+		struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
+
+		for (unsigned i = 0; i < servo_count; i++) {
+			if (set_mask & 1<<i) {
+				pwm_values.values[i] = pwm_value;
+				if (print_verbose)
+					warnx("Channel %d: max PWM: %d", i+1, pwm_value);
+			}
+			pwm_values.channel_count++;
+		}
+
+		if (pwm_values.channel_count == 0) {
+			usage("no PWM values added");
+		} else {
+
+			ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
 			if (ret != OK)
-				err(1, "PWM_SERVO_ARM");
-			continue;
+				errx(ret, "failed setting max values");
 		}
-		if (!strcmp(arg, "disarm")) {
-			ret = ioctl(fd, PWM_SERVO_DISARM, 0);
+		exit(0);
+
+	} else if (!strcmp(argv[1], "disarmed")) {
+
+		if (set_mask == 0) {
+			usage("no channels set");
+		}
+		if (pwm_value == 0)
+			warnx("reading disarmed value of zero, disabling disarmed PWM");
+
+		struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
+
+		for (unsigned i = 0; i < servo_count; i++) {
+			if (set_mask & 1<<i) {
+				pwm_values.values[i] = pwm_value;
+				if (print_verbose)
+					warnx("channel %d: disarmed PWM: %d", i+1, pwm_value);
+			}
+			pwm_values.channel_count++;
+		}
+
+		if (pwm_values.channel_count == 0) {
+			usage("no PWM values added");
+		} else {
+
+			ret = ioctl(fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_values);
 			if (ret != OK)
-				err(1, "PWM_SERVO_DISARM");
-			continue;
+				errx(ret, "failed setting disarmed values");
+		}
+		exit(0);
+
+	} else if (!strcmp(argv[1], "failsafe")) {
+
+		if (set_mask == 0) {
+			usage("no channels set");
 		}
-		unsigned pwm_value = strtol(arg, &ep, 0);
-		if (*ep == '\0') {
-			ret = ioctl(fd, PWM_SERVO_SET(channel), pwm_value);
+		if (pwm_value == 0)
+			usage("no PWM value provided");
+
+		struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
+
+		for (unsigned i = 0; i < servo_count; i++) {
+			if (set_mask & 1<<i) {
+				pwm_values.values[i] = pwm_value;
+				if (print_verbose)
+					warnx("Channel %d: failsafe PWM: %d", i+1, pwm_value);
+			}
+			pwm_values.channel_count++;
+		}
+
+		if (pwm_values.channel_count == 0) {
+			usage("no PWM values added");
+		} else {
+
+			ret = ioctl(fd, PWM_SERVO_SET_FAILSAFE_PWM, (long unsigned int)&pwm_values);
 			if (ret != OK)
-				err(1, "PWM_SERVO_SET(%d)", channel);
-			channel++;
-			continue;
+				errx(ret, "failed setting failsafe values");
 		}
-		usage("unrecognised option");
-	}
+		exit(0);
 
-	/* print verbose info */
-	if (print_info) {
-		/* get the number of servo channels */
-		unsigned count;
-		ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&count);
-		if (ret != OK)
-			err(1, "PWM_SERVO_GET_COUNT");
+	} else if (!strcmp(argv[1], "test")) {
+
+		if (set_mask == 0) {
+			usage("no channels set");
+		}
+		if (pwm_value == 0)
+			usage("no PWM value provided");
+
+		/* 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("Press CTRL-C or 'c' to abort.");
+
+		while (1) {
+			for (unsigned i = 0; i < servo_count; i++) {
+				if (set_mask & 1<<i) {
+					ret = ioctl(fd, PWM_SERVO_SET(i), pwm_value);
+					if (ret != OK)
+						err(1, "PWM_SERVO_SET(%d)", i);
+				}
+			}
+
+			/* abort on user request */
+			char c;
+			ret = poll(&fds, 1, 0);
+			if (ret > 0) {
+
+			read(0, &c, 1);
+				if (c == 0x03 || c == 0x63 || c == 'q') {
+					/* 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("User abort\n");
+					exit(0);
+				}
+			}
+		}
+		exit(0);
+
+
+	} else if (!strcmp(argv[1], "info")) {
+
+		printf("device: %s\n", dev);
+
+		uint32_t info_default_rate;
+		uint32_t info_alt_rate;
+		uint32_t info_alt_rate_mask;
+
+		ret = ioctl(fd, PWM_SERVO_GET_DEFAULT_UPDATE_RATE, (unsigned long)&info_default_rate);
+		if (ret != OK) {
+			err(1, "PWM_SERVO_GET_DEFAULT_UPDATE_RATE");
+		}
+
+		ret = ioctl(fd, PWM_SERVO_GET_UPDATE_RATE, (unsigned long)&info_alt_rate);
+		if (ret != OK) {
+			err(1, "PWM_SERVO_GET_UPDATE_RATE");
+		}
+
+		ret = ioctl(fd, PWM_SERVO_GET_SELECT_UPDATE_RATE, (unsigned long)&info_alt_rate_mask);
+		if (ret != OK) {
+			err(1, "PWM_SERVO_GET_SELECT_UPDATE_RATE");
+		}
+
+		struct pwm_output_values failsafe_pwm;
+		struct pwm_output_values disarmed_pwm;
+		struct pwm_output_values min_pwm;
+		struct pwm_output_values max_pwm;
+
+		ret = ioctl(fd, PWM_SERVO_GET_FAILSAFE_PWM, (unsigned long)&failsafe_pwm);
+		if (ret != OK) {
+			err(1, "PWM_SERVO_GET_FAILSAFE_PWM");
+		}
+		ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (unsigned long)&disarmed_pwm);
+		if (ret != OK) {
+			err(1, "PWM_SERVO_GET_DISARMED_PWM");
+		}
+		ret = ioctl(fd, PWM_SERVO_GET_MIN_PWM, (unsigned long)&min_pwm);
+		if (ret != OK) {
+			err(1, "PWM_SERVO_GET_MIN_PWM");
+		}
+		ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, (unsigned long)&max_pwm);
+		if (ret != OK) {
+			err(1, "PWM_SERVO_GET_MAX_PWM");
+		}
 
 		/* print current servo values */
-		for (unsigned i = 0; i < count; i++) {
+		for (unsigned i = 0; i < servo_count; i++) {
 			servo_position_t spos;
 
 			ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos);
 			if (ret == OK) {
-				printf("channel %u: %uus\n", i, spos);
+				printf("channel %u: %u us", i+1, spos);
+
+				if (info_alt_rate_mask & (1<<i))
+					printf(" (alternative rate: %d Hz", info_alt_rate);
+				else
+					printf(" (default rate: %d Hz", info_default_rate);
+
+
+				printf(" failsafe: %d, disarmed: %d us, min: %d us, max: %d us)",
+					failsafe_pwm.values[i], disarmed_pwm.values[i], min_pwm.values[i], max_pwm.values[i]);
+				printf("\n");
 			} else {
 				printf("%u: ERROR\n", i);
 			}
 		}
-
 		/* print rate groups */
-		for (unsigned i = 0; i < count; i++) {
+		for (unsigned i = 0; i < servo_count; i++) {
 			uint32_t group_mask;
 
 			ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask);
@@ -238,11 +524,14 @@ pwm_main(int argc, char *argv[])
 				printf("channel group %u: channels", i);
 				for (unsigned j = 0; j < 32; j++)
 					if (group_mask & (1 << j))
-						printf(" %u", j);
+						printf(" %u", j+1);
 				printf("\n");
 			}
 		}
-		fflush(stdout);
+		exit(0);
+
 	}
-	exit(0);
-}
\ No newline at end of file
+	usage("specify arm|disarm|rate|failsafe|disarmed|min|max|test|info");
+	return 0;
+}
+
diff --git a/src/systemcmds/reboot/reboot.c b/src/systemcmds/reboot/reboot.c
index 47d3cd948..91a2c2eb8 100644
--- a/src/systemcmds/reboot/reboot.c
+++ b/src/systemcmds/reboot/reboot.c
@@ -40,14 +40,31 @@
 #include <nuttx/config.h>
 #include <unistd.h>
 #include <stdio.h>
+#include <getopt.h>
 
 #include <systemlib/systemlib.h>
+#include <systemlib/err.h>
 
 __EXPORT int reboot_main(int argc, char *argv[]);
 
 int reboot_main(int argc, char *argv[])
 {
-	up_systemreset();
+	int ch;
+	bool to_bootloader = false;
+
+	while ((ch = getopt(argc, argv, "b")) != -1) {
+		switch (ch) {
+		case 'b':
+			to_bootloader = true;
+			break;
+		default:
+			errx(1, "usage: reboot [-b]\n"
+				"   -b   reboot into the bootloader");
+
+		}
+	}
+
+	systemreset(to_bootloader);
 }
 
 
diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk
index 754d3a0da..beb9ad13d 100644
--- a/src/systemcmds/tests/module.mk
+++ b/src/systemcmds/tests/module.mk
@@ -23,6 +23,12 @@ SRCS			 = test_adc.c \
 			   test_uart_console.c \
 			   test_uart_loopback.c \
 			   test_uart_send.c \
-			   tests_file.c \
+			   test_mixer.cpp \
+			   test_file.c \
 			   tests_main.c \
-			   tests_param.c
+			   test_param.c \
+			   test_ppm_loopback.c \
+			   test_rc.c \
+			   test_conv.cpp \
+			   test_mount.c \
+			   test_mtd.c
diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/test_conv.cpp
index 6f75b9812..50dece816 100644
--- a/src/systemcmds/tests/tests_file.c
+++ b/src/systemcmds/tests/test_conv.cpp
@@ -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,53 +32,45 @@
  ****************************************************************************/
 
 /**
- * @file tests_file.c
+ * @file test_conv.cpp
+ * Tests conversions used across the system.
  *
- * File write test.
  */
 
-#include <sys/stat.h>
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
 #include <stdio.h>
+#include <poll.h>
+#include <stdlib.h>
 #include <unistd.h>
 #include <fcntl.h>
-#include <systemlib/err.h>
-#include <systemlib/perf_counter.h>
-#include <string.h>
-
-#include <drivers/drv_hrt.h>
+#include <errno.h>
 
 #include "tests.h"
 
-int
-test_file(int argc, char *argv[])
-{
-	/* check if microSD card is mounted */
-	struct stat buffer;
-	if (stat("/fs/microsd/", &buffer)) {
-		warnx("no microSD card mounted, aborting file test");
-		return 1;
-	}
+#include <math.h>
+#include <float.h>
 
-	uint8_t buf[512];
-	hrt_abstime start, end;
-	perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes");
+#include <systemlib/err.h>
+#include <unit_test/unit_test.h>
+#include <px4iofirmware/protocol.h>
 
-	int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
-	memset(buf, 0, sizeof(buf));
+int test_conv(int argc, char *argv[])
+{
+	warnx("Testing system conversions");
 
-	start = hrt_absolute_time();
-	for (unsigned i = 0; i < 1024; i++) {
-		perf_begin(wperf);
-		write(fd, buf, sizeof(buf));
-		perf_end(wperf);
+	for (int i = -10000; i <= 10000; i+=1) {
+		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);
+			return 1;
+		}
 	}
-	end = hrt_absolute_time();
-
-	close(fd);
 
-	warnx("512KiB in %llu microseconds", end - start);
-	perf_print_counter(wperf);
-	perf_free(wperf);
+	warnx("All conversions clean");
 
 	return 0;
 }
diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c
new file mode 100644
index 000000000..96be1e8df
--- /dev/null
+++ b/src/systemcmds/tests/test_file.c
@@ -0,0 +1,322 @@
+/****************************************************************************
+ *
+ *   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 test_file.c
+ *
+ * File write test.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <sys/stat.h>
+#include <poll.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 <drivers/drv_hrt.h>
+
+#include "tests.h"
+
+static int check_user_abort(int fd);
+
+int check_user_abort(int fd) {
+	/* check if user wants to abort */
+	char c;
+
+	struct pollfd fds;
+	int ret;
+	fds.fd = 0; /* stdin */
+	fds.events = POLLIN;
+	ret = poll(&fds, 1, 0);
+
+	if (ret > 0) {
+
+		read(0, &c, 1);
+
+		switch (c) {
+		case 0x03: // ctrl-c
+		case 0x1b: // esc
+		case 'c':
+		case 'q':
+		{
+			warnx("Test aborted.");
+			fsync(fd);
+			close(fd);
+			return OK;
+			/* not reached */
+			}
+		}
+	}
+
+	return 1;
+}
+
+int
+test_file(int argc, char *argv[])
+{
+	const unsigned iterations = 100;
+	const unsigned alignments = 65;
+
+	/* check if microSD card is mounted */
+	struct stat buffer;
+	if (stat("/fs/microsd/", &buffer)) {
+		warnx("no microSD card mounted, aborting file test");
+		return 1;
+	}
+
+	/* 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};
+
+	for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) {
+
+		printf("\n====== FILE TEST: %u bytes chunks ======\n", chunk_sizes[c]);
+
+		for (unsigned a = 0; a < alignments; a++) {
+
+			printf("\n");
+			warnx("----- alignment test: %u bytes -----", a);
+
+			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++) {
+				/* 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);
+
+			warnx("testing unaligned writes - please wait..");
+
+			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]) {
+					warn("WRITE ERROR!");
+
+					if ((0x3 & (uintptr_t)(write_buf + a)))
+						warnx("memory is unaligned, align shift: %d", a);
+
+					return 1;
+				}
+
+				fsync(fd);
+
+				if (!check_user_abort(fd))
+					return OK;
+
+			}
+			end = hrt_absolute_time();
+
+			close(fd);
+			fd = open("/fs/microsd/testfile", O_RDONLY);
+
+			/* read back data for validation */
+			for (unsigned i = 0; i < iterations; i++) {
+				int rret = read(fd, read_buf, chunk_sizes[c]);
+
+				if (rret != chunk_sizes[c]) {
+					warnx("READ ERROR!");
+					return 1;
+				}
+				
+				/* compare value */
+				bool compare_ok = true;
+
+				for (int 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;
+						break;
+					}
+				}
+
+				if (!compare_ok) {
+					warnx("ABORTING FURTHER COMPARISON DUE TO ERROR");
+					return 1;
+				}
+
+				if (!check_user_abort(fd))
+					return OK;
+
+			}
+
+			/*
+			 * ALIGNED WRITES AND UNALIGNED READS
+			 */
+
+			close(fd);
+			int ret = unlink("/fs/microsd/testfile");
+			fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
+
+			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]);
+
+				if (wret != chunk_sizes[c]) {
+					warnx("WRITE ERROR!");
+					return 1;
+				}
+
+				if (!check_user_abort(fd))
+					return OK;
+
+			}
+
+			fsync(fd);
+
+			warnx("reading data aligned..");
+
+			close(fd);
+			fd = open("/fs/microsd/testfile", O_RDONLY);
+
+			bool align_read_ok = true;
+
+			/* read back data unaligned */
+			for (unsigned i = 0; i < iterations; i++) {
+				int rret = read(fd, read_buf, chunk_sizes[c]);
+
+				if (rret != chunk_sizes[c]) {
+					warnx("READ ERROR!");
+					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]);
+						align_read_ok = false;
+						break;
+					}
+
+					if (!check_user_abort(fd))
+						return OK;
+				}
+
+				if (!align_read_ok) {
+					warnx("ABORTING FURTHER COMPARISON DUE TO ERROR");
+					return 1;
+				}
+
+			}
+
+			warnx("align read result: %s\n", (align_read_ok) ? "OK" : "ERROR");
+
+			warnx("reading data unaligned..");
+
+			close(fd);
+			fd = open("/fs/microsd/testfile", O_RDONLY);
+
+			bool unalign_read_ok = true;
+			int unalign_read_err_count = 0;
+
+			memset(read_buf, 0, sizeof(read_buf));
+
+			/* read back data unaligned */
+			for (unsigned i = 0; i < iterations; i++) {
+				int rret = read(fd, read_buf + a, chunk_sizes[c]);
+
+				if (rret != chunk_sizes[c]) {
+					warnx("READ ERROR!");
+					return 1;
+				}
+
+				for (int j = 0; j < chunk_sizes[c]; j++) {
+
+					if ((read_buf + a)[j] != write_buf[j]) {
+						warnx("COMPARISON ERROR: byte %d, align shift: %d: %u != %u", j, a, (unsigned int)read_buf[j + a], (unsigned int)write_buf[j]);
+						unalign_read_ok = false;
+						unalign_read_err_count++;
+						
+						if (unalign_read_err_count > 10)
+							break;
+					}
+
+					if (!check_user_abort(fd))
+						return OK;
+				}
+
+				if (!unalign_read_ok) {
+					warnx("ABORTING FURTHER COMPARISON DUE TO ERROR");
+					return 1;
+				}
+
+			}
+
+			ret = unlink("/fs/microsd/testfile");
+			close(fd);
+
+			if (ret) {
+				warnx("UNLINKING FILE FAILED");
+				return 1;
+			}
+		}
+	}
+
+	/* list directory */
+	DIR		*d;
+	struct dirent	*dir;
+	d = opendir("/fs/microsd");
+	if (d) {
+
+		while ((dir = readdir(d)) != NULL) {
+			//printf("%s\n", dir->d_name);
+		}
+
+		closedir(d);
+
+		warnx("directory listing ok (FS mounted and readable)");
+
+	} else {
+		/* failed opening dir */
+		warnx("FAILED LISTING MICROSD ROOT DIRECTORY");
+		return 1;
+	}
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/test_gpio.c b/src/systemcmds/tests/test_gpio.c
index ab536d956..62a873270 100644
--- a/src/systemcmds/tests/test_gpio.c
+++ b/src/systemcmds/tests/test_gpio.c
@@ -92,7 +92,7 @@ int test_gpio(int argc, char *argv[])
 	int		fd;
 	int		ret = 0;
 
-	fd = open(GPIO_DEVICE_PATH, 0);
+	fd = open(PX4IO_DEVICE_PATH, 0);
 
 	if (fd < 0) {
 		printf("GPIO: open fail\n");
diff --git a/src/systemcmds/tests/test_hrt.c b/src/systemcmds/tests/test_hrt.c
index f21dd115b..5690997a9 100644
--- a/src/systemcmds/tests/test_hrt.c
+++ b/src/systemcmds/tests/test_hrt.c
@@ -94,7 +94,7 @@ extern uint16_t ppm_pulse_history[];
 
 int test_ppm(int argc, char *argv[])
 {
-#ifdef CONFIG_HRT_PPM
+#ifdef HRT_PPM_CHANNEL
 	unsigned i;
 
 	printf("channels: %u\n", ppm_decoded_channels);
@@ -124,10 +124,10 @@ int test_tone(int argc, char *argv[])
 	int fd, result;
 	unsigned long tone;
 
-	fd = open("/dev/tone_alarm", O_WRONLY);
+	fd = open(TONEALARM_DEVICE_PATH, O_WRONLY);
 
 	if (fd < 0) {
-		printf("failed opening /dev/tone_alarm\n");
+		printf("failed opening " TONEALARM_DEVICE_PATH "\n");
 		goto out;
 	}
 
@@ -137,7 +137,7 @@ int test_tone(int argc, char *argv[])
 		tone = atoi(argv[1]);
 
 	if (tone  == 0) {
-		result = ioctl(fd, TONE_SET_ALARM, 0);
+		result = ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE);
 
 		if (result < 0) {
 			printf("failed clearing alarms\n");
@@ -148,7 +148,7 @@ int test_tone(int argc, char *argv[])
 		}
 
 	} else {
-		result = ioctl(fd, TONE_SET_ALARM, 0);
+		result = ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE);
 
 		if (result < 0) {
 			printf("failed clearing alarms\n");
diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp
new file mode 100644
index 000000000..df382e2c6
--- /dev/null
+++ b/src/systemcmds/tests/test_mixer.cpp
@@ -0,0 +1,392 @@
+/****************************************************************************
+ *
+ *   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_mixer.hpp
+ *
+ * Mixer load test
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <string.h>
+#include <time.h>
+#include <math.h>
+
+#include <systemlib/err.h>
+#include <systemlib/mixer/mixer.h>
+#include <systemlib/pwm_limit/pwm_limit.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_pwm_output.h>
+
+#include "tests.h"
+
+static int	mixer_callback(uintptr_t handle,
+			       uint8_t control_group,
+			       uint8_t control_index,
+			       float &control);
+
+const unsigned output_max = 8;
+static float actuator_controls[output_max];
+
+int test_mixer(int argc, char *argv[])
+{
+	/*
+	 * PWM limit structure
+	 */
+	pwm_limit_t pwm_limit;
+	static bool should_arm = false;
+	uint16_t r_page_servo_disarmed[output_max];
+	uint16_t r_page_servo_control_min[output_max];
+	uint16_t r_page_servo_control_max[output_max];
+	uint16_t r_page_servos[output_max];
+	uint16_t servo_predicted[output_max];
+
+	warnx("testing mixer");
+
+	char *filename = "/etc/mixers/IO_pass.mix";
+
+	if (argc > 1)
+		filename = argv[1];
+
+	warnx("loading: %s", filename);
+
+	char		buf[2048];
+
+	load_mixer_file(filename, &buf[0], sizeof(buf));
+	unsigned loaded = strlen(buf);
+
+	warnx("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);
+
+	/* load the mixer in chunks, like
+	 * in the case of a remote load,
+	 * e.g. on PX4IO.
+	 */
+
+	unsigned nused = 0;
+
+	const unsigned chunk_size = 64;
+
+	MixerGroup mixer_group(mixer_callback, 0);
+
+	/* load at once test */
+	unsigned xx = loaded;
+	mixer_group.load_from_buf(&buf[0], xx);
+	warnx("complete buffer load: loaded %u mixers", mixer_group.count());
+	if (mixer_group.count() != 8)
+		return 1;
+
+	unsigned empty_load = 2;
+	char empty_buf[2];
+	empty_buf[0] = ' ';
+	empty_buf[1] = '\0';
+	mixer_group.reset();
+	mixer_group.load_from_buf(&empty_buf[0], empty_load);
+	warnx("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load);
+	if (empty_load != 0)
+		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 */
+	unsigned mixer_text_length = 0;
+
+	unsigned transmitted = 0;
+
+	warnx("transmitted: %d, loaded: %d", transmitted, loaded);
+
+	while (transmitted < loaded) {
+
+		unsigned	text_length = (loaded - transmitted > chunk_size) ? chunk_size : loaded - transmitted;
+
+		/* check for overflow - this would be really fatal */
+		if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
+			bool mixer_ok = false;
+			return 1;
+		}
+
+		/* append mixer text and nul-terminate */
+		memcpy(&mixer_text[mixer_text_length], &buf[transmitted], text_length);
+		mixer_text_length += text_length;
+		mixer_text[mixer_text_length] = '\0';
+		warnx("buflen %u, text:\n\"%s\"", mixer_text_length, &mixer_text[0]);
+
+		/* process the text buffer, adding new mixers as their descriptions can be parsed */
+		unsigned resid = mixer_text_length;
+		mixer_group.load_from_buf(&mixer_text[0], resid);
+
+		/* 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 */
+			if (resid > 0)
+				memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
+
+			mixer_text_length = resid;
+		}
+
+		transmitted += text_length;
+	}
+
+	warnx("chunked load: loaded %u mixers", mixer_group.count());
+
+	if (mixer_group.count() != 8)
+		return 1;
+
+	/* execute the mixer */
+	
+	float	outputs[output_max];
+	unsigned mixed;
+	const int jmax = 5;
+
+	pwm_limit_init(&pwm_limit);
+	should_arm = true;
+
+	/* run through arming phase */
+	for (int 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;
+		r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
+	}
+
+	warnx("ARMING TEST: STARTING RAMP");
+	unsigned sleep_quantum_us = 10000;
+
+	hrt_abstime starttime = hrt_absolute_time();
+	unsigned sleepcount = 0;
+
+	while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US) {
+		
+		/* mix */
+		mixed = mixer_group.mix(&outputs[0], output_max);
+
+		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++)
+		{
+			/* check mixed outputs to be zero during init phase */
+			if (hrt_elapsed_time(&starttime) < INIT_TIME_US &&
+				r_page_servos[i] != r_page_servo_disarmed[i]) {
+				warnx("disarmed servo value mismatch");
+				return 1;
+			}
+
+			if (hrt_elapsed_time(&starttime) >= INIT_TIME_US &&
+				r_page_servos[i] + 1 <= r_page_servo_disarmed[i]) {
+				warnx("ramp servo value mismatch");
+				return 1;
+			}
+
+			//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]);
+		}
+		usleep(sleep_quantum_us);
+		sleepcount++;
+
+		if (sleepcount % 10 == 0) {
+			printf(".");
+			fflush(stdout);
+		}
+	}
+	printf("\n");
+
+	warnx("ARMING TEST: NORMAL OPERATION");
+
+	for (int j = -jmax; j <= jmax; j++) {
+
+		for (int 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;
+			r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
+		}
+
+		/* mix */
+		mixed = mixer_group.mix(&outputs[0], output_max);
+
+		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++)
+		{
+			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]);
+				warnx("mixer violated predicted value");
+				return 1;
+			}
+		}
+	}
+
+	warnx("ARMING TEST: DISARMING");
+
+	starttime = hrt_absolute_time();
+	sleepcount = 0;
+	should_arm = false;
+
+	while (hrt_elapsed_time(&starttime) < 600000) {
+		
+		/* mix */
+		mixed = mixer_group.mix(&outputs[0], output_max);
+
+		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++)
+		{
+			/* check mixed outputs to be zero during init phase */
+			if (r_page_servos[i] != r_page_servo_disarmed[i]) {
+				warnx("disarmed servo value mismatch");
+				return 1;
+			}
+
+			//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]);
+		}
+		usleep(sleep_quantum_us);
+		sleepcount++;
+
+		if (sleepcount % 10 == 0) {
+			printf(".");
+			fflush(stdout);
+		}
+	}
+	printf("\n");
+
+	warnx("ARMING TEST: REARMING: STARTING RAMP");
+
+	starttime = hrt_absolute_time();
+	sleepcount = 0;
+	should_arm = true;
+
+	while (hrt_elapsed_time(&starttime) < 600000 + RAMP_TIME_US) {
+		
+		/* mix */
+		mixed = mixer_group.mix(&outputs[0], output_max);
+
+		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++)
+		{
+			/* predict value */
+			servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
+
+			/* check ramp */
+
+			if (hrt_elapsed_time(&starttime) < RAMP_TIME_US &&
+				(r_page_servos[i] + 1 <= r_page_servo_disarmed[i] ||
+					r_page_servos[i] > servo_predicted[i])) {
+				warnx("ramp servo value mismatch");
+				return 1;
+			}
+
+			/* 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]);
+				warnx("mixer violated predicted value");
+				return 1;
+			}
+
+			//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]);
+		}
+		usleep(sleep_quantum_us);
+		sleepcount++;
+
+		if (sleepcount % 10 == 0) {
+			printf(".");
+			fflush(stdout);
+		}
+	}
+	printf("\n");
+
+	/* load multirotor at once test */
+	mixer_group.reset();
+
+	if (argc > 2)
+		filename = argv[2];
+	else
+		filename = "/etc/mixers/FMU_quad_w.mix";
+
+	load_mixer_file(filename, &buf[0], sizeof(buf));
+	loaded = strlen(buf);
+
+	warnx("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);
+
+	unsigned mc_loaded = loaded;
+	mixer_group.load_from_buf(&buf[0], mc_loaded);
+	warnx("complete buffer load: loaded %u mixers", mixer_group.count());
+	if (mixer_group.count() != 5) {
+		warnx("FAIL: Quad W mixer load failed");
+		return 1;
+	}
+
+	warnx("SUCCESS: No errors in mixer test");
+}
+
+static int
+mixer_callback(uintptr_t handle,
+	       uint8_t control_group,
+	       uint8_t control_index,
+	       float &control)
+{
+	if (control_group != 0)
+		return -1;
+
+	if (control_index > (sizeof(actuator_controls) / sizeof(actuator_controls[0])))
+		return -1;
+
+	control = actuator_controls[control_index];
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/test_mount.c b/src/systemcmds/tests/test_mount.c
new file mode 100644
index 000000000..4b6303cfb
--- /dev/null
+++ b/src/systemcmds/tests/test_mount.c
@@ -0,0 +1,285 @@
+/****************************************************************************
+ *
+ *   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 test_mount.c
+ *
+ * Device mount / unmount stress test
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#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/systemlib.h>
+#include <systemlib/perf_counter.h>
+#include <string.h>
+
+#include <drivers/drv_hrt.h>
+
+#include "tests.h"
+
+const int fsync_tries = 1;
+const int abort_tries = 10;
+
+int
+test_mount(int argc, char *argv[])
+{
+	const unsigned iterations = 2000;
+	const unsigned alignments = 10;
+
+	const char* cmd_filename = "/fs/microsd/mount_test_cmds.txt";
+
+
+	/* check if microSD card is mounted */
+	struct stat buffer;
+	if (stat("/fs/microsd/", &buffer)) {
+		warnx("no microSD card mounted, aborting file test");
+		return 1;
+	}
+
+	/* list directory */
+	DIR		*d;
+	struct dirent	*dir;
+	d = opendir("/fs/microsd");
+	if (d) {
+
+		while ((dir = readdir(d)) != NULL) {
+			//printf("%s\n", dir->d_name);
+		}
+
+		closedir(d);
+
+		warnx("directory listing ok (FS mounted and readable)");
+
+	} else {
+		/* failed opening dir */
+		warnx("FAILED LISTING MICROSD ROOT DIRECTORY");
+
+		if (stat(cmd_filename, &buffer) == OK) {
+			(void)unlink(cmd_filename);
+		}
+
+		return 1;
+	}
+
+	/* read current test status from file, write test instructions for next round */
+
+	/* initial values */
+	int it_left_fsync = fsync_tries;
+	int it_left_abort = abort_tries;
+
+	int cmd_fd;
+	if (stat(cmd_filename, &buffer) == OK) {
+
+		/* command file exists, read off state */
+		cmd_fd = open(cmd_filename, O_RDWR | O_NONBLOCK);
+		char buf[64];
+		int ret = read(cmd_fd, buf, sizeof(buf));
+
+		if (ret > 0) {
+			int count = 0;
+			ret = sscanf(buf, "TEST: %u %u %n", &it_left_fsync, &it_left_abort, &count);
+		} else {
+			buf[0] = '\0';
+		}
+
+		if (it_left_fsync > fsync_tries)
+			it_left_fsync = fsync_tries;
+
+		if (it_left_abort > abort_tries)
+			it_left_abort = abort_tries;
+
+		warnx("Iterations left: #%d / #%d of %d / %d\n(%s)", it_left_fsync, it_left_abort,
+			fsync_tries, abort_tries, buf);
+
+		int it_left_fsync_prev = it_left_fsync;
+
+		/* now write again what to do next */
+		if (it_left_fsync > 0)
+			it_left_fsync--;
+
+		if (it_left_fsync == 0 && it_left_abort > 0) {
+
+			it_left_abort--;
+
+			/* 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(fileno(stdout));
+				fsync(fileno(stderr));
+				usleep(20000);
+			}
+
+		}
+
+		if (it_left_abort == 0) {
+			(void)unlink(cmd_filename);
+			return 0;
+		}
+
+	} else {
+
+		/* this must be the first iteration, do something */
+		cmd_fd = open(cmd_filename, O_TRUNC | O_WRONLY | O_CREAT);
+
+		warnx("First iteration of file test\n");
+	}
+
+	char buf[64];
+	(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);
+
+	/* perform tests for a range of chunk sizes */
+	unsigned chunk_sizes[] = {32, 64, 128, 256, 512, 600, 1200};
+
+	for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) {
+
+		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(fileno(stdout));
+		fsync(fileno(stderr));
+		usleep(50000);
+
+		for (unsigned a = 0; a < alignments; a++) {
+
+			printf(".");
+
+			uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
+
+			/* fill write buffer with known values */
+			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)));
+
+			int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
+
+			for (unsigned i = 0; i < iterations; i++) {
+
+				int wret = write(fd, write_buf + a, chunk_sizes[c]);
+
+				if (wret != (int)chunk_sizes[c]) {
+					warn("WRITE ERROR!");
+
+					if ((0x3 & (uintptr_t)(write_buf + a)))
+						warnx("memory is unaligned, align shift: %d", a);
+
+					return 1;
+
+				}
+
+				if (it_left_fsync > 0) {
+					fsync(fd);
+				} else {
+					printf("#");
+					fsync(fileno(stdout));
+					fsync(fileno(stderr));
+				}
+			}
+
+			if (it_left_fsync > 0) {
+				printf("#");
+			}
+
+			printf(".");
+			fsync(fileno(stdout));
+			fsync(fileno(stderr));
+			usleep(200000);
+
+			close(fd);
+			fd = open("/fs/microsd/testfile", O_RDONLY);
+
+			/* read back data for validation */
+			for (unsigned i = 0; i < iterations; i++) {
+				int rret = read(fd, read_buf, chunk_sizes[c]);
+
+				if (rret != (int)chunk_sizes[c]) {
+					warnx("READ ERROR!");
+					return 1;
+				}
+				
+				/* compare value */
+				bool compare_ok = true;
+
+				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;
+						break;
+					}
+				}
+
+				if (!compare_ok) {
+					warnx("ABORTING FURTHER COMPARISON DUE TO ERROR");
+					return 1;
+				}
+
+			}
+
+			int ret = unlink("/fs/microsd/testfile");
+			close(fd);
+
+			if (ret) {
+				warnx("UNLINKING FILE FAILED");
+				return 1;
+			}
+
+		}
+	}
+
+	fsync(fileno(stdout));
+	fsync(fileno(stderr));
+	usleep(20000);
+
+
+
+	/* we always reboot for the next test if we get here */
+	warnx("Iteration done, rebooting..");
+	fsync(fileno(stdout));
+	fsync(fileno(stderr));
+	usleep(50000);
+	systemreset(false);
+
+	/* never going to get here */
+	return 0;
+}
diff --git a/src/systemcmds/tests/test_mtd.c b/src/systemcmds/tests/test_mtd.c
new file mode 100644
index 000000000..bac9efbdb
--- /dev/null
+++ b/src/systemcmds/tests/test_mtd.c
@@ -0,0 +1,229 @@
+/****************************************************************************
+ *
+ *   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 test_mtd.c
+ *
+ * Param storage / file test.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <sys/stat.h>
+#include <poll.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 <drivers/drv_hrt.h>
+
+#include "tests.h"
+
+#define PARAM_FILE_NAME "/fs/mtd_params"
+
+static int check_user_abort(int fd);
+
+int check_user_abort(int fd) {
+	/* check if user wants to abort */
+	char c;
+
+	struct pollfd fds;
+	int ret;
+	fds.fd = 0; /* stdin */
+	fds.events = POLLIN;
+	ret = poll(&fds, 1, 0);
+
+	if (ret > 0) {
+
+		read(0, &c, 1);
+
+		switch (c) {
+		case 0x03: // ctrl-c
+		case 0x1b: // esc
+		case 'c':
+		case 'q':
+		{
+			warnx("Test aborted.");
+			fsync(fd);
+			close(fd);
+			return OK;
+			/* not reached */
+			}
+		}
+	}
+
+	return 1;
+}
+
+void print_fail()
+{
+	printf("<[T]: MTD: FAIL>\n");
+}
+
+void print_success()
+{
+	printf("<[T]: MTD: OK>\n");
+}
+
+int
+test_mtd(int argc, char *argv[])
+{
+	unsigned iterations= 0;
+
+	/* check if microSD card is mounted */
+	struct stat buffer;
+	if (stat(PARAM_FILE_NAME, &buffer)) {
+		warnx("file %s not found, aborting MTD test", PARAM_FILE_NAME);
+		print_fail();
+		return 1;
+	}
+
+	// XXX get real storage space here
+	unsigned file_size = 4096;
+
+	/* perform tests for a range of chunk sizes */
+	unsigned chunk_sizes[] = {256, 512, 4096};
+
+	for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) {
+
+		printf("\n====== FILE TEST: %u bytes chunks ======\n", chunk_sizes[c]);
+
+		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++) {
+			/* this will wrap, but we just need a known value with spacing */
+			write_buf[i] = i+11;
+		}
+
+		uint8_t read_buf[chunk_sizes[c]] __attribute__((aligned(64)));
+		hrt_abstime start, end;
+
+		int fd = open(PARAM_FILE_NAME, O_RDONLY);
+		int rret = read(fd, read_buf, chunk_sizes[c]);
+		close(fd);
+
+		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++) {
+			printf("%02X", read_buf[i]);
+		}
+		printf("\n");
+
+		iterations = file_size / chunk_sizes[c];
+
+		start = hrt_absolute_time();
+		for (unsigned i = 0; i < iterations; i++) {
+			int wret = write(fd, write_buf, chunk_sizes[c]);
+
+			if (wret != (int)chunk_sizes[c]) {
+				warn("WRITE ERROR!");
+				print_fail();
+				return 1;
+			}
+
+			fsync(fd);
+
+			if (!check_user_abort(fd))
+				return OK;
+
+		}
+		end = hrt_absolute_time();
+
+		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]);
+
+			if (rret != chunk_sizes[c]) {
+				warnx("READ ERROR!");
+				print_fail();
+				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", j);
+					print_fail();
+					compare_ok = false;
+					break;
+				}
+			}
+
+			if (!compare_ok) {
+				warnx("ABORTING FURTHER COMPARISON DUE TO ERROR");
+				print_fail();
+				return 1;
+			}
+
+			if (!check_user_abort(fd))
+				return OK;
+
+		}
+
+
+		close(fd);
+
+	}
+
+	/* fill the file with 0xFF to make it look new again */
+	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++) {
+		int ret = write(fd, ffbuf, sizeof(ffbuf));
+
+		if (ret != sizeof(ffbuf)) {
+			warnx("ERROR! Could not fill file with 0xFF");
+			close(fd);
+			print_fail();
+			return 1;
+		}
+	}
+
+	(void)close(fd);
+	print_success();
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/tests_param.c b/src/systemcmds/tests/test_param.c
index 13f17bc43..318d2505b 100644
--- a/src/systemcmds/tests/tests_param.c
+++ b/src/systemcmds/tests/test_param.c
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file tests_param.c
+ * @file test_param.c
  *
  * Tests related to the parameter system.
  */
diff --git a/src/systemcmds/tests/test_ppm_loopback.c b/src/systemcmds/tests/test_ppm_loopback.c
new file mode 100644
index 000000000..b9041b013
--- /dev/null
+++ b/src/systemcmds/tests/test_ppm_loopback.c
@@ -0,0 +1,178 @@
+/****************************************************************************
+ *
+ *   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 test_ppm_loopback.c
+ * Tests the PWM outputs and PPM input
+ *
+ */
+
+#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_pwm_output.h>
+#include <drivers/drv_rc_input.h>
+#include <uORB/topics/rc_channels.h>
+#include <systemlib/err.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+
+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);
+
+	if (servo_fd < 0) {
+		printf("failed opening /dev/pwm_servo\n");
+	}
+
+	printf("Servo readback, pairs of values should match defaults\n");
+
+	unsigned servo_count;
+	result = ioctl(servo_fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
+	if (result != OK) {
+		warnx("PWM_SERVO_GET_COUNT");
+		return ERROR;
+	}
+
+	for (unsigned i = 0; i < servo_count; i++) {
+		result = ioctl(servo_fd, PWM_SERVO_GET(i), (unsigned long)&pos);
+
+		if (result < 0) {
+			printf("failed reading channel %u\n", i);
+		}
+
+		//printf("%u: %u %u\n", i, pos, data[i]);
+
+	}
+
+	// /* tell safety that its ok to disable it with the switch */
+	// result = ioctl(servo_fd, PWM_SERVO_SET_ARM_OK, 0);
+	// if (result != OK)
+	// 	warnx("FAIL: PWM_SERVO_SET_ARM_OK");
+	//  tell output device that the system is armed (it will output values if safety is off) 
+	// result = ioctl(servo_fd, PWM_SERVO_ARM, 0);
+	// if (result != OK)
+	// 	warnx("FAIL: PWM_SERVO_ARM");
+
+	int pwm_values[] = {1200, 1300, 1900, 1700, 1500, 1250, 1800, 1400};
+
+
+	// for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) {
+	// 	result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]);
+
+	// 	if (result) {
+	// 		(void)close(servo_fd);
+	// 		return ERROR;
+	// 	} else {
+	// 		warnx("channel %d set to %d", i, pwm_values[i]);
+	// 	}
+	// }
+
+	warnx("servo count: %d", servo_count);
+
+	struct pwm_output_values pwm_out = {.values = {0}, .channel_count = 0};
+
+	for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) {
+		pwm_out.values[i] = pwm_values[i];
+		//warnx("channel %d: disarmed PWM: %d", i+1, pwm_values[i]);
+		pwm_out.channel_count++;
+	}
+
+	result = ioctl(servo_fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_out);
+
+	/* give driver 10 ms to propagate */
+
+	/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
+	struct rc_input_values	rc_input;
+	orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
+	usleep(100000);
+
+	/* open PPM input and expect values close to the output values */
+
+	bool rc_updated;
+	orb_check(_rc_sub, &rc_updated);
+
+	if (rc_updated) {
+
+		orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
+
+		// int ppm_fd = open(RC_INPUT_DEVICE_PATH, O_RDONLY);
+
+
+
+		// struct rc_input_values rc;
+		// result = read(ppm_fd, &rc, sizeof(rc));
+
+		// if (result != sizeof(rc)) {
+		// 	warnx("Error reading RC output");
+		// 	(void)close(servo_fd);
+		// 	(void)close(ppm_fd);
+		// 	return ERROR;
+		// }
+
+		/* go and check values */
+		for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) {
+			if (fabsf(rc_input.values[i] - pwm_values[i]) > 10) {
+				warnx("comparison fail: RC: %d, expected: %d", rc_input.values[i], pwm_values[i]);
+				(void)close(servo_fd);
+				return ERROR;
+			}
+		}
+	} else {
+		warnx("failed reading RC input data");
+		return ERROR;
+	}
+
+	warnx("PPM LOOPBACK TEST PASSED SUCCESSFULLY!");
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c
new file mode 100644
index 000000000..57c0e7f4c
--- /dev/null
+++ b/src/systemcmds/tests/test_rc.c
@@ -0,0 +1,146 @@
+/****************************************************************************
+ *
+ *   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 test_rc.c
+ * Tests RC input.
+ *
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <poll.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_pwm_output.h>
+#include <drivers/drv_rc_input.h>
+#include <systemlib/err.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+
+int test_rc(int argc, char *argv[])
+{
+
+	int _rc_sub = orb_subscribe(ORB_ID(input_rc));
+
+	/* 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_last;
+	orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
+	usleep(100000);
+
+	/* open PPM input and expect values close to the output values */
+
+	bool rc_updated;
+	orb_check(_rc_sub, &rc_updated);
+
+	warnx("Reading PPM values - press any key to abort");
+	warnx("This test guarantees: 10 Hz update rates, no glitches (channel values), no channel count changes.");
+
+	if (rc_updated) {
+
+		/* copy initial set */
+		for (unsigned i = 0; i < rc_input.channel_count; i++) {
+			rc_last.values[i] = rc_input.values[i];
+		}
+
+		rc_last.channel_count = rc_input.channel_count;
+
+		/* poll descriptor */
+		struct pollfd fds[2];
+		fds[0].fd = _rc_sub;
+		fds[0].events = POLLIN;
+		fds[1].fd = 0;
+		fds[1].events = POLLIN;
+
+		while (true) {
+
+			int ret = poll(fds, 2, 200);
+
+			if (ret > 0) {
+
+				if (fds[0].revents & POLLIN) {
+
+					orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
+
+					/* go and check values */
+					for (unsigned i = 0; i < rc_input.channel_count; i++) {
+						if (fabsf(rc_input.values[i] - rc_last.values[i]) > 20) {
+							warnx("comparison fail: RC: %d, expected: %d", rc_input.values[i], rc_last.values[i]);
+							(void)close(_rc_sub);
+							return ERROR;
+						}
+
+						rc_last.values[i] = rc_input.values[i];
+					}
+
+					if (rc_last.channel_count != rc_input.channel_count) {
+						warnx("channel count mismatch: last: %d, now: %d", rc_last.channel_count, rc_input.channel_count);
+						(void)close(_rc_sub);
+						return ERROR;
+					}
+
+					if (hrt_absolute_time() - rc_input.timestamp_last_signal > 100000) {
+						warnx("TIMEOUT, less than 10 Hz updates");
+						(void)close(_rc_sub);
+						return ERROR;
+					} 
+
+				} else {
+					/* key pressed, bye bye */
+					return 0;
+				}
+
+			}
+		}
+
+	} else {
+		warnx("failed reading RC input data");
+		return ERROR;
+	}
+
+	warnx("PPM CONTINUITY TEST PASSED SUCCESSFULLY!");
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c
index 14503276c..096106ff3 100644
--- a/src/systemcmds/tests/test_sensors.c
+++ b/src/systemcmds/tests/test_sensors.c
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   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
@@ -49,6 +48,8 @@
 #include <fcntl.h>
 #include <errno.h>
 #include <debug.h>
+#include <math.h>
+#include <systemlib/err.h>
 
 #include <arch/board/board.h>
 
@@ -77,6 +78,8 @@ static int accel(int argc, char *argv[]);
 static int gyro(int argc, char *argv[]);
 static int mag(int argc, char *argv[]);
 static int baro(int argc, char *argv[]);
+static int accel1(int argc, char *argv[]);
+static int gyro1(int argc, char *argv[]);
 
 /****************************************************************************
  * Private Data
@@ -91,6 +94,8 @@ struct {
 	{"gyro",	"/dev/gyro",	gyro},
 	{"mag",		"/dev/mag",	mag},
 	{"baro",	"/dev/baro",	baro},
+	{"accel1",	"/dev/accel1",	accel1},
+	{"gyro1",	"/dev/gyro1",	gyro1},
 	{NULL, NULL, NULL}
 };
 
@@ -133,23 +138,58 @@ accel(int argc, char *argv[])
 		printf("\tACCEL accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z);
 	}
 
-	// /* wait at least 10ms, sensor should have data after no more than 2ms */
-	// usleep(100000);
+	if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) {
+		warnx("ACCEL1 acceleration values out of range!");
+		return ERROR;
+	}
+
+	/* Let user know everything is ok */
+	printf("\tOK: ACCEL passed all tests successfully\n");
+	close(fd);
+
+	return OK;
+}
+
+static int
+accel1(int argc, char *argv[])
+{
+	printf("\tACCEL1: test start\n");
+	fflush(stdout);
+
+	int		fd;
+	struct accel_report buf;
+	int		ret;
 
-	// ret = read(fd, buf, sizeof(buf));
+	fd = open("/dev/accel1", O_RDONLY);
 
-	// if (ret != sizeof(buf)) {
-	// 	printf("\tMPU-6000: read2 fail (%d)\n", ret);
-	// 	return ERROR;
+	if (fd < 0) {
+		printf("\tACCEL1: open fail, run <mpu6000 start> or <lsm303d start> first.\n");
+		return ERROR;
+	}
+
+	/* wait at least 100ms, sensor should have data after no more than 20ms */
+	usleep(100000);
+
+	/* read data - expect samples */
+	ret = read(fd, &buf, sizeof(buf));
+
+	if (ret != sizeof(buf)) {
+		printf("\tACCEL1: read1 fail (%d)\n", ret);
+		return ERROR;
 
-	// } else {
-	// 	printf("\tMPU-6000 values: acc: x:%d\ty:%d\tz:%d\tgyro: r:%d\tp:%d\ty:%d\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
-	// }
+	} else {
+		printf("\tACCEL1 accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z);
+	}
 
-	/* XXX more tests here */
+	if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) {
+		warnx("ACCEL1 acceleration values out of range!");
+		return ERROR;
+	}
 
 	/* Let user know everything is ok */
-	printf("\tOK: ACCEL passed all tests successfully\n");
+	printf("\tOK: ACCEL1 passed all tests successfully\n");
+
+	close(fd);
 
 	return OK;
 }
@@ -187,6 +227,45 @@ gyro(int argc, char *argv[])
 
 	/* Let user know everything is ok */
 	printf("\tOK: GYRO passed all tests successfully\n");
+	close(fd);
+
+	return OK;
+}
+
+static int
+gyro1(int argc, char *argv[])
+{
+	printf("\tGYRO1: test start\n");
+	fflush(stdout);
+
+	int		fd;
+	struct gyro_report buf;
+	int		ret;
+
+	fd = open("/dev/gyro1", O_RDONLY);
+
+	if (fd < 0) {
+		printf("\tGYRO1: open fail, run <l3gd20 start> or <mpu6000 start> first.\n");
+		return ERROR;
+	}
+
+	/* wait at least 5 ms, sensor should have data after that */
+	usleep(5000);
+
+	/* read data - expect samples */
+	ret = read(fd, &buf, sizeof(buf));
+
+	if (ret != sizeof(buf)) {
+		printf("\tGYRO1: read fail (%d)\n", ret);
+		return ERROR;
+
+	} else {
+		printf("\tGYRO1 rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z);
+	}
+
+	/* Let user know everything is ok */
+	printf("\tOK: GYRO1 passed all tests successfully\n");
+	close(fd);
 
 	return OK;
 }
@@ -224,6 +303,7 @@ mag(int argc, char *argv[])
 
 	/* Let user know everything is ok */
 	printf("\tOK: MAG passed all tests successfully\n");
+	close(fd);
 
 	return OK;
 }
@@ -261,6 +341,7 @@ baro(int argc, char *argv[])
 
 	/* Let user know everything is ok */
 	printf("\tOK: BARO passed all tests successfully\n");
+	close(fd);
 
 	return OK;
 }
diff --git a/src/systemcmds/tests/test_servo.c b/src/systemcmds/tests/test_servo.c
index f95760ca8..ef8512bf5 100644
--- a/src/systemcmds/tests/test_servo.c
+++ b/src/systemcmds/tests/test_servo.c
@@ -1,7 +1,6 @@
 /****************************************************************************
- * px4/sensors/test_hrt.c
  *
- *  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
@@ -13,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 NuttX nor the names of its contributors may be
+ * 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.
  *
@@ -32,9 +31,11 @@
  *
  ****************************************************************************/
 
-/****************************************************************************
- * Included Files
- ****************************************************************************/
+/**
+ * @file test_servo.c
+ * Tests the servo outputs
+ *
+ */
 
 #include <nuttx/config.h>
 
@@ -55,39 +56,6 @@
 
 #include "tests.h"
 
-/****************************************************************************
- * Pre-processor Definitions
- ****************************************************************************/
-
-/****************************************************************************
- * Private Types
- ****************************************************************************/
-
-/****************************************************************************
- * Private Function Prototypes
- ****************************************************************************/
-
-/****************************************************************************
- * Private Data
- ****************************************************************************/
-
-/****************************************************************************
- * Public Data
- ****************************************************************************/
-
-/****************************************************************************
- * Private Functions
- ****************************************************************************/
-
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-/****************************************************************************
- * Name: test_servo
- ****************************************************************************/
-
 int test_servo(int argc, char *argv[])
 {
 	int fd, result;
@@ -110,7 +78,14 @@ int test_servo(int argc, char *argv[])
 
 	printf("Servo readback, pairs of values should match defaults\n");
 
-	for (unsigned i = 0; i < PWM_OUTPUT_MAX_CHANNELS; i++) {
+	unsigned servo_count;
+	result = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
+	if (result != OK) {
+		warnx("PWM_SERVO_GET_COUNT");
+		return ERROR;
+	}
+
+	for (unsigned i = 0; i < servo_count; i++) {
 		result = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&pos);
 
 		if (result < 0) {
@@ -122,11 +97,20 @@ int test_servo(int argc, char *argv[])
 
 	}
 
-	printf("Servos arming at default values\n");
+	/* tell safety that its ok to disable it with the switch */
+	result = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
+	if (result != OK)
+		warnx("FAIL: PWM_SERVO_SET_ARM_OK");
+	/* tell output device that the system is armed (it will output values if safety is off) */
 	result = ioctl(fd, PWM_SERVO_ARM, 0);
+	if (result != OK)
+		warnx("FAIL: PWM_SERVO_ARM");
+
 	usleep(5000000);
 	printf("Advancing channel 0 to 1500\n");
 	result = ioctl(fd, PWM_SERVO_SET(0), 1500);
+	printf("Advancing channel 1 to 1800\n");
+	result = ioctl(fd, PWM_SERVO_SET(1), 1800);
 out:
 	return 0;
 }
diff --git a/src/systemcmds/tests/test_uart_loopback.c b/src/systemcmds/tests/test_uart_loopback.c
index 3be152004..f1d41739b 100644
--- a/src/systemcmds/tests/test_uart_loopback.c
+++ b/src/systemcmds/tests/test_uart_loopback.c
@@ -1,8 +1,6 @@
 /****************************************************************************
- * px4/sensors/test_gpio.c
  *
- *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *            Lorenz Meier <lm@inf.ethz.ch>
+ *   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
@@ -14,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 NuttX nor the names of its contributors may be
+ * 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.
  *
@@ -33,9 +31,11 @@
  *
  ****************************************************************************/
 
-/****************************************************************************
- * Included Files
- ****************************************************************************/
+/**
+ * @file test_uart_loopback.c
+ * Tests the uart outputs
+ *
+ */
 
 #include <nuttx/config.h>
 
@@ -55,40 +55,6 @@
 #include <math.h>
 #include <float.h>
 
-
-/****************************************************************************
- * Pre-processor Definitions
- ****************************************************************************/
-
-/****************************************************************************
- * Private Types
- ****************************************************************************/
-
-/****************************************************************************
- * Private Function Prototypes
- ****************************************************************************/
-
-/****************************************************************************
- * Private Data
- ****************************************************************************/
-
-/****************************************************************************
- * Public Data
- ****************************************************************************/
-
-/****************************************************************************
- * Private Functions
- ****************************************************************************/
-
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-/****************************************************************************
- * Name: test_led
- ****************************************************************************/
-
 int test_uart_loopback(int argc, char *argv[])
 {
 
@@ -97,11 +63,11 @@ int test_uart_loopback(int argc, char *argv[])
 	int uart5_nwrite = 0;
 	int uart2_nwrite = 0;
 
-	int uart1 = open("/dev/ttyS0", O_RDWR | O_NOCTTY); //
+	/* opening stdout */
+	int stdout_fd = 1;
 
-	/* assuming NuttShell is on UART1 (/dev/ttyS0) */
-	int uart2 = open("/dev/ttyS1", O_RDWR | O_NONBLOCK | O_NOCTTY); //
-	int uart5 = open("/dev/ttyS2", O_RDWR | O_NONBLOCK | O_NOCTTY); //
+	int uart2 = open("/dev/ttyS1", O_RDWR | O_NONBLOCK | O_NOCTTY);
+	int uart5 = open("/dev/ttyS2", O_RDWR | O_NONBLOCK | O_NOCTTY);
 
 	if (uart2 < 0) {
 		printf("ERROR opening UART2, aborting..\n");
@@ -113,7 +79,7 @@ int test_uart_loopback(int argc, char *argv[])
 		exit(uart5);
 	}
 
-	uint8_t sample_uart1[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', '\n'};
+	uint8_t sample_stdout_fd[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', '\n'};
 	uint8_t sample_uart2[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', 0};
 	uint8_t sample_uart5[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', 0};
 
@@ -121,7 +87,7 @@ int test_uart_loopback(int argc, char *argv[])
 
 	for (i = 0; i < 1000; i++) {
 //		printf("TEST #%d\n",i);
-		write(uart1, sample_uart1, sizeof(sample_uart1));
+		write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd));
 
 		/* uart2 -> uart5 */
 		r = write(uart2, sample_uart2, sizeof(sample_uart2));
@@ -130,7 +96,7 @@ int test_uart_loopback(int argc, char *argv[])
 			uart2_nwrite += r;
 
 //		printf("TEST #%d\n",i);
-		write(uart1, sample_uart1, sizeof(sample_uart1));
+		write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd));
 
 		/* uart2 -> uart5 */
 		r = write(uart5, sample_uart5, sizeof(sample_uart5));
@@ -139,7 +105,7 @@ int test_uart_loopback(int argc, char *argv[])
 			uart5_nwrite += r;
 
 //		printf("TEST #%d\n",i);
-		write(uart1, sample_uart1, sizeof(sample_uart1));
+		write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd));
 
 		/* try to read back values */
 		do {
@@ -150,7 +116,7 @@ int test_uart_loopback(int argc, char *argv[])
 		} while (r > 0);
 
 //		printf("TEST #%d\n",i);
-		write(uart1, sample_uart1, sizeof(sample_uart1));
+		write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd));
 
 		do {
 			r = read(uart2, sample_uart5, sizeof(sample_uart5));
@@ -160,7 +126,7 @@ int test_uart_loopback(int argc, char *argv[])
 		} while (r > 0);
 
 //		printf("TEST #%d\n",i);
-//		write(uart1, sample_uart1, sizeof(sample_uart5));
+//		write(stdout_fd, sample_stdout_fd, sizeof(sample_uart5));
 	}
 
 	for (i = 0; i < 200000; i++) {
@@ -181,7 +147,7 @@ int test_uart_loopback(int argc, char *argv[])
 	}
 
 
-	close(uart1);
+	close(stdout_fd);
 	close(uart2);
 	close(uart5);
 
diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h
index c02ea6808..82de05dff 100644
--- a/src/systemcmds/tests/tests.h
+++ b/src/systemcmds/tests/tests.h
@@ -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
@@ -34,6 +34,12 @@
 #ifndef __APPS_PX4_TESTS_H
 #define __APPS_PX4_TESTS_H
 
+/**
+ * @file tests.h
+ * Tests declaration file.
+ *
+ */
+
 /****************************************************************************
  * Included Files
  ****************************************************************************/
@@ -76,6 +82,8 @@
  * Public Function Prototypes
  ****************************************************************************/
 
+__BEGIN_DECLS
+
 extern int	test_sensors(int argc, char *argv[]);
 extern int	test_gpio(int argc, char *argv[]);
 extern int	test_hrt(int argc, char *argv[]);
@@ -86,6 +94,7 @@ extern int	test_int(int argc, char *argv[]);
 extern int	test_float(int argc, char *argv[]);
 extern int	test_ppm(int argc, char *argv[]);
 extern int	test_servo(int argc, char *argv[]);
+extern int	test_ppm_loopback(int argc, char *argv[]);
 extern int	test_uart_loopback(int argc, char *argv[]);
 extern int	test_uart_baudchange(int argc, char *argv[]);
 extern int	test_cpuload(int argc, char *argv[]);
@@ -98,5 +107,12 @@ 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_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[]);
+
+__END_DECLS
 
 #endif /* __APPS_PX4_TESTS_H */
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
index 9f8c5c9ea..77a4df618 100644
--- a/src/systemcmds/tests/tests_main.c
+++ b/src/systemcmds/tests/tests_main.c
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *   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
@@ -35,6 +34,8 @@
 /**
  * @file tests_main.c
  * Tests main file, loads individual tests.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #include <nuttx/config.h>
@@ -58,14 +59,6 @@
 #include "tests.h"
 
 /****************************************************************************
- * Pre-processor Definitions
- ****************************************************************************/
-
-/****************************************************************************
- * Private Types
- ****************************************************************************/
-
-/****************************************************************************
  * Private Function Prototypes
  ****************************************************************************/
 
@@ -94,6 +87,7 @@ const struct {
 	{"hrt",			test_hrt,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"ppm",			test_ppm,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"servo",		test_servo,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"ppm_loopback",	test_ppm_loopback,	OPT_NOALLTEST},
 	{"adc",			test_adc,	OPT_NOJIGTEST},
 	{"jig_voltages",	test_jig_voltages,	OPT_NOALLTEST},
 	{"uart_loopback",	test_uart_loopback,	OPT_NOJIGTEST | OPT_NOALLTEST},
@@ -110,6 +104,11 @@ const struct {
 	{"param",		test_param,	0},
 	{"bson",		test_bson,	0},
 	{"file",		test_file,	0},
+	{"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},
 	{"help",		test_help,	OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
 	{NULL,			NULL, 		0}
 };
diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c
index efe62685c..37e913040 100644
--- a/src/systemcmds/top/top.c
+++ b/src/systemcmds/top/top.c
@@ -51,19 +51,46 @@
 #include <systemlib/cpuload.h>
 #include <drivers/drv_hrt.h>
 
+#define CL "\033[K" // clear line
+
 /**
  * Start the top application.
  */
-__EXPORT int top_main(int argc, char *argv[]);
+__EXPORT int top_main(void);
 
 extern struct system_load_s system_load;
 
-bool top_sigusr1_rcvd = false;
-
-int top_main(int argc, char *argv[])
+static const char *
+tstate_name(const tstate_t s)
 {
-	int t;
+	switch (s) {
+	case TSTATE_TASK_INVALID:    return "init";
+
+	case TSTATE_TASK_PENDING:    return "PEND";
+	case TSTATE_TASK_READYTORUN: return "READY";
+	case TSTATE_TASK_RUNNING:    return "RUN";
+
+	case TSTATE_TASK_INACTIVE:   return "inact";
+	case TSTATE_WAIT_SEM:        return "w:sem";
+#ifndef CONFIG_DISABLE_SIGNALS
+	case TSTATE_WAIT_SIG:        return "w:sig";
+#endif
+#ifndef CONFIG_DISABLE_MQUEUE
+	case TSTATE_WAIT_MQNOTEMPTY: return "w:mqe";
+	case TSTATE_WAIT_MQNOTFULL:  return "w:mqf";
+#endif
+#ifdef CONFIG_PAGING
+	case TSTATE_WAIT_PAGEFILL:   return "w:pgf";
+#endif
+
+	default:
+		return "ERROR";
+	}
+}
 
+int
+top_main(void)
+{
 	uint64_t total_user_time = 0;
 
 	int running_count = 0;
@@ -75,24 +102,21 @@ int top_main(int argc, char *argv[])
 	uint64_t last_times[CONFIG_MAX_TASKS];
 	float curr_loads[CONFIG_MAX_TASKS];
 
-	for (t = 0; t < CONFIG_MAX_TASKS; t++)
+	for (int t = 0; t < CONFIG_MAX_TASKS; t++)
 		last_times[t] = 0;
 
 	float interval_time_ms_inv = 0.f;
 
-	/* Open console directly to grab CTRL-C signal */
-	int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
-
-	while (true)
-//	for (t = 0; t < 10; t++)
-	{
-		int i;
+	/* clear screen */
+	printf("\033[2J");
 
-		uint64_t curr_time_ms = (hrt_absolute_time() / 1000LLU);
-		unsigned int curr_time_s = curr_time_ms / 1000LLU;
+	for (;;) {
+		int   i;
+		uint64_t curr_time_us;
+		uint64_t idle_time_us;
 
-		uint64_t idle_time_total_ms = (system_load.tasks[0].total_runtime / 1000LLU);
-		unsigned int idle_time_total_s = idle_time_total_ms / 1000LLU;
+		curr_time_us = hrt_absolute_time();
+		idle_time_us = system_load.tasks[0].total_runtime;
 
 		if (new_time > interval_start_time)
 			interval_time_ms_inv = 1.f / ((float)((new_time - interval_start_time) / 1000));
@@ -102,7 +126,38 @@ int top_main(int argc, char *argv[])
 		total_user_time = 0;
 
 		for (i = 0; i < CONFIG_MAX_TASKS; i++) {
-			uint64_t interval_runtime = (system_load.tasks[i].valid && last_times[i] > 0 && system_load.tasks[i].total_runtime > last_times[i]) ? (system_load.tasks[i].total_runtime - last_times[i]) / 1000 : 0;
+			uint64_t interval_runtime;
+
+			if (system_load.tasks[i].valid) {
+				switch (system_load.tasks[i].tcb->task_state) {
+				case TSTATE_TASK_PENDING:
+				case TSTATE_TASK_READYTORUN:
+				case TSTATE_TASK_RUNNING:
+					running_count++;
+					break;
+
+				case TSTATE_TASK_INVALID:
+				case TSTATE_TASK_INACTIVE:
+				case TSTATE_WAIT_SEM:
+#ifndef CONFIG_DISABLE_SIGNALS
+				case TSTATE_WAIT_SIG:
+#endif
+#ifndef CONFIG_DISABLE_MQUEUE
+				case TSTATE_WAIT_MQNOTEMPTY:
+				case TSTATE_WAIT_MQNOTFULL:
+#endif
+#ifdef CONFIG_PAGING
+				case TSTATE_WAIT_PAGEFILL:
+#endif
+					blocked_count++;
+					break;
+				}
+			}
+
+			interval_runtime = (system_load.tasks[i].valid && last_times[i] > 0 &&
+								system_load.tasks[i].total_runtime > last_times[i])
+				? (system_load.tasks[i].total_runtime - last_times[i]) / 1000
+				: 0;
 
 			last_times[i] = system_load.tasks[i].total_runtime;
 
@@ -111,7 +166,6 @@ int top_main(int argc, char *argv[])
 
 				if (i > 0)
 					total_user_time += interval_runtime;
-
 			} else
 				curr_loads[i] = 0;
 		}
@@ -119,127 +173,106 @@ int top_main(int argc, char *argv[])
 		for (i = 0; i < CONFIG_MAX_TASKS; i++) {
 			if (system_load.tasks[i].valid && (new_time > interval_start_time)) {
 				if (system_load.tasks[i].tcb->pid == 0) {
-					float idle = curr_loads[0];
-					float task_load = (float)(total_user_time) * interval_time_ms_inv;
+					float idle;
+					float task_load;
+					float sched_load;
+
+					idle = curr_loads[0];
+					task_load = (float)(total_user_time) * interval_time_ms_inv;
 
-					if (task_load > (1.f - idle)) task_load = (1.f - idle);	/* this can happen if one tasks total runtime was not computed correctly by the scheduler instrumentation TODO */
+					/* this can happen if one tasks total runtime was not computed
+					   correctly by the scheduler instrumentation TODO */
+					if (task_load > (1.f - idle))
+						task_load = (1.f - idle);
 
-					float sched_load = 1.f - idle - task_load;
+					sched_load = 1.f - idle - task_load;
 
 					/* print system information */
-					printf("\033[H"); /* cursor home */
-					printf("\033[KProcesses: %d total, %d running, %d sleeping\n", system_load.total_count, running_count, blocked_count);
-					printf("\033[KCPU usage: %d.%02d%% tasks, %d.%02d%% sched, %d.%02d%% idle\n", (int)(task_load * 100), (int)((task_load * 10000.0f) - (int)(task_load * 100.0f) * 100), (int)(sched_load * 100), (int)((sched_load * 10000.0f) - (int)(sched_load * 100.0f) * 100), (int)(idle * 100), (int)((idle * 10000.0f) - ((int)(idle * 100)) * 100));
-					printf("\033[KUptime: %u.%03u s total, %d.%03d s idle\n\033[K\n", curr_time_s, (unsigned int)(curr_time_ms - curr_time_s * 1000LLU), idle_time_total_s, (int)(idle_time_total_ms - idle_time_total_s * 1000));
-
-					/* 34 chars command name length (32 chars plus two spaces) */
-					char header_spaces[CONFIG_TASK_NAME_SIZE + 1];
-					memset(header_spaces, ' ', CONFIG_TASK_NAME_SIZE);
-					header_spaces[CONFIG_TASK_NAME_SIZE] = '\0';
+					printf("\033[H"); /* move cursor home and clear screen */
+					printf(CL "Processes: %d total, %d running, %d sleeping\n",
+						   system_load.total_count,
+						   running_count,
+						   blocked_count);
+					printf(CL "CPU usage: %.2f%% tasks, %.2f%% sched, %.2f%% idle\n",
+						   (double)(task_load * 100.f),
+						   (double)(sched_load * 100.f),
+						   (double)(idle * 100.f));
+					printf(CL "Uptime: %.3fs total, %.3fs idle\n\n",
+						   (double)curr_time_us / 1000000.d,
+						   (double)idle_time_us / 1000000.d);
+
+					/* header for task list */
+					printf(CL "%4s %*-s %8s %6s %11s %10s %-6s\n",
+						   "PID",
+						   CONFIG_TASK_NAME_SIZE, "COMMAND",
+						   "CPU(ms)",
+						   "CPU(%)",
+						   "USED/STACK",
+						   "PRIO(BASE)",
 #if CONFIG_RR_INTERVAL > 0
-					printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tSTACK USE\tCURR (BASE) PRIO\tRR SLICE\n", header_spaces);
+						   "TSLICE"
 #else
-					printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tSTACK USE\tCURR (BASE) PRIO\n", header_spaces);
+						   "STATE"
 #endif
+						   );
+				}
 
-				} else {
-					enum tstate_e task_state = (enum tstate_e)system_load.tasks[i].tcb->task_state;
-
-					if (task_state == TSTATE_TASK_PENDING ||
-					    task_state == TSTATE_TASK_READYTORUN ||
-					    task_state == TSTATE_TASK_RUNNING) {
-						running_count++;
-					}
-
-					if (task_state == TSTATE_TASK_INACTIVE ||      /* BLOCKED      - Initialized but not yet activated */
-					    task_state == TSTATE_WAIT_SEM           /* BLOCKED      - Waiting for a semaphore */
-#ifndef CONFIG_DISABLE_SIGNALS
-					    || task_state == TSTATE_WAIT_SIG           /* BLOCKED      - Waiting for a signal */
-#endif
-#ifndef CONFIG_DISABLE_MQUEUE
-					    || task_state == TSTATE_WAIT_MQNOTEMPTY     /* BLOCKED      - Waiting for a MQ to become not empty. */
-					    || task_state == TSTATE_WAIT_MQNOTFULL      /* BLOCKED      - Waiting for a MQ to become not full. */
-#endif
-#ifdef CONFIG_PAGING
-					    || task_state == TSTATE_WAIT_PAGEFILL       /* BLOCKED      - Waiting for page fill */
-#endif
-					   ) {
-						blocked_count++;
-					}
-
-					char spaces[CONFIG_TASK_NAME_SIZE + 2];
-
-					/* count name len */
-					int namelen = 0;
-
-					while (namelen < CONFIG_TASK_NAME_SIZE) {
-						if (system_load.tasks[i].tcb->name[namelen] == '\0') break;
-
-						namelen++;
-					}
-
-					int s = 0;
-
-					for (s = 0; s < CONFIG_TASK_NAME_SIZE + 2 - namelen; s++) {
-						spaces[s] = ' ';
-					}
-
-					spaces[s] = '\0';
-
-					char *runtime_spaces = "  ";
-
-					if ((system_load.tasks[i].total_runtime / 1000) < 99) {
-						runtime_spaces = "";
-					}
+				unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr -
+					(uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr;
+				unsigned stack_free = 0;
+				uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr;
 
-					unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr -
-							      (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr;
-					unsigned stack_free = 0;
-					uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr;
+				while (stack_free < stack_size) {
+					if (*stack_sweeper++ != 0xff)
+						break;
 
-					while (stack_free < stack_size) {
-						if (*stack_sweeper++ != 0xff)
-							break;
+					stack_free++;
+				}
 
-						stack_free++;
-					}
+				printf(CL "%4d %*-s %8lld %2d.%03d %5u/%5u %3u (%3u) ",
+					   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.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,
+					   system_load.tasks[i].tcb->base_priority);
 
-					printf("\033[K % 2d\t%s%s % 8lld ms%s  \t % 2d.%03d \t % 4u / % 4u",
-					       (int)system_load.tasks[i].tcb->pid,
-					       system_load.tasks[i].tcb->name,
-					       spaces,
-					       (system_load.tasks[i].total_runtime / 1000),
-					       runtime_spaces,
-					       (int)(curr_loads[i] * 100),
-					       (int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100),
-					       stack_size - stack_free,
-					       stack_size);
-					/* Print scheduling info with RR time slice */
 #if CONFIG_RR_INTERVAL > 0
-					printf("\t%d\t(%d)\t\t%d\n", (int)system_load.tasks[i].tcb->sched_priority, (int)system_load.tasks[i].tcb->base_priority, (int)system_load.tasks[i].tcb->timeslice);
+				/* print scheduling info with RR time slice */
+				printf(" %6d\n", system_load.tasks[i].tcb->timeslice);
 #else
-					/* Print scheduling info without time slice*/
-					printf("\t%d (%d)\n", (int)system_load.tasks[i].tcb->sched_priority, (int)system_load.tasks[i].tcb->base_priority);
+				// print task state instead
+				printf(" %-6s\n", tstate_name(system_load.tasks[i].tcb->task_state));
 #endif
-				}
 			}
 		}
 
-		printf("\033[K[ Hit Ctrl-C to quit. ]\n\033[J");
-		fflush(stdout);
-
 		interval_start_time = new_time;
 
-		char c;
+		/* Sleep 200 ms waiting for user input five times ~ 1s */
+		for (int k = 0; k < 5; k++) {
+			char c;
+
+			struct pollfd fds;
+			int ret;
+			fds.fd = 0; /* stdin */
+			fds.events = POLLIN;
+			ret = poll(&fds, 1, 0);
 
-		/* Sleep 200 ms waiting for user input four times */
-		/* XXX use poll ... */
-		for (int k = 0; k < 4; k++) {
-			if (read(console, &c, 1) == 1) {
-				if (c == 0x03 || c == 0x63) {
-					printf("Abort\n");
-					close(console);
+			if (ret > 0) {
+
+				read(0, &c, 1);
+
+				switch (c) {
+				case 0x03: // ctrl-c
+				case 0x1b: // esc
+				case 'c':
+				case 'q':
 					return OK;
+					/* not reached */
 				}
 			}
 
@@ -249,7 +282,5 @@ int top_main(int argc, char *argv[])
 		new_time = hrt_absolute_time();
 	}
 
-	close(console);
-
 	return OK;
 }