Merge branch 'master' into takeoff_fix

author: Anton Babushkin <anton.babushkin@me.com> 2014-09-28 13:41:21 +0400
committer: Anton Babushkin <anton.babushkin@me.com> 2014-09-28 13:41:21 +0400
commit: 1cc5f3778dd6c18f3a14c64959d28f978b0305ef (patch)
tree: f2b8138e6f85b5f760fa22bbe3aecbce61539277
parent: cf4604f5c3614bc5cd1b82dfee693cbac36181ab (diff)
parent: 8a18cfa3869555389e7e9ff8f104d83f9c54cb43 (diff)
download: px4-firmware-1cc5f3778dd6c18f3a14c64959d28f978b0305ef.tar.gz
px4-firmware-1cc5f3778dd6c18f3a14c64959d28f978b0305ef.tar.bz2
px4-firmware-1cc5f3778dd6c18f3a14c64959d28f978b0305ef.zip
115 files changed, 5023 insertions, 1294 deletions
diff --git a/LICENSE.md b/LICENSE.md
new file mode 100644
index 000000000..2ad83eba4
--- /dev/null
+++ b/LICENSE.md
@@ -0,0 +1,41 @@
+The PX4 firmware is licensed generally under a permissive 3-clause BSD license. Contributions are required
+to be made under the same license. Any exception to this general rule is listed below.
+
+	/****************************************************************************
+	 *
+	 *   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.
+	 *
+	 ****************************************************************************/
+
+
+  - PX4 middleware: BSD 3-clause
+  - PX4 flight control stack: BSD 3-clause
+  - NuttX operating system: BSD 3-clause
+  - Exceptions: Currently only this [400 LOC file](https://github.com/PX4/Firmware/blob/master/src/lib/external_lgpl/tecs/tecs.cpp) remains LGPL, but will be replaced with a BSD implementation.
+\ No newline at end of file
diff --git a/NuttX b/NuttX
-Subproject 088146b90eee5b614ea6386a64dae343a49a517
+Subproject 41fffa0df192e4a26d2325c7ac5d9b5d7ba0211
diff --git a/README.md b/README.md
new file mode 100644
index 000000000..84fb02e3b
--- /dev/null
+++ b/README.md
@@ -0,0 +1,10 @@
+## PX4 Aerial Middleware and Flight Control Stack ##
+
+*   Official Website: http://px4.io
+*   License: BSD 3-clause (see LICENSE.md)
+*   Supported airframes:
+  * Multicopters
+  * Fixed wing
+*   Binaries (always up-to-date from master):
+  * [Downloads](https://pixhawk.org/downloads)
+*   Mailing list: [Google Groups](http://groups.google.com/group/px4users)
diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom
index 31dfe7100..53c48d8aa 100644
--- a/ROMFS/px4fmu_common/init.d/3031_phantom
+++ b/ROMFS/px4fmu_common/init.d/3031_phantom
@@ -30,6 +30,9 @@ then
 	param set FW_RR_P 0.08
 	param set FW_R_LIM 50
 	param set FW_R_RMAX 0
+	# Bottom of bay and nominal zero-pitch attitude differ
+	# the payload bay is pitched up about 7 degrees
+	param set SENS_BOARD_Y_OFF 7.0
 fi
 
 set MIXER phantom
diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper
index a4c1e832d..3714b612f 100644
--- a/ROMFS/px4fmu_common/init.d/3035_viper
+++ b/ROMFS/px4fmu_common/init.d/3035_viper
@@ -8,3 +8,5 @@
 sh /etc/init.d/rc.fw_defaults
 
 set MIXER Viper
+
+set FAILSAFE "-c567 -p 1000"
diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface
index 1de0abb58..e44cd0953 100644
--- a/ROMFS/px4fmu_common/init.d/rc.interface
+++ b/ROMFS/px4fmu_common/init.d/rc.interface
@@ -77,4 +77,9 @@ then
 			pwm max -c $PWM_OUTPUTS -p $PWM_MAX
 		fi
 	fi
+
+	if [ $FAILSAFE != none ]
+	then
+		pwm failsafe -d $OUTPUT_DEV $FAILSAFE
+	fi
 fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.uavcan b/ROMFS/px4fmu_common/init.d/rc.uavcan
new file mode 100644
index 000000000..9a470a6b8
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.uavcan
@@ -0,0 +1,18 @@
+#!nsh
+#
+# UAVCAN initialization script.
+#
+
+if param compare UAVCAN_ENABLE 1
+then
+	if uavcan start
+	then
+		# First sensor publisher to initialize takes lowest instance ID
+		# This delay ensures that UAVCAN-interfaced sensors would be allocated on lowest instance IDs
+		sleep 1
+		echo "[init] UAVCAN started"
+	else
+		echo "[init] ERROR: Could not start UAVCAN"
+		tone_alarm $TUNE_OUT_ERROR
+	fi
+fi
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index c9e6a27ca..ea04ece34 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -66,6 +66,9 @@ then
 	#
 	sercon
 
+	# Try to get an USB console
+	nshterm /dev/ttyACM0 &
+
 	#
 	# Start the ORB (first app to start)
 	#
@@ -96,11 +99,9 @@ then
 	#
 	if rgbled start
 	then
-		echo "[init] RGB Led"
 	else
 		if blinkm start
 		then
-			echo "[init] BlinkM"
 			blinkm systemstate
 		fi
 	fi
@@ -129,6 +130,7 @@ then
 	set LOAD_DEFAULT_APPS yes
 	set GPS yes
 	set GPS_FAKE no
+	set FAILSAFE none
 
 	#
 	# Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts
@@ -279,9 +281,6 @@ then
 		fi
 	fi
 
-	# Try to get an USB console
-	nshterm /dev/ttyACM0 &
-
 	#
 	# Start the datamanager (and do not abort boot if it fails)
 	#
@@ -304,11 +303,10 @@ then
 	then
 		if [ $OUTPUT_MODE == uavcan_esc ]
 		then
-			if uavcan start 1
+			if param compare UAVCAN_ENABLE 0
 			then
-				echo "CAN UP"
-			else
-				echo "CAN ERR"
+				echo "[init] OVERRIDING UAVCAN_ENABLE = 1"
+				param set UAVCAN_ENABLE 1
 			fi
 		fi
 
@@ -448,6 +446,11 @@ then
 	mavlink start $MAVLINK_FLAGS
 
 	#
+	# UAVCAN
+	#
+	sh /etc/init.d/rc.uavcan
+
+	#
 	# Sensors, Logging, GPS
 	#
 	sh /etc/init.d/rc.sensors
diff --git a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix
index 0ec663e35..7fed488af 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix
@@ -64,21 +64,22 @@ O:      10000  10000      0 -10000  10000
 S: 0 3      0  20000 -10000 -10000  10000
 
 
-Gimbal / flaps / payload mixer for last four channels
+Gimbal / flaps / payload mixer for last four channels,
+using the payload control group
 -----------------------------------------------------
 
 M: 1
 O:      10000  10000      0 -10000  10000
-S: 0 4  10000  10000      0 -10000  10000
+S: 2 0  10000  10000      0 -10000  10000
 
 M: 1
 O:      10000  10000      0 -10000  10000
-S: 0 5  10000  10000      0 -10000  10000
+S: 2 1  10000  10000      0 -10000  10000
 
 M: 1
 O:      10000  10000      0 -10000  10000
-S: 0 6  10000  10000      0 -10000  10000
+S: 2 2  10000  10000      0 -10000  10000
 
 M: 1
 O:      10000  10000      0 -10000  10000
-S: 0 7  10000  10000      0 -10000  10000
+S: 2 3  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/Viper.mix b/ROMFS/px4fmu_common/mixers/Viper.mix
index 79c4447be..5a0381bd8 100755
--- a/ROMFS/px4fmu_common/mixers/Viper.mix
+++ b/ROMFS/px4fmu_common/mixers/Viper.mix
@@ -52,21 +52,20 @@ M: 1
 O:      10000  10000      0 -10000  10000
 S: 0 3      0  20000 -10000 -10000  10000
 
-Gimbal / flaps / payload mixer for last four channels
+Inputs to the mixer come from channel group 2 (payload), channels 0
+(bay servo 1), 1 (bay servo 2) and 3 (drop release).
 -----------------------------------------------------
 
 M: 1
 O:      10000  10000      0 -10000  10000
-S: 0 4  10000  10000      0 -10000  10000
+S: 2 0  10000  10000      0 -10000  10000
 
 M: 1
 O:      10000  10000      0 -10000  10000
-S: 0 5  10000  10000      0 -10000  10000
+S: 2 1  10000  10000      0 -10000  10000
 
 M: 1
 O:      10000  10000      0 -10000  10000
-S: 0 6  10000  10000      0 -10000  10000
+S: 2 2 -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_test/unit_test_data/mavlink_tests/empty_dir/.gitignore b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/empty_dir/.gitignore
new file mode 100644
index 000000000..e69de29bb
--- /dev/null
+++ b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/empty_dir/.gitignore
diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_238.data b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_238.data
new file mode 100644
index 000000000..973de16e5
--- /dev/null
+++ b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_238.data
diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_239.data b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_239.data
new file mode 100644
index 000000000..7e8764c03
--- /dev/null
+++ b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_239.data
diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_240.data b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_240.data
new file mode 100644
index 000000000..f3fe31687
--- /dev/null
+++ b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_240.data
diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py
index d8f4884bc..b46db00b5 100755
--- a/Tools/px_uploader.py
+++ b/Tools/px_uploader.py
@@ -178,9 +178,9 @@ class uploader(object):
         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):
+        def __init__(self, portname, baudrate, interCharTimeout=0.001, timeout=0.5):
                 # open the port, keep the default timeout short so we can poll quickly
-                self.port = serial.Serial(portname, baudrate, timeout=2.0)
+                self.port = serial.Serial(portname, baudrate)
                 self.otp = b''
                 self.sn = b''
 
@@ -195,7 +195,7 @@ class uploader(object):
         def __recv(self, count=1):
                 c = self.port.read(count)
                 if len(c) < 1:
-                        raise RuntimeError("timeout waiting for data")
+                        raise RuntimeError("timeout waiting for data (%u bytes)", count)
 #               print("recv " + binascii.hexlify(c))
                 return c
 
diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index a7c10f52f..6f39b56a1 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -25,11 +25,8 @@ MODULES		+= drivers/mpu6000
 MODULES		+= drivers/hmc5883
 MODULES		+= drivers/ms5611
 MODULES		+= drivers/mb12xx
-MODULES		+= drivers/ll40ls
 MODULES		+= drivers/gps
 MODULES		+= drivers/hil
-MODULES		+= drivers/hott/hott_telemetry
-MODULES		+= drivers/hott/hott_sensors
 MODULES		+= drivers/blinkm
 MODULES		+= drivers/rgbled
 MODULES		+= drivers/mkblctrl
@@ -43,8 +40,6 @@ MODULES		+= modules/sensors
 # System commands
 #
 MODULES		+= systemcmds/mtd
-MODULES		+= systemcmds/bl_update
-MODULES		+= systemcmds/i2c
 MODULES		+= systemcmds/mixer
 MODULES		+= systemcmds/param
 MODULES		+= systemcmds/perf
@@ -152,5 +147,4 @@ endef
 #                  command                 priority                   stack  entrypoint
 BUILTIN_COMMANDS := \
 	$(call _B, sercon,                 ,                          2048,  sercon_main                ) \
-	$(call _B, serdis,                 ,                          2048,  serdis_main                ) \
-	$(call _B, sysinfo,                ,                          2048,  sysinfo_main               )
+	$(call _B, serdis,                 ,                          2048,  serdis_main                )
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index d0a40445d..5f146686c 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -41,6 +41,7 @@ MODULES		+= drivers/frsky_telemetry
 MODULES		+= modules/sensors
 MODULES		+= drivers/mkblctrl
 MODULES		+= drivers/pca8574
+MODULES		+= drivers/px4flow
 
 
 # Needs to be burned to the ground and re-written; for now,
diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk
index 932f92261..a41670a77 100644
--- a/makefiles/config_px4fmu-v2_test.mk
+++ b/makefiles/config_px4fmu-v2_test.mk
@@ -54,6 +54,9 @@ MODULES		+= lib/conversion
 #
 LIBRARIES	+= lib/mathlib/CMSIS
 
+MODULES 	+= modules/unit_test
+MODULES		+= modules/mavlink/mavlink_tests
+
 #
 # Transitional support - add commands from the NuttX export archive.
 #
diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0
-Subproject 4d7487c2bc5f5ccf87bca82970fb2c08d6d8bd5
+Subproject 2423e47b4f9169e77f7194b36fa2118e018c94e
diff --git a/nuttx-configs/aerocore/nsh/defconfig b/nuttx-configs/aerocore/nsh/defconfig
index 8d0bae7b9..317194f68 100644
--- a/nuttx-configs/aerocore/nsh/defconfig
+++ b/nuttx-configs/aerocore/nsh/defconfig
@@ -314,7 +314,7 @@ CONFIG_STM32_I2CTIMEOTICKS=500
 # CONFIG_ARCH_NOINTC is not set
 # CONFIG_ARCH_VECNOTIRQ is not set
 CONFIG_ARCH_DMA=y
-CONFIG_ARCH_IRQPRIO=y
+# CONFIG_ARCH_IRQPRIO is not set
 # CONFIG_CUSTOM_STACK is not set
 # CONFIG_ADDRENV is not set
 CONFIG_ARCH_HAVE_VFORK=y
diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig
index a651faffa..cd9c05b13 100644
--- a/nuttx-configs/px4fmu-v1/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v1/nsh/defconfig
@@ -287,8 +287,7 @@ CONFIG_STM32_USART_SINGLEWIRE=y
 #
 # CONFIG_STM32_I2C_DYNTIMEO is not set
 CONFIG_STM32_I2CTIMEOSEC=0
-CONFIG_STM32_I2CTIMEOMS=10
-CONFIG_STM32_I2CTIMEOTICKS=500
+CONFIG_STM32_I2CTIMEOMS=1
 # CONFIG_STM32_I2C_DUTY16_9 is not set
 
 #
@@ -309,7 +308,7 @@ CONFIG_STM32_I2CTIMEOTICKS=500
 # CONFIG_ARCH_NOINTC is not set
 # CONFIG_ARCH_VECNOTIRQ is not set
 CONFIG_ARCH_DMA=y
-CONFIG_ARCH_IRQPRIO=y
+# CONFIG_ARCH_IRQPRIO is not set
 # CONFIG_CUSTOM_STACK is not set
 # CONFIG_ADDRENV is not set
 CONFIG_ARCH_HAVE_VFORK=y
diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig
index a55c95a29..e31f40cd2 100644
--- a/nuttx-configs/px4fmu-v2/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v2/nsh/defconfig
@@ -323,8 +323,7 @@ CONFIG_STM32_USART_SINGLEWIRE=y
 #
 # CONFIG_STM32_I2C_DYNTIMEO is not set
 CONFIG_STM32_I2CTIMEOSEC=0
-CONFIG_STM32_I2CTIMEOMS=10
-CONFIG_STM32_I2CTIMEOTICKS=500
+CONFIG_STM32_I2CTIMEOMS=1
 # CONFIG_STM32_I2C_DUTY16_9 is not set
 
 #
@@ -350,7 +349,7 @@ CONFIG_SDIO_PRI=128
 # CONFIG_ARCH_NOINTC is not set
 # CONFIG_ARCH_VECNOTIRQ is not set
 CONFIG_ARCH_DMA=y
-CONFIG_ARCH_IRQPRIO=y
+# CONFIG_ARCH_IRQPRIO is not set
 # CONFIG_CUSTOM_STACK is not set
 # CONFIG_ADDRENV is not set
 CONFIG_ARCH_HAVE_VFORK=y
diff --git a/nuttx-configs/px4io-v1/nsh/defconfig b/nuttx-configs/px4io-v1/nsh/defconfig
index 3785e0367..7c76be7ec 100755
--- a/nuttx-configs/px4io-v1/nsh/defconfig
+++ b/nuttx-configs/px4io-v1/nsh/defconfig
@@ -83,7 +83,6 @@ 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
@@ -134,6 +133,8 @@ CONFIG_STM32_USART2=y
 CONFIG_STM32_USART3=y
 CONFIG_STM32_I2C1=y
 CONFIG_STM32_I2C2=n
+CONFIG_STM32_I2CTIMEOSEC=0
+CONFIG_STM32_I2CTIMEOMS=1
 CONFIG_STM32_BKP=n
 CONFIG_STM32_PWR=n
 CONFIG_STM32_DAC=n
diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig
index e6563e587..02b51e3d7 100755
--- a/nuttx-configs/px4io-v2/nsh/defconfig
+++ b/nuttx-configs/px4io-v2/nsh/defconfig
@@ -79,7 +79,6 @@ 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
diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
index 41942aacd..293690d27 100644
--- a/src/drivers/airspeed/airspeed.cpp
+++ b/src/drivers/airspeed/airspeed.cpp
@@ -165,7 +165,7 @@ Airspeed::probe()
 	*/
 	_retries = 4;
 	int ret = measure();
-	_retries = 2;
+	_retries = 0;
 	return ret;
 }
 
@@ -381,7 +381,10 @@ Airspeed::cycle_trampoline(void *arg)
 	Airspeed *dev = (Airspeed *)arg;
 
 	dev->cycle();
-	dev->update_status();
+	// XXX we do not know if this is
+	// really helping - do not update the
+	// subsys state right now
+	//dev->update_status();
 }
 
 void
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 84815fdfb..bc586f395 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -95,6 +95,11 @@ __BEGIN_DECLS
 #define PWM_LOWEST_MAX 1700
 
 /**
+ * Do not output a channel with this value
+ */
+#define PWM_IGNORE_THIS_CHANNEL UINT16_MAX
+
+/**
  * Servo output signal type, value is actual servo output pulse
  * width in microseconds.
  */
@@ -200,10 +205,13 @@ ORB_DECLARE(output_pwm);
 #define PWM_SERVO_GET_DISABLE_LOCKDOWN		_IOC(_PWM_SERVO_BASE, 22)
 
 /** force safety switch off (to disable use of safety switch) */
-#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23)
+#define PWM_SERVO_SET_FORCE_SAFETY_OFF		_IOC(_PWM_SERVO_BASE, 23)
 
 /** force failsafe mode (failsafe values are set immediately even if failsafe condition not met) */
-#define PWM_SERVO_SET_FORCE_FAILSAFE _IOC(_PWM_SERVO_BASE, 24)
+#define PWM_SERVO_SET_FORCE_FAILSAFE		_IOC(_PWM_SERVO_BASE, 24)
+
+/** make failsafe non-recoverable (termination) if it occurs */
+#define PWM_SERVO_SET_TERMINATION_FAILSAFE	_IOC(_PWM_SERVO_BASE, 25)
 
 /*
  *
diff --git a/src/drivers/drv_px4flow.h b/src/drivers/drv_px4flow.h
index 76ec55c3e..ab640837b 100644
--- a/src/drivers/drv_px4flow.h
+++ b/src/drivers/drv_px4flow.h
@@ -46,37 +46,6 @@
 
 #define PX4FLOW_DEVICE_PATH	"/dev/px4flow"
 
-/**
- * @addtogroup topics
- * @{
- */
-
-/**
- * Optical flow in NED body frame in SI units.
- *
- * @see http://en.wikipedia.org/wiki/International_System_of_Units
- *
- * @warning If possible the usage of the raw flow and performing rotation-compensation
- *		using the autopilot angular rate estimate is recommended.
- */
-struct px4flow_report {
-
-	uint64_t timestamp;		/**< in microseconds since system start          */
-
-	int16_t flow_raw_x;		/**< flow in pixels in X direction, not rotation-compensated */
-	int16_t flow_raw_y;		/**< flow in pixels in Y direction, not rotation-compensated */
-	float flow_comp_x_m;		/**< speed over ground in meters per second, rotation-compensated */
-	float flow_comp_y_m;		/**< speed over ground in meters per second, rotation-compensated */
-	float ground_distance_m;	/**< Altitude / distance to ground in meters */
-	uint8_t	quality;		/**< Quality of the measurement, 0: bad quality, 255: maximum quality */
-	uint8_t sensor_id;		/**< id of the sensor emitting the flow value */
-
-};
-
-/**
- * @}
- */
-
 /*
  * ObjDev tag for px4flow data.
  */
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index f98d615a2..0f77bb805 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -155,7 +155,6 @@ ETSAirspeed::collect()
 	}
 
 	uint16_t diff_pres_pa_raw = val[1] << 8 | val[0];
-	uint16_t diff_pres_pa;
         if (diff_pres_pa_raw == 0) {
 		// a zero value means the pressure sensor cannot give us a
 		// value. We need to return, and not report a value or the
@@ -166,28 +165,21 @@ ETSAirspeed::collect()
 		return -1;
         }
 
-	if (diff_pres_pa_raw < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
-		diff_pres_pa = 0;
-	} else {
-		diff_pres_pa = diff_pres_pa_raw - _diff_pres_offset;
-	}
-
 	// The raw value still should be compensated for the known offset
 	diff_pres_pa_raw -= _diff_pres_offset;
 
 	// Track maximum differential pressure measured (so we can work out top speed).
-	if (diff_pres_pa > _max_differential_pressure_pa) {
-		_max_differential_pressure_pa = diff_pres_pa;
+	if (diff_pres_pa_raw > _max_differential_pressure_pa) {
+		_max_differential_pressure_pa = diff_pres_pa_raw;
 	}
 
 	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;
 
 	// XXX we may want to smooth out the readings to remove noise.
-	report.differential_pressure_filtered_pa = (float)diff_pres_pa;
-	report.differential_pressure_raw_pa = (float)diff_pres_pa_raw;
+	report.differential_pressure_filtered_pa = diff_pres_pa_raw;
+	report.differential_pressure_raw_pa = diff_pres_pa_raw;
 	report.temperature = -1000.0f;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;
 
@@ -369,7 +361,7 @@ test()
 		err(1, "immediate read failed");
 
 	warnx("single read");
-	warnx("diff pressure: %f pa", (double)report.differential_pressure_pa);
+	warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
 
 	/* start the sensor polling at 2Hz */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
@@ -394,7 +386,7 @@ test()
 			err(1, "periodic read failed");
 
 		warnx("periodic read %u", i);
-		warnx("diff pressure: %f pa", (double)report.differential_pressure_pa);
+		warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
 	}
 
 	/* reset the sensor polling to its default rate */
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 0e9a961ac..e4ecfa6b5 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -1283,14 +1283,13 @@ int HMC5883::set_excitement(unsigned enable)
 	if (OK != ret)
 		perf_count(_comms_errors);
 
+	_conf_reg &= ~0x03; // reset previous excitement mode
 	if (((int)enable) < 0) {
 		_conf_reg |= 0x01;
 
 	} else if (enable > 0) {
 		_conf_reg |= 0x02;
 
-	} else {
-		_conf_reg &= ~0x03;
 	}
 
         // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)_conf_reg);
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 159706278..1d9a463ad 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -228,44 +228,23 @@ MEASAirspeed::collect()
 	// the raw value still should be compensated for the known offset
 	diff_press_pa_raw -= _diff_pres_offset;
 
-	float diff_press_pa = fabsf(diff_press_pa_raw);
-	
 	/*
-	  note that we return both the absolute value with offset
-	  applied and a raw value without the offset applied. This
-	  makes it possible for higher level code to detect if the
-	  user has the tubes connected backwards, and also makes it
-	  possible to correctly use offsets calculated by a higher
-	  level airspeed driver.
-
 	  With the above calculation the MS4525 sensor will produce a
 	  positive number when the top port is used as a dynamic port
 	  and bottom port is used as the static port
-
-	  Also note that the _diff_pres_offset is applied before the
-	  fabsf() not afterwards. It needs to be done this way to
-	  prevent a bias at low speeds, but this also means that when
-	  setting a offset you must set it based on the raw value, not
-	  the offset value
 	 */
-	
+
 	struct differential_pressure_s report;
 
 	/* track maximum differential pressure measured (so we can work out top speed). */
-	if (diff_press_pa > _max_differential_pressure_pa) {
-		_max_differential_pressure_pa = diff_press_pa;
+	if (diff_press_pa_raw > _max_differential_pressure_pa) {
+		_max_differential_pressure_pa = diff_press_pa_raw;
 	}
 
 	report.timestamp = hrt_absolute_time();
 	report.error_count = perf_event_count(_comms_errors);
 	report.temperature = temperature;
-	report.differential_pressure_pa = diff_press_pa;
-	report.differential_pressure_filtered_pa =  _filter.apply(diff_press_pa);
-
-	/* the dynamics of the filter can make it overshoot into the negative range */
-	if (report.differential_pressure_filtered_pa < 0.0f) {
-		report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa);
-	}
+	report.differential_pressure_filtered_pa =  _filter.apply(diff_press_pa_raw);
 
 	report.differential_pressure_raw_pa = diff_press_pa_raw;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;
@@ -345,7 +324,7 @@ MEASAirspeed::cycle()
 /**
    correct for 5V rail voltage if the system_power ORB topic is
    available
-   
+
    See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of
    offset versus voltage for 3 sensors
  */
@@ -394,7 +373,7 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
 	if (voltage_diff < -1.0f) {
 		voltage_diff = -1.0f;
 	}
-	temperature -= voltage_diff * temp_slope;	
+	temperature -= voltage_diff * temp_slope;
 #endif // CONFIG_ARCH_BOARD_PX4FMU_V2
 }
 
@@ -523,7 +502,7 @@ test()
 	}
 
 	warnx("single read");
-	warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
+	warnx("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa);
 
 	/* start the sensor polling at 2Hz */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
@@ -551,7 +530,7 @@ test()
 		}
 
 		warnx("periodic read %u", i);
-		warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
+		warnx("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa);
 		warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature);
 	}
 
diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 873fa62c4..889643d0d 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -130,7 +130,7 @@ protected:
 	float			_T;
 
 	/* altitude conversion calibration */
-	unsigned		_msl_pressure;	/* in kPa */
+	unsigned		_msl_pressure;	/* in Pa */
 
 	orb_advert_t		_baro_topic;
 
@@ -466,7 +466,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 			irqrestore(flags);
 			return -ENOMEM;
 		}
-		irqrestore(flags);		
+		irqrestore(flags);
 		return OK;
 	}
 
diff --git a/src/drivers/pca9685/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt b/src/drivers/pca9685/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt
new file mode 100644
index 000000000..9c5eb42eb
--- /dev/null
+++ b/src/drivers/pca9685/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt
@@ -0,0 +1,29 @@
+The following license agreement covers re-used code from the arduino driver
+for the Adafruit I2C to PWM converter.
+
+Software License Agreement (BSD License)
+
+Copyright (c) 2012, Adafruit Industries
+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 of the copyright holders nor the
+names of its contributors may be used to endorse or promote products
+derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(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/src/drivers/pca9685/module.mk b/src/drivers/pca9685/module.mk
new file mode 100644
index 000000000..7a5c7996e
--- /dev/null
+++ b/src/drivers/pca9685/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Driver for the PCA9685 I2C PWM controller
+# The chip is used on the adafruit I2C PWM converter,
+# which allows to control servos via I2C.
+# https://www.adafruit.com/product/815
+
+MODULE_COMMAND	= pca9685
+
+SRCS		= pca9685.cpp
diff --git a/src/drivers/pca9685/pca9685.cpp b/src/drivers/pca9685/pca9685.cpp
new file mode 100644
index 000000000..6f69ce8a1
--- /dev/null
+++ b/src/drivers/pca9685/pca9685.cpp
@@ -0,0 +1,651 @@
+/****************************************************************************
+ *
+ *   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 pca9685.cpp
+ *
+ * Driver for the PCA9685 I2C PWM module
+ * The chip is used on the Adafruit I2C/PWM converter https://www.adafruit.com/product/815
+ *
+ * Parts of the code are adapted from the arduino library for the board
+ * https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library
+ * for the license of these parts see the
+ * arduino_Adafruit_PWM_Servo_Driver_Library_license.txt file
+ * see https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library for contributors
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <nuttx/config.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <ctype.h>
+#include <math.h>
+
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/actuator_controls.h>
+
+#include <board_config.h>
+#include <drivers/drv_io_expander.h>
+
+#define PCA9685_SUBADR1 0x2
+#define PCA9685_SUBADR2 0x3
+#define PCA9685_SUBADR3 0x4
+
+#define PCA9685_MODE1 0x0
+#define PCA9685_PRESCALE 0xFE
+
+#define LED0_ON_L 0x6
+#define LED0_ON_H 0x7
+#define LED0_OFF_L 0x8
+#define LED0_OFF_H 0x9
+
+#define ALLLED_ON_L 0xFA
+#define ALLLED_ON_H 0xFB
+#define ALLLED_OFF_L 0xFC
+#define ALLLED_OF
+
+#define ADDR 0x40	// I2C adress
+
+#define PCA9685_DEVICE_PATH "/dev/pca9685"
+#define PCA9685_BUS PX4_I2C_BUS_EXPANSION
+#define PCA9685_PWMFREQ 60.0f
+#define PCA9685_NCHANS 16 // total amount of pwm outputs
+
+#define PCA9685_PWMMIN 150 // this is the 'minimum' pulse length count (out of 4096)
+#define PCA9685_PWMMAX 600 // this is the 'maximum' pulse length count (out of 4096)_PWMFREQ 60.0f
+
+#define PCA9685_PWMCENTER ((PCA9685_PWMMAX + PCA9685_PWMMIN)/2)
+#define PCA9685_MAXSERVODEG 90.0f /* maximal servo deflection in degrees
+				     PCA9685_PWMMIN <--> -PCA9685_MAXSERVODEG
+				     PCA9685_PWMMAX <--> PCA9685_MAXSERVODEG
+				     */
+#define PCA9685_SCALE ((PCA9685_PWMMAX - PCA9685_PWMCENTER)/(M_DEG_TO_RAD_F * PCA9685_MAXSERVODEG)) // scales from rad to PWM
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+class PCA9685 : public device::I2C
+{
+public:
+	PCA9685(int bus=PCA9685_BUS, uint8_t address=ADDR);
+	virtual ~PCA9685();
+
+
+	virtual int		init();
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+	virtual int		info();
+	virtual int		reset();
+	bool			is_running() { return _running; }
+
+private:
+	work_s			_work;
+
+
+	enum IOX_MODE		_mode;
+	bool			_running;
+	int			_i2cpwm_interval;
+	bool			_should_run;
+	perf_counter_t		_comms_errors;
+
+	uint8_t			_msg[6];
+
+	int			_actuator_controls_sub;
+	struct actuator_controls_s  _actuator_controls;
+	uint16_t	    	_current_values[NUM_ACTUATOR_CONTROLS]; /**< stores the current pwm output
+										  values as sent to the setPin() */
+
+	bool _mode_on_initialized;  /** Set to true after the first call of i2cpwm in mode IOX_MODE_ON */
+
+	static void		i2cpwm_trampoline(void *arg);
+	void			i2cpwm();
+
+	/**
+	 * Helper function to set the pwm frequency
+	 */
+	int setPWMFreq(float freq);
+
+	/**
+	 * Helper function to set the demanded pwm value
+	 * @param num pwm output number
+	 */
+	int setPWM(uint8_t num, uint16_t on, uint16_t off);
+
+	/**
+	 * Sets pin without having to deal with on/off tick placement and properly handles
+	 * a zero value as completely off.  Optional invert parameter supports inverting
+	 * the pulse for sinking to ground.
+	 * @param num pwm output number
+	 * @param val should be a value from 0 to 4095 inclusive.
+	 */
+	int setPin(uint8_t num, uint16_t val, bool invert = false);
+
+
+	/* Wrapper to read a byte from addr */
+	int read8(uint8_t addr, uint8_t &value);
+
+	/* Wrapper to wite a byte to addr */
+	int write8(uint8_t addr, uint8_t value);
+
+};
+
+/* for now, we only support one board */
+namespace
+{
+PCA9685 *g_pca9685;
+}
+
+void pca9685_usage();
+
+extern "C" __EXPORT int pca9685_main(int argc, char *argv[]);
+
+PCA9685::PCA9685(int bus, uint8_t address) :
+	I2C("pca9685", PCA9685_DEVICE_PATH, bus, address, 100000),
+	_mode(IOX_MODE_OFF),
+	_running(false),
+	_i2cpwm_interval(SEC2TICK(1.0f/60.0f)),
+	_should_run(false),
+	_comms_errors(perf_alloc(PC_COUNT, "actuator_controls_2_comms_errors")),
+	_actuator_controls_sub(-1),
+	_actuator_controls(),
+	_mode_on_initialized(false)
+{
+	memset(&_work, 0, sizeof(_work));
+	memset(_msg, 0, sizeof(_msg));
+	memset(_current_values, 0, sizeof(_current_values));
+}
+
+PCA9685::~PCA9685()
+{
+}
+
+int
+PCA9685::init()
+{
+	int ret;
+	ret = I2C::init();
+	if (ret != OK) {
+		return ret;
+	}
+
+	ret = reset();
+	if (ret != OK) {
+		return ret;
+	}
+
+	ret = setPWMFreq(PCA9685_PWMFREQ);
+
+	return ret;
+}
+
+int
+PCA9685::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	int ret = -EINVAL;
+	switch (cmd) {
+
+	case IOX_SET_MODE:
+
+		if (_mode != (IOX_MODE)arg) {
+
+			switch ((IOX_MODE)arg) {
+			case IOX_MODE_OFF:
+				warnx("shutting down");
+				break;
+			case IOX_MODE_ON:
+				warnx("starting");
+				break;
+			case IOX_MODE_TEST_OUT:
+				warnx("test starting");
+				break;
+
+			default:
+				return -1;
+			}
+
+			_mode = (IOX_MODE)arg;
+		}
+
+		// if not active, kick it
+		if (!_running) {
+			_running = true;
+			work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, 1);
+		}
+
+
+		return OK;
+
+	default:
+		// see if the parent class can make any use of it
+		ret = CDev::ioctl(filp, cmd, arg);
+		break;
+	}
+
+	return ret;
+}
+
+int
+PCA9685::info()
+{
+	int ret = OK;
+
+	if (is_running()) {
+		warnx("Driver is running, mode: %u", _mode);
+	} else {
+		warnx("Driver started but not running");
+	}
+
+	return ret;
+}
+
+void
+PCA9685::i2cpwm_trampoline(void *arg)
+{
+	PCA9685 *i2cpwm = reinterpret_cast<PCA9685 *>(arg);
+
+	i2cpwm->i2cpwm();
+}
+
+/**
+ * Main loop function
+ */
+void
+PCA9685::i2cpwm()
+{
+	if (_mode == IOX_MODE_TEST_OUT) {
+		setPin(0, PCA9685_PWMCENTER);
+		_should_run = true;
+	} else if (_mode == IOX_MODE_OFF) {
+		_should_run = false;
+	} else {
+		if (!_mode_on_initialized) {
+			/* Subscribe to actuator control 2 (payload group for gimbal) */
+			_actuator_controls_sub = orb_subscribe(ORB_ID(actuator_controls_2));
+			/* set the uorb update interval lower than the driver pwm interval */
+			orb_set_interval(_actuator_controls_sub, 1000.0f / PCA9685_PWMFREQ - 5);
+
+			_mode_on_initialized = true;
+		}
+
+		/* Read the servo setpoints from the actuator control topics (gimbal) */
+		bool updated;
+		orb_check(_actuator_controls_sub, &updated);
+		if (updated) {
+			orb_copy(ORB_ID(actuator_controls_2), _actuator_controls_sub, &_actuator_controls);
+			for (int i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
+				/* Scale the controls to PWM, first multiply by pi to get rad,
+				 * the control[i] values are on the range -1 ... 1 */
+				uint16_t new_value = PCA9685_PWMCENTER +
+					(_actuator_controls.control[i] * M_PI_F * PCA9685_SCALE);
+				debug("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value,
+						(double)_actuator_controls.control[i]);
+				if (new_value != _current_values[i] &&
+						isfinite(new_value) &&
+						new_value >= PCA9685_PWMMIN &&
+						new_value <= PCA9685_PWMMAX) {
+					/* This value was updated, send the command to adjust the PWM value */
+					setPin(i, new_value);
+					_current_values[i] = new_value;
+				}
+			}
+		}
+		_should_run = true;
+	}
+
+	// check if any activity remains, else stop
+	if (!_should_run) {
+		_running = false;
+		return;
+	}
+
+	// re-queue ourselves to run again later
+	_running = true;
+	work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, _i2cpwm_interval);
+}
+
+int
+PCA9685::setPWM(uint8_t num, uint16_t on, uint16_t off)
+{
+	int ret;
+	/* convert to correct message */
+	_msg[0] = LED0_ON_L + 4 * num;
+	_msg[1] = on;
+	_msg[2] = on >> 8;
+	_msg[3] = off;
+	_msg[4] = off >> 8;
+
+	/* try i2c transfer */
+	ret = transfer(_msg, 5, nullptr, 0);
+
+	if (OK != ret) {
+		perf_count(_comms_errors);
+		log("i2c::transfer returned %d", ret);
+	}
+
+	return ret;
+}
+
+int
+PCA9685::setPin(uint8_t num, uint16_t val, bool invert)
+{
+	// Clamp value between 0 and 4095 inclusive.
+	if (val > 4095) {
+		val = 4095;
+	}
+	if (invert) {
+		if (val == 0) {
+			// Special value for signal fully on.
+			return setPWM(num, 4096, 0);
+		} else if (val == 4095) {
+			// Special value for signal fully off.
+			return setPWM(num, 0, 4096);
+		} else {
+			return setPWM(num, 0, 4095-val);
+		}
+	} else {
+		if (val == 4095) {
+			// Special value for signal fully on.
+			return setPWM(num, 4096, 0);
+		} else if (val == 0) {
+			// Special value for signal fully off.
+			return setPWM(num, 0, 4096);
+		} else {
+			return setPWM(num, 0, val);
+		}
+	}
+
+	return ERROR;
+}
+
+int
+PCA9685::setPWMFreq(float freq)
+{
+	int ret  = OK;
+	freq *= 0.9f;  /* Correct for overshoot in the frequency setting (see issue
+		https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library/issues/11). */
+	float prescaleval = 25000000;
+	prescaleval /= 4096;
+	prescaleval /= freq;
+	prescaleval -= 1;
+	uint8_t prescale = uint8_t(prescaleval + 0.5f); //implicit floor()
+	uint8_t oldmode;
+	ret = read8(PCA9685_MODE1, oldmode);
+	if (ret != OK) {
+		return ret;
+	}
+	uint8_t newmode = (oldmode&0x7F) | 0x10; // sleep
+
+	ret = write8(PCA9685_MODE1, newmode); // go to sleep
+	if (ret != OK) {
+		return ret;
+	}
+	ret = write8(PCA9685_PRESCALE, prescale); // set the prescaler
+	if (ret != OK) {
+		return ret;
+	}
+	ret = write8(PCA9685_MODE1, oldmode);
+	if (ret != OK) {
+		return ret;
+	}
+
+	usleep(5000); //5ms delay (from arduino driver)
+
+	ret = write8(PCA9685_MODE1, oldmode | 0xa1);  //  This sets the MODE1 register to turn on auto increment.
+	if (ret != OK) {
+		return ret;
+	}
+
+	return ret;
+}
+
+/* Wrapper to read a byte from addr */
+int
+PCA9685::read8(uint8_t addr, uint8_t &value)
+{
+	int ret = OK;
+
+	/* send addr */
+	ret = transfer(&addr, sizeof(addr), nullptr, 0);
+	if (ret != OK) {
+		goto fail_read;
+	}
+
+	/* get value */
+	ret = transfer(nullptr, 0, &value, 1);
+	if (ret != OK) {
+		goto fail_read;
+	}
+
+	return ret;
+
+fail_read:
+	perf_count(_comms_errors);
+	log("i2c::transfer returned %d", ret);
+
+	return ret;
+}
+
+int PCA9685::reset(void) {
+	warnx("resetting");
+	return write8(PCA9685_MODE1, 0x0);
+}
+
+/* Wrapper to wite a byte to addr */
+int
+PCA9685::write8(uint8_t addr, uint8_t value) {
+	int ret = OK;
+	_msg[0] = addr;
+	_msg[1] = value;
+	/* send addr and value */
+	ret = transfer(_msg, 2, nullptr, 0);
+	if (ret != OK) {
+		perf_count(_comms_errors);
+		log("i2c::transfer returned %d", ret);
+	}
+	return ret;
+}
+
+void
+pca9685_usage()
+{
+	warnx("missing command: try 'start', 'test', 'stop', 'info'");
+	warnx("options:");
+	warnx("    -b i2cbus (%d)", PX4_I2C_BUS_EXPANSION);
+	warnx("    -a addr (0x%x)", ADDR);
+}
+
+int
+pca9685_main(int argc, char *argv[])
+{
+	int i2cdevice = -1;
+	int i2caddr = 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':
+			i2caddr = strtol(optarg, NULL, 0);
+			break;
+
+		case 'b':
+			i2cdevice = strtol(optarg, NULL, 0);
+			break;
+
+		default:
+			pca9685_usage();
+			exit(0);
+		}
+	}
+
+	if (optind >= argc) {
+		pca9685_usage();
+		exit(1);
+	}
+
+	const char *verb = argv[optind];
+
+	int fd;
+	int ret;
+
+	if (!strcmp(verb, "start")) {
+		if (g_pca9685 != nullptr) {
+			errx(1, "already started");
+		}
+
+		if (i2cdevice == -1) {
+			// try the external bus first
+			i2cdevice = PX4_I2C_BUS_EXPANSION;
+			g_pca9685 = new PCA9685(PX4_I2C_BUS_EXPANSION, i2caddr);
+
+			if (g_pca9685 != nullptr && OK != g_pca9685->init()) {
+				delete g_pca9685;
+				g_pca9685 = nullptr;
+			}
+
+			if (g_pca9685 == nullptr) {
+				errx(1, "init failed");
+			}
+		}
+
+		if (g_pca9685 == nullptr) {
+			g_pca9685 = new PCA9685(i2cdevice, i2caddr);
+
+			if (g_pca9685 == nullptr) {
+				errx(1, "new failed");
+			}
+
+			if (OK != g_pca9685->init()) {
+				delete g_pca9685;
+				g_pca9685 = nullptr;
+				errx(1, "init failed");
+			}
+		}
+		fd = open(PCA9685_DEVICE_PATH, 0);
+		if (fd == -1) {
+			errx(1, "Unable to open " PCA9685_DEVICE_PATH);
+		}
+		ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_ON);
+		close(fd);
+
+
+		exit(0);
+	}
+
+	// need the driver past this point
+	if (g_pca9685 == nullptr) {
+		warnx("not started, run pca9685 start");
+		exit(1);
+	}
+
+	if (!strcmp(verb, "info")) {
+		g_pca9685->info();
+		exit(0);
+	}
+
+	if (!strcmp(verb, "reset")) {
+		g_pca9685->reset();
+		exit(0);
+	}
+
+
+	if (!strcmp(verb, "test")) {
+		fd = open(PCA9685_DEVICE_PATH, 0);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " PCA9685_DEVICE_PATH);
+		}
+
+		ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_TEST_OUT);
+
+		close(fd);
+		exit(ret);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		fd = open(PCA9685_DEVICE_PATH, 0);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " PCA9685_DEVICE_PATH);
+		}
+
+		ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF);
+		close(fd);
+
+		// wait until we're not running any more
+		for (unsigned i = 0; i < 15; i++) {
+			if (!g_pca9685->is_running()) {
+				break;
+			}
+
+			usleep(50000);
+			printf(".");
+			fflush(stdout);
+		}
+		printf("\n");
+		fflush(stdout);
+
+		if (!g_pca9685->is_running()) {
+			delete g_pca9685;
+			g_pca9685= nullptr;
+			warnx("stopped, exiting");
+			exit(0);
+		} else {
+			warnx("stop failed.");
+			exit(1);
+		}
+	}
+
+	pca9685_usage();
+	exit(0);
+}
diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp
index f214b5964..60ad3c1af 100644
--- a/src/drivers/px4flow/px4flow.cpp
+++ b/src/drivers/px4flow/px4flow.cpp
@@ -37,7 +37,7 @@
  *
  * Driver for the PX4FLOW module connected via I2C.
  */
-	 
+
 #include <nuttx/config.h>
 
 #include <drivers/device/i2c.h>
@@ -68,7 +68,7 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/subsystem_info.h>
-//#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/optical_flow.h>
 
 #include <board_config.h>
 
@@ -80,7 +80,7 @@
 /* PX4FLOW Registers addresses */
 #define PX4FLOW_REG	0x00		/* Measure Register */
 
-#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz
+#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz */
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -115,17 +115,17 @@ class PX4FLOW : public device::I2C
 public:
 	PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS);
 	virtual ~PX4FLOW();
-	
+
 	virtual int 		init();
-	
+
 	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
 	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
-	
+
 	/**
 	* Diagnostics - print some basic information about the driver.
 	*/
 	void				print_info();
-	
+
 protected:
 	virtual int			probe();
 
@@ -136,13 +136,13 @@ private:
 	bool				_sensor_ok;
 	int					_measure_ticks;
 	bool				_collect_phase;
-	
+
 	orb_advert_t		_px4flow_topic;
 
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_comms_errors;
 	perf_counter_t		_buffer_overflows;
-	
+
 	/**
 	* Test whether the device supported by the driver is present at a
 	* specific address.
@@ -151,7 +151,7 @@ private:
 	* @return		True if the device is present.
 	*/
 	int					probe_address(uint8_t address);
-	
+
 	/**
 	* Initialise the automatic measurement state machine and start it.
 	*
@@ -159,12 +159,12 @@ private:
 	*       to make it more aggressive about resetting the bus in case of errors.
 	*/
 	void				start();
-	
+
 	/**
 	* Stop the automatic measurement state machine.
 	*/
 	void				stop();
-	
+
 	/**
 	* Perform a poll cycle; collect from the previous measurement
 	* and start a new one.
@@ -179,8 +179,8 @@ private:
 	* @param arg		Instance pointer for the driver that is polling.
 	*/
 	static void		cycle_trampoline(void *arg);
-	
-	
+
+
 };
 
 /*
@@ -201,7 +201,7 @@ PX4FLOW::PX4FLOW(int bus, int address) :
 {
 	// enable debug() calls
 	_debug_enabled = true;
-	
+
 	// work_cancel in the dtor will explode if we don't do this...
 	memset(&_work, 0, sizeof(_work));
 }
@@ -226,13 +226,13 @@ PX4FLOW::init()
 		goto out;
 
 	/* allocate basic report buffers */
-	_reports = new RingBuffer(2, sizeof(px4flow_report));
+	_reports = new RingBuffer(2, sizeof(struct optical_flow_s));
 
 	if (_reports == nullptr)
 		goto out;
 
 	/* get a publish handle on the px4flow topic */
-	struct px4flow_report zero_report;
+	struct optical_flow_s zero_report;
 	memset(&zero_report, 0, sizeof(zero_report));
 	_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report);
 
@@ -323,24 +323,24 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
 		/* lower bound is mandatory, upper bound is a sanity check */
 		if ((arg < 1) || (arg > 100))
 			return -EINVAL;
-		
+
 		irqstate_t flags = irqsave();
 		if (!_reports->resize(arg)) {
 			irqrestore(flags);
 			return -ENOMEM;
 		}
 		irqrestore(flags);
-		
+
 		return OK;
 	}
 
 	case SENSORIOCGQUEUEDEPTH:
 		return _reports->size();
-		
+
 	case SENSORIOCRESET:
 		/* XXX implement this */
 		return -EINVAL;
-	
+
 	default:
 		/* give it to the superclass */
 		return I2C::ioctl(filp, cmd, arg);
@@ -350,8 +350,8 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
 ssize_t
 PX4FLOW::read(struct file *filp, char *buffer, size_t buflen)
 {
-	unsigned count = buflen / sizeof(struct px4flow_report);
-	struct px4flow_report *rbuf = reinterpret_cast<struct px4flow_report *>(buffer);
+	unsigned count = buflen / sizeof(struct optical_flow_s);
+	struct optical_flow_s *rbuf = reinterpret_cast<struct optical_flow_s *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
@@ -425,7 +425,7 @@ PX4FLOW::measure()
 		return ret;
 	}
 	ret = OK;
-	
+
 	return ret;
 }
 
@@ -433,14 +433,14 @@ int
 PX4FLOW::collect()
 {
 	int	ret = -EIO;
-	
+
 	/* read from the sensor */
 	uint8_t val[22] = {0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0};
-	
+
 	perf_begin(_sample_perf);
-	
+
 	ret = transfer(nullptr, 0, &val[0], 22);
-	
+
 	if (ret < 0)
 	{
 		log("error reading from sensor: %d", ret);
@@ -448,7 +448,7 @@ PX4FLOW::collect()
 		perf_end(_sample_perf);
 		return ret;
 	}
-	
+
 //	f.frame_count = val[1] << 8 | val[0];
 //	f.pixel_flow_x_sum= val[3] << 8 | val[2];
 //	f.pixel_flow_y_sum= val[5] << 8 | val[4];
@@ -466,7 +466,7 @@ PX4FLOW::collect()
 	int16_t flowcy = val[9] << 8 | val[8];
 	int16_t gdist = val[21] << 8 | val[20];
 
-	struct px4flow_report report;
+	struct optical_flow_s report;
 	report.flow_comp_x_m = float(flowcx)/1000.0f;
 	report.flow_comp_y_m = float(flowcy)/1000.0f;
 	report.flow_raw_x= val[3] << 8 | val[2];
@@ -503,7 +503,7 @@ PX4FLOW::start()
 
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1);
-	
+
 	/* notify about state change */
 	struct subsystem_info_s info = {
 		true,
@@ -644,7 +644,7 @@ start()
 
 fail:
 
-	if (g_dev != nullptr) 
+	if (g_dev != nullptr)
 	{
 		delete g_dev;
 		g_dev = nullptr;
@@ -678,7 +678,7 @@ void stop()
 void
 test()
 {
-	struct px4flow_report report;
+	struct optical_flow_s report;
 	ssize_t sz;
 	int ret;
 
@@ -777,7 +777,7 @@ px4flow_main(int argc, char *argv[])
 	 */
 	if (!strcmp(argv[1], "start"))
 		px4flow::start();
-	
+
 	 /*
 	  * Stop the driver
 	  */
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 82977a032..122a3cd17 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -1272,7 +1272,9 @@ PX4FMU::write(file *filp, const char *buffer, size_t len)
 	memcpy(values, buffer, count * 2);
 
 	for (uint8_t i = 0; i < count; i++) {
-		up_pwm_servo_set(i, values[i]);
+		if (values[i] != PWM_IGNORE_THIS_CHANNEL) {
+			up_pwm_servo_set(i, values[i]);
+		}
 	}
 
 	return count * 2;
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 32069cf09..3fdb1dd1f 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1175,6 +1175,14 @@ PX4IO::io_set_arming_state()
 		clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
 	}
 
+	// XXX this is for future support in the commander
+	// but can be removed if unneeded
+	// if (armed.termination_failsafe) {
+	// 	set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
+	// } else {
+	// 	clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
+	// }
+
 	if (armed.ready_to_arm) {
 		set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
 
@@ -1968,7 +1976,7 @@ PX4IO::print_status(bool extended_status)
 	printf("actuators");
 
 	for (unsigned i = 0; i < _max_actuators; i++)
-		printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i));
+		printf(" %hi", int16_t(io_reg_get(PX4IO_PAGE_ACTUATORS, i)));
 
 	printf("\n");
 	printf("servos");
@@ -2038,7 +2046,8 @@ PX4IO::print_status(bool extended_status)
 	       ((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" : ""),
-	       ((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE)		? " FORCE_FAILSAFE" : "")
+	       ((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE)		? " FORCE_FAILSAFE" : ""),
+	       ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : "")
 	       );
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
@@ -2262,6 +2271,17 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 		}
 		break;
 
+	case PWM_SERVO_SET_TERMINATION_FAILSAFE:
+		/* if failsafe occurs, do not allow the system to recover */
+		if (arg == 0) {
+			/* clear termination failsafe flag */
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE, 0);
+		} else {
+			/* set termination failsafe flag */
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE);
+		}
+		break;
+
 	case DSM_BIND_START:
 
 		/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */
diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp
index bca1715fa..d382d08d0 100644
--- a/src/drivers/sf0x/sf0x.cpp
+++ b/src/drivers/sf0x/sf0x.cpp
@@ -520,11 +520,9 @@ SF0X::collect()
 	/* clear buffer if last read was too long ago */
 	uint64_t read_elapsed = hrt_elapsed_time(&_last_read);
 
+	/* timed out - retry */
 	if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
 		_linebuf_index = 0;
-	} else if (_linebuf_index > 0) {
-		/* increment to next read position */
-		_linebuf_index++;
 	}
 
 	/* the buffer for read chars is buflen minus null termination */
@@ -550,18 +548,19 @@ SF0X::collect()
 		return -EAGAIN;
 	}
 
-	/* we did increment the index to the next position already, so just add the additional fields */
-	_linebuf_index += (ret - 1);
+	/* let the write pointer point to the next free entry */
+	_linebuf_index += ret;
 
 	_last_read = hrt_absolute_time();
 
-	if (_linebuf_index < 1) {
-		/* we need at least the two end bytes to make sense of this string */
+	/* require a reasonable amount of minimum bytes */
+	if (_linebuf_index < 6) {
+		/* we need at this format: x.xx\r\n */
 		return -EAGAIN;
 
-	} else if (_linebuf[_linebuf_index - 1] != '\r' || _linebuf[_linebuf_index] != '\n') {
+	} else if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') {
 
-		if (_linebuf_index >= readlen - 1) {
+		if (_linebuf_index == readlen) {
 			/* we have a full buffer, but no line ending - abort */
 			_linebuf_index = 0;
 			perf_count(_comms_errors);
@@ -577,9 +576,7 @@ SF0X::collect()
 	bool valid;
 
 	/* enforce line ending */
-	unsigned lend = (_linebuf_index < (sizeof(_linebuf) - 1)) ? _linebuf_index : (sizeof(_linebuf) - 1);
-
-	_linebuf[lend] = '\0';
+	_linebuf[_linebuf_index] = '\0';
 
 	if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') {
 		si_units = -1.0f;
@@ -591,14 +588,16 @@ SF0X::collect()
 		valid = false;
 
 		/* wipe out partially read content from last cycle(s), check for dot */
-		for (unsigned i = 0; i < (lend - 2); i++) {
+		for (unsigned i = 0; i < (_linebuf_index - 2); i++) {
 			if (_linebuf[i] == '\n') {
-				char buf[sizeof(_linebuf)];
-				memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1));
-				memcpy(_linebuf, buf, (lend + 1) - (i + 1));
+				/* wipe out any partial measurements */
+				for (unsigned j = 0; j <= i; j++) {
+					_linebuf[j] = ' ';
+				}
 			}
 
-			if (_linebuf[i] == '.') {
+			/* we need a digit before the dot and a dot for a valid number */
+			if (i > 0 && ((_linebuf[i - 1] >= '0') && (_linebuf[i - 1] <= '9')) && (_linebuf[i] == '.')) {
 				valid = true;
 			}
 		}
@@ -606,7 +605,7 @@ SF0X::collect()
 		if (valid) {
 			si_units = strtod(_linebuf, &end);
 
-			/* we require at least 3 characters for a valid number */
+			/* we require at least four characters for a valid number */
 			if (end > _linebuf + 3) {
 				valid = true;
 			} else {
@@ -616,7 +615,7 @@ SF0X::collect()
 		}
 	}
 
-	debug("val (float): %8.4f, raw: %s, valid: %s\n", (double)si_units, _linebuf, ((valid) ? "OK" : "NO"));
+	debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO"));
 
 	/* done with this chunk, resetting - even if invalid */
 	_linebuf_index = 0;
@@ -708,12 +707,12 @@ SF0X::cycle()
 		int collect_ret = collect();
 
 		if (collect_ret == -EAGAIN) {
-			/* reschedule to grab the missing bits, time to transmit 10 bytes @9600 bps */
+			/* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */
 			work_queue(HPWORK,
 				   &_work,
 				   (worker_t)&SF0X::cycle_trampoline,
 				   this,
-				   USEC2TICK(1100));
+				   USEC2TICK(1042 * 8));
 			return;
 		}
 
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index 46db788a6..926a8db2a 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -169,7 +169,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
 
 	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
 
-		float id = _rate_error * dt;
+		float id = _rate_error * dt * scaler;
 
 		/*
 		 * anti-windup: do not allow integrator to increase if actuator is at limit
@@ -190,7 +190,9 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
 	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
 
 	/* Apply PI rate controller and store non-limited output */
-	_last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained) * scaler * scaler;  //scaler is proportional to 1/airspeed
+	_last_output = _bodyrate_setpoint * _k_ff * scaler +
+		_rate_error * _k_p * scaler * scaler
+		+ integrator_constrained;  //scaler is proportional to 1/airspeed
 //	warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
 //	warnx("roll: _last_output %.4f", (double)_last_output);
 	return math::constrain(_last_output, -1.0f, 1.0f);
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index 9894a34d7..94bd26f03 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -135,7 +135,7 @@ float ECL_RollController::control_bodyrate(float pitch,
 
 	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
 
-		float id = _rate_error * dt;
+		float id = _rate_error * dt * scaler;
 
 		/*
 		* anti-windup: do not allow integrator to increase if actuator is at limit
@@ -157,7 +157,9 @@ float ECL_RollController::control_bodyrate(float pitch,
 	//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);
 
 	/* Apply PI rate controller and store non-limited output */
-	_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler;  //scaler is proportional to 1/airspeed
+	_last_output = _bodyrate_setpoint * _k_ff * scaler +
+		_rate_error * _k_p * scaler * scaler
+		+ integrator_constrained;  //scaler is proportional to 1/airspeed
 
 	return math::constrain(_last_output, -1.0f, 1.0f);
 }
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index a57a0481a..023bd71bf 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -238,7 +238,7 @@ void TECS::_update_height_demand(float demand, float state)
 	_hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last;
 	_hgt_dem_adj_last = _hgt_dem_adj;
 
-	_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p;
+	_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT;
 	// Limit height rate of change
 	if (_hgt_rate_dem > _maxClimbRate) {
 		_hgt_rate_dem = _maxClimbRate;
@@ -252,6 +252,11 @@ void TECS::_update_height_demand(float demand, float state)
 
 void TECS::_detect_underspeed(void)
 {
+	if (!_detect_underspeed_enabled) {
+		_underspeed = false;
+		return;
+	}
+
 	if (((_integ5_state < _TASmin * 0.9f) && (_throttle_dem >= _THRmaxf * 0.95f)) || ((_integ3_state < _hgt_dem_adj) && _underspeed)) {
 		_underspeed = true;
 
diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h
index bcc2d90e5..8ac31de6f 100644
--- a/src/lib/external_lgpl/tecs/tecs.h
+++ b/src/lib/external_lgpl/tecs/tecs.h
@@ -47,6 +47,7 @@ public:
 		_rollComp(0.0f),
 		_spdWeight(0.5f),
 		_heightrate_p(0.0f),
+		_heightrate_ff(0.0f),
 		_speedrate_p(0.0f),
 		_throttle_dem(0.0f),
 		_pitch_dem(0.0f),
@@ -66,6 +67,9 @@ public:
 		_hgt_dem_prev(0.0f),
 		_TAS_dem_adj(0.0f),
 		_STEdotErrLast(0.0f),
+		_underspeed(false),
+		_detect_underspeed_enabled(true),
+		_badDescent(false),
 		_climbOutDem(false),
 		_SPE_dem(0.0f),
 		_SKE_dem(0.0f),
@@ -217,10 +221,18 @@ public:
 		_heightrate_p = heightrate_p;
 	}
 
+	void set_heightrate_ff(float heightrate_ff) {
+		_heightrate_ff = heightrate_ff;
+	}
+
 	void set_speedrate_p(float speedrate_p) {
 		_speedrate_p = speedrate_p;
 	}
 
+	void set_detect_underspeed_enabled(bool enabled) {
+		_detect_underspeed_enabled = enabled;
+	}
+
 private:
 
 	struct tecs_state _tecs_state;
@@ -249,6 +261,7 @@ private:
 	float _rollComp;
 	float _spdWeight;
 	float _heightrate_p;
+	float _heightrate_ff;
 	float _speedrate_p;
 
 	// throttle demand in the range from 0.0 to 1.0
@@ -323,6 +336,9 @@ private:
 	// Underspeed condition
 	bool _underspeed;
 
+	// Underspeed detection enabled
+	bool _detect_underspeed_enabled;
+
 	// Bad descent condition caused by unachievable airspeed demand
 	bool _badDescent;
 
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp
index c555a0a69..2ea1c414b 100644
--- a/src/lib/launchdetection/CatapultLaunchMethod.cpp
+++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp
@@ -49,9 +49,11 @@ CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) :
 	SuperBlock(parent, "CAT"),
 	last_timestamp(hrt_absolute_time()),
 	integrator(0.0f),
-	launchDetected(false),
-	threshold_accel(this, "A"),
-	threshold_time(this, "T")
+	state(LAUNCHDETECTION_RES_NONE),
+	thresholdAccel(this, "A"),
+	thresholdTime(this, "T"),
+	motorDelay(this, "MDEL"),
+	pitchMaxPreThrottle(this, "PMAX")
 {
 
 }
@@ -65,34 +67,66 @@ void CatapultLaunchMethod::update(float accel_x)
 	float dt = (float)hrt_elapsed_time(&last_timestamp) * 1e-6f;
 	last_timestamp = hrt_absolute_time();
 
-	if (accel_x > threshold_accel.get()) {
-		integrator += accel_x * dt;
-//		warnx("*** integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f",
-//				(double)integrator, (double)threshold_accel.get(),  (double)threshold_time.get(), (double)accel_x, (double)dt);
-		if (integrator > threshold_accel.get() * threshold_time.get()) {
-			launchDetected = true;
+	switch (state) {
+	case LAUNCHDETECTION_RES_NONE:
+		/* Detect a acceleration that is longer and stronger as the minimum given by the params */
+		if (accel_x > thresholdAccel.get()) {
+			integrator += dt;
+			if (integrator > thresholdTime.get()) {
+				if (motorDelay.get() > 0.0f) {
+					state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL;
+					warnx("Launch detected: state: enablecontrol, waiting %.2fs until using full"
+							" throttle", (double)motorDelay.get());
+				} else {
+					/* No motor delay set: go directly to enablemotors state */
+					state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
+					warnx("Launch detected: state: enablemotors (delay not activated)");
+				}
+			}
+		} else {
+			/* reset */
+			reset();
 		}
+		break;
+
+	case LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL:
+		/* Vehicle is currently controlling attitude but not with full throttle. Waiting undtil delay is
+		 * over to allow full throttle */
+		motorDelayCounter += dt;
+		if (motorDelayCounter > motorDelay.get()) {
+			warnx("Launch detected: state enablemotors");
+			state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
+		}
+		break;
+
+	default:
+		break;
 
-	} else {
-//		warnx("integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f",
-//				(double)integrator, (double)threshold_accel.get(),  (double)threshold_time.get(), (double)accel_x, (double)dt);
-		/* reset integrator */
-		integrator = 0.0f;
-		launchDetected = false;
 	}
 
 }
 
-bool CatapultLaunchMethod::getLaunchDetected()
+LaunchDetectionResult CatapultLaunchMethod::getLaunchDetected() const
 {
-	return launchDetected;
+	return state;
 }
 
 
 void CatapultLaunchMethod::reset()
 {
 	integrator = 0.0f;
-	launchDetected = false;
+	motorDelayCounter = 0.0f;
+	state = LAUNCHDETECTION_RES_NONE;
+}
+
+float CatapultLaunchMethod::getPitchMax(float pitchMaxDefault) {
+	/* If motor is turned on do not impose the extra limit on maximum pitch */
+	if (state == LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
+		return pitchMaxDefault;
+	} else {
+		return pitchMaxPreThrottle.get();
+	}
+
 }
 
 }
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h
index 23757f6f3..321dfb1de 100644
--- a/src/lib/launchdetection/CatapultLaunchMethod.h
+++ b/src/lib/launchdetection/CatapultLaunchMethod.h
@@ -57,16 +57,23 @@ public:
 	~CatapultLaunchMethod();
 
 	void update(float accel_x);
-	bool getLaunchDetected();
+	LaunchDetectionResult getLaunchDetected() const;
 	void reset();
+	float getPitchMax(float pitchMaxDefault);
 
 private:
 	hrt_abstime last_timestamp;
 	float integrator;
-	bool launchDetected;
+	float motorDelayCounter;
 
-	control::BlockParamFloat threshold_accel;
-	control::BlockParamFloat threshold_time;
+	LaunchDetectionResult state;
+
+	control::BlockParamFloat thresholdAccel;
+	control::BlockParamFloat thresholdTime;
+	control::BlockParamFloat motorDelay;
+	control::BlockParamFloat pitchMaxPreThrottle; /**< Upper pitch limit before throttle is turned on.
+						       Can be used to make sure that the AC does not climb
+						       too much while attached to a bungee */
 
 };
 
diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp
index 3bf47bbb0..52f5c3257 100644
--- a/src/lib/launchdetection/LaunchDetector.cpp
+++ b/src/lib/launchdetection/LaunchDetector.cpp
@@ -46,6 +46,7 @@ namespace launchdetection
 
 LaunchDetector::LaunchDetector() :
 	SuperBlock(NULL, "LAUN"),
+	activeLaunchDetectionMethodIndex(-1),
 	launchdetection_on(this, "ALL_ON"),
 	throttlePreTakeoff(this, "THR_PRE")
 {
@@ -65,7 +66,14 @@ LaunchDetector::~LaunchDetector()
 void LaunchDetector::reset()
 {
 	/* Reset all detectors */
-	launchMethods[0]->reset();
+	for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
+		launchMethods[i]->reset();
+	}
+
+	/* Reset active launchdetector */
+	activeLaunchDetectionMethodIndex  = -1;
+
+
 }
 
 void LaunchDetector::update(float accel_x)
@@ -77,17 +85,44 @@ void LaunchDetector::update(float accel_x)
 	}
 }
 
-bool LaunchDetector::getLaunchDetected()
+LaunchDetectionResult LaunchDetector::getLaunchDetected()
 {
 	if (launchdetection_on.get() == 1) {
-		for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
-			if(launchMethods[i]->getLaunchDetected()) {
-				return true;
+		if (activeLaunchDetectionMethodIndex < 0) {
+		/* None of the active launchmethods has detected a launch, check all launchmethods */
+			for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
+				if(launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) {
+					warnx("selecting launchmethod %d", i);
+					activeLaunchDetectionMethodIndex = i; // from now on only check this method
+					return launchMethods[i]->getLaunchDetected();
+				}
 			}
+		} else {
+			return launchMethods[activeLaunchDetectionMethodIndex]->getLaunchDetected();
 		}
 	}
 
-	return false;
+	return LAUNCHDETECTION_RES_NONE;
 }
 
+float LaunchDetector::getPitchMax(float pitchMaxDefault) {
+	if (!launchdetection_on.get()) {
+		return pitchMaxDefault;
+	}
+
+	/* if a lauchdetectionmethod is active or only one exists return the pitch limit from this method,
+	 * otherwise use the default limit */
+	if (activeLaunchDetectionMethodIndex < 0) {
+		if (sizeof(launchMethods)/sizeof(LaunchMethod) > 1) {
+			return pitchMaxDefault;
+		} else {
+			return launchMethods[0]->getPitchMax(pitchMaxDefault);
+		}
+	} else {
+		return launchMethods[activeLaunchDetectionMethodIndex]->getPitchMax(pitchMaxDefault);
+	}
+
+}
+
+
 }
diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h
index 1a214b66e..4215b49d2 100644
--- a/src/lib/launchdetection/LaunchDetector.h
+++ b/src/lib/launchdetection/LaunchDetector.h
@@ -59,14 +59,23 @@ public:
 	void reset();
 
 	void update(float accel_x);
-	bool getLaunchDetected();
+	LaunchDetectionResult getLaunchDetected();
 	bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); };
 
 	float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); }
 
+	/* Returns a maximum pitch in deg. Different launch methods may impose upper pitch limits during launch */
+	float getPitchMax(float pitchMaxDefault);
+
 //	virtual bool getLaunchDetected();
 protected:
 private:
+	int activeLaunchDetectionMethodIndex; /**< holds a index to the launchMethod in the array launchMethods
+					       which detected a Launch. If no launchMethod has detected a launch yet the
+					       value is -1. Once one launchMetthod has detected a launch only this
+					       method is checked for further adavancing in the state machine (e.g. when
+					       to power up the motors) */
+
 	LaunchMethod* launchMethods[1];
 	control::BlockParamInt launchdetection_on;
 	control::BlockParamFloat throttlePreTakeoff;
diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h
index e04467f6a..8b5220cb3 100644
--- a/src/lib/launchdetection/LaunchMethod.h
+++ b/src/lib/launchdetection/LaunchMethod.h
@@ -44,13 +44,27 @@
 namespace launchdetection
 {
 
+enum LaunchDetectionResult {
+	LAUNCHDETECTION_RES_NONE = 0, /**< No launch has been detected */
+	LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL = 1, /**< Launch has been detected, the controller should
+							  control the attitude. However any motors should not throttle
+							  up and still be set to 'throttlePreTakeoff'.
+							  For instance this is used to have a delay for the motor
+							  when launching a fixed wing aircraft from a bungee */
+	LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS = 2 /**< Launch has been detected, teh controller should control
+							attitude and also throttle up the motors. */
+};
+
 class LaunchMethod
 {
 public:
 	virtual void update(float accel_x) = 0;
-	virtual bool getLaunchDetected() = 0;
+	virtual LaunchDetectionResult getLaunchDetected() const = 0;
 	virtual void reset() = 0;
 
+	/* Returns a upper pitch limit if required, otherwise returns pitchMaxDefault */
+	virtual float getPitchMax(float pitchMaxDefault) = 0;
+
 protected:
 private:
 };
diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c
index 8df8c696c..e3aa7ab2d 100644
--- a/src/lib/launchdetection/launchdetection_params.c
+++ b/src/lib/launchdetection/launchdetection_params.c
@@ -78,6 +78,31 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f);
 PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f);
 
 /**
+ * Motor delay
+ *
+ * Delay between starting attitude control and powering up the throttle (giving throttle control to the controller)
+ * Before this timespan is up the throttle will be set to LAUN_THR_PRE, set to 0 to deactivate
+ *
+ * @unit seconds
+ * @min 0
+ * @group Launch detection
+ */
+PARAM_DEFINE_FLOAT(LAUN_CAT_MDEL, 0.0f);
+
+/**
+ * Maximum pitch before the throttle is powered up (during motor delay phase)
+ *
+ * This is an extra limit for the maximum pitch which is imposed in the phase before the throttle turns on.
+ * This allows to limit the maximum pitch angle during a bungee launch (make the launch less steep).
+ *
+ * @unit deg
+ * @min 0
+ * @max 45
+ * @group Launch detection
+ */
+PARAM_DEFINE_FLOAT(LAUN_CAT_PMAX, 30.0f);
+
+/**
  * Throttle setting while detecting launch.
  *
  * The throttle is set to this value while the system is waiting for the take-off.
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
index 0e58c68b6..339b11bbe 100644
--- a/src/modules/commander/airspeed_calibration.cpp
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -180,11 +180,13 @@ int do_airspeed_calibration(int mavlink_fd)
 		return ERROR;
 	}
 
-	mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset);
+	mavlink_log_critical(mavlink_fd, "Offset of %d Pascal", (int)diff_pres_offset);
 
 	/* wait 500 ms to ensure parameter propagated through the system */
 	usleep(500 * 1000);
 
+	mavlink_log_critical(mavlink_fd, "Create airflow now");
+
 	calibration_counter = 0;
 	const unsigned maxcount = 3000;
 
@@ -204,18 +206,18 @@ int do_airspeed_calibration(int mavlink_fd)
 			calibration_counter++;
 
 			if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
-				if (calibration_counter % 100 == 0) {
-					mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)",
-					(int)diff_pres.differential_pressure_raw_pa);
+				if (calibration_counter % 500 == 0) {
+					mavlink_log_info(mavlink_fd, "Create airflow! (%d, wanted: 50 Pa)",
+						(int)diff_pres.differential_pressure_raw_pa);
 				}
 				continue;
 			}
 
 			/* do not allow negative values */
 			if (diff_pres.differential_pressure_raw_pa < 0.0f) {
-				mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)",
-					(int)diff_pres.differential_pressure_raw_pa);
-				mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa);
+				mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!");
+				mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)",
+						(int)diff_pres.differential_pressure_raw_pa);
 				close(diff_pres_sub);
 
 				/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
@@ -236,7 +238,7 @@ int do_airspeed_calibration(int mavlink_fd)
 				mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
 				return ERROR;
 			} else {
-				mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)",
+				mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)",
 					(int)diff_pres.differential_pressure_raw_pa);
 				break;
 			}
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 6c2c03070..a5a772825 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -129,7 +129,7 @@ extern struct system_load_s system_load;
 #define POSITION_TIMEOUT		(2 * 1000 * 1000)	/**< consider the local or global position estimate invalid after 600ms */
 #define FAILSAFE_DEFAULT_TIMEOUT	(3 * 1000 * 1000)	/**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
 #define RC_TIMEOUT			500000
-#define DL_TIMEOUT			5 * 1000* 1000
+#define DL_TIMEOUT			(10 * 1000 * 1000)
 #define OFFBOARD_TIMEOUT		500000
 #define DIFFPRESS_TIMEOUT		2000000
 
@@ -615,6 +615,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 	case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
 	case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
 	case VEHICLE_CMD_PREFLIGHT_STORAGE:
+	case VEHICLE_CMD_CUSTOM_0:
+	case VEHICLE_CMD_CUSTOM_1:
+	case VEHICLE_CMD_CUSTOM_2:
+	case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY:
+	case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY:
 		/* ignore commands that handled in low prio loop */
 		break;
 
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index f8589d24b..684c61a17 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -182,12 +182,19 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 
 						// Fail transition if power levels on the avionics rail
 						// are measured but are insufficient
-						if (status->condition_power_input_valid && ((status->avionics_power_rail_voltage > 0.0f) &&
-							(status->avionics_power_rail_voltage < 4.9f))) {
-
-							mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
-							feedback_provided = true;
-							valid_transition = false;
+						if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) {
+							// Check avionics rail voltages
+							if (status->avionics_power_rail_voltage < 4.75f) {
+								mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
+								feedback_provided = true;
+								valid_transition = false;
+							} else if (status->avionics_power_rail_voltage < 4.9f) {
+								mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
+								feedback_provided = true;
+							} else if (status->avionics_power_rail_voltage > 5.4f) {
+								mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage);
+								feedback_provided = true;
+							}
 						}
 					}
 
diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
index ca1fe9bbb..b2355d4d8 100644
--- a/src/modules/dataman/dataman.c
+++ b/src/modules/dataman/dataman.c
@@ -797,7 +797,7 @@ start(void)
 	sem_init(&g_init_sema, 1, 0);
 
 	/* start the worker thread */
-	if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, task_main, NULL)) <= 0) {
+	if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2000, task_main, NULL)) <= 0) {
 		warn("task start failed");
 		return -1;
 	}
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 91d17e787..97abb76a9 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -58,6 +58,7 @@
 #include <drivers/drv_accel.h>
 #include <drivers/drv_mag.h>
 #include <drivers/drv_baro.h>
+#include <drivers/drv_range_finder.h>
 #ifdef SENSOR_COMBINED_SUB
 #include <uORB/topics/sensor_combined.h>
 #endif
@@ -96,7 +97,10 @@ extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]);
 
 __EXPORT uint32_t millis();
 
+__EXPORT uint64_t getMicros();
+
 static uint64_t IMUmsec = 0;
+static uint64_t IMUusec = 0;
 static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000;
 
 uint32_t millis()
@@ -104,6 +108,11 @@ uint32_t millis()
 	return IMUmsec;
 }
 
+uint64_t getMicros()
+{
+	return IMUusec;
+}
+
 class FixedwingEstimator
 {
 public:
@@ -171,6 +180,7 @@ private:
 #else
 	int		_sensor_combined_sub;
 #endif
+	int		_distance_sub;			/**< distance measurement */
 	int		_airspeed_sub;			/**< airspeed subscription */
 	int		_baro_sub;			/**< barometer subscription */
 	int		_gps_sub;			/**< GPS subscription */
@@ -196,7 +206,8 @@ private:
 	struct vehicle_global_position_s		_global_pos;		/**< global vehicle position */
 	struct vehicle_local_position_s			_local_pos;		/**< local vehicle position */
 	struct vehicle_gps_position_s			_gps;			/**< GPS position */
-	struct wind_estimate_s				_wind;			/**< Wind estimate */
+	struct wind_estimate_s				_wind;			/**< wind estimate */
+	struct range_finder_report			_distance;		/**< distance estimate */
 
 	struct gyro_scale				_gyro_offsets;
 	struct accel_scale				_accel_offsets;
@@ -226,6 +237,7 @@ private:
 	hrt_abstime					_filter_start_time;
 	hrt_abstime					_last_sensor_timestamp;
 	hrt_abstime					_last_run;
+	hrt_abstime					_distance_last_valid;
 	bool						_gyro_valid;
 	bool						_accel_valid;
 	bool						_mag_valid;
@@ -342,6 +354,7 @@ FixedwingEstimator::FixedwingEstimator() :
 #else
 	_sensor_combined_sub(-1),
 #endif
+	_distance_sub(-1),
 	_airspeed_sub(-1),
 	_baro_sub(-1),
 	_gps_sub(-1),
@@ -399,6 +412,7 @@ FixedwingEstimator::FixedwingEstimator() :
 	_filter_start_time(0),
 	_last_sensor_timestamp(0),
 	_last_run(0),
+	_distance_last_valid(0),
 	_gyro_valid(false),
 	_accel_valid(false),
 	_mag_valid(false),
@@ -549,6 +563,7 @@ FixedwingEstimator::parameters_update()
 		_ekf->gyroProcessNoise = _parameters.gyro_pnoise;
         	_ekf->accelProcessNoise = _parameters.acc_pnoise;
 		_ekf->airspeedMeasurementSigma = _parameters.eas_noise;
+		_ekf->rngFinderPitch = 0.0f; // XXX base on SENS_BOARD_Y_OFF
 	}
 
 	return OK;
@@ -704,6 +719,7 @@ FixedwingEstimator::task_main()
 	/*
 	 * do subscriptions
 	 */
+	_distance_sub = orb_subscribe(ORB_ID(sensor_range_finder));
 	_baro_sub = orb_subscribe(ORB_ID(sensor_baro0));
 	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
@@ -753,6 +769,7 @@ FixedwingEstimator::task_main()
 	bool newHgtData = false;
 	bool newAdsData = false;
 	bool newDataMag = false;
+	bool newRangeData = false;
 
 	float posNED[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m)
 
@@ -850,7 +867,8 @@ FixedwingEstimator::task_main()
 			}
 
 			_last_sensor_timestamp = _gyro.timestamp;
-			IMUmsec = _gyro.timestamp / 1e3f;
+			IMUmsec = _gyro.timestamp / 1e3;
+			IMUusec = _gyro.timestamp;
 
 			float deltaT = (_gyro.timestamp - _last_run) / 1e6f;
 			_last_run = _gyro.timestamp;
@@ -914,7 +932,8 @@ FixedwingEstimator::task_main()
 
 			// Copy gyro and accel
 			_last_sensor_timestamp = _sensor_combined.timestamp;
-			IMUmsec = _sensor_combined.timestamp / 1e3f;
+			IMUmsec = _sensor_combined.timestamp / 1e3;
+			IMUusec = _sensor_combined.timestamp;
 
 			float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
 
@@ -994,8 +1013,6 @@ FixedwingEstimator::task_main()
 
 			if (gps_updated) {
 
-				last_gps = _gps.timestamp_position;
-
 				orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
 				perf_count(_perf_gps);
 
@@ -1008,11 +1025,17 @@ FixedwingEstimator::task_main()
 					_gps_start_time = hrt_absolute_time();
 
 					/* check if we had a GPS outage for a long time */
-					if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) {
+					float gps_elapsed = hrt_elapsed_time(&last_gps) / 1e6f;
+
+					const float pos_reset_threshold = 5.0f; // seconds
+
+					/* timeout of 5 seconds */
+					if (gps_elapsed > pos_reset_threshold) {
 						_ekf->ResetPosition();
 						_ekf->ResetVelocity();
 						_ekf->ResetStoredStates();
 					}
+					_ekf->updateDtGpsFilt(math::constrain((_gps.timestamp_position - last_gps) / 1e6f, 0.01f, pos_reset_threshold));
 
 					/* fuse GPS updates */
 
@@ -1044,6 +1067,8 @@ FixedwingEstimator::task_main()
 
 					newDataGps = true;
 
+					last_gps = _gps.timestamp_position;
+
 				}
 
 			}
@@ -1052,8 +1077,15 @@ FixedwingEstimator::task_main()
 			orb_check(_baro_sub, &baro_updated);
 
 			if (baro_updated) {
+
+				hrt_abstime baro_last = _baro.timestamp;
+
 				orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
 
+				float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f;
+
+				_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1));
+
 				_ekf->baroHgt = _baro.altitude;
 
 				if (!_baro_init) {
@@ -1114,6 +1146,19 @@ FixedwingEstimator::task_main()
 				newDataMag = false;
 			}
 
+			orb_check(_distance_sub, &newRangeData);
+
+			if (newRangeData) {
+				orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance);
+
+				if (_distance.valid) {
+					_ekf->rngMea = _distance.distance;
+					_distance_last_valid = _distance.timestamp;
+				} else {
+					newRangeData = false;
+				}
+			}
+
 			/*
 			 *    CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY
 			 */
@@ -1197,6 +1242,7 @@ FixedwingEstimator::task_main()
 				} else if (_ekf->statesInitialised) {
 
 					// We're apparently initialized in this case now
+					// check (and reset the filter as needed)
 					int check = check_filter_state();
 
 					if (check) {
@@ -1206,21 +1252,7 @@ FixedwingEstimator::task_main()
 
 					// Run the strapdown INS equations every IMU update
 					_ekf->UpdateStrapdownEquationsNED();
-	#if 0
-					// debug code - could be tunred into a filter mnitoring/watchdog function
-					float tempQuat[4];
-
-					for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j];
-
-					quat2eul(eulerEst, tempQuat);
 
-					for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j];
-
-					if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi;
-
-					if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi;
-
-	#endif
 					// store the predicted states for subsequent use by measurement fusion
 					_ekf->StoreStates(IMUmsec);
 					// Check if on ground - status is used by covariance prediction
@@ -1334,6 +1366,13 @@ FixedwingEstimator::task_main()
 						_ekf->fuseVtasData = false;
 					}
 
+					if (newRangeData) {
+						_ekf->fuseRngData = true;
+						_ekf->useRangeFinder = true;
+						_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 500.0f));
+						_ekf->GroundEKF();
+					}
+
 
 					// Output results
 					math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
@@ -1447,6 +1486,10 @@ FixedwingEstimator::task_main()
 							_global_pos.vel_d = _local_pos.vz;
 						}
 
+						/* terrain altitude */
+						_global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1];
+						_global_pos.terrain_alt_valid = (_distance_last_valid > 0) &&
+						(hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000);
 
 						_global_pos.yaw = _local_pos.yaw;
 
@@ -1467,8 +1510,10 @@ FixedwingEstimator::task_main()
 
 						if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
 							_wind.timestamp = _global_pos.timestamp;
-							_wind.windspeed_north = _ekf->states[14];
-							_wind.windspeed_east = _ekf->states[15];
+							_wind.windspeed_north = _ekf->windSpdFiltNorth;
+							_wind.windspeed_east = _ekf->windSpdFiltEast;
+							// XXX we need to do something smart about the covariance here
+							// but we default to the estimate covariance for now
 							_wind.covariance_north = _ekf->P[14][14];
 							_wind.covariance_east = _ekf->P[15][15];
 
diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
index c7c7305b2..94a6d165f 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
@@ -2,6 +2,11 @@
 #include <string.h>
 #include <stdio.h>
 #include <stdarg.h>
+#include <math.h>
+
+#ifndef M_PI_F
+#define M_PI_F ((float)M_PI)
+#endif
 
 #define EKF_COVARIANCE_DIVERGED 1.0e8f
 
@@ -38,13 +43,14 @@ AttPosEKF::AttPosEKF() :
     resetStates{},
     storedStates{},
     statetimeStamp{},
+    lastVelPosFusion(millis()),
     statesAtVelTime{},
     statesAtPosTime{},
     statesAtHgtTime{},
     statesAtMagMeasTime{},
     statesAtVtasMeasTime{},
     statesAtRngTime{},
-    statesAtOptFlowTime{},
+    statesAtFlowTime{},
     correctedDelAng(),
     correctedDelVel(),
     summedDelAng(),
@@ -59,7 +65,16 @@ AttPosEKF::AttPosEKF() :
     accel(),
     dVelIMU(),
     dAngIMU(),
-    dtIMU(0),
+    dtIMU(0.005f),
+    dtIMUfilt(0.005f),
+    dtVelPos(0.01f),
+    dtVelPosFilt(0.01f),
+    dtHgtFilt(0.01f),
+    dtGpsFilt(0.1f),
+    windSpdFiltNorth(0.0f),
+    windSpdFiltEast(0.0f),
+    windSpdFiltAltitude(0.0f),
+    windSpdFiltClimb(0.0f),
     fusionModeGPS(0),
     innovVelPos{},
     varInnovVelPos{},
@@ -103,13 +118,13 @@ AttPosEKF::AttPosEKF() :
 
     inhibitWindStates(true),
     inhibitMagStates(true),
-    inhibitGndHgtState(true),
+    inhibitGndState(true),
 
     onGround(true),
     staticMode(true),
     useAirspeed(true),
     useCompass(true),
-    useRangeFinder(false),
+    useRangeFinder(true),
     useOpticalFlow(false),
 
     ekfDiverged(false),
@@ -117,7 +132,24 @@ AttPosEKF::AttPosEKF() :
     current_ekf_state{},
     last_ekf_error{},
     numericalProtection(true),
-    storeIndex(0)
+    storeIndex(0),
+    storedOmega{},
+    Popt{},
+    flowStates{},
+    prevPosN(0.0f),
+    prevPosE(0.0f),
+    auxFlowObsInnov{},
+    auxFlowObsInnovVar{},
+    fScaleFactorVar(0.0f),
+    Tnb_flow{},
+    R_LOS(0.0f),
+    auxFlowTestRatio{},
+    auxRngTestRatio(0.0f),
+    flowInnovGate(0.0f),
+    auxFlowInnovGate(0.0f),
+    rngInnovGate(0.0f),
+    minFlowRng(0.0f),
+    moCompR_LOS(0.0f)
 {
     memset(&last_ekf_error, 0, sizeof(last_ekf_error));
     memset(&current_ekf_state, 0, sizeof(current_ekf_state));
@@ -260,6 +292,9 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
 
     // Constrain states (to protect against filter divergence)
     ConstrainStates();
+
+    // update filtered IMU time step length
+    dtIMUfilt = 0.99f * dtIMUfilt + 0.01f * dtIMU;
 }
 
 void AttPosEKF::CovariancePrediction(float dt)
@@ -312,7 +347,7 @@ void AttPosEKF::CovariancePrediction(float dt)
     } else {
         for (uint8_t i=16; i<=21; i++) processNoise[i] = 0;
     }
-    if (!inhibitGndHgtState) {
+    if (!inhibitGndState) {
         processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma;
     } else {
         processNoise[22] = 0;
@@ -962,6 +997,21 @@ void AttPosEKF::CovariancePrediction(float dt)
         ConstrainVariances();
 }
 
+void AttPosEKF::updateDtGpsFilt(float dt)
+{
+    dtGpsFilt = ConstrainFloat(dt, 0.001f, 2.0f) * 0.05f + dtGpsFilt * 0.95f;
+}
+
+void AttPosEKF::updateDtHgtFilt(float dt)
+{
+    dtHgtFilt = ConstrainFloat(dt, 0.001f, 2.0f) * 0.05f + dtHgtFilt * 0.95f;
+}
+
+void AttPosEKF::updateDtVelPosFilt(float dt)
+{
+    dtVelPosFilt = ConstrainFloat(dt, 0.0005f, 2.0f) * 0.05f + dtVelPosFilt * 0.95f;
+}
+
 void AttPosEKF::FuseVelposNED()
 {
 
@@ -998,6 +1048,18 @@ void AttPosEKF::FuseVelposNED()
 // associated with sequential fusion
     if (fuseVelData || fusePosData || fuseHgtData)
     {
+        uint64_t tNow = getMicros();
+        updateDtVelPosFilt((tNow - lastVelPosFusion) / 1e6f);
+        lastVelPosFusion = tNow;
+
+        // scaler according to the number of repetitions of the
+        // same measurement in one fusion step
+        float gpsVarianceScaler = dtGpsFilt / dtVelPosFilt;
+
+        // scaler according to the number of repetitions of the
+        // same measurement in one fusion step
+        float hgtVarianceScaler = dtHgtFilt / dtVelPosFilt;
+
         // set the GPS data timeout depending on whether airspeed data is present
         if (useAirspeed) horizRetryTime = gpsRetryTime;
         else horizRetryTime = gpsRetryTimeNoTAS;
@@ -1010,12 +1072,12 @@ void AttPosEKF::FuseVelposNED()
         // Estimate the GPS Velocity, GPS horiz position and height measurement variances.
         velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring
         posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring
-        R_OBS[0] = sq(vneSigma) + sq(velErr);
+        R_OBS[0] = gpsVarianceScaler * sq(vneSigma) + sq(velErr);
         R_OBS[1] = R_OBS[0];
-        R_OBS[2] = sq(vdSigma) + sq(velErr);
-        R_OBS[3] = sq(posNeSigma) + sq(posErr);
+        R_OBS[2] = gpsVarianceScaler * sq(vdSigma) + sq(velErr);
+        R_OBS[3] = gpsVarianceScaler * sq(posNeSigma) + sq(posErr);
         R_OBS[4] = R_OBS[3];
-        R_OBS[5] = sq(posDSigma) + sq(posErr);
+        R_OBS[5] = hgtVarianceScaler * sq(posDSigma) + sq(posErr);
 
         // calculate innovations and check GPS data validity using an innovation consistency check
         if (fuseVelData)
@@ -1173,7 +1235,7 @@ void AttPosEKF::FuseVelposNED()
                     }
                 }
                 // Don't update terrain state if inhibited
-                if (inhibitGndHgtState) {
+                if (inhibitGndState) {
                     Kfusion[22] = 0;
                }
 
@@ -1356,7 +1418,7 @@ void AttPosEKF::FuseMagnetometer()
                     Kfusion[i] = 0;
                 }
             }
-            if (!inhibitGndHgtState) {
+            if (!inhibitGndState) {
                 Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]);
             } else {
                 Kfusion[22] = 0;
@@ -1430,11 +1492,6 @@ void AttPosEKF::FuseMagnetometer()
                 Kfusion[20] = 0;
                 Kfusion[21] = 0;
             }
-            if (!inhibitGndHgtState) {
-                Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]);
-            } else {
-                Kfusion[22] = 0;
-            }
             varInnovMag[1] = 1.0f/SK_MY[0];
             innovMag[1] = MagPred[1] - magData.y;
         }
@@ -1505,11 +1562,6 @@ void AttPosEKF::FuseMagnetometer()
                 Kfusion[20] = 0;
                 Kfusion[21] = 0;
             }
-            if (!inhibitGndHgtState) {
-                Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] + P[22][3]*SH_MAG[0] - P[22][1]*SK_MZ[2] + P[22][2]*SK_MZ[3] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[5] - P[22][17]*SK_MZ[4]);
-            } else {
-                Kfusion[22] = 0;
-            }
             varInnovMag[2] = 1.0f/SK_MZ[0];
             innovMag[2] = MagPred[2] - magData.z;
 
@@ -1616,6 +1668,32 @@ void AttPosEKF::FuseAirspeed()
     // Perform fusion of True Airspeed measurement
     if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
     {
+
+        float altDiff = fabsf(windSpdFiltAltitude - hgtMea);
+
+        if (isfinite(windSpdFiltClimb)) {
+            windSpdFiltClimb = ((1.0f - 0.0002f) * windSpdFiltClimb) + (0.0002f * states[6]);
+        } else {
+            windSpdFiltClimb = states[6];
+        }
+
+        if (altDiff < 20.0f) {
+            // Lowpass the output of the wind estimate - we want a long-term
+            // stable estimate, but not start to load into the overall dynamics
+            // of the system (which adjusting covariances would do)
+
+            // Change filter coefficient based on altitude change rate
+            float windFiltCoeff = ConstrainFloat(fabsf(windSpdFiltClimb) / 1000.0f, 0.00005f, 0.2f);
+
+            windSpdFiltNorth = ((1.0f - windFiltCoeff) * windSpdFiltNorth) + (windFiltCoeff * vwn);
+            windSpdFiltEast = ((1.0f - windFiltCoeff) * windSpdFiltEast) + (windFiltCoeff * vwe);
+        } else {
+            windSpdFiltNorth = vwn;
+            windSpdFiltEast = vwe;
+        }
+
+        windSpdFiltAltitude = hgtMea;
+
         // Calculate observation jacobians
         SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
         SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f;
@@ -1675,11 +1753,6 @@ void AttPosEKF::FuseAirspeed()
                 Kfusion[i] = 0;
             }
         }
-        if (!inhibitGndHgtState) {
-            Kfusion[22] = SK_TAS*(P[22][4]*SH_TAS[2] - P[22][14]*SH_TAS[2] + P[22][5]*SH_TAS[1] - P[22][15]*SH_TAS[1] + P[22][6]*vd*SH_TAS[0]);
-        } else {
-            Kfusion[22] = 0;
-        }
         varInnovVtas = 1.0f/SK_TAS;
 
         // Calculate the measurement innovation
@@ -1811,8 +1884,11 @@ void AttPosEKF::FuseRangeFinder()
         rngPred = (ptd - pd)/cosRngTilt;
         innovRng = rngPred - rngMea;
 
-        // Check the innovation for consistency and don't fuse if > 5Sigma
-        if ((innovRng*innovRng*SK_RNG[0]) < 25)
+        // calculate the innovation consistency test ratio
+        auxRngTestRatio = sq(innovRng) / (sq(rngInnovGate) * varInnovRng);
+
+        // Check the innovation for consistency and don't fuse if out of bounds
+        if (auxRngTestRatio < 1.0f)
         {
             // correct the state vector
             states[22] = states[22] - Kfusion[22] * innovRng;
@@ -1827,285 +1903,387 @@ void AttPosEKF::FuseRangeFinder()
 
 void AttPosEKF::FuseOptFlow()
 {
-    static uint8_t obsIndex;
-    static float SH_LOS[13];
-    static float SKK_LOS[15];
-    static float SK_LOS[2];
-    static float q0 = 0.0f;
-    static float q1 = 0.0f;
-    static float q2 = 0.0f;
-    static float q3 = 1.0f;
-    static float vn = 0.0f;
-    static float ve = 0.0f;
-    static float vd = 0.0f;
-    static float pd = 0.0f;
-    static float ptd = 0.0f;
-    static float R_LOS = 0.01f;
-    static float losPred[2];
-
-    // Transformation matrix from nav to body axes
-    Mat3f Tnb_local;
-    // Transformation matrix from body to sensor axes
-    // assume camera is aligned with Z body axis plus a misalignment
-    // defined by 3 small angles about X, Y and Z body axis
-    Mat3f Tbs;
-    Tbs.x.y =  a3;
-    Tbs.y.x = -a3;
-    Tbs.x.z = -a2;
-    Tbs.z.x =  a2;
-    Tbs.y.z =  a1;
-    Tbs.z.y = -a1;
-    // Transformation matrix from navigation to sensor axes
-    Mat3f Tns;
-    float H_LOS[n_states];
-    for (uint8_t i = 0; i < n_states; i++) {
-        H_LOS[i] = 0.0f;
-    }
-    Vector3f velNED_local;
-    Vector3f relVelSensor;
+//     static uint8_t obsIndex;
+//     static float SH_LOS[13];
+//     static float SKK_LOS[15];
+//     static float SK_LOS[2];
+//     static float q0 = 0.0f;
+//     static float q1 = 0.0f;
+//     static float q2 = 0.0f;
+//     static float q3 = 1.0f;
+//     static float vn = 0.0f;
+//     static float ve = 0.0f;
+//     static float vd = 0.0f;
+//     static float pd = 0.0f;
+//     static float ptd = 0.0f;
+//     static float R_LOS = 0.01f;
+//     static float losPred[2];
+
+//     // Transformation matrix from nav to body axes
+//     Mat3f Tnb_local;
+//     // Transformation matrix from body to sensor axes
+//     // assume camera is aligned with Z body axis plus a misalignment
+//     // defined by 3 small angles about X, Y and Z body axis
+//     Mat3f Tbs;
+//     Tbs.x.y =  a3;
+//     Tbs.y.x = -a3;
+//     Tbs.x.z = -a2;
+//     Tbs.z.x =  a2;
+//     Tbs.y.z =  a1;
+//     Tbs.z.y = -a1;
+//     // Transformation matrix from navigation to sensor axes
+//     Mat3f Tns;
+//     float H_LOS[n_states];
+//     for (uint8_t i = 0; i < n_states; i++) {
+//         H_LOS[i] = 0.0f;
+//     }
+//     Vector3f velNED_local;
+//     Vector3f relVelSensor;
+
+// // Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg.
+//     if (useOpticalFlow && (fuseOptFlowData || obsIndex == 1) && !onGround && Tbs.z.z > 0.866f && rngMea > 5.0f && rngMea < 39.0f)
+//     {
+//         // Sequential fusion of XY components to spread processing load across
+//         // two prediction time steps.
+
+//         // Calculate observation jacobians and Kalman gains
+//         if (fuseOptFlowData)
+//         {
+//             // Copy required states to local variable names
+//             q0       = statesAtOptFlowTime[0];
+//             q1       = statesAtOptFlowTime[1];
+//             q2       = statesAtOptFlowTime[2];
+//             q3       = statesAtOptFlowTime[3];
+//             vn       = statesAtOptFlowTime[4];
+//             ve       = statesAtOptFlowTime[5];
+//             vd       = statesAtOptFlowTime[6];
+//             pd       = statesAtOptFlowTime[9];
+//             ptd      = statesAtOptFlowTime[22];
+//             velNED_local.x = vn;
+//             velNED_local.y = ve;
+//             velNED_local.z = vd;
+
+//             // calculate rotation from NED to body axes
+//             float q00 = sq(q0);
+//             float q11 = sq(q1);
+//             float q22 = sq(q2);
+//             float q33 = sq(q3);
+//             float q01 = q0 * q1;
+//             float q02 = q0 * q2;
+//             float q03 = q0 * q3;
+//             float q12 = q1 * q2;
+//             float q13 = q1 * q3;
+//             float q23 = q2 * q3;
+//             Tnb_local.x.x = q00 + q11 - q22 - q33;
+//             Tnb_local.y.y = q00 - q11 + q22 - q33;
+//             Tnb_local.z.z = q00 - q11 - q22 + q33;
+//             Tnb_local.y.x = 2*(q12 - q03);
+//             Tnb_local.z.x = 2*(q13 + q02);
+//             Tnb_local.x.y = 2*(q12 + q03);
+//             Tnb_local.z.y = 2*(q23 - q01);
+//             Tnb_local.x.z = 2*(q13 - q02);
+//             Tnb_local.y.z = 2*(q23 + q01);
+
+//             // calculate transformation from NED to sensor axes
+//             Tns = Tbs*Tnb_local;
+
+//             // calculate range from ground plain to centre of sensor fov assuming flat earth
+//             float range = ConstrainFloat(((ptd - pd)/Tns.z.z),0.5f,100.0f);
+
+//             // calculate relative velocity in sensor frame
+//             relVelSensor = Tns*velNED_local;
+
+//             // divide velocity by range  and include angular rate effects to get predicted angular LOS rates relative to X and Y axes
+//             losPred[0] =  relVelSensor.y/range;
+//             losPred[1] = -relVelSensor.x/range;
+
+//             //printf("relVelSensor.x=%5.1f, relVelSensor.y=%5.1f\n", relVelSensor.x, relVelSensor.y);
+//             //printf("Xpred=%5.2f, Xmea=%5.2f, Ypred=%5.2f, Ymea=%5.2f, delAng.x=%4.4f, delAng.y=%4.4f\n", losPred[0], losData[0], losPred[1], losData[1], delAng.x, delAng.y);
+//             //printf("omegaX=%5.2f, omegaY=%5.2f, velY=%5.1f velX=%5.1f\n, range=%5.1f\n", delAngRel.x/dt, delAngRel.y/dt, relVelSensor.y, relVelSensor.x, range);
+
+//             // Calculate observation jacobians
+//             SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3);
+//             SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3));
+//             SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3));
+//             SH_LOS[3] = 1/(pd - ptd);
+//             SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2;
+//             SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3;
+//             SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1;
+//             SH_LOS[7] = 1/sq(pd - ptd);
+//             SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1;
+//             SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0;
+//             SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3;
+//             SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0;
+//             SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2;
+
+//             for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0;
+//             H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]);
+//             H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]);
+//             H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3);
+//             H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]);
+//             H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3));
+//             H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3));
+//             H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3));
+//             H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7];
+//             H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7];
+
+//             // Calculate Kalman gain
+//             SKK_LOS[0] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3);
+//             SKK_LOS[1] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3);
+//             SKK_LOS[2] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3);
+//             SKK_LOS[3] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3);
+//             SKK_LOS[4] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3);
+//             SKK_LOS[5] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3);
+//             SKK_LOS[6] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]);
+//             SKK_LOS[7] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]);
+//             SKK_LOS[8] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]);
+//             SKK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]);
+//             SKK_LOS[10] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]);
+//             SKK_LOS[11] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]);
+//             SKK_LOS[12] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]);
+//             SKK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]);
+//             SKK_LOS[14] = SH_LOS[0];
+
+//             SK_LOS[0] = 1/(R_LOS + SKK_LOS[8]*(P[0][0]*SKK_LOS[8] + P[1][0]*SKK_LOS[7] + P[2][0]*SKK_LOS[9] - P[3][0]*SKK_LOS[6] - P[9][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][0]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][0]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[7]*(P[0][1]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] + P[2][1]*SKK_LOS[9] - P[3][1]*SKK_LOS[6] - P[9][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][1]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][1]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[9]*(P[0][2]*SKK_LOS[8] + P[1][2]*SKK_LOS[7] + P[2][2]*SKK_LOS[9] - P[3][2]*SKK_LOS[6] - P[9][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][2]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][2]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SKK_LOS[6]*(P[0][3]*SKK_LOS[8] + P[1][3]*SKK_LOS[7] + P[2][3]*SKK_LOS[9] - P[3][3]*SKK_LOS[6] - P[9][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][3]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][3]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[8] + P[1][9]*SKK_LOS[7] + P[2][9]*SKK_LOS[9] - P[3][9]*SKK_LOS[6] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][9]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][9]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[8] + P[1][22]*SKK_LOS[7] + P[2][22]*SKK_LOS[9] - P[3][22]*SKK_LOS[6] - P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][22]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][22]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14]*(P[0][4]*SKK_LOS[8] + P[1][4]*SKK_LOS[7] + P[2][4]*SKK_LOS[9] - P[3][4]*SKK_LOS[6] - P[9][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][4]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14]*(P[0][5]*SKK_LOS[8] + P[1][5]*SKK_LOS[7] + P[2][5]*SKK_LOS[9] - P[3][5]*SKK_LOS[6] - P[9][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]*(P[0][6]*SKK_LOS[8] + P[1][6]*SKK_LOS[7] + P[2][6]*SKK_LOS[9] - P[3][6]*SKK_LOS[6] - P[9][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]));
+//             Kfusion[0] = -SK_LOS[0]*(P[0][0]*SKK_LOS[8] + P[0][1]*SKK_LOS[7] - P[0][3]*SKK_LOS[6] + P[0][2]*SKK_LOS[9] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[0][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[0][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+//             Kfusion[1] = -SK_LOS[0]*(P[1][0]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] - P[1][3]*SKK_LOS[6] + P[1][2]*SKK_LOS[9] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[1][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[1][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+//             Kfusion[2] = -SK_LOS[0]*(P[2][0]*SKK_LOS[8] + P[2][1]*SKK_LOS[7] - P[2][3]*SKK_LOS[6] + P[2][2]*SKK_LOS[9] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[2][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[2][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+//             Kfusion[3] = -SK_LOS[0]*(P[3][0]*SKK_LOS[8] + P[3][1]*SKK_LOS[7] - P[3][3]*SKK_LOS[6] + P[3][2]*SKK_LOS[9] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[3][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[3][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+//             Kfusion[4] = -SK_LOS[0]*(P[4][0]*SKK_LOS[8] + P[4][1]*SKK_LOS[7] - P[4][3]*SKK_LOS[6] + P[4][2]*SKK_LOS[9] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[4][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+//             Kfusion[5] = -SK_LOS[0]*(P[5][0]*SKK_LOS[8] + P[5][1]*SKK_LOS[7] - P[5][3]*SKK_LOS[6] + P[5][2]*SKK_LOS[9] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+//             Kfusion[6] = -SK_LOS[0]*(P[6][0]*SKK_LOS[8] + P[6][1]*SKK_LOS[7] - P[6][3]*SKK_LOS[6] + P[6][2]*SKK_LOS[9] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+//             Kfusion[7] = -SK_LOS[0]*(P[7][0]*SKK_LOS[8] + P[7][1]*SKK_LOS[7] - P[7][3]*SKK_LOS[6] + P[7][2]*SKK_LOS[9] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[7][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[7][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+//             Kfusion[8] = -SK_LOS[0]*(P[8][0]*SKK_LOS[8] + P[8][1]*SKK_LOS[7] - P[8][3]*SKK_LOS[6] + P[8][2]*SKK_LOS[9] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[8][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[8][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+//             Kfusion[9] = -SK_LOS[0]*(P[9][0]*SKK_LOS[8] + P[9][1]*SKK_LOS[7] - P[9][3]*SKK_LOS[6] + P[9][2]*SKK_LOS[9] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[9][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[9][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+//             Kfusion[10] = -SK_LOS[0]*(P[10][0]*SKK_LOS[8] + P[10][1]*SKK_LOS[7] - P[10][3]*SKK_LOS[6] + P[10][2]*SKK_LOS[9] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[10][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[10][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+//             Kfusion[11] = -SK_LOS[0]*(P[11][0]*SKK_LOS[8] + P[11][1]*SKK_LOS[7] - P[11][3]*SKK_LOS[6] + P[11][2]*SKK_LOS[9] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[11][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[11][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+//             Kfusion[12] = -SK_LOS[0]*(P[12][0]*SKK_LOS[8] + P[12][1]*SKK_LOS[7] - P[12][3]*SKK_LOS[6] + P[12][2]*SKK_LOS[9] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[12][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[12][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+//             Kfusion[13] = 0.0f;//-SK_LOS[0]*(P[13][0]*SKK_LOS[8] + P[13][1]*SKK_LOS[7] - P[13][3]*SKK_LOS[6] + P[13][2]*SKK_LOS[9] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[13][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[13][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+//             Kfusion[14] = -SK_LOS[0]*(P[14][0]*SKK_LOS[8] + P[14][1]*SKK_LOS[7] - P[14][3]*SKK_LOS[6] + P[14][2]*SKK_LOS[9] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[14][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[14][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+//             Kfusion[15] = -SK_LOS[0]*(P[15][0]*SKK_LOS[8] + P[15][1]*SKK_LOS[7] - P[15][3]*SKK_LOS[6] + P[15][2]*SKK_LOS[9] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[15][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[15][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+//             Kfusion[16] = -SK_LOS[0]*(P[16][0]*SKK_LOS[8] + P[16][1]*SKK_LOS[7] - P[16][3]*SKK_LOS[6] + P[16][2]*SKK_LOS[9] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[16][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[16][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+//             Kfusion[17] = -SK_LOS[0]*(P[17][0]*SKK_LOS[8] + P[17][1]*SKK_LOS[7] - P[17][3]*SKK_LOS[6] + P[17][2]*SKK_LOS[9] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[17][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[17][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+//             Kfusion[18] = -SK_LOS[0]*(P[18][0]*SKK_LOS[8] + P[18][1]*SKK_LOS[7] - P[18][3]*SKK_LOS[6] + P[18][2]*SKK_LOS[9] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[18][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[18][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+//             Kfusion[19] = -SK_LOS[0]*(P[19][0]*SKK_LOS[8] + P[19][1]*SKK_LOS[7] - P[19][3]*SKK_LOS[6] + P[19][2]*SKK_LOS[9] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[19][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[19][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+//             Kfusion[20] = -SK_LOS[0]*(P[20][0]*SKK_LOS[8] + P[20][1]*SKK_LOS[7] - P[20][3]*SKK_LOS[6] + P[20][2]*SKK_LOS[9] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[20][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[20][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+//             Kfusion[21] = -SK_LOS[0]*(P[21][0]*SKK_LOS[8] + P[21][1]*SKK_LOS[7] - P[21][3]*SKK_LOS[6] + P[21][2]*SKK_LOS[9] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[21][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[21][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+//             Kfusion[22] = -SK_LOS[0]*(P[22][0]*SKK_LOS[8] + P[22][1]*SKK_LOS[7] - P[22][3]*SKK_LOS[6] + P[22][2]*SKK_LOS[9] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[22][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[22][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+//             varInnovOptFlow[0] = 1.0f/SK_LOS[0];
+//             innovOptFlow[0] = losPred[0] - losData[0];
+
+//             // set the observation index to 1 to fuse the y component next time round and reset the commence fusion flag
+//             obsIndex = 1;
+//             fuseOptFlowData = false;
+//         }
+//         else if (obsIndex == 1) // we are now fusing the Y measurement
+//         {
+//             // Calculate observation jacobians
+//             for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0;
+//             H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]);
+//             H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]);
+//             H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]);
+//             H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1);
+//             H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3));
+//             H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3));
+//             H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3));
+//             H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7];
+//             H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7];
+
+//             // Calculate Kalman gains
+//             SK_LOS[1] = 1/(R_LOS + SKK_LOS[12]*(P[0][2]*SKK_LOS[10] + P[1][2]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[3][2]*SKK_LOS[13] - P[9][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][2]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][2]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SKK_LOS[13]*(P[0][3]*SKK_LOS[10] + P[1][3]*SKK_LOS[11] + P[2][3]*SKK_LOS[12] - P[3][3]*SKK_LOS[13] - P[9][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][3]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][3]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[10]*(P[0][0]*SKK_LOS[10] + P[1][0]*SKK_LOS[11] + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][0]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][0]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[11]*(P[0][1]*SKK_LOS[10] + P[1][1]*SKK_LOS[11] + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][1]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][1]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[10] + P[1][9]*SKK_LOS[11] + P[2][9]*SKK_LOS[12] - P[3][9]*SKK_LOS[13] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][9]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][9]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[10] + P[1][22]*SKK_LOS[11] + P[2][22]*SKK_LOS[12] - P[3][22]*SKK_LOS[13] - P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][22]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][22]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14]*(P[0][4]*SKK_LOS[10] + P[1][4]*SKK_LOS[11] + P[2][4]*SKK_LOS[12] - P[3][4]*SKK_LOS[13] - P[9][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][4]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]*(P[0][5]*SKK_LOS[10] + P[1][5]*SKK_LOS[11] + P[2][5]*SKK_LOS[12] - P[3][5]*SKK_LOS[13] - P[9][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]*(P[0][6]*SKK_LOS[10] + P[1][6]*SKK_LOS[11] + P[2][6]*SKK_LOS[12] - P[3][6]*SKK_LOS[13] - P[9][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]));
+//             Kfusion[0] = SK_LOS[1]*(P[0][0]*SKK_LOS[10] - P[0][3]*SKK_LOS[13] + P[0][1]*SKK_LOS[11] + P[0][2]*SKK_LOS[12] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[0][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+//             Kfusion[1] = SK_LOS[1]*(P[1][0]*SKK_LOS[10] - P[1][3]*SKK_LOS[13] + P[1][1]*SKK_LOS[11] + P[1][2]*SKK_LOS[12] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[1][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+//             Kfusion[2] = SK_LOS[1]*(P[2][0]*SKK_LOS[10] - P[2][3]*SKK_LOS[13] + P[2][1]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[2][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+//             Kfusion[3] = SK_LOS[1]*(P[3][0]*SKK_LOS[10] - P[3][3]*SKK_LOS[13] + P[3][1]*SKK_LOS[11] + P[3][2]*SKK_LOS[12] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[3][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+//             Kfusion[4] = SK_LOS[1]*(P[4][0]*SKK_LOS[10] - P[4][3]*SKK_LOS[13] + P[4][1]*SKK_LOS[11] + P[4][2]*SKK_LOS[12] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[4][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+//             Kfusion[5] = SK_LOS[1]*(P[5][0]*SKK_LOS[10] - P[5][3]*SKK_LOS[13] + P[5][1]*SKK_LOS[11] + P[5][2]*SKK_LOS[12] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+//             Kfusion[6] = SK_LOS[1]*(P[6][0]*SKK_LOS[10] - P[6][3]*SKK_LOS[13] + P[6][1]*SKK_LOS[11] + P[6][2]*SKK_LOS[12] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+//             Kfusion[7] = SK_LOS[1]*(P[7][0]*SKK_LOS[10] - P[7][3]*SKK_LOS[13] + P[7][1]*SKK_LOS[11] + P[7][2]*SKK_LOS[12] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[7][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+//             Kfusion[8] = SK_LOS[1]*(P[8][0]*SKK_LOS[10] - P[8][3]*SKK_LOS[13] + P[8][1]*SKK_LOS[11] + P[8][2]*SKK_LOS[12] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[8][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+//             Kfusion[9] = SK_LOS[1]*(P[9][0]*SKK_LOS[10] - P[9][3]*SKK_LOS[13] + P[9][1]*SKK_LOS[11] + P[9][2]*SKK_LOS[12] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[9][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+//             Kfusion[10] = SK_LOS[1]*(P[10][0]*SKK_LOS[10] - P[10][3]*SKK_LOS[13] + P[10][1]*SKK_LOS[11] + P[10][2]*SKK_LOS[12] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[10][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+//             Kfusion[11] = SK_LOS[1]*(P[11][0]*SKK_LOS[10] - P[11][3]*SKK_LOS[13] + P[11][1]*SKK_LOS[11] + P[11][2]*SKK_LOS[12] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[11][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+//             Kfusion[12] = SK_LOS[1]*(P[12][0]*SKK_LOS[10] - P[12][3]*SKK_LOS[13] + P[12][1]*SKK_LOS[11] + P[12][2]*SKK_LOS[12] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[12][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+//             Kfusion[13] = 0.0f;//SK_LOS[1]*(P[13][0]*SKK_LOS[10] - P[13][3]*SKK_LOS[13] + P[13][1]*SKK_LOS[11] + P[13][2]*SKK_LOS[12] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[13][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+//             Kfusion[14] = SK_LOS[1]*(P[14][0]*SKK_LOS[10] - P[14][3]*SKK_LOS[13] + P[14][1]*SKK_LOS[11] + P[14][2]*SKK_LOS[12] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[14][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+//             Kfusion[15] = SK_LOS[1]*(P[15][0]*SKK_LOS[10] - P[15][3]*SKK_LOS[13] + P[15][1]*SKK_LOS[11] + P[15][2]*SKK_LOS[12] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[15][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+//             Kfusion[16] = SK_LOS[1]*(P[16][0]*SKK_LOS[10] - P[16][3]*SKK_LOS[13] + P[16][1]*SKK_LOS[11] + P[16][2]*SKK_LOS[12] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[16][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+//             Kfusion[17] = SK_LOS[1]*(P[17][0]*SKK_LOS[10] - P[17][3]*SKK_LOS[13] + P[17][1]*SKK_LOS[11] + P[17][2]*SKK_LOS[12] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[17][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+//             Kfusion[18] = SK_LOS[1]*(P[18][0]*SKK_LOS[10] - P[18][3]*SKK_LOS[13] + P[18][1]*SKK_LOS[11] + P[18][2]*SKK_LOS[12] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[18][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+//             Kfusion[19] = SK_LOS[1]*(P[19][0]*SKK_LOS[10] - P[19][3]*SKK_LOS[13] + P[19][1]*SKK_LOS[11] + P[19][2]*SKK_LOS[12] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[19][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+//             Kfusion[20] = SK_LOS[1]*(P[20][0]*SKK_LOS[10] - P[20][3]*SKK_LOS[13] + P[20][1]*SKK_LOS[11] + P[20][2]*SKK_LOS[12] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[20][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+//             Kfusion[21] = SK_LOS[1]*(P[21][0]*SKK_LOS[10] - P[21][3]*SKK_LOS[13] + P[21][1]*SKK_LOS[11] + P[21][2]*SKK_LOS[12] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[21][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+//             Kfusion[22] = SK_LOS[1]*(P[22][0]*SKK_LOS[10] - P[22][3]*SKK_LOS[13] + P[22][1]*SKK_LOS[11] + P[22][2]*SKK_LOS[12] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[22][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+//             varInnovOptFlow[1] = 1.0f/SK_LOS[1];
+//             innovOptFlow[1] = losPred[1] - losData[1];
+
+//             // reset the observation index
+//             obsIndex = 0;
+//             fuseOptFlowData = false;
+//         }
+
+//         // Check the innovation for consistency and don't fuse if > 3Sigma
+//         if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 9.0f)
+//         {
+//             // correct the state vector
+//             for (uint8_t j = 0; j < n_states; j++)
+//             {
+//                 states[j] = states[j] - Kfusion[j] * innovOptFlow[obsIndex];
+//             }
+//             // normalise the quaternion states
+//             float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
+//             if (quatMag > 1e-12f)
+//             {
+//                 for (uint8_t j= 0; j<=3; j++)
+//                 {
+//                     float quatMagInv = 1.0f/quatMag;
+//                     states[j] = states[j] * quatMagInv;
+//                 }
+//             }
+//             // correct the covariance P = (I - K*H)*P
+//             // take advantage of the empty columns in KH to reduce the
+//             // number of operations
+//             for (uint8_t i = 0; i < n_states; i++)
+//             {
+//                 for (uint8_t j = 0; j <= 6; j++)
+//                 {
+//                     KH[i][j] = Kfusion[i] * H_LOS[j];
+//                 }
+//                 for (uint8_t j = 7; j <= 8; j++)
+//                 {
+//                     KH[i][j] = 0.0f;
+//                 }
+//                 KH[i][9] = Kfusion[i] * H_LOS[9];
+//                 for (uint8_t j = 10; j <= 21; j++)
+//                 {
+//                     KH[i][j] = 0.0f;
+//                 }
+//                 KH[i][22] = Kfusion[i] * H_LOS[22];
+//             }
+//             for (uint8_t i = 0; i < n_states; i++)
+//             {
+//                 for (uint8_t j = 0; j < n_states; j++)
+//                 {
+//                     KHP[i][j] = 0.0f;
+//                     for (uint8_t k = 0; k <= 6; k++)
+//                     {
+//                         KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
+//                     }
+//                     KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j];
+//                     KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j];
+//                 }
+//             }
+//         }
+//         for (uint8_t i = 0; i <  n_states; i++)
+//         {
+//             for (uint8_t j = 0; j <  n_states; j++)
+//             {
+//                 P[i][j] = P[i][j] - KHP[i][j];
+//             }
+//         }
+//         ForceSymmetry();
+//         ConstrainVariances();
+//     }
+}
 
-// Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg.
-    if (useOpticalFlow && (fuseOptFlowData || obsIndex == 1) && !onGround && Tbs.z.z > 0.866f && rngMea > 5.0f && rngMea < 39.0f)
-    {
-        // Sequential fusion of XY components to spread processing load across
-        // two prediction time steps.
+/*
+Estimation of optical flow sensor focal length scale factor and terrain height using a two state EKF
+This fiter requires optical flow rates that are not motion compensated
+Range to ground measurement is assumed to be via a narrow beam type sensor - eg laser
+*/
+void AttPosEKF::GroundEKF()
+{
+    // propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption
+    // limit distance to prevent intialisation afer bad gps causing bad numerical conditioning
+    if (!inhibitGndState) {
+        float distanceTravelledSq;
+        distanceTravelledSq = sq(statesAtRngTime[7] - prevPosN) + sq(statesAtRngTime[8] - prevPosE);
+        prevPosN = statesAtRngTime[7];
+        prevPosE = statesAtRngTime[8];
+        distanceTravelledSq = min(distanceTravelledSq, 100.0f);
+        Popt[1][1] += (distanceTravelledSq * sq(gndHgtSigma));
+    }
+    // we aren't using optical flow measurements in this hacked implementation so set the covariances for this state to zero to avoid numerical problems
+    Popt[0][0] = 0.0f;
+    Popt[0][1] = 0.0f;
+    Popt[1][0] = 0.0f;
+
+    // Fuse range finder data
+    // Need to check that our range finder tilt angle is less than 30 degrees
+    float cosRngTilt = - Tbn.z.x * sinf(rngFinderPitch) + Tbn.z.z * cosf(rngFinderPitch);
+    if (useRangeFinder && fuseRngData && cosRngTilt > 0.87f) {
+        float range; // range from camera to centre of image
+        float q0; // quaternion at optical flow measurement time
+        float q1; // quaternion at optical flow measurement time
+        float q2; // quaternion at optical flow measurement time
+        float q3; // quaternion at optical flow measurement time
+        float R_RNG = 0.5; // range measurement variance (m^2) TODO make this a function of range and tilt to allow for sensor, alignment and AHRS errors
+
+        // Copy required states to local variable names
+        q0             = statesAtRngTime[0];
+        q1             = statesAtRngTime[1];
+        q2             = statesAtRngTime[2];
+        q3             = statesAtRngTime[3];
+
+        // calculate Kalman gains
+        float SK_RNG[3];
+        SK_RNG[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
+        SK_RNG[1] = 1/(R_RNG + Popt[1][1]/sq(SK_RNG[0]));
+        SK_RNG[2] = 1/SK_RNG[0];
+        float K_RNG[2];
+        if (!inhibitScaleState) {
+            K_RNG[0] = Popt[0][1]*SK_RNG[1]*SK_RNG[2];
+        } else {
+            K_RNG[0] = 0.0f;
+        }
+        if (!inhibitGndState) {
+            K_RNG[1] = Popt[1][1]*SK_RNG[1]*SK_RNG[2];
+        } else {
+            K_RNG[1] = 0.0f;
+        }
 
-        // Calculate observation jacobians and Kalman gains
-        if (fuseOptFlowData)
-        {
-            // Copy required states to local variable names
-            q0       = statesAtOptFlowTime[0];
-            q1       = statesAtOptFlowTime[1];
-            q2       = statesAtOptFlowTime[2];
-            q3       = statesAtOptFlowTime[3];
-            vn       = statesAtOptFlowTime[4];
-            ve       = statesAtOptFlowTime[5];
-            vd       = statesAtOptFlowTime[6];
-            pd       = statesAtOptFlowTime[9];
-            ptd      = statesAtOptFlowTime[22];
-            velNED_local.x = vn;
-            velNED_local.y = ve;
-            velNED_local.z = vd;
-
-            // calculate rotation from NED to body axes
-            float q00 = sq(q0);
-            float q11 = sq(q1);
-            float q22 = sq(q2);
-            float q33 = sq(q3);
-            float q01 = q0 * q1;
-            float q02 = q0 * q2;
-            float q03 = q0 * q3;
-            float q12 = q1 * q2;
-            float q13 = q1 * q3;
-            float q23 = q2 * q3;
-            Tnb_local.x.x = q00 + q11 - q22 - q33;
-            Tnb_local.y.y = q00 - q11 + q22 - q33;
-            Tnb_local.z.z = q00 - q11 - q22 + q33;
-            Tnb_local.y.x = 2*(q12 - q03);
-            Tnb_local.z.x = 2*(q13 + q02);
-            Tnb_local.x.y = 2*(q12 + q03);
-            Tnb_local.z.y = 2*(q23 - q01);
-            Tnb_local.x.z = 2*(q13 - q02);
-            Tnb_local.y.z = 2*(q23 + q01);
-
-            // calculate transformation from NED to sensor axes
-            Tns = Tbs*Tnb_local;
-
-            // calculate range from ground plain to centre of sensor fov assuming flat earth
-            float range = ConstrainFloat(((ptd - pd)/Tns.z.z),0.5f,100.0f);
-
-            // calculate relative velocity in sensor frame
-            relVelSensor = Tns*velNED_local;
-
-            // divide velocity by range  and include angular rate effects to get predicted angular LOS rates relative to X and Y axes
-            losPred[0] =  relVelSensor.y/range;
-            losPred[1] = -relVelSensor.x/range;
-
-            //printf("relVelSensor.x=%5.1f, relVelSensor.y=%5.1f\n", relVelSensor.x, relVelSensor.y);
-            //printf("Xpred=%5.2f, Xmea=%5.2f, Ypred=%5.2f, Ymea=%5.2f, delAng.x=%4.4f, delAng.y=%4.4f\n", losPred[0], losData[0], losPred[1], losData[1], delAng.x, delAng.y);
-            //printf("omegaX=%5.2f, omegaY=%5.2f, velY=%5.1f velX=%5.1f\n, range=%5.1f\n", delAngRel.x/dt, delAngRel.y/dt, relVelSensor.y, relVelSensor.x, range);
+        // Calculate the innovation variance for data logging
+        varInnovRng = 1.0f/SK_RNG[1];
 
-            // Calculate observation jacobians
-            SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3);
-            SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3));
-            SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3));
-            SH_LOS[3] = 1/(pd - ptd);
-            SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2;
-            SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3;
-            SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1;
-            SH_LOS[7] = 1/sq(pd - ptd);
-            SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1;
-            SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0;
-            SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3;
-            SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0;
-            SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2;
-
-            for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0;
-            H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]);
-            H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]);
-            H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3);
-            H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]);
-            H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3));
-            H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3));
-            H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3));
-            H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7];
-            H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7];
+        // constrain terrain height to be below the vehicle
+        flowStates[1] = maxf(flowStates[1], statesAtRngTime[9] + minFlowRng);
 
-            // Calculate Kalman gain
-            SKK_LOS[0] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3);
-            SKK_LOS[1] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3);
-            SKK_LOS[2] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3);
-            SKK_LOS[3] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3);
-            SKK_LOS[4] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3);
-            SKK_LOS[5] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3);
-            SKK_LOS[6] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]);
-            SKK_LOS[7] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]);
-            SKK_LOS[8] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]);
-            SKK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]);
-            SKK_LOS[10] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]);
-            SKK_LOS[11] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]);
-            SKK_LOS[12] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]);
-            SKK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]);
-            SKK_LOS[14] = SH_LOS[0];
-
-            SK_LOS[0] = 1/(R_LOS + SKK_LOS[8]*(P[0][0]*SKK_LOS[8] + P[1][0]*SKK_LOS[7] + P[2][0]*SKK_LOS[9] - P[3][0]*SKK_LOS[6] - P[9][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][0]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][0]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[7]*(P[0][1]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] + P[2][1]*SKK_LOS[9] - P[3][1]*SKK_LOS[6] - P[9][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][1]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][1]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[9]*(P[0][2]*SKK_LOS[8] + P[1][2]*SKK_LOS[7] + P[2][2]*SKK_LOS[9] - P[3][2]*SKK_LOS[6] - P[9][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][2]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][2]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SKK_LOS[6]*(P[0][3]*SKK_LOS[8] + P[1][3]*SKK_LOS[7] + P[2][3]*SKK_LOS[9] - P[3][3]*SKK_LOS[6] - P[9][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][3]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][3]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[8] + P[1][9]*SKK_LOS[7] + P[2][9]*SKK_LOS[9] - P[3][9]*SKK_LOS[6] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][9]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][9]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[8] + P[1][22]*SKK_LOS[7] + P[2][22]*SKK_LOS[9] - P[3][22]*SKK_LOS[6] - P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][22]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][22]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14]*(P[0][4]*SKK_LOS[8] + P[1][4]*SKK_LOS[7] + P[2][4]*SKK_LOS[9] - P[3][4]*SKK_LOS[6] - P[9][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][4]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14]*(P[0][5]*SKK_LOS[8] + P[1][5]*SKK_LOS[7] + P[2][5]*SKK_LOS[9] - P[3][5]*SKK_LOS[6] - P[9][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]*(P[0][6]*SKK_LOS[8] + P[1][6]*SKK_LOS[7] + P[2][6]*SKK_LOS[9] - P[3][6]*SKK_LOS[6] - P[9][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]));
-            Kfusion[0] = -SK_LOS[0]*(P[0][0]*SKK_LOS[8] + P[0][1]*SKK_LOS[7] - P[0][3]*SKK_LOS[6] + P[0][2]*SKK_LOS[9] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[0][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[0][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-            Kfusion[1] = -SK_LOS[0]*(P[1][0]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] - P[1][3]*SKK_LOS[6] + P[1][2]*SKK_LOS[9] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[1][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[1][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-            Kfusion[2] = -SK_LOS[0]*(P[2][0]*SKK_LOS[8] + P[2][1]*SKK_LOS[7] - P[2][3]*SKK_LOS[6] + P[2][2]*SKK_LOS[9] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[2][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[2][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-            Kfusion[3] = -SK_LOS[0]*(P[3][0]*SKK_LOS[8] + P[3][1]*SKK_LOS[7] - P[3][3]*SKK_LOS[6] + P[3][2]*SKK_LOS[9] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[3][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[3][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-            Kfusion[4] = -SK_LOS[0]*(P[4][0]*SKK_LOS[8] + P[4][1]*SKK_LOS[7] - P[4][3]*SKK_LOS[6] + P[4][2]*SKK_LOS[9] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[4][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-            Kfusion[5] = -SK_LOS[0]*(P[5][0]*SKK_LOS[8] + P[5][1]*SKK_LOS[7] - P[5][3]*SKK_LOS[6] + P[5][2]*SKK_LOS[9] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-            Kfusion[6] = -SK_LOS[0]*(P[6][0]*SKK_LOS[8] + P[6][1]*SKK_LOS[7] - P[6][3]*SKK_LOS[6] + P[6][2]*SKK_LOS[9] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-            Kfusion[7] = -SK_LOS[0]*(P[7][0]*SKK_LOS[8] + P[7][1]*SKK_LOS[7] - P[7][3]*SKK_LOS[6] + P[7][2]*SKK_LOS[9] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[7][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[7][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-            Kfusion[8] = -SK_LOS[0]*(P[8][0]*SKK_LOS[8] + P[8][1]*SKK_LOS[7] - P[8][3]*SKK_LOS[6] + P[8][2]*SKK_LOS[9] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[8][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[8][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-            Kfusion[9] = -SK_LOS[0]*(P[9][0]*SKK_LOS[8] + P[9][1]*SKK_LOS[7] - P[9][3]*SKK_LOS[6] + P[9][2]*SKK_LOS[9] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[9][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[9][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-            Kfusion[10] = -SK_LOS[0]*(P[10][0]*SKK_LOS[8] + P[10][1]*SKK_LOS[7] - P[10][3]*SKK_LOS[6] + P[10][2]*SKK_LOS[9] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[10][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[10][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-            Kfusion[11] = -SK_LOS[0]*(P[11][0]*SKK_LOS[8] + P[11][1]*SKK_LOS[7] - P[11][3]*SKK_LOS[6] + P[11][2]*SKK_LOS[9] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[11][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[11][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-            Kfusion[12] = -SK_LOS[0]*(P[12][0]*SKK_LOS[8] + P[12][1]*SKK_LOS[7] - P[12][3]*SKK_LOS[6] + P[12][2]*SKK_LOS[9] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[12][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[12][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-            Kfusion[13] = 0.0f;//-SK_LOS[0]*(P[13][0]*SKK_LOS[8] + P[13][1]*SKK_LOS[7] - P[13][3]*SKK_LOS[6] + P[13][2]*SKK_LOS[9] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[13][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[13][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-            Kfusion[14] = -SK_LOS[0]*(P[14][0]*SKK_LOS[8] + P[14][1]*SKK_LOS[7] - P[14][3]*SKK_LOS[6] + P[14][2]*SKK_LOS[9] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[14][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[14][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-            Kfusion[15] = -SK_LOS[0]*(P[15][0]*SKK_LOS[8] + P[15][1]*SKK_LOS[7] - P[15][3]*SKK_LOS[6] + P[15][2]*SKK_LOS[9] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[15][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[15][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-            Kfusion[16] = -SK_LOS[0]*(P[16][0]*SKK_LOS[8] + P[16][1]*SKK_LOS[7] - P[16][3]*SKK_LOS[6] + P[16][2]*SKK_LOS[9] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[16][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[16][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-            Kfusion[17] = -SK_LOS[0]*(P[17][0]*SKK_LOS[8] + P[17][1]*SKK_LOS[7] - P[17][3]*SKK_LOS[6] + P[17][2]*SKK_LOS[9] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[17][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[17][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-            Kfusion[18] = -SK_LOS[0]*(P[18][0]*SKK_LOS[8] + P[18][1]*SKK_LOS[7] - P[18][3]*SKK_LOS[6] + P[18][2]*SKK_LOS[9] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[18][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[18][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-            Kfusion[19] = -SK_LOS[0]*(P[19][0]*SKK_LOS[8] + P[19][1]*SKK_LOS[7] - P[19][3]*SKK_LOS[6] + P[19][2]*SKK_LOS[9] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[19][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[19][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-            Kfusion[20] = -SK_LOS[0]*(P[20][0]*SKK_LOS[8] + P[20][1]*SKK_LOS[7] - P[20][3]*SKK_LOS[6] + P[20][2]*SKK_LOS[9] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[20][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[20][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-            Kfusion[21] = -SK_LOS[0]*(P[21][0]*SKK_LOS[8] + P[21][1]*SKK_LOS[7] - P[21][3]*SKK_LOS[6] + P[21][2]*SKK_LOS[9] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[21][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[21][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-            Kfusion[22] = -SK_LOS[0]*(P[22][0]*SKK_LOS[8] + P[22][1]*SKK_LOS[7] - P[22][3]*SKK_LOS[6] + P[22][2]*SKK_LOS[9] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[22][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[22][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-            varInnovOptFlow[0] = 1.0f/SK_LOS[0];
-            innovOptFlow[0] = losPred[0] - losData[0];
-
-            // reset the observation index to 0 (we start by fusing the X
-            // measurement)
-            obsIndex = 0;
-            fuseOptFlowData = false;
-        }
-        else if (obsIndex == 1) // we are now fusing the Y measurement
-        {
-            // Calculate observation jacobians
-            for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0;
-            H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]);
-            H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]);
-            H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]);
-            H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1);
-            H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3));
-            H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3));
-            H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3));
-            H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7];
-            H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7];
-
-            // Calculate Kalman gains
-            SK_LOS[1] = 1/(R_LOS + SKK_LOS[12]*(P[0][2]*SKK_LOS[10] + P[1][2]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[3][2]*SKK_LOS[13] - P[9][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][2]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][2]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SKK_LOS[13]*(P[0][3]*SKK_LOS[10] + P[1][3]*SKK_LOS[11] + P[2][3]*SKK_LOS[12] - P[3][3]*SKK_LOS[13] - P[9][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][3]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][3]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[10]*(P[0][0]*SKK_LOS[10] + P[1][0]*SKK_LOS[11] + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][0]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][0]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[11]*(P[0][1]*SKK_LOS[10] + P[1][1]*SKK_LOS[11] + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][1]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][1]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[10] + P[1][9]*SKK_LOS[11] + P[2][9]*SKK_LOS[12] - P[3][9]*SKK_LOS[13] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][9]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][9]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[10] + P[1][22]*SKK_LOS[11] + P[2][22]*SKK_LOS[12] - P[3][22]*SKK_LOS[13] - P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][22]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][22]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14]*(P[0][4]*SKK_LOS[10] + P[1][4]*SKK_LOS[11] + P[2][4]*SKK_LOS[12] - P[3][4]*SKK_LOS[13] - P[9][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][4]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]*(P[0][5]*SKK_LOS[10] + P[1][5]*SKK_LOS[11] + P[2][5]*SKK_LOS[12] - P[3][5]*SKK_LOS[13] - P[9][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]*(P[0][6]*SKK_LOS[10] + P[1][6]*SKK_LOS[11] + P[2][6]*SKK_LOS[12] - P[3][6]*SKK_LOS[13] - P[9][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]));
-            Kfusion[0] = SK_LOS[1]*(P[0][0]*SKK_LOS[10] - P[0][3]*SKK_LOS[13] + P[0][1]*SKK_LOS[11] + P[0][2]*SKK_LOS[12] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[0][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-            Kfusion[1] = SK_LOS[1]*(P[1][0]*SKK_LOS[10] - P[1][3]*SKK_LOS[13] + P[1][1]*SKK_LOS[11] + P[1][2]*SKK_LOS[12] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[1][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-            Kfusion[2] = SK_LOS[1]*(P[2][0]*SKK_LOS[10] - P[2][3]*SKK_LOS[13] + P[2][1]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[2][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-            Kfusion[3] = SK_LOS[1]*(P[3][0]*SKK_LOS[10] - P[3][3]*SKK_LOS[13] + P[3][1]*SKK_LOS[11] + P[3][2]*SKK_LOS[12] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[3][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-            Kfusion[4] = SK_LOS[1]*(P[4][0]*SKK_LOS[10] - P[4][3]*SKK_LOS[13] + P[4][1]*SKK_LOS[11] + P[4][2]*SKK_LOS[12] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[4][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-            Kfusion[5] = SK_LOS[1]*(P[5][0]*SKK_LOS[10] - P[5][3]*SKK_LOS[13] + P[5][1]*SKK_LOS[11] + P[5][2]*SKK_LOS[12] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-            Kfusion[6] = SK_LOS[1]*(P[6][0]*SKK_LOS[10] - P[6][3]*SKK_LOS[13] + P[6][1]*SKK_LOS[11] + P[6][2]*SKK_LOS[12] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-            Kfusion[7] = SK_LOS[1]*(P[7][0]*SKK_LOS[10] - P[7][3]*SKK_LOS[13] + P[7][1]*SKK_LOS[11] + P[7][2]*SKK_LOS[12] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[7][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-            Kfusion[8] = SK_LOS[1]*(P[8][0]*SKK_LOS[10] - P[8][3]*SKK_LOS[13] + P[8][1]*SKK_LOS[11] + P[8][2]*SKK_LOS[12] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[8][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-            Kfusion[9] = SK_LOS[1]*(P[9][0]*SKK_LOS[10] - P[9][3]*SKK_LOS[13] + P[9][1]*SKK_LOS[11] + P[9][2]*SKK_LOS[12] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[9][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-            Kfusion[10] = SK_LOS[1]*(P[10][0]*SKK_LOS[10] - P[10][3]*SKK_LOS[13] + P[10][1]*SKK_LOS[11] + P[10][2]*SKK_LOS[12] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[10][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-            Kfusion[11] = SK_LOS[1]*(P[11][0]*SKK_LOS[10] - P[11][3]*SKK_LOS[13] + P[11][1]*SKK_LOS[11] + P[11][2]*SKK_LOS[12] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[11][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-            Kfusion[12] = SK_LOS[1]*(P[12][0]*SKK_LOS[10] - P[12][3]*SKK_LOS[13] + P[12][1]*SKK_LOS[11] + P[12][2]*SKK_LOS[12] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[12][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-            Kfusion[13] = 0.0f;//SK_LOS[1]*(P[13][0]*SKK_LOS[10] - P[13][3]*SKK_LOS[13] + P[13][1]*SKK_LOS[11] + P[13][2]*SKK_LOS[12] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[13][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-            Kfusion[14] = SK_LOS[1]*(P[14][0]*SKK_LOS[10] - P[14][3]*SKK_LOS[13] + P[14][1]*SKK_LOS[11] + P[14][2]*SKK_LOS[12] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[14][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-            Kfusion[15] = SK_LOS[1]*(P[15][0]*SKK_LOS[10] - P[15][3]*SKK_LOS[13] + P[15][1]*SKK_LOS[11] + P[15][2]*SKK_LOS[12] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[15][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-            Kfusion[16] = SK_LOS[1]*(P[16][0]*SKK_LOS[10] - P[16][3]*SKK_LOS[13] + P[16][1]*SKK_LOS[11] + P[16][2]*SKK_LOS[12] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[16][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-            Kfusion[17] = SK_LOS[1]*(P[17][0]*SKK_LOS[10] - P[17][3]*SKK_LOS[13] + P[17][1]*SKK_LOS[11] + P[17][2]*SKK_LOS[12] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[17][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-            Kfusion[18] = SK_LOS[1]*(P[18][0]*SKK_LOS[10] - P[18][3]*SKK_LOS[13] + P[18][1]*SKK_LOS[11] + P[18][2]*SKK_LOS[12] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[18][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-            Kfusion[19] = SK_LOS[1]*(P[19][0]*SKK_LOS[10] - P[19][3]*SKK_LOS[13] + P[19][1]*SKK_LOS[11] + P[19][2]*SKK_LOS[12] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[19][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-            Kfusion[20] = SK_LOS[1]*(P[20][0]*SKK_LOS[10] - P[20][3]*SKK_LOS[13] + P[20][1]*SKK_LOS[11] + P[20][2]*SKK_LOS[12] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[20][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-            Kfusion[21] = SK_LOS[1]*(P[21][0]*SKK_LOS[10] - P[21][3]*SKK_LOS[13] + P[21][1]*SKK_LOS[11] + P[21][2]*SKK_LOS[12] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[21][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-            Kfusion[22] = SK_LOS[1]*(P[22][0]*SKK_LOS[10] - P[22][3]*SKK_LOS[13] + P[22][1]*SKK_LOS[11] + P[22][2]*SKK_LOS[12] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[22][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-            varInnovOptFlow[1] = 1.0f/SK_LOS[1];
-            innovOptFlow[1] = losPred[1] - losData[1];
-        }
+        // estimate range to centre of image
+        range = (flowStates[1] - statesAtRngTime[9]) * SK_RNG[2];
 
-        // Check the innovation for consistency and don't fuse if > 3Sigma
-        if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 9.0f)
-        {
-            // correct the state vector
-            for (uint8_t j = 0; j < n_states; j++)
-            {
-                states[j] = states[j] - Kfusion[j] * innovOptFlow[obsIndex];
-            }
-            // normalise the quaternion states
-            float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
-            if (quatMag > 1e-12f)
-            {
-                for (uint8_t j= 0; j<=3; j++)
-                {
-                    float quatMagInv = 1.0f/quatMag;
-                    states[j] = states[j] * quatMagInv;
-                }
-            }
-            // correct the covariance P = (I - K*H)*P
-            // take advantage of the empty columns in KH to reduce the
-            // number of operations
-            for (uint8_t i = 0; i < n_states; i++)
-            {
-                for (uint8_t j = 0; j <= 6; j++)
-                {
-                    KH[i][j] = Kfusion[i] * H_LOS[j];
-                }
-                for (uint8_t j = 7; j <= 8; j++)
-                {
-                    KH[i][j] = 0.0f;
-                }
-                KH[i][9] = Kfusion[i] * H_LOS[9];
-                for (uint8_t j = 10; j <= 21; j++)
-                {
-                    KH[i][j] = 0.0f;
-                }
-                KH[i][22] = Kfusion[i] * H_LOS[22];
-            }
-            for (uint8_t i = 0; i < n_states; i++)
-            {
-                for (uint8_t j = 0; j < n_states; j++)
-                {
-                    KHP[i][j] = 0.0f;
-                    for (uint8_t k = 0; k <= 6; k++)
-                    {
-                        KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
-                    }
-                    KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j];
-                    KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j];
-                }
-            }
-        }
-        for (uint8_t i = 0; i <  n_states; i++)
+        // Calculate the measurement innovation
+        innovRng = range - rngMea;
+
+        // calculate the innovation consistency test ratio
+        auxRngTestRatio = sq(innovRng) / (sq(rngInnovGate) * varInnovRng);
+
+        // Check the innovation for consistency and don't fuse if out of bounds
+        if (auxRngTestRatio < 1.0f)
         {
-            for (uint8_t j = 0; j <  n_states; j++)
-            {
-                P[i][j] = P[i][j] - KHP[i][j];
+            // correct the state
+            for (uint8_t i = 0; i < 2 ; i++) {
+                flowStates[i] -= K_RNG[i] * innovRng;
             }
+            // constrain the states
+
+            // constrain focal length to 0.1 to 10 mm
+            flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f);
+            // constrain altitude
+            flowStates[1] = maxf(flowStates[1], statesAtRngTime[9] + minFlowRng);
+
+            // correct the covariance matrix
+            float nextPopt[2][2];
+            nextPopt[0][0] = Popt[0][0] - (Popt[0][1]*Popt[1][0]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2];
+            nextPopt[0][1] = Popt[0][1] - (Popt[0][1]*Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2];
+            nextPopt[1][0] = -Popt[1][0]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f);
+            nextPopt[1][1] = -Popt[1][1]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f);
+            // prevent the state variances from becoming negative and maintain symmetry
+            Popt[0][0] = maxf(nextPopt[0][0],0.0f);
+            Popt[1][1] = maxf(nextPopt[1][1],0.0f);
+            Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]);
+            Popt[1][0] = Popt[0][1];
         }
     }
-    obsIndex = obsIndex + 1;
-    ForceSymmetry();
-    ConstrainVariances();
 }
 
 void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
@@ -2126,6 +2304,24 @@ float AttPosEKF::sq(float valIn)
     return valIn*valIn;
 }
 
+float AttPosEKF::maxf(float valIn1, float valIn2)
+{
+    if (valIn1 >= valIn2) {
+        return valIn1;
+    } else {
+        return valIn2;
+    }
+}
+
+float AttPosEKF::min(float valIn1, float valIn2)
+{
+    if (valIn1 <= valIn2) {
+        return valIn1;
+    } else {
+        return valIn2;
+    }
+}
+
 // Store states in a history array along with time stamp
 void AttPosEKF::StoreStates(uint64_t timestamp_ms)
 {
@@ -2322,9 +2518,9 @@ void AttPosEKF::OnGroundCheck()
     }
     // don't update terrain offset state if on ground
     if (onGround) {
-        inhibitGndHgtState = true;
+        inhibitGndState = true;
     } else {
-        inhibitGndHgtState = false;
+        inhibitGndState = false;
     }
 }
 
@@ -3006,9 +3202,14 @@ void AttPosEKF::ZeroVariables()
 {
 
     // Initialize on-init initialized variables
-
+    dtIMUfilt = ConstrainFloat(dtIMU, 0.001f, 0.02f);
+    dtVelPosFilt = ConstrainFloat(dtVelPos, 0.04f, 0.5f);
+    dtGpsFilt = 1.0f / 5.0f;
+    dtHgtFilt = 1.0f / 100.0f;
     storeIndex = 0;
 
+    lastVelPosFusion = millis();
+
     // Do the data structure init
     for (unsigned i = 0; i < n_states; i++) {
         for (unsigned j = 0; j < n_states; j++) {
@@ -3028,6 +3229,13 @@ void AttPosEKF::ZeroVariables()
     dVelIMU.zero();
     lastGyroOffset.zero();
 
+    windSpdFiltNorth = 0.0f;
+    windSpdFiltEast = 0.0f;
+    // setting the altitude to zero will give us a higher
+    // gain to adjust faster in the first step
+    windSpdFiltAltitude = 0.0f;
+    windSpdFiltClimb = 0.0f;
+
     for (unsigned i = 0; i < data_buffer_size; i++) {
 
         for (unsigned j = 0; j < n_states; j++) {
diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h
index faa6735ca..a607955a8 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_23states.h
+++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h
@@ -80,6 +80,14 @@ public:
         airspeedMeasurementSigma = 1.4f;
         gyroProcessNoise = 1.4544411e-2f;
         accelProcessNoise = 0.5f;
+
+        gndHgtSigma  = 0.1f; // terrain gradient 1-sigma
+        R_LOS = 0.03f; // optical flow measurement noise variance (rad/sec)^2
+        flowInnovGate = 3.0f; // number of standard deviations applied to the optical flow innovation consistency check
+        auxFlowInnovGate = 10.0f; // number of standard deviations applied to the optical flow innovation consistency check used by the auxiliary filter
+        rngInnovGate = 10.0f; // number of standard deviations applied to the rnage finder innovation consistency check
+        minFlowRng = 0.01f; //minimum range between ground and flow sensor
+        moCompR_LOS = 0.2; // scaler from sensor gyro rate to uncertainty in LOS rate
     }
 
     struct mag_state_struct {
@@ -116,13 +124,16 @@ public:
     float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
     uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
 
+    // Times
+    uint64_t lastVelPosFusion;  // the time of the last velocity fusion, in the standard time unit of the filter
+
     float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
     float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
     float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
     float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
     float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
     float statesAtRngTime[n_states]; // filter states at the effective measurement time
-    float statesAtOptFlowTime[n_states]; // States at the effective optical flow measurement time
+    float statesAtFlowTime[n_states]; // States at the effective optical flow measurement time
 
     Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
     Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
@@ -140,7 +151,16 @@ public:
     Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
     Vector3f dVelIMU;
     Vector3f dAngIMU;
-    float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
+    float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec), this may have significant jitter
+    float dtIMUfilt; // average time between IMU measurements (sec)
+    float dtVelPos; // time lapsed since the last position / velocity fusion (seconds), this may have significant jitter
+    float dtVelPosFilt; // average time between position / velocity fusion steps
+    float dtHgtFilt; // average time between height measurement updates
+    float dtGpsFilt; // average time between gps measurement updates
+    float windSpdFiltNorth; // average wind speed north component
+    float windSpdFiltEast; // average wind speed east component
+    float windSpdFiltAltitude; // the last altitude used to filter wind speed
+    float windSpdFiltClimb; // filtered climb rate
     uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
     float innovVelPos[6]; // innovation output
     float varInnovVelPos[6]; // innovation variance output
@@ -192,7 +212,8 @@ public:
 
     bool inhibitWindStates; // true when wind states and covariances are to remain constant
     bool inhibitMagStates;  // true when magnetic field states and covariances are to remain constant
-    bool inhibitGndHgtState; // true when the terrain ground height offset state and covariances are to remain constant
+    bool inhibitGndState; // true when the terrain ground height offset state and covariances are to remain constant
+    bool inhibitScaleState; // true when the focal length scale factor state and covariances are to remain constant
 
     bool onGround;    ///< boolean true when the flight vehicle is on the ground (not flying)
     bool staticMode;    ///< boolean true if no position feedback is fused
@@ -211,6 +232,30 @@ public:
 
     unsigned storeIndex;
 
+    // Optical Flow error estimation
+    float storedOmega[3][data_buffer_size]; // angular rate vector stored for the last 50 time steps used by optical flow eror estimators
+
+    // Two state EKF used to estimate focal length scale factor and terrain position
+    float Popt[2][2];                       // state covariance matrix
+    float flowStates[2];                    // flow states [scale factor, terrain position]
+    float prevPosN;                         // north position at last measurement
+    float prevPosE;                         // east position at last measurement
+    float auxFlowObsInnov[2];               // optical flow observation innovations from focal length scale factor estimator
+    float auxFlowObsInnovVar[2];            // innovation variance for optical flow observations from focal length scale factor estimator
+    float fScaleFactorVar;                  // optical flow sensor focal length scale factor variance
+    Mat3f Tnb_flow;                         // Transformation matrix from nav to body at the time fo the optical flow measurement
+    float R_LOS;                            // Optical flow observation noise variance (rad/sec)^2
+    float auxFlowTestRatio[2];              // ratio of X and Y flow observation innovations to fault threshold
+    float auxRngTestRatio;                  // ratio of range observation innovations to fault threshold
+    float flowInnovGate;                    // number of standard deviations used for the innovation consistency check
+    float auxFlowInnovGate;                 // number of standard deviations applied to the optical flow innovation consistency check
+    float rngInnovGate;                     // number of standard deviations used for the innovation consistency check
+    float minFlowRng;                       // minimum range over which to fuse optical flow measurements
+    float moCompR_LOS;                      // scaler from sensor gyro rate to uncertainty in LOS rate
+
+void updateDtGpsFilt(float dt);
+
+void updateDtHgtFilt(float dt);
 
 void  UpdateStrapdownEquationsNED();
 
@@ -226,6 +271,8 @@ void FuseRangeFinder();
 
 void FuseOptFlow();
 
+void GroundEKF();
+
 void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
 
 void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
@@ -268,6 +315,10 @@ static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
 
 static float sq(float valIn);
 
+static float maxf(float valIn1, float valIn2);
+
+static float min(float valIn1, float valIn2);
+
 void OnGroundCheck();
 
 void CovarianceInit();
@@ -300,6 +351,8 @@ void InitializeDynamic(float (&initvelNED)[3], float declination);
 
 protected:
 
+void updateDtVelPosFilt(float dt);
+
 bool FilterHealthy();
 
 bool GyroOffsetsDiverged();
@@ -314,3 +367,5 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl
 
 uint32_t millis();
 
+uint64_t getMicros();
+
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 0cea13cc4..c60d8d348 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -63,6 +63,7 @@
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_status.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/pid/pid.h>
@@ -124,6 +125,7 @@ private:
 	int 		_params_sub;			/**< notification of parameter updates */
 	int 		_manual_sub;			/**< notification of manual control updates */
 	int		_global_pos_sub;		/**< global position subscription */
+	int		_vehicle_status_sub;		/**< vehicle status subscription */
 
 	orb_advert_t	_rate_sp_pub;			/**< rate setpoint publication */
 	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint point */
@@ -139,12 +141,14 @@ private:
 	struct actuator_controls_s			_actuators;		/**< actuator control inputs */
 	struct actuator_controls_s			_actuators_airframe;	/**< actuator control inputs */
 	struct vehicle_global_position_s		_global_pos;		/**< global position */
+	struct vehicle_status_s				_vehicle_status;	/**< vehicle status */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 	perf_counter_t	_nonfinite_input_perf;		/**< performance counter for non finite input */
 	perf_counter_t	_nonfinite_output_perf;		/**< performance counter for non finite output */
 
 	bool		_setpoint_valid;		/**< flag if the position control setpoint is valid */
+	bool		_debug;				/**< if set to true, print debug output */
 
 	struct {
 		float tconst;
@@ -275,6 +279,11 @@ private:
 	void		global_pos_poll();
 
 	/**
+	 * Check for vehicle status updates.
+	 */
+	void		vehicle_status_poll();
+
+	/**
 	 * Shim for calling task_main from task_create.
 	 */
 	static void	task_main_trampoline(int argc, char *argv[]);
@@ -312,6 +321,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_params_sub(-1),
 	_manual_sub(-1),
 	_global_pos_sub(-1),
+	_vehicle_status_sub(-1),
 
 /* publications */
 	_rate_sp_pub(-1),
@@ -324,7 +334,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")),
 	_nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")),
 /* states */
-	_setpoint_valid(false)
+	_setpoint_valid(false),
+	_debug(false)
 {
 	/* safely initialize structs */
 	_att = {};
@@ -336,6 +347,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_actuators = {};
 	_actuators_airframe = {};
 	_global_pos = {};
+	_vehicle_status = {};
 
 
 	_parameter_handles.tconst = param_find("FW_ATT_TC");
@@ -559,6 +571,18 @@ FixedwingAttitudeControl::global_pos_poll()
 }
 
 void
+FixedwingAttitudeControl::vehicle_status_poll()
+{
+	/* check if there is new status information */
+	bool vehicle_status_updated;
+	orb_check(_vehicle_status_sub, &vehicle_status_updated);
+
+	if (vehicle_status_updated) {
+		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
+	}
+}
+
+void
 FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[])
 {
 	att_control::g_control->task_main();
@@ -583,6 +607,7 @@ FixedwingAttitudeControl::task_main()
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 
 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_vcontrol_mode_sub, 200);
@@ -597,6 +622,7 @@ FixedwingAttitudeControl::task_main()
 	vehicle_accel_poll();
 	vehicle_control_mode_poll();
 	vehicle_manual_poll();
+	vehicle_status_poll();
 
 	/* wakeup source(s) */
 	struct pollfd fds[2];
@@ -665,6 +691,8 @@ FixedwingAttitudeControl::task_main()
 
 			global_pos_poll();
 
+			vehicle_status_poll();
+
 			/* lock integrator until control is started */
 			bool lock_integrator;
 
@@ -700,7 +728,8 @@ FixedwingAttitudeControl::task_main()
 						perf_count(_nonfinite_input_perf);
 					}
 				} else {
-					airspeed = _airspeed.true_airspeed_m_s;
+					/* prevent numerical drama by requiring 0.5 m/s minimal speed */
+					airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s);
 				}
 
 				/*
@@ -776,6 +805,13 @@ FixedwingAttitudeControl::task_main()
 					}
 				}
 
+				/* If the aircraft is on ground reset the integrators */
+				if (_vehicle_status.condition_landed) {
+					_roll_ctrl.reset_integrator();
+					_pitch_ctrl.reset_integrator();
+					_yaw_ctrl.reset_integrator();
+				}
+
 				/* Prepare speed_body_u and speed_body_w */
 				float speed_body_u = 0.0f;
 				float speed_body_v = 0.0f;
@@ -785,7 +821,7 @@ FixedwingAttitudeControl::task_main()
 					speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d;
 					speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d;
 				} else	{
-					if (loop_counter % 10 == 0) {
+					if (_debug && loop_counter % 10 == 0) {
 						warnx("Did not get a valid R\n");
 					}
 				}
@@ -808,7 +844,7 @@ FixedwingAttitudeControl::task_main()
 						_roll_ctrl.reset_integrator();
 						perf_count(_nonfinite_output_perf);
 
-						if (loop_counter % 10 == 0) {
+						if (_debug && loop_counter % 10 == 0) {
 							warnx("roll_u %.4f", (double)roll_u);
 						}
 					}
@@ -821,7 +857,7 @@ FixedwingAttitudeControl::task_main()
 					if (!isfinite(pitch_u)) {
 						_pitch_ctrl.reset_integrator();
 						perf_count(_nonfinite_output_perf);
-						if (loop_counter % 10 == 0) {
+						if (_debug && loop_counter % 10 == 0) {
 							warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
 								" airspeed %.4f, airspeed_scaling %.4f,"
 								" roll_sp %.4f, pitch_sp %.4f,"
@@ -845,7 +881,7 @@ FixedwingAttitudeControl::task_main()
 					if (!isfinite(yaw_u)) {
 						_yaw_ctrl.reset_integrator();
 						perf_count(_nonfinite_output_perf);
-						if (loop_counter % 10 == 0) {
+						if (_debug && loop_counter % 10 == 0) {
 							warnx("yaw_u %.4f", (double)yaw_u);
 						}
 					}
@@ -853,13 +889,13 @@ FixedwingAttitudeControl::task_main()
 					/* throttle passed through */
 					_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
 					if (!isfinite(throttle_sp)) {
-						if (loop_counter % 10 == 0) {
+						if (_debug && loop_counter % 10 == 0) {
 							warnx("throttle_sp %.4f", (double)throttle_sp);
 						}
 					}
 				} else {
 					perf_count(_nonfinite_input_perf);
-					if (loop_counter % 10 == 0) {
+					if (_debug && loop_counter % 10 == 0) {
 						warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
 					}
 				}
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 350ce6dec..23167eef4 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -102,6 +102,8 @@ static int	_control_task = -1;			/**< task handle for sensor task */
  */
 extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]);
 
+using namespace launchdetection;
+
 class FixedwingPositionControl
 {
 public:
@@ -171,8 +173,7 @@ private:
 	bool land_onslope;
 
 	/* takeoff/launch states */
-	bool launch_detected;
-	bool usePreTakeoffThrust;
+	LaunchDetectionResult launch_detection_state;
 
 	bool last_manual;				///< true if the last iteration was in manual mode (used to determine when a reset is needed)
 
@@ -210,6 +211,7 @@ private:
 		float max_climb_rate;
 		float climbout_diff;
 		float heightrate_p;
+		float heightrate_ff;
 		float speedrate_p;
 		float throttle_damp;
 		float integrator_gain;
@@ -255,6 +257,7 @@ private:
 		param_t max_climb_rate;
 		param_t climbout_diff;
 		param_t heightrate_p;
+		param_t heightrate_ff;
 		param_t speedrate_p;
 		param_t throttle_damp;
 		param_t integrator_gain;
@@ -380,7 +383,8 @@ private:
 			bool climbout_mode, float climbout_pitch_min_rad,
 			float altitude,
 			const math::Vector<3> &ground_speed,
-			tecs_mode mode = TECS_MODE_NORMAL);
+			tecs_mode mode = TECS_MODE_NORMAL,
+			bool pitch_max_special = false);
 
 };
 
@@ -438,8 +442,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	land_stayonground(false),
 	land_motor_lim(false),
 	land_onslope(false),
-	launch_detected(false),
-	usePreTakeoffThrust(false),
+	launch_detection_state(LAUNCHDETECTION_RES_NONE),
 	last_manual(false),
 	landingslope(),
 	flare_curve_alt_rel_last(0.0f),
@@ -494,6 +497,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.speed_weight = 			param_find("FW_T_SPDWEIGHT");
 	_parameter_handles.pitch_damping = 			param_find("FW_T_PTCH_DAMP");
 	_parameter_handles.heightrate_p =			param_find("FW_T_HRATE_P");
+	_parameter_handles.heightrate_ff =			param_find("FW_T_HRATE_FF");
 	_parameter_handles.speedrate_p =			param_find("FW_T_SRATE_P");
 
 	/* fetch initial parameter values */
@@ -563,12 +567,19 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.climbout_diff, &(_parameters.climbout_diff));
 
 	param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p));
+	param_get(_parameter_handles.heightrate_ff, &(_parameters.heightrate_ff));
 	param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p));
 
 	param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle));
 	param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
 	param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
 	param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative));
+
+	/* check if negative value for 2/3 of flare altitude is set for throttle cut */
+	if (_parameters.land_thrust_lim_alt_relative < 0.0f) {
+		_parameters.land_thrust_lim_alt_relative = 0.66f * _parameters.land_flare_alt_relative;
+	}
+
 	param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));
 
 	param_get(_parameter_handles.range_finder_rel_alt, &(_parameters.range_finder_rel_alt));
@@ -594,6 +605,7 @@ FixedwingPositionControl::parameters_update()
 	_tecs.set_indicated_airspeed_max(_parameters.airspeed_max);
 	_tecs.set_max_climb_rate(_parameters.max_climb_rate);
 	_tecs.set_heightrate_p(_parameters.heightrate_p);
+	_tecs.set_heightrate_ff(_parameters.heightrate_ff);
 	_tecs.set_speedrate_p(_parameters.speedrate_p);
 
 	/* sanity check parameters */
@@ -823,7 +835,7 @@ float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt,
 	 * the measurement is valid
 	 * the estimated relative altitude (from global altitude estimate and landing waypoint) <= range_finder_use_relative_alt
 	 */
-	if (range_finder_use_relative_alt < 0 || !range_finder.valid || rel_alt_estimated > range_finder_use_relative_alt ) {
+	if (range_finder_use_relative_alt < 0 || !range_finder.valid || range_finder.distance > range_finder_use_relative_alt ) {
 		return rel_alt_estimated;
 	}
 
@@ -1076,41 +1088,46 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) {
 
 			/* Perform launch detection */
-			if(!launch_detected) { //do not do further checks once a launch was detected
-				if (launchDetector.launchDetectionEnabled()) {
-					static hrt_abstime last_sent = 0;
-					if(hrt_absolute_time() - last_sent > 4e6) {
-						mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
-						last_sent = hrt_absolute_time();
-					}
-
-					/* Tell the attitude controller to stop integrating while we are waiting
-					 * for the launch */
-					_att_sp.roll_reset_integral = true;
-					_att_sp.pitch_reset_integral = true;
-					_att_sp.yaw_reset_integral = true;
-
-					/* Detect launch */
-					launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
-					if (launchDetector.getLaunchDetected()) {
-						launch_detected = true;
-						mavlink_log_info(_mavlink_fd, "#audio: Takeoff");
-					}
-				} else	{
-					/* no takeoff detection --> fly */
-					launch_detected = true;
-					warnx("launchdetection off");
+			if (launchDetector.launchDetectionEnabled() &&
+					launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
+				/* Inform user that launchdetection is running */
+				static hrt_abstime last_sent = 0;
+				if(hrt_absolute_time() - last_sent > 4e6) {
+					mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
+					last_sent = hrt_absolute_time();
 				}
-			}
 
-			_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
-			_att_sp.roll_body = _l1_control.nav_roll();
-			_att_sp.yaw_body = _l1_control.nav_bearing();
+				/* Detect launch */
+				launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
 
-			if (launch_detected) {
-				usePreTakeoffThrust = false;
+				/* update our copy of the laucn detection state */
+				launch_detection_state = launchDetector.getLaunchDetected();
+			} else	{
+				/* no takeoff detection --> fly */
+				launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
+			}
+
+			/* Set control values depending on the detection state */
+			if (launch_detection_state != LAUNCHDETECTION_RES_NONE) {
+				/* Launch has been detected, hence we have to control the plane. */
 
-				/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
+				_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
+				_att_sp.roll_body = _l1_control.nav_roll();
+				_att_sp.yaw_body = _l1_control.nav_bearing();
+
+				/* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use
+				 * full throttle, otherwise we use the preTakeOff Throttle */
+				float takeoff_throttle = launch_detection_state !=
+					LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ?
+					launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max;
+
+				/* select maximum pitch: the launchdetector may impose another limit for the pitch
+				 * depending on the state of the launch */
+				float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max);
+				float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg);
+
+				/* apply minimum pitch and limit roll if target altitude is not within climbout_diff
+				 * meters */
 				if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) {
 
 					/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
@@ -1118,18 +1135,20 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 							calculate_target_airspeed(1.3f * _parameters.airspeed_min),
 							eas2tas,
 							math::radians(_parameters.pitch_limit_min),
-							math::radians(_parameters.pitch_limit_max),
-							_parameters.throttle_min, _parameters.throttle_max,
+							takeoff_pitch_max_rad,
+							_parameters.throttle_min, takeoff_throttle,
 							_parameters.throttle_cruise,
 							true,
 							math::max(math::radians(pos_sp_triplet.current.pitch_min),
 							math::radians(10.0f)),
 							_global_pos.alt,
 							ground_speed,
-							TECS_MODE_TAKEOFF);
+							TECS_MODE_TAKEOFF,
+							takeoff_pitch_max_deg != _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));
+					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f),
+							math::radians(15.0f));
 
 				} else {
 					tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
@@ -1138,17 +1157,26 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 								math::radians(_parameters.pitch_limit_min),
 								math::radians(_parameters.pitch_limit_max),
 								_parameters.throttle_min,
-								_parameters.throttle_max,
+								takeoff_throttle,
 								_parameters.throttle_cruise,
 								false,
 								math::radians(_parameters.pitch_limit_min),
 								_global_pos.alt,
 								ground_speed);
 				}
-
 			} else {
-				usePreTakeoffThrust = true;
+				/* Tell the attitude controller to stop integrating while we are waiting
+				 * for the launch */
+				_att_sp.roll_reset_integral = true;
+				_att_sp.pitch_reset_integral = true;
+				_att_sp.yaw_reset_integral = true;
+
+				/* Set default roll and pitch setpoints during detection phase */
+				_att_sp.roll_body = 0.0f;
+				_att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min),
+						math::radians(10.0f));
 			}
+
 		}
 
 		/* reset landing state */
@@ -1182,13 +1210,22 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		}
 	}
 
-	if (usePreTakeoffThrust) {
+	/* Copy thrust and pitch values from tecs
+	 * making sure again that the correct thrust is used,
+	 * without depending on library calls for safety reasons */
+	if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
+			launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
 		_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
 	}
 	else {
 		_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : _tecs.get_throttle_demand(), throttle_max);
 	}
-	_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
+	/* During a takeoff waypoint while waiting for launch the pitch sp is set
+	 * already (not by tecs) */
+	if (!(pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
+			launch_detection_state == LAUNCHDETECTION_RES_NONE)) {
+		_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
+	}
 
 	if (_control_mode.flag_control_position_enabled) {
 		last_manual = false;
@@ -1342,8 +1379,7 @@ FixedwingPositionControl::task_main()
 
 void FixedwingPositionControl::reset_takeoff_state()
 {
-	launch_detected = false;
-	usePreTakeoffThrust = false;
+	launch_detection_state = LAUNCHDETECTION_RES_NONE;
 	launchDetector.reset();
 }
 
@@ -1362,7 +1398,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		bool climbout_mode, float climbout_pitch_min_rad,
 		float altitude,
 		const math::Vector<3> &ground_speed,
-		tecs_mode mode)
+		tecs_mode mode, bool pitch_max_special)
 {
 	if (_mTecs.getEnabled()) {
 		/* Using mtecs library: prepare arguments for mtecs call */
@@ -1377,9 +1413,20 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		} else {
 			limitOverride.disablePitchMinOverride();
 		}
+
+		if (pitch_max_special) {
+			/* Use the maximum pitch from the argument */
+			limitOverride.enablePitchMaxOverride(M_RAD_TO_DEG_F * pitch_max_rad);
+		} else {
+			/* use pitch max set by MT param */
+			limitOverride.disablePitchMaxOverride();
+		}
 		_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
 				limitOverride);
 	} else {
+		/* No underspeed protection in landing mode */
+		_tecs.set_detect_underspeed_enabled(!(mode == TECS_MODE_LAND || mode == TECS_MODE_LAND_THROTTLELIM));
+
 		/* Using tecs library */
 		_tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
 					    _airspeed.indicated_airspeed_m_s, eas2tas,
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
index 41c374407..847ecbb5c 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -131,8 +131,8 @@ PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f);
 /**
  * Throttle limit max
  *
- * This is the maximum throttle % that can be used by the controller. 
- * For overpowered aircraft, this should be reduced to a value that 
+ * This is the maximum throttle % that can be used by the controller.
+ * For overpowered aircraft, this should be reduced to a value that
  * provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX.
  *
  * @group L1 Control
@@ -142,10 +142,10 @@ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
 /**
  * Throttle limit min
  *
- * This is the minimum throttle % that can be used by the controller. 
- * For electric aircraft this will normally be set to zero, but can be set 
- * to a small non-zero value if a folding prop is fitted to prevent the 
- * prop from folding and unfolding repeatedly in-flight or to provide 
+ * This is the minimum throttle % that can be used by the controller.
+ * For electric aircraft this will normally be set to zero, but can be set
+ * to a small non-zero value if a folding prop is fitted to prevent the
+ * prop from folding and unfolding repeatedly in-flight or to provide
  * some aerodynamic drag from a turning prop to improve the descent rate.
  *
  * For aircraft with internal combustion engine this parameter should be set
@@ -158,7 +158,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
 /**
  * Throttle limit value before flare
  *
- * This throttle value will be set as throttle limit at FW_LND_TLALT, 
+ * This throttle value will be set as throttle limit at FW_LND_TLALT,
  * before arcraft will flare.
  *
  * @group L1 Control
@@ -180,17 +180,17 @@ PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f);
 /**
  * Maximum climb rate
  *
- * This is the best climb rate that the aircraft can achieve with 
- * the throttle set to THR_MAX and the airspeed set to the 
- * default value. For electric aircraft make sure this number can be 
- * achieved towards the end of flight when the battery voltage has reduced. 
- * The setting of this parameter can be checked by commanding a positive 
- * altitude change of 100m in loiter, RTL or guided mode. If the throttle 
- * required to climb is close to THR_MAX and the aircraft is maintaining 
- * airspeed, then this parameter is set correctly. If the airspeed starts 
- * to reduce, then the parameter is set to high, and if the throttle 
- * demand required to climb and maintain speed is noticeably less than 
- * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or 
+ * This is the best climb rate that the aircraft can achieve with
+ * the throttle set to THR_MAX and the airspeed set to the
+ * default value. For electric aircraft make sure this number can be
+ * achieved towards the end of flight when the battery voltage has reduced.
+ * The setting of this parameter can be checked by commanding a positive
+ * altitude change of 100m in loiter, RTL or guided mode. If the throttle
+ * required to climb is close to THR_MAX and the aircraft is maintaining
+ * airspeed, then this parameter is set correctly. If the airspeed starts
+ * to reduce, then the parameter is set to high, and if the throttle
+ * demand required to climb and maintain speed is noticeably less than
+ * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or
  * FW_THR_MAX reduced.
  *
  * @group L1 Control
@@ -200,8 +200,8 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
 /**
  * Minimum descent rate
  *
- * This is the sink rate of the aircraft with the throttle 
- * set to THR_MIN and flown at the same airspeed as used 
+ * This is the sink rate of the aircraft with the throttle
+ * set to THR_MIN and flown at the same airspeed as used
  * to measure FW_T_CLMB_MAX.
  *
  * @group Fixed Wing TECS
@@ -211,10 +211,10 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
 /**
  * Maximum descent rate
  *
- * This sets the maximum descent rate that the controller will use. 
- * If this value is too large, the aircraft can over-speed on descent. 
- * This should be set to a value that can be achieved without 
- * exceeding the lower pitch angle limit and without over-speeding 
+ * This sets the maximum descent rate that the controller will use.
+ * If this value is too large, the aircraft can over-speed on descent.
+ * This should be set to a value that can be achieved without
+ * exceeding the lower pitch angle limit and without over-speeding
  * the aircraft.
  *
  * @group Fixed Wing TECS
@@ -224,7 +224,7 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
 /**
  * TECS time constant
  *
- * This is the time constant of the TECS control algorithm (in seconds). 
+ * This is the time constant of the TECS control algorithm (in seconds).
  * Smaller values make it faster to respond, larger values make it slower
  * to respond.
  *
@@ -235,7 +235,7 @@ PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
 /**
  * TECS Throttle time constant
  *
- * This is the time constant of the TECS throttle control algorithm (in seconds). 
+ * This is the time constant of the TECS throttle control algorithm (in seconds).
  * Smaller values make it faster to respond, larger values make it slower
  * to respond.
  *
@@ -246,7 +246,7 @@ PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f);
 /**
  * Throttle damping factor
  *
- * This is the damping gain for the throttle demand loop. 
+ * This is the damping gain for the throttle demand loop.
  * Increase to add damping to correct for oscillations in speed and height.
  *
  * @group Fixed Wing TECS
@@ -256,9 +256,9 @@ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
 /**
  * Integrator gain
  *
- * This is the integrator gain on the control loop. 
- * Increasing this gain increases the speed at which speed 
- * and height offsets are trimmed out, but reduces damping and 
+ * This is the integrator gain on the control loop.
+ * Increasing this gain increases the speed at which speed
+ * and height offsets are trimmed out, but reduces damping and
  * increases overshoot.
  *
  * @group Fixed Wing TECS
@@ -269,9 +269,9 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
  * Maximum vertical acceleration
  *
  * This is the maximum vertical acceleration (in metres/second square)
- * either up or down that the controller will use to correct speed 
- * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) 
- * allows for reasonably aggressive pitch changes if required to recover 
+ * either up or down that the controller will use to correct speed
+ * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g)
+ * allows for reasonably aggressive pitch changes if required to recover
  * from under-speed conditions.
  *
  * @group Fixed Wing TECS
@@ -281,10 +281,10 @@ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
 /**
  * Complementary filter "omega" parameter for height
  *
- * This is the cross-over frequency (in radians/second) of the complementary 
- * filter used to fuse vertical acceleration and barometric height to obtain 
- * an estimate of height rate and height. Increasing this frequency weights 
- * the solution more towards use of the barometer, whilst reducing it weights 
+ * This is the cross-over frequency (in radians/second) of the complementary
+ * filter used to fuse vertical acceleration and barometric height to obtain
+ * an estimate of height rate and height. Increasing this frequency weights
+ * the solution more towards use of the barometer, whilst reducing it weights
  * the solution more towards use of the accelerometer data.
  *
  * @group Fixed Wing TECS
@@ -294,10 +294,10 @@ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
 /**
  * Complementary filter "omega" parameter for speed
  *
- * This is the cross-over frequency (in radians/second) of the complementary 
- * filter used to fuse longitudinal acceleration and airspeed to obtain an 
+ * This is the cross-over frequency (in radians/second) of the complementary
+ * filter used to fuse longitudinal acceleration and airspeed to obtain an
  * improved airspeed estimate. Increasing this frequency weights the solution
- * more towards use of the arispeed sensor, whilst reducing it weights the 
+ * more towards use of the arispeed sensor, whilst reducing it weights the
  * solution more towards use of the accelerometer data.
  *
  * @group Fixed Wing TECS
@@ -307,13 +307,13 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
 /**
  * Roll -> Throttle feedforward
  *
- * Increasing this gain turn increases the amount of throttle that will 
- * be used to compensate for the additional drag created by turning. 
- * Ideally this should be set to  approximately 10 x the extra sink rate 
- * in m/s created by a 45 degree bank turn. Increase this gain if 
- * the aircraft initially loses energy in turns and reduce if the 
- * aircraft initially gains energy in turns. Efficient high aspect-ratio 
- * aircraft (eg powered sailplanes) can use a lower value, whereas 
+ * Increasing this gain turn increases the amount of throttle that will
+ * be used to compensate for the additional drag created by turning.
+ * Ideally this should be set to  approximately 10 x the extra sink rate
+ * in m/s created by a 45 degree bank turn. Increase this gain if
+ * the aircraft initially loses energy in turns and reduce if the
+ * aircraft initially gains energy in turns. Efficient high aspect-ratio
+ * aircraft (eg powered sailplanes) can use a lower value, whereas
  * inefficient low aspect-ratio models (eg delta wings) can use a higher value.
  *
  * @group Fixed Wing TECS
@@ -323,15 +323,15 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
 /**
  * Speed <--> Altitude priority
  *
- * This parameter adjusts the amount of weighting that the pitch control 
- * applies to speed vs height errors. Setting it to 0.0 will cause the 
- * pitch control to control height and ignore speed errors. This will 
- * normally improve height accuracy but give larger airspeed errors. 
- * Setting it to 2.0 will cause the pitch control loop to control speed 
- * and ignore height errors. This will normally reduce airspeed errors, 
- * but give larger height errors. The default value of 1.0 allows the pitch 
- * control to simultaneously control height and speed. 
- * Note to Glider Pilots - set this parameter to 2.0 (The glider will 
+ * This parameter adjusts the amount of weighting that the pitch control
+ * applies to speed vs height errors. Setting it to 0.0 will cause the
+ * pitch control to control height and ignore speed errors. This will
+ * normally improve height accuracy but give larger airspeed errors.
+ * Setting it to 2.0 will cause the pitch control loop to control speed
+ * and ignore height errors. This will normally reduce airspeed errors,
+ * but give larger height errors. The default value of 1.0 allows the pitch
+ * control to simultaneously control height and speed.
+ * Note to Glider Pilots - set this parameter to 2.0 (The glider will
  * adjust its pitch angle to maintain airspeed, ignoring changes in height).
  *
  * @group Fixed Wing TECS
@@ -341,9 +341,9 @@ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
 /**
  * Pitch damping factor
  *
- * This is the damping gain for the pitch demand loop. Increase to add 
- * damping to correct for oscillations in height. The default value of 0.0 
- * will work well provided the pitch to servo controller has been tuned 
+ * This is the damping gain for the pitch demand loop. Increase to add
+ * damping to correct for oscillations in height. The default value of 0.0
+ * will work well provided the pitch to servo controller has been tuned
  * properly.
  *
  * @group Fixed Wing TECS
@@ -358,6 +358,13 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
 PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
 
 /**
+ * Height rate FF factor
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.0f);
+
+/**
  * Speed rate P factor
  *
  * @group Fixed Wing TECS
@@ -379,18 +386,23 @@ PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f);
 PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f);
 
 /**
- * Landing flare altitude (relative)
+ * Landing flare altitude (relative to landing altitude)
  *
+ * @unit meter
  * @group L1 Control
  */
-PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f);
+PARAM_DEFINE_FLOAT(FW_LND_FLALT, 8.0f);
 
 /**
- * Landing throttle limit altitude (relative)
+ * Landing throttle limit altitude (relative landing altitude)
+ *
+ * Default of -1.0f lets the system default to applying throttle
+ * limiting at 2/3 of the flare altitude.
  *
+ * @unit meter
  * @group L1 Control
  */
-PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f);
+PARAM_DEFINE_FLOAT(FW_LND_TLALT, -1.0f);
 
 /**
  * Landing heading hold horizontal distance
diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index 00c8df18c..2a85c3702 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -31,38 +31,38 @@
  *
  ****************************************************************************/
 
-#include <crc32.h>
+/// @file mavlink_ftp.cpp
+///	@author px4dev, Don Gagne <don@thegagnes.com>
+
 #include <unistd.h>
 #include <stdio.h>
 #include <fcntl.h>
 #include <sys/stat.h>
+#include <errno.h>
 
 #include "mavlink_ftp.h"
+#include "mavlink_tests/mavlink_ftp_test.h"
 
 // Uncomment the line below to get better debug output. Never commit with this left on.
 //#define MAVLINK_FTP_DEBUG
 
-MavlinkFTP *MavlinkFTP::_server;
-
 MavlinkFTP *
-MavlinkFTP::getServer()
+MavlinkFTP::get_server(void)
 {
-	// XXX this really cries out for some locking...
-	if (_server == nullptr) {
-		_server = new MavlinkFTP;
-	}
-	return _server;
+	static MavlinkFTP server;
+        return &server;
 }
 
 MavlinkFTP::MavlinkFTP() :
-	_session_fds{},
-	_workBufs{},
-	_workFree{},
-	_lock{}
+	_request_bufs{},
+	_request_queue{},
+	_request_queue_sem{},
+	_utRcvMsgFunc{},
+	_ftp_test{}
 {
 	// initialise the request freelist
-	dq_init(&_workFree);
-	sem_init(&_lock, 0, 1);
+	dq_init(&_request_queue);
+	sem_init(&_request_queue_sem, 0, 1);
 
 	// initialize session list
 	for (size_t i=0; i<kMaxSession; i++) {
@@ -71,161 +71,240 @@ MavlinkFTP::MavlinkFTP() :
 
 	// drop work entries onto the free list
 	for (unsigned i = 0; i < kRequestQueueSize; i++) {
-		_qFree(&_workBufs[i]);
+		_return_request(&_request_bufs[i]);
 	}
 
 }
 
+#ifdef MAVLINK_FTP_UNIT_TEST
+void
+MavlinkFTP::set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, MavlinkFtpTest *ftp_test)
+{
+	_utRcvMsgFunc = rcvMsgFunc;
+	_ftp_test = ftp_test;
+}
+#endif
+
 void
 MavlinkFTP::handle_message(Mavlink* mavlink, mavlink_message_t *msg)
 {
 	// get a free request
-	auto req = _dqFree();
+	struct Request* req = _get_request();
 
 	// if we couldn't get a request slot, just drop it
-	if (req != nullptr) {
-
-		// decode the request
-		if (req->decode(mavlink, msg)) {
+	if (req == nullptr) {
+		warnx("Dropping FTP request: queue full\n");
+		return;
+	}
 
-			// and queue it for the worker
-			work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0);
-		} else {
-			_qFree(req);
+	if (msg->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
+		mavlink_msg_file_transfer_protocol_decode(msg, &req->message);
+		
+#ifdef MAVLINK_FTP_UNIT_TEST
+		if (!_utRcvMsgFunc) {
+			warnx("Incorrectly written unit test\n");
+			return;
+		}
+		// We use fake ids when unit testing
+		req->serverSystemId = MavlinkFtpTest::serverSystemId;
+		req->serverComponentId = MavlinkFtpTest::serverComponentId;
+		req->serverChannel = MavlinkFtpTest::serverChannel;
+#else
+		// Not unit testing, use the real thing
+		req->serverSystemId = mavlink->get_system_id();
+		req->serverComponentId = mavlink->get_component_id();
+		req->serverChannel = mavlink->get_channel();
+#endif
+		
+		// This is the system id we want to target when sending
+		req->targetSystemId = msg->sysid;
+			
+		if (req->message.target_system == req->serverSystemId) {
+			req->mavlink = mavlink;
+#ifdef MAVLINK_FTP_UNIT_TEST
+			// We are running in Unit Test mode. Don't queue, just call _worket directly.
+			_process_request(req);
+#else
+			// We are running in normal mode. Queue the request to the worker
+			work_queue(LPWORK, &req->work, &MavlinkFTP::_worker_trampoline, req, 0);
+#endif
+            return;
 		}
 	}
+
+	_return_request(req);
 }
 
+/// @brief Queued static work queue routine to handle mavlink messages
 void
-MavlinkFTP::_workerTrampoline(void *arg)
+MavlinkFTP::_worker_trampoline(void *arg)
 {
-	auto req = reinterpret_cast<Request *>(arg);
-	auto server = MavlinkFTP::getServer();
+	Request* req = reinterpret_cast<Request *>(arg);
+	MavlinkFTP* server = MavlinkFTP::get_server();
 
 	// call the server worker with the work item
-	server->_worker(req);
+	server->_process_request(req);
 }
 
+/// @brief Processes an FTP message
 void
-MavlinkFTP::_worker(Request *req)
+MavlinkFTP::_process_request(Request *req)
 {
-	auto hdr = req->header();
+	PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&req->message.payload[0]);
+
 	ErrorCode errorCode = kErrNone;
-	uint32_t messageCRC;
 
 	// basic sanity checks; must validate length before use
-	if ((hdr->magic != kProtocolMagic) || (hdr->size > kMaxDataLength)) {
-		errorCode = kErrNoRequest;
+	if (payload->size > kMaxDataLength) {
+		errorCode = kErrInvalidDataSize;
 		goto out;
 	}
 
-	// check request CRC to make sure this is one of ours
-	messageCRC = hdr->crc32;
-	hdr->crc32 = 0;
-	if (crc32(req->rawData(), req->dataSize()) != messageCRC) {
-		errorCode = kErrNoRequest;
-		goto out;
-		warnx("ftp: bad crc");
-	}
-
 #ifdef MAVLINK_FTP_DEBUG
-	printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset);
+	printf("ftp: channel %u opc %u size %u offset %u\n", req->serverChannel, payload->opcode, payload->size, payload->offset);
 #endif
 
-	switch (hdr->opcode) {
+	switch (payload->opcode) {
 	case kCmdNone:
 		break;
 
-	case kCmdTerminate:
-		errorCode = _workTerminate(req);
+	case kCmdTerminateSession:
+		errorCode = _workTerminate(payload);
 		break;
 
-	case kCmdReset:
-		errorCode = _workReset();
+	case kCmdResetSessions:
+		errorCode = _workReset(payload);
 		break;
 
-	case kCmdList:
-		errorCode = _workList(req);
+	case kCmdListDirectory:
+		errorCode = _workList(payload);
 		break;
 
-	case kCmdOpen:
-		errorCode = _workOpen(req, false);
+	case kCmdOpenFile:
+		errorCode = _workOpen(payload, false);
 		break;
 
-	case kCmdCreate:
-		errorCode = _workOpen(req, true);
+	case kCmdCreateFile:
+		errorCode = _workOpen(payload, true);
 		break;
 
-	case kCmdRead:
-		errorCode = _workRead(req);
+	case kCmdReadFile:
+		errorCode = _workRead(payload);
 		break;
 
-	case kCmdWrite:
-		errorCode = _workWrite(req);
+	case kCmdWriteFile:
+		errorCode = _workWrite(payload);
 		break;
 
-	case kCmdRemove:
-		errorCode = _workRemove(req);
+	case kCmdRemoveFile:
+		errorCode = _workRemoveFile(payload);
 		break;
 
+	case kCmdCreateDirectory:
+		errorCode = _workCreateDirectory(payload);
+		break;
+			
+	case kCmdRemoveDirectory:
+		errorCode = _workRemoveDirectory(payload);
+		break;
+			
 	default:
-		errorCode = kErrNoRequest;
+		errorCode = kErrUnknownCommand;
 		break;	
 	}
 
 out:
 	// handle success vs. error
 	if (errorCode == kErrNone) {
-		hdr->opcode = kRspAck;
+		payload->req_opcode = payload->opcode;
+		payload->opcode = kRspAck;
 #ifdef MAVLINK_FTP_DEBUG
 		warnx("FTP: ack\n");
 #endif
 	} else {
 		warnx("FTP: nak %u", errorCode);
-		hdr->opcode = kRspNak;
-		hdr->size = 1;
-		hdr->data[0] = errorCode;
+		payload->req_opcode = payload->opcode;
+		payload->opcode = kRspNak;
+		payload->size = 1;
+		payload->data[0] = errorCode;
+		if (errorCode == kErrFailErrno) {
+			payload->size = 2;
+			payload->data[1] = errno;
+		}
 	}
 
+	
 	// respond to the request
 	_reply(req);
 
-	// free the request buffer back to the freelist
-	_qFree(req);
+	_return_request(req);
 }
 
+/// @brief Sends the specified FTP reponse message out through mavlink
 void
 MavlinkFTP::_reply(Request *req)
 {
-	auto hdr = req->header();
+	PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&req->message.payload[0]);
 	
-	hdr->magic = kProtocolMagic;
+	payload->seqNumber = payload->seqNumber + 1;
 
-	// message is assumed to be already constructed in the request buffer, so generate the CRC
-	hdr->crc32 = 0;
-	hdr->crc32 = crc32(req->rawData(), req->dataSize());
+	mavlink_message_t msg;
+	msg.checksum = 0;
+#ifndef MAVLINK_FTP_UNIT_TEST
+	uint16_t len =
+#endif
+	mavlink_msg_file_transfer_protocol_pack_chan(req->serverSystemId,	// Sender system id
+								    req->serverComponentId,	// Sender component id
+								    req->serverChannel,		// Channel to send on
+								    &msg,			// Message to pack payload into
+								    0,				// Target network
+								    req->targetSystemId,	// Target system id
+								    0,				// Target component id
+								    (const uint8_t*)payload);	// Payload to pack into message
+	
+	bool success = true;
+#ifdef MAVLINK_FTP_UNIT_TEST
+	// Unit test hook is set, call that instead
+	_utRcvMsgFunc(&msg, _ftp_test);
+#else
+	Mavlink	*mavlink = req->mavlink;
+	
+	mavlink->lockMessageBufferMutex();
+	success = mavlink->message_buffer_write(&msg, len);
+	mavlink->unlockMessageBufferMutex();
+		
+#endif
 
-	// then pack and send the reply back to the request source
-	req->reply();
+	if (!success) {
+		warnx("FTP TX ERR");
+	}
+#ifdef MAVLINK_FTP_DEBUG
+	else {
+		warnx("wrote: sys: %d, comp: %d, chan: %d, checksum: %d",
+		      req->serverSystemId,
+		      req->serverComponentId,
+		      req->serverChannel,
+		      msg.checksum);
+	}
+#endif
 }
 
+/// @brief Responds to a List command
 MavlinkFTP::ErrorCode
-MavlinkFTP::_workList(Request *req)
+MavlinkFTP::_workList(PayloadHeader* payload)
 {
-	auto hdr = req->header();
-    
     char dirPath[kMaxDataLength];
-    strncpy(dirPath, req->dataAsCString(), kMaxDataLength);
+    strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength);
     
 	DIR *dp = opendir(dirPath);
 
 	if (dp == nullptr) {
 		warnx("FTP: can't open path '%s'", dirPath);
-		return kErrNotDir;
+		return kErrFailErrno;
 	}
     
 #ifdef MAVLINK_FTP_DEBUG
-	warnx("FTP: list %s offset %d", dirPath, hdr->offset);
+	warnx("FTP: list %s offset %d", dirPath, payload->offset);
 #endif
 
 	ErrorCode errorCode = kErrNone;
@@ -233,19 +312,19 @@ MavlinkFTP::_workList(Request *req)
 	unsigned offset = 0;
 
 	// move to the requested offset
-	seekdir(dp, hdr->offset);
+	seekdir(dp, payload->offset);
 
 	for (;;) {
 		// read the directory entry
 		if (readdir_r(dp, &entry, &result)) {
 			warnx("FTP: list %s readdir_r failure\n", dirPath);
-			errorCode = kErrIO;
+			errorCode = kErrFailErrno;
 			break;
 		}
 
 		// no more entries?
 		if (result == nullptr) {
-			if (hdr->offset != 0 && offset == 0) {
+			if (payload->offset != 0 && offset == 0) {
 				// User is requesting subsequent dir entries but there were none. This means the user asked
 				// to seek past EOF.
 				errorCode = kErrEOF;
@@ -270,14 +349,22 @@ MavlinkFTP::_workList(Request *req)
 			}
 			break;
 		case DTYPE_DIRECTORY:
-			direntType = kDirentDir;
+			if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) {
+				// Don't bother sending these back
+				direntType = kDirentSkip;
+			} else {
+				direntType = kDirentDir;
+			}
 			break;
 		default:
-			direntType = kDirentUnknown;
-			break;
+			// We only send back file and diretory entries, skip everything else
+			direntType = kDirentSkip;
 		}
 		
-		if (entry.d_type == DTYPE_FILE) {
+		if (direntType == kDirentSkip) {
+			// Skip send only dirent identifier
+			buf[0] = '\0';
+		} else if (direntType == kDirentFile) {
 			// Files send filename and file length
 			snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize);
 		} else {
@@ -293,94 +380,95 @@ MavlinkFTP::_workList(Request *req)
 		}
 		
 		// Move the data into the buffer
-		hdr->data[offset++] = direntType;
-		strcpy((char *)&hdr->data[offset], buf);
+		payload->data[offset++] = direntType;
+		strcpy((char *)&payload->data[offset], buf);
 #ifdef MAVLINK_FTP_DEBUG
-		printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]);
+		printf("FTP: list %s %s\n", dirPath, (char *)&payload->data[offset-1]);
 #endif
 		offset += nameLen + 1;
 	}
 
 	closedir(dp);
-	hdr->size = offset;
+	payload->size = offset;
 
 	return errorCode;
 }
 
+/// @brief Responds to an Open command
 MavlinkFTP::ErrorCode
-MavlinkFTP::_workOpen(Request *req, bool create)
+MavlinkFTP::_workOpen(PayloadHeader* payload, bool create)
 {
-	auto hdr = req->header();
-    
-	int session_index = _findUnusedSession();
+	int session_index = _find_unused_session();
 	if (session_index < 0) {
-		return kErrNoSession;
+		warnx("FTP: Open failed - out of sessions\n");
+		return kErrNoSessionsAvailable;
 	}
-
+	
+	char *filename = _data_as_cstring(payload);
 	
 	uint32_t fileSize = 0;
 	if (!create) {
 		struct stat st;
-		if (stat(req->dataAsCString(), &st) != 0) {
-			return kErrNotFile;
+		if (stat(filename, &st) != 0) {
+			return kErrFailErrno;
 		}
 		fileSize = st.st_size;
 	}
 
 	int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
     
-	int fd = ::open(req->dataAsCString(), oflag);
+	int fd = ::open(filename, oflag);
 	if (fd < 0) {
-		return create ? kErrPerm : kErrNotFile;
+		return kErrFailErrno;
 	}
 	_session_fds[session_index] = fd;
 
-	hdr->session = session_index;
+	payload->session = session_index;
 	if (create) {
-		hdr->size = 0;
+		payload->size = 0;
 	} else {
-		hdr->size = sizeof(uint32_t);
-		*((uint32_t*)hdr->data) = fileSize;
+		payload->size = sizeof(uint32_t);
+		*((uint32_t*)payload->data) = fileSize;
 	}
 
 	return kErrNone;
 }
 
+/// @brief Responds to a Read command
 MavlinkFTP::ErrorCode
-MavlinkFTP::_workRead(Request *req)
+MavlinkFTP::_workRead(PayloadHeader* payload)
 {
-	auto hdr = req->header();
-
-	int session_index = hdr->session;
+	int session_index = payload->session;
 
-	if (!_validSession(session_index)) {
-		return kErrNoSession;
+	if (!_valid_session(session_index)) {
+		return kErrInvalidSession;
 	}
 
 	// Seek to the specified position
 #ifdef MAVLINK_FTP_DEBUG
-	warnx("seek %d", hdr->offset);
+	warnx("seek %d", payload->offset);
 #endif
-	if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) {
+	if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) {
 		// Unable to see to the specified location
 		warnx("seek fail");
 		return kErrEOF;
 	}
 
-	int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength);
+	int bytes_read = ::read(_session_fds[session_index], &payload->data[0], kMaxDataLength);
 	if (bytes_read < 0) {
 		// Negative return indicates error other than eof
 		warnx("read fail %d", bytes_read);
-		return kErrIO;
+		return kErrFailErrno;
 	}
 
-	hdr->size = bytes_read;
+	payload->size = bytes_read;
 
 	return kErrNone;
 }
 
+/// @brief Responds to a Write command
 MavlinkFTP::ErrorCode
-MavlinkFTP::_workWrite(Request *req)
+MavlinkFTP::_workWrite(PayloadHeader* payload)
 {
 #if 0
     // NYI: Coming soon
@@ -403,35 +491,44 @@ MavlinkFTP::_workWrite(Request *req)
 	hdr->size = result;
 	return kErrNone;
 #else
-	return kErrPerm;
+	return kErrUnknownCommand;
 #endif
 }
 
+/// @brief Responds to a RemoveFile command
 MavlinkFTP::ErrorCode
-MavlinkFTP::_workRemove(Request *req)
+MavlinkFTP::_workRemoveFile(PayloadHeader* payload)
 {
-	//auto hdr = req->header();
-
-	// for now, send error reply
-	return kErrPerm;
+	char file[kMaxDataLength];
+	strncpy(file, _data_as_cstring(payload), kMaxDataLength);
+	
+	if (unlink(file) == 0) {
+		payload->size = 0;
+		return kErrNone;
+	} else {
+		return kErrFailErrno;
+	}
 }
 
+/// @brief Responds to a Terminate command
 MavlinkFTP::ErrorCode
-MavlinkFTP::_workTerminate(Request *req)
+MavlinkFTP::_workTerminate(PayloadHeader* payload)
 {
-	auto hdr = req->header();
-    
-	if (!_validSession(hdr->session)) {
-		return kErrNoSession;
+	if (!_valid_session(payload->session)) {
+		return kErrInvalidSession;
 	}
     
-	::close(_session_fds[hdr->session]);
+	::close(_session_fds[payload->session]);
+	_session_fds[payload->session] = -1;
+	
+	payload->size = 0;
 
 	return kErrNone;
 }
 
+/// @brief Responds to a Reset command
 MavlinkFTP::ErrorCode
-MavlinkFTP::_workReset(void)
+MavlinkFTP::_workReset(PayloadHeader* payload)
 {
 	for (size_t i=0; i<kMaxSession; i++) {
 		if (_session_fds[i] != -1) {
@@ -440,11 +537,44 @@ MavlinkFTP::_workReset(void)
 		}
 	}
 
+	payload->size = 0;
+	
 	return kErrNone;
 }
 
+/// @brief Responds to a RemoveDirectory command
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workRemoveDirectory(PayloadHeader* payload)
+{
+	char dir[kMaxDataLength];
+	strncpy(dir, _data_as_cstring(payload), kMaxDataLength);
+	
+	if (rmdir(dir) == 0) {
+		payload->size = 0;
+		return kErrNone;
+	} else {
+		return kErrFailErrno;
+	}
+}
+
+/// @brief Responds to a CreateDirectory command
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workCreateDirectory(PayloadHeader* payload)
+{
+	char dir[kMaxDataLength];
+	strncpy(dir, _data_as_cstring(payload), kMaxDataLength);
+	
+	if (mkdir(dir, S_IRWXU | S_IRWXG | S_IRWXO) == 0) {
+		payload->size = 0;
+		return kErrNone;
+	} else {
+		return kErrFailErrno;
+	}
+}
+
+/// @brief Returns true if the specified session is a valid open session
 bool
-MavlinkFTP::_validSession(unsigned index)
+MavlinkFTP::_valid_session(unsigned index)
 {
 	if ((index >= kMaxSession) || (_session_fds[index] < 0)) {
 		return false;
@@ -452,8 +582,9 @@ MavlinkFTP::_validSession(unsigned index)
 	return true;
 }
 
+/// @brief Returns an unused session index
 int
-MavlinkFTP::_findUnusedSession(void)
+MavlinkFTP::_find_unused_session(void)
 {
 	for (size_t i=0; i<kMaxSession; i++) {
 		if (_session_fds[i] == -1) {
@@ -464,16 +595,53 @@ MavlinkFTP::_findUnusedSession(void)
 	return -1;
 }
 
+/// @brief Guarantees that the payload data is null terminated.
+///     @return Returns a pointer to the payload data as a char *
 char *
-MavlinkFTP::Request::dataAsCString()
+MavlinkFTP::_data_as_cstring(PayloadHeader* payload)
 {
 	// guarantee nul termination
-	if (header()->size < kMaxDataLength) {
-		requestData()[header()->size] = '\0';
+	if (payload->size < kMaxDataLength) {
+		payload->data[payload->size] = '\0';
 	} else {
-		requestData()[kMaxDataLength - 1] = '\0';
+		payload->data[kMaxDataLength - 1] = '\0';
 	}
 
 	// and return data
-	return (char *)&(header()->data[0]);
+	return (char *)&(payload->data[0]);
+}
+
+/// @brief Returns a unused Request entry. NULL if none available.
+MavlinkFTP::Request *
+MavlinkFTP::_get_request(void)
+{
+	_lock_request_queue();
+	Request* req = reinterpret_cast<Request *>(dq_remfirst(&_request_queue));
+	_unlock_request_queue();
+	return req;
+}
+
+/// @brief Locks a semaphore to provide exclusive access to the request queue
+void
+MavlinkFTP::_lock_request_queue(void)
+{
+	do {}
+	while (sem_wait(&_request_queue_sem) != 0);
+}
+
+/// @brief Unlocks the semaphore providing exclusive access to the request queue
+void
+MavlinkFTP::_unlock_request_queue(void)
+{
+	sem_post(&_request_queue_sem);
 }
+
+/// @brief Returns a no longer needed request to the queue
+void
+MavlinkFTP::_return_request(Request *req)
+{
+	_lock_request_queue();
+	dq_addlast(&req->work.dq, &_request_queue);
+	_unlock_request_queue();
+}
+
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index 43de89de9..657e2f855 100644
--- a/src/modules/mavlink/mavlink_ftp.h
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -33,20 +33,8 @@
 
 #pragma once
 
-/**
- * @file mavlink_ftp.h
- *
- * MAVLink remote file server.
- *
- * Messages are wrapped in ENCAPSULATED_DATA messages. Every message includes
- * a session ID and sequence number.
- *
- * A limited number of requests (currently 2) may be outstanding at a time.
- * Additional messages will be discarded.
- *
- * Messages consist of a fixed header, followed by a data area.
- *
- */
+/// @file mavlink_ftp.h
+///     @author px4dev, Don Gagne <don@thegagnes.com>
  
 #include <dirent.h>
 #include <queue.h>
@@ -57,174 +45,128 @@
 #include "mavlink_messages.h"
 #include "mavlink_main.h"
 
+class MavlinkFtpTest;
+
+/// @brief MAVLink remote file server. Support FTP like commands using MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL message.
+/// A limited number of requests (kRequestQueueSize) may be outstanding at a time. Additional messages will be discarded.
 class MavlinkFTP
 {
 public:
+	/// @brief Returns the one Mavlink FTP server in the system.
+	static MavlinkFTP* get_server(void);
+    
+	/// @brief Contructor is only public so unit test code can new objects.
 	MavlinkFTP();
-
-	static MavlinkFTP	*getServer();
-
-	// static interface
-	void			handle_message(Mavlink* mavlink,
-					mavlink_message_t *msg);
-
-private:
-
-	static const unsigned	kRequestQueueSize = 2;
-
-	static MavlinkFTP	*_server;
-
-	struct RequestHeader
-	{
-		uint8_t		magic;
-		uint8_t		session;
-		uint8_t		opcode;
-		uint8_t		size;
-		uint32_t	crc32;
-		uint32_t	offset;
-		uint8_t		data[];
-	};
-
+	
+	/// @brief Adds the specified message to the work queue.
+	void handle_message(Mavlink* mavlink, mavlink_message_t *msg);
+    
+	typedef void (*ReceiveMessageFunc_t)(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest);
+	
+	/// @brief Sets up the server to run in unit test mode.
+	///	@param rcvmsgFunc Function which will be called to handle outgoing mavlink messages.
+	///	@param ftp_test MavlinkFtpTest object which the function is associated with
+	void set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, MavlinkFtpTest *ftp_test);
+
+	/// @brief This is the payload which is in mavlink_file_transfer_protocol_t.payload. We pad the structure ourselves to
+	/// 32 bit alignment to avoid usage of any pack pragmas.
+	struct PayloadHeader
+        {
+		uint16_t	seqNumber;	///< sequence number for message
+		uint8_t		session;	///< Session id for read and write commands
+		uint8_t		opcode;		///< Command opcode
+		uint8_t		size;		///< Size of data
+		uint8_t		req_opcode;	///< Request opcode returned in kRspAck, kRspNak message
+		uint8_t		padding[2];	///< 32 bit aligment padding
+		uint32_t	offset;		///< Offsets for List and Read commands
+		uint8_t		data[];		///< command data, varies by Opcode
+        };
+	
+	/// @brief Command opcodes
 	enum Opcode : uint8_t
 	{
-		kCmdNone,	// ignored, always acked
-		kCmdTerminate,	// releases sessionID, closes file
-		kCmdReset,	// terminates all sessions
-		kCmdList,	// list files in <path> from <offset>
-		kCmdOpen,	// opens <path> for reading, returns <session>
-		kCmdRead,	// reads <size> bytes from <offset> in <session>
-		kCmdCreate,	// creates <path> for writing, returns <session>
-		kCmdWrite,	// appends <size> bytes at <offset> in <session>
-		kCmdRemove,	// remove file (only if created by server?)
-
-		kRspAck,
-		kRspNak
+		kCmdNone,		///< ignored, always acked
+		kCmdTerminateSession,	///< Terminates open Read session
+		kCmdResetSessions,	///< Terminates all open Read sessions
+		kCmdListDirectory,	///< List files in <path> from <offset>
+		kCmdOpenFile,		///< Opens file at <path> for reading, returns <session>
+		kCmdReadFile,		///< Reads <size> bytes from <offset> in <session>
+		kCmdCreateFile,		///< Creates file at <path> for writing, returns <session>
+		kCmdWriteFile,		///< Appends <size> bytes to file in <session>
+		kCmdRemoveFile,		///< Remove file at <path>
+		kCmdCreateDirectory,	///< Creates directory at <path>
+		kCmdRemoveDirectory,	///< Removes Directory at <path>, must be empty
+		
+		kRspAck = 128,		///< Ack response
+		kRspNak			///< Nak response
 	};
-
+	
+	/// @brief Error codes returned in Nak response PayloadHeader.data[0].
 	enum ErrorCode : uint8_t
-	{
+        {
 		kErrNone,
-		kErrNoRequest,
-		kErrNoSession,
-		kErrSequence,
-		kErrNotDir,
-		kErrNotFile,
-		kErrEOF,
-		kErrNotAppend,
-		kErrTooBig,
-		kErrIO,
-		kErrPerm
-	};
-
-    int _findUnusedSession(void);
-    bool _validSession(unsigned index);
-
-    static const unsigned kMaxSession = 2;
-    int	_session_fds[kMaxSession];
-
-	class Request
+		kErrFail,			///< Unknown failure
+		kErrFailErrno,			///< Command failed, errno sent back in PayloadHeader.data[1]
+		kErrInvalidDataSize,		///< PayloadHeader.size is invalid
+		kErrInvalidSession,		///< Session is not currently open
+		kErrNoSessionsAvailable,	///< All available Sessions in use
+		kErrEOF,			///< Offset past end of file for List and Read commands
+		kErrUnknownCommand		///< Unknown command opcode
+        };
+	
+private:
+	/// @brief Unit of work which is queued to work_queue
+	struct Request
 	{
-	public:
-		union {
-			dq_entry_t	entry;
-			work_s		work;
-		};
-
-		bool		decode(Mavlink *mavlink, mavlink_message_t *fromMessage) {
-			if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) {
-				_mavlink = mavlink;
-				mavlink_msg_encapsulated_data_decode(fromMessage, &_message);
-				return true;
-			}
-			return false;
-		}
-
-		void		reply() {
-
-			// XXX the proper way would be an IOCTL / uORB call, rather than exploiting the
-			// flat memory architecture, as we're operating between threads here.
-			mavlink_message_t msg;
-			msg.checksum = 0;
-			unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(),
-				_mavlink->get_channel(), &msg, sequence()+1, rawData());
-
-			_mavlink->lockMessageBufferMutex();
-			bool success = _mavlink->message_buffer_write(&msg, len);
-			_mavlink->unlockMessageBufferMutex();
-
-			if (!success) {
-				warnx("FTP TX ERR");
-			}
-#ifdef MAVLINK_FTP_DEBUG
-			else {
-				warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d",
-				      _mavlink->get_system_id(),
-				      _mavlink->get_component_id(),
-				      _mavlink->get_channel(),
-				      len,
-				      msg.checksum);
-			}
-#endif
-		}
-
-		uint8_t		*rawData() { return &_message.data[0]; }
-		RequestHeader 	*header()  { return reinterpret_cast<RequestHeader *>(&_message.data[0]); }
-		uint8_t         *requestData() { return &(header()->data[0]); }
-		unsigned	dataSize() { return header()->size + sizeof(RequestHeader); }
-		uint16_t	sequence() const { return _message.seqnr; }
-		mavlink_channel_t channel() { return _mavlink->get_channel(); }
-
-		char		*dataAsCString();
-
-	private:
-		Mavlink			*_mavlink;
-		mavlink_encapsulated_data_t _message;
-
+		work_s	work;			///< work queue entry
+		Mavlink	*mavlink;		///< Mavlink to reply to
+		uint8_t serverSystemId;		///< System ID to send from
+		uint8_t serverComponentId;	///< Component ID to send from
+		uint8_t serverChannel;		///< Channel to send to
+		uint8_t targetSystemId;		///< System ID to target reply to
+
+		mavlink_file_transfer_protocol_t message;	///< Protocol message
 	};
-
-	static const uint8_t	kProtocolMagic = 'f';
-	static const char	kDirentFile = 'F';
-	static const char	kDirentDir = 'D';
-	static const char	kDirentUnknown = 'U';
-	static const uint8_t	kMaxDataLength = MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN - sizeof(RequestHeader);
-
-	/// Request worker; runs on the low-priority work queue to service
-	/// remote requests.
-	///
-	static void		_workerTrampoline(void *arg);
-	void			_worker(Request *req);
-
-	/// Reply to a request (XXX should be a Request method)
-	///
-	void			_reply(Request *req);
-
-	ErrorCode		_workList(Request *req);
-	ErrorCode		_workOpen(Request *req, bool create);
-	ErrorCode		_workRead(Request *req);
-	ErrorCode		_workWrite(Request *req);
-	ErrorCode		_workRemove(Request *req);
-	ErrorCode		_workTerminate(Request *req);
-	ErrorCode		_workReset();
-
-	// work freelist
-	Request			_workBufs[kRequestQueueSize];
-	dq_queue_t		_workFree;
-	sem_t			_lock;
-
-	void			_qLock() { do {} while (sem_wait(&_lock) != 0); }
-	void			_qUnlock() { sem_post(&_lock); }
-
-	void			_qFree(Request *req) {
-		_qLock();
-		dq_addlast(&req->entry, &_workFree);
-		_qUnlock();
-	}
-
-	Request			*_dqFree() {
-		_qLock();
-		auto req = reinterpret_cast<Request *>(dq_remfirst(&_workFree));
-		_qUnlock();
-		return req;
-	}
-    
+	
+	Request		*_get_request(void);
+	void		_return_request(Request *req);
+	void		_lock_request_queue(void);
+	void		_unlock_request_queue(void);
+	
+	char		*_data_as_cstring(PayloadHeader* payload);
+	
+	static void	_worker_trampoline(void *arg);
+	void		_process_request(Request *req);
+	void		_reply(Request *req);
+
+	ErrorCode	_workList(PayloadHeader *payload);
+	ErrorCode	_workOpen(PayloadHeader *payload, bool create);
+	ErrorCode	_workRead(PayloadHeader *payload);
+	ErrorCode	_workWrite(PayloadHeader *payload);
+	ErrorCode	_workTerminate(PayloadHeader *payload);
+	ErrorCode	_workReset(PayloadHeader* payload);
+	ErrorCode	_workRemoveDirectory(PayloadHeader *payload);
+	ErrorCode	_workCreateDirectory(PayloadHeader *payload);
+	ErrorCode	_workRemoveFile(PayloadHeader *payload);
+
+	static const unsigned	kRequestQueueSize = 2;			///< Max number of queued requests
+	Request			_request_bufs[kRequestQueueSize];	///< Request buffers which hold work
+	dq_queue_t		_request_queue;				///< Queue of available Request buffers
+	sem_t			_request_queue_sem;			///< Semaphore for locking access to _request_queue
+	
+	int _find_unused_session(void);
+	bool _valid_session(unsigned index);
+	
+	static const char	kDirentFile = 'F';	///< Identifies File returned from List command
+	static const char	kDirentDir = 'D';	///< Identifies Directory returned from List command
+	static const char	kDirentSkip = 'S';	///< Identifies Skipped entry from List command
+	
+	/// @brief Maximum data size in RequestHeader::data
+	static const uint8_t	kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(PayloadHeader);
+	
+	static const unsigned kMaxSession = 2;	///< Max number of active sessions
+	int	_session_fds[kMaxSession];	///< Session file descriptors, 0 for empty slot
+	
+	ReceiveMessageFunc_t _utRcvMsgFunc;	///< Unit test override for mavlink message sending
+	MavlinkFtpTest *_ftp_test;		///< Additional parameter to _utRcvMsgFunc;
 };
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 408605939..940e64144 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -134,44 +134,44 @@ Mavlink::Mavlink() :
 	_mode(MAVLINK_MODE_NORMAL),
 	_channel(MAVLINK_COMM_0),
 	_logbuffer {},
-	   _total_counter(0),
-	   _receive_thread {},
-	   _verbose(false),
-	   _forwarding_on(false),
-	   _passing_on(false),
-	   _ftp_on(false),
-	   _uart_fd(-1),
-	   _baudrate(57600),
-	   _datarate(1000),
-	   _datarate_events(500),
-	   _rate_mult(1.0f),
-	   _mavlink_param_queue_index(0),
-	   mavlink_link_termination_allowed(false),
-	   _subscribe_to_stream(nullptr),
-	   _subscribe_to_stream_rate(0.0f),
-	   _flow_control_enabled(true),
-	   _last_write_success_time(0),
-	   _last_write_try_time(0),
-	   _bytes_tx(0),
-	   _bytes_txerr(0),
-	   _bytes_rx(0),
-	   _bytes_timestamp(0),
-	   _rate_tx(0.0f),
-	   _rate_txerr(0.0f),
-	   _rate_rx(0.0f),
-	   _rstatus {},
-	   _message_buffer {},
-	   _message_buffer_mutex {},
-	   _send_mutex {},
-	   _param_initialized(false),
-	   _param_system_id(0),
-	   _param_component_id(0),
-	   _param_system_type(0),
-	   _param_use_hil_gps(0),
-
-	   /* performance counters */
-	   _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
-	   _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
+	_total_counter(0),
+	_receive_thread {},
+	_verbose(false),
+	_forwarding_on(false),
+	_passing_on(false),
+	_ftp_on(false),
+	_uart_fd(-1),
+	_baudrate(57600),
+	_datarate(1000),
+	_datarate_events(500),
+	_rate_mult(1.0f),
+	_mavlink_param_queue_index(0),
+	mavlink_link_termination_allowed(false),
+	_subscribe_to_stream(nullptr),
+	_subscribe_to_stream_rate(0.0f),
+	_flow_control_enabled(true),
+	_last_write_success_time(0),
+	_last_write_try_time(0),
+	_bytes_tx(0),
+	_bytes_txerr(0),
+	_bytes_rx(0),
+	_bytes_timestamp(0),
+	_rate_tx(0.0f),
+	_rate_txerr(0.0f),
+	_rate_rx(0.0f),
+	_rstatus {},
+	_message_buffer {},
+	_message_buffer_mutex {},
+	_send_mutex {},
+	_param_initialized(false),
+	_param_system_id(0),
+	_param_component_id(0),
+	_param_system_type(0),
+	_param_use_hil_gps(0),
+
+	/* performance counters */
+	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
+	_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
 {
 	fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl;
 
@@ -217,6 +217,8 @@ Mavlink::Mavlink() :
 		errx(1, "instance ID is out of range");
 		break;
 	}
+
+	_rstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
 }
 
 Mavlink::~Mavlink()
@@ -1227,7 +1229,10 @@ Mavlink::task_main(int argc, char *argv[])
 				_mode = MAVLINK_MODE_CUSTOM;
 
 			} else if (strcmp(optarg, "camera") == 0) {
-				_mode = MAVLINK_MODE_CAMERA;
+				// left in here for compatibility
+				_mode = MAVLINK_MODE_ONBOARD;
+			} else if (strcmp(optarg, "onboard") == 0) {
+				_mode = MAVLINK_MODE_ONBOARD;
 			}
 
 			break;
@@ -1287,8 +1292,8 @@ Mavlink::task_main(int argc, char *argv[])
 		warnx("mode: CUSTOM");
 		break;
 
-	case MAVLINK_MODE_CAMERA:
-		warnx("mode: CAMERA");
+	case MAVLINK_MODE_ONBOARD:
+		warnx("mode: ONBOARD");
 		break;
 
 	default:
@@ -1391,13 +1396,14 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
 		configure_stream("ATTITUDE_TARGET", 3.0f);
 		configure_stream("DISTANCE_SENSOR", 0.5f);
+		configure_stream("OPTICAL_FLOW", 20.0f);
 		break;
 
-	case MAVLINK_MODE_CAMERA:
+	case MAVLINK_MODE_ONBOARD:
 		configure_stream("SYS_STATUS", 1.0f);
-		configure_stream("ATTITUDE", 15.0f);
-		configure_stream("GLOBAL_POSITION_INT", 15.0f);
-		configure_stream("CAMERA_CAPTURE", 1.0f);
+		configure_stream("ATTITUDE", 50.0f);
+		configure_stream("GLOBAL_POSITION_INT", 50.0f);
+		configure_stream("CAMERA_CAPTURE", 2.0f);
 		break;
 
 	default:
@@ -1653,6 +1659,8 @@ Mavlink::display_status()
 		printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time));
 	}
 
+	printf("\tmavlink chan: #%u\n", _channel);
+
 	if (_rstatus.timestamp > 0) {
 
 		printf("\ttype:\t\t");
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 1e2e94cef..7f9d225bb 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -127,7 +127,7 @@ public:
 	enum MAVLINK_MODE {
 		MAVLINK_MODE_NORMAL = 0,
 		MAVLINK_MODE_CUSTOM,
-		MAVLINK_MODE_CAMERA
+		MAVLINK_MODE_ONBOARD
 	};
 
 	void			set_mode(enum MAVLINK_MODE);
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index af4d46a36..a2f3828ff 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -774,6 +774,9 @@ private:
 	MavlinkOrbSubscription *_airspeed_sub;
 	uint64_t _airspeed_time;
 
+	MavlinkOrbSubscription *_sensor_combined_sub;
+	uint64_t _sensor_combined_time;
+
 	/* do not allow top copying this class */
 	MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &);
 	MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &);
@@ -789,7 +792,9 @@ protected:
 		_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))),
 		_act_time(0),
 		_airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))),
-		_airspeed_time(0)
+		_airspeed_time(0),
+		_sensor_combined_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))),
+		_sensor_combined_time(0)
 	{}
 
 	void send(const hrt_abstime t)
@@ -799,12 +804,14 @@ protected:
 		struct actuator_armed_s armed;
 		struct actuator_controls_s act;
 		struct airspeed_s airspeed;
+		struct sensor_combined_s sensor_combined;
 
 		bool updated = _att_sub->update(&_att_time, &att);
 		updated |= _pos_sub->update(&_pos_time, &pos);
 		updated |= _armed_sub->update(&_armed_time, &armed);
 		updated |= _act_sub->update(&_act_time, &act);
 		updated |= _airspeed_sub->update(&_airspeed_time, &airspeed);
+		updated |= _sensor_combined_sub->update(&_sensor_combined_time, &sensor_combined);
 
 		if (updated) {
 			mavlink_vfr_hud_t msg;
@@ -813,7 +820,7 @@ protected:
 			msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
 			msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
 			msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
-			msg.alt = pos.alt;
+			msg.alt = sensor_combined.baro_alt_meter;
 			msg.climb = -pos.vel_d;
 
 			_mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg);
@@ -879,8 +886,8 @@ protected:
 			msg.eph = cm_uint16_from_m_float(gps.eph);
 			msg.epv = cm_uint16_from_m_float(gps.epv);
 			msg.vel = cm_uint16_from_m_float(gps.vel_m_s),
-		    msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
-		    msg.satellites_visible = gps.satellites_used;
+			msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
+			msg.satellites_visible = gps.satellites_used;
 
 			_mavlink->send_message(MAVLINK_MSG_ID_GPS_RAW_INT, &msg);
 		}
@@ -950,11 +957,11 @@ protected:
 			msg.lat = pos.lat * 1e7;
 			msg.lon = pos.lon * 1e7;
 			msg.alt = pos.alt * 1000.0f;
-		    msg.relative_alt = (pos.alt - home.alt) * 1000.0f;
-		    msg.vx = pos.vel_n * 100.0f;
-		    msg.vy = pos.vel_e * 100.0f;
-		    msg.vz = pos.vel_d * 100.0f;
-		    msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f;
+			msg.relative_alt = (pos.alt - home.alt) * 1000.0f;
+			msg.vx = pos.vel_n * 100.0f;
+			msg.vy = pos.vel_e * 100.0f;
+			msg.vz = pos.vel_d * 100.0f;
+			msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f;
 
 			_mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &msg);
 		}
@@ -1015,9 +1022,9 @@ protected:
 			msg.x = pos.x;
 			msg.y = pos.y;
 			msg.z = pos.z;
-		    msg.vx = pos.vx;
-		    msg.vy = pos.vy;
-		    msg.vz = pos.vz;
+			msg.vx = pos.vx;
+			msg.vy = pos.vy;
+			msg.vz = pos.vz;
 
 			_mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &msg);
 		}
@@ -1078,9 +1085,9 @@ protected:
 			msg.x = pos.x;
 			msg.y = pos.y;
 			msg.z = pos.z;
-		    msg.roll = pos.roll;
-		    msg.pitch = pos.pitch;
-		    msg.yaw = pos.yaw;
+			msg.roll = pos.roll;
+			msg.pitch = pos.pitch;
+			msg.yaw = pos.yaw;
 
 			_mavlink->send_message(MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, &msg);
 		}
diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index 7a761588a..a3c127cdc 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -58,6 +58,10 @@
 #endif
 static const int ERROR = -1;
 
+#define CHECK_SYSID_COMPID_MISSION(_msg)		(_msg.target_system == mavlink_system.sysid && \
+						((_msg.target_component == mavlink_system.compid) || \
+						(_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \
+						(_msg.target_component == MAV_COMP_ID_ALL)))
 
 MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink),
 	_state(MAVLINK_WPM_STATE_IDLE),
@@ -79,8 +83,7 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(m
 	_mission_result_sub(-1),
 	_offboard_mission_pub(-1),
 	_slow_rate_limiter(_interval / 10.0f),
-	_verbose(false),
-	_comp_id(MAV_COMP_ID_MISSIONPLANNER)
+	_verbose(false)
 {
 	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
 	_mission_result_sub = orb_subscribe(ORB_ID(mission_result));
@@ -384,7 +387,7 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg)
 	mavlink_mission_ack_t wpa;
 	mavlink_msg_mission_ack_decode(msg, &wpa);
 
-	if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) {
+	if (CHECK_SYSID_COMPID_MISSION(wpa)) {
 		if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) {
 			if (_state == MAVLINK_WPM_STATE_SENDLIST) {
 				_time_last_recv = hrt_absolute_time();
@@ -416,7 +419,7 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg)
 	mavlink_mission_set_current_t wpc;
 	mavlink_msg_mission_set_current_decode(msg, &wpc);
 
-	if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) {
+	if (CHECK_SYSID_COMPID_MISSION(wpc)) {
 		if (_state == MAVLINK_WPM_STATE_IDLE) {
 			_time_last_recv = hrt_absolute_time();
 
@@ -451,7 +454,7 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg)
 	mavlink_mission_request_list_t wprl;
 	mavlink_msg_mission_request_list_decode(msg, &wprl);
 
-	if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) {
+	if (CHECK_SYSID_COMPID_MISSION(wprl)) {
 		if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) {
 			_time_last_recv = hrt_absolute_time();
 
@@ -487,7 +490,7 @@ MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg)
 	mavlink_mission_request_t wpr;
 	mavlink_msg_mission_request_decode(msg, &wpr);
 
-	if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) {
+	if (CHECK_SYSID_COMPID_MISSION(wpr)) {
 		if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) {
 			if (_state == MAVLINK_WPM_STATE_SENDLIST) {
 				_time_last_recv = hrt_absolute_time();
@@ -558,7 +561,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
 	mavlink_mission_count_t wpc;
 	mavlink_msg_mission_count_decode(msg, &wpc);
 
-	if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) {
+	if (CHECK_SYSID_COMPID_MISSION(wpc)) {
 		if (_state == MAVLINK_WPM_STATE_IDLE) {
 			_time_last_recv = hrt_absolute_time();
 
@@ -624,7 +627,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
 	mavlink_mission_item_t wp;
 	mavlink_msg_mission_item_decode(msg, &wp);
 
-	if (wp.target_system == mavlink_system.sysid && wp.target_component == _comp_id) {
+	if (CHECK_SYSID_COMPID_MISSION(wp)) {
 		if (_state == MAVLINK_WPM_STATE_GETLIST) {
 			_time_last_recv = hrt_absolute_time();
 
@@ -710,7 +713,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
 	mavlink_mission_clear_all_t wpca;
 	mavlink_msg_mission_clear_all_decode(msg, &wpca);
 
-	if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) {
+	if (CHECK_SYSID_COMPID_MISSION(wpca)) {
 
 		if (_state == MAVLINK_WPM_STATE_IDLE) {
 			/* don't touch mission items storage itself, but only items count in mission state */
@@ -795,10 +798,10 @@ int
 MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item)
 {
 	if (mission_item->altitude_is_relative) {
-		mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
+		mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
 
 	} else {
-		mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
+		mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
 	}
 
 	switch (mission_item->nav_cmd) {
diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h
index 8ff27f87d..a7bb94c22 100644
--- a/src/modules/mavlink/mavlink_mission.h
+++ b/src/modules/mavlink/mavlink_mission.h
@@ -126,8 +126,6 @@ private:
 
 	bool _verbose;
 
-	uint8_t			_comp_id;
-
 	/* do not allow top copying this class */
 	MavlinkMissionManager(MavlinkMissionManager &);
 	MavlinkMissionManager& operator = (const MavlinkMissionManager &);
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index c0fae0a2f..373d06d4d 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -54,6 +54,7 @@
 #include <drivers/drv_gyro.h>
 #include <drivers/drv_mag.h>
 #include <drivers/drv_baro.h>
+#include <drivers/drv_range_finder.h>
 #include <time.h>
 #include <float.h>
 #include <unistd.h>
@@ -102,6 +103,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_battery_pub(-1),
 	_cmd_pub(-1),
 	_flow_pub(-1),
+	_range_pub(-1),
 	_offboard_control_sp_pub(-1),
 	_local_pos_sp_pub(-1),
 	_global_vel_sp_pub(-1),
@@ -112,7 +114,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_telemetry_status_pub(-1),
 	_rc_pub(-1),
 	_manual_pub(-1),
-	_radio_status_available(false),
 	_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
 	_hil_frames(0),
 	_old_timestamp(0),
@@ -122,7 +123,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 {
 
 	// make sure the FTP server is started
-	(void)MavlinkFTP::getServer();
+	(void)MavlinkFTP::get_server();
 }
 
 MavlinkReceiver::~MavlinkReceiver()
@@ -137,6 +138,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_command_long(msg);
 		break;
 
+	case MAVLINK_MSG_ID_COMMAND_INT:
+		handle_message_command_int(msg);
+		break;
+
 	case MAVLINK_MSG_ID_OPTICAL_FLOW:
 		handle_message_optical_flow(msg);
 		break;
@@ -169,8 +174,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_request_data_stream(msg);
 		break;
 
-	case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
-		MavlinkFTP::getServer()->handle_message(_mavlink, msg);
+	case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
+		MavlinkFTP::get_server()->handle_message(_mavlink, msg);
 		break;
 
 	default:
@@ -197,6 +202,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 			handle_message_hil_state_quaternion(msg);
 			break;
 
+		case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW:
+			handle_message_hil_optical_flow(msg);
+			break;
+
 		default:
 			break;
 		}
@@ -277,6 +286,62 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 }
 
 void
+MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
+{
+	/* command */
+	mavlink_command_int_t cmd_mavlink;
+	mavlink_msg_command_int_decode(msg, &cmd_mavlink);
+
+	if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
+			|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
+		//check for MAVLINK terminate command
+		if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
+			/* This is the link shutdown command, terminate mavlink */
+			warnx("terminated by remote command");
+			fflush(stdout);
+			usleep(50000);
+
+			/* terminate other threads and this thread */
+			_mavlink->_task_should_exit = true;
+
+		} else {
+
+			if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
+				warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID",
+				      mavlink_system.sysid, mavlink_system.compid);
+				return;
+			}
+
+			struct vehicle_command_s vcmd;
+			memset(&vcmd, 0, sizeof(vcmd));
+
+			/* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */
+			vcmd.param1 = cmd_mavlink.param1;
+			vcmd.param2 = cmd_mavlink.param2;
+			vcmd.param3 = cmd_mavlink.param3;
+			vcmd.param4 = cmd_mavlink.param4;
+			/* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */
+			vcmd.param5 = ((double)cmd_mavlink.x) / 1e7;
+			vcmd.param6 = ((double)cmd_mavlink.y) / 1e7;
+			vcmd.param7 = cmd_mavlink.z;
+			// XXX do proper translation
+			vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command;
+			vcmd.target_system = cmd_mavlink.target_system;
+			vcmd.target_component = cmd_mavlink.target_component;
+			vcmd.source_system = msg->sysid;
+			vcmd.source_component = msg->compid;
+
+			if (_cmd_pub < 0) {
+				_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
+
+			} else {
+				orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
+			}
+		}
+	}
+}
+
+void
 MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
 {
 	/* optical flow */
@@ -305,6 +370,52 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
 }
 
 void
+MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
+{
+	/* optical flow */
+	mavlink_hil_optical_flow_t flow;
+	mavlink_msg_hil_optical_flow_decode(msg, &flow);
+
+	struct optical_flow_s f;
+	memset(&f, 0, sizeof(f));
+
+	f.timestamp = hrt_absolute_time();
+	f.flow_timestamp = flow.time_usec;
+	f.flow_raw_x = flow.flow_x;
+	f.flow_raw_y = flow.flow_y;
+	f.flow_comp_x_m = flow.flow_comp_m_x;
+	f.flow_comp_y_m = flow.flow_comp_m_y;
+	f.ground_distance_m = flow.ground_distance;
+	f.quality = flow.quality;
+	f.sensor_id = flow.sensor_id;
+
+	if (_flow_pub < 0) {
+		_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
+
+	} else {
+		orb_publish(ORB_ID(optical_flow), _flow_pub, &f);
+	}
+
+	/* Use distance value for range finder report */
+	struct range_finder_report r;
+	memset(&r, 0, sizeof(f));
+
+	r.timestamp = hrt_absolute_time();
+	r.error_count = 0;
+	r.type = RANGE_FINDER_TYPE_LASER;
+	r.distance = flow.ground_distance;
+	r.minimum_distance = 0.0f;
+	r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable
+	r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance);
+
+	if (_range_pub < 0) {
+		_range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r);
+	} else {
+		orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r);
+	}
+}
+
+void
 MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
 {
 	mavlink_set_mode_t new_mode;
@@ -385,7 +496,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 	vision_position.vx = 0.0f;
 	vision_position.vy = 0.0f;
 	vision_position.vz = 0.0f;
-	
+
 	math::Quaternion q;
 	q.from_euler(pos.roll, pos.pitch, pos.yaw);
 
@@ -430,9 +541,6 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 		} else {
 			orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
 		}
-
-		/* this means that heartbeats alone won't be published to the radio status no more */
-		_radio_status_available = true;
 	}
 }
 
@@ -474,25 +582,17 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
 
 			struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
 
-			hrt_abstime tnow = hrt_absolute_time();
+			/* set heartbeat time and topic time and publish -
+			 * the telem status also gets updated on telemetry events
+			 */
+			tstatus.timestamp = hrt_absolute_time();
+			tstatus.heartbeat_time = tstatus.timestamp;
 
-			/* always set heartbeat, publish only if telemetry link not up */
-			tstatus.heartbeat_time = tnow;
+			if (_telemetry_status_pub < 0) {
+				_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
 
-			/* if no radio status messages arrive, lets at least publish that heartbeats were received */
-			if (!_radio_status_available) {
-
-				tstatus.timestamp = tnow;
-				/* telem_time indicates the timestamp of a telemetry status packet and we got none */
-				tstatus.telem_time = 0;
-				tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
-
-				if (_telemetry_status_pub < 0) {
-					_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
-
-				} else {
-					orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
-				}
+			} else {
+				orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
 			}
 		}
 	}
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 014193100..c4e12d8d8 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -110,7 +110,9 @@ private:
 
 	void handle_message(mavlink_message_t *msg);
 	void handle_message_command_long(mavlink_message_t *msg);
+	void handle_message_command_int(mavlink_message_t *msg);
 	void handle_message_optical_flow(mavlink_message_t *msg);
+	void handle_message_hil_optical_flow(mavlink_message_t *msg);
 	void handle_message_set_mode(mavlink_message_t *msg);
 	void handle_message_vicon_position_estimate(mavlink_message_t *msg);
 	void handle_message_vision_position_estimate(mavlink_message_t *msg);
@@ -141,6 +143,7 @@ private:
 	orb_advert_t _battery_pub;
 	orb_advert_t _cmd_pub;
 	orb_advert_t _flow_pub;
+	orb_advert_t _range_pub;
 	orb_advert_t _offboard_control_sp_pub;
 	orb_advert_t _local_pos_sp_pub;
 	orb_advert_t _global_vel_sp_pub;
@@ -151,7 +154,6 @@ private:
 	orb_advert_t _telemetry_status_pub;
 	orb_advert_t _rc_pub;
 	orb_advert_t _manual_pub;
-	bool _radio_status_available;
 	int _control_mode_sub;
 	int _hil_frames;
 	uint64_t _old_timestamp;
diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
new file mode 100644
index 000000000..022041c74
--- /dev/null
+++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
@@ -0,0 +1,757 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/// @file mavlink_ftp_test.cpp
+///	@author Don Gagne <don@thegagnes.com>
+
+#include <sys/stat.h>
+#include <crc32.h>
+#include <stdio.h>
+#include <fcntl.h>
+
+#include "mavlink_ftp_test.h"
+#include "../mavlink_ftp.h"
+
+/// @brief Test case file name for Read command. File are generated using mavlink_ftp_test_data.py
+const MavlinkFtpTest::ReadTestCase MavlinkFtpTest::_rgReadTestCases[] = {
+	{ "/etc/unit_test_data/mavlink_tests/test_238.data",	MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) - 1},	// Read takes less than single packet
+	{ "/etc/unit_test_data/mavlink_tests/test_239.data",	MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) },		// Read completely fills single packet
+	{ "/etc/unit_test_data/mavlink_tests/test_240.data",	MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) + 1 },	// Read take two packets
+};
+
+const char MavlinkFtpTest::_unittest_microsd_dir[] = "/fs/microsd/ftp_unit_test_dir";
+const char MavlinkFtpTest::_unittest_microsd_file[] = "/fs/microsd/ftp_unit_test_dir/file";
+
+MavlinkFtpTest::MavlinkFtpTest() :
+	_ftp_server{},
+	_reply_msg{},
+	_lastOutgoingSeqNumber{}
+{
+}
+
+MavlinkFtpTest::~MavlinkFtpTest()
+{
+	
+}
+
+/// @brief Called before every test to initialize the FTP Server.
+void MavlinkFtpTest::init(void)
+{
+	_ftp_server = new MavlinkFTP;;
+	_ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message, this);
+
+	_cleanup_microsd();
+}
+
+/// @brief Called after every test to take down the FTP Server.
+void MavlinkFtpTest::cleanup(void)
+{
+	delete _ftp_server;
+	
+	_cleanup_microsd();
+}
+
+/// @brief Tests for correct behavior of an Ack response.
+bool MavlinkFtpTest::_ack_test(void)
+{
+	MavlinkFTP::PayloadHeader		payload;
+	mavlink_file_transfer_protocol_t	ftp_msg;
+	MavlinkFTP::PayloadHeader		*reply;
+	
+	payload.opcode = MavlinkFTP::kCmdNone;
+
+	bool success = _send_receive_msg(&payload,	// FTP payload header
+					 0,		// size in bytes of data
+					 nullptr,	// Data to start into FTP message payload
+					 &ftp_msg,	// Response from server
+					 &reply);	// Payload inside FTP message response
+	if (!success) {
+		return false;
+	}
+	
+	ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+	ut_compare("Incorrect payload size", reply->size, 0);
+	
+	return true;
+}
+
+/// @brief Tests for correct response to an invalid opcpde.
+bool MavlinkFtpTest::_bad_opcode_test(void)
+{
+	MavlinkFTP::PayloadHeader		payload;
+	mavlink_file_transfer_protocol_t	ftp_msg;
+	MavlinkFTP::PayloadHeader		*reply;
+	
+	payload.opcode = 0xFF;	// bogus opcode
+
+	bool success = _send_receive_msg(&payload,	// FTP payload header
+					 0,		// size in bytes of data
+					 nullptr,	// Data to start into FTP message payload
+					 &ftp_msg,	// Response from server
+					 &reply);	// Payload inside FTP message response
+	if (!success) {
+		return false;
+	}
+	
+	ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
+	ut_compare("Incorrect payload size", reply->size, 1);
+	ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrUnknownCommand);
+	
+	return true;
+}
+
+/// @brief Tests for correct reponse to a payload which an invalid data size field.
+bool MavlinkFtpTest::_bad_datasize_test(void)
+{
+	mavlink_message_t			msg;
+	MavlinkFTP::PayloadHeader		payload;
+	mavlink_file_transfer_protocol_t	ftp_msg;
+	MavlinkFTP::PayloadHeader		*reply;
+	
+	payload.opcode = MavlinkFTP::kCmdListDirectory;
+	
+	_setup_ftp_msg(&payload, 0, nullptr, &msg);
+	
+	// Set the data size to be one larger than is legal
+	((MavlinkFTP::PayloadHeader*)((mavlink_file_transfer_protocol_t*)msg.payload64)->payload)->size = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN + 1;
+	
+	_ftp_server->handle_message(nullptr /* mavlink */, &msg);
+	
+	if (!_decode_message(&_reply_msg, &ftp_msg, &reply)) {
+		return false;
+	}
+	
+	ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
+	ut_compare("Incorrect payload size", reply->size, 1);
+	ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidDataSize);
+	
+	return true;
+}
+
+bool MavlinkFtpTest::_list_test(void)
+{
+	MavlinkFTP::PayloadHeader		payload;
+	mavlink_file_transfer_protocol_t	ftp_msg;
+	MavlinkFTP::PayloadHeader		*reply;
+	
+	char response1[] = "Dempty_dir|Ftest_238.data\t238|Ftest_239.data\t239|Ftest_240.data\t240";
+	char response2[] = "Ddev|Detc|Dfs|Dobj";
+	
+	struct _testCase {
+		const char	*dir;		///< Directory to run List command on
+		char		*response;	///< Expected response entries from List command
+		int		response_count;	///< Number of directories that should be returned
+		bool		success;	///< true: List command should succeed, false: List command should fail
+	};
+	struct _testCase rgTestCases[] = {
+		{ "/bogus",				nullptr,	0,	false },
+		{ "/etc/unit_test_data/mavlink_tests",	response1,	4,	true },
+		{ "/",					response2,	4,	true },
+	};
+
+	for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
+		const struct _testCase *test = &rgTestCases[i];
+		
+		payload.opcode = MavlinkFTP::kCmdListDirectory;
+		payload.offset = 0;
+		
+		bool success = _send_receive_msg(&payload,		// FTP payload header
+						strlen(test->dir)+1,	// size in bytes of data
+						(uint8_t*)test->dir,	// Data to start into FTP message payload
+						&ftp_msg,		// Response from server
+						&reply);		// Payload inside FTP message response
+		if (!success) {
+			return false;
+		}
+		
+		if (test->success) {
+			ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+			ut_compare("Incorrect payload size", reply->size, strlen(test->response) + 1);
+			
+			// The return order of directories from the List command is not repeatable. So we can't do a direct comparison
+			// to a hardcoded return result string.
+			
+			// Convert null terminators to seperator char so we can use strok to parse returned data
+			for (uint8_t j=0; j<reply->size-1; j++) {
+				if (reply->data[j] == 0) {
+					reply->data[j] = '|';
+				}
+			}
+			
+			// Loop over returned directory entries trying to find then in the response list
+			char *dir;
+			int response_count = 0;
+			dir = strtok((char *)&reply->data[0], "|");
+			while (dir != nullptr) {
+				ut_assert("Returned directory not found in expected response", strstr(test->response, dir));
+				response_count++;
+				dir = strtok(nullptr, "|");
+			}
+			
+			ut_compare("Incorrect number of directory entires returned", test->response_count, response_count);
+		} else {
+			ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
+			ut_compare("Incorrect payload size", reply->size, 2);
+			ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
+		}
+	}
+	
+	return true;
+}
+
+/// @brief Tests for correct reponse to a List command on a valid directory, but with an offset that
+/// is beyond the last directory entry.
+bool MavlinkFtpTest::_list_eof_test(void)
+{
+	MavlinkFTP::PayloadHeader		payload;
+	mavlink_file_transfer_protocol_t	ftp_msg;
+	MavlinkFTP::PayloadHeader		*reply;
+	const char				*dir = "/";
+	
+	payload.opcode = MavlinkFTP::kCmdListDirectory;
+	payload.offset = 4;	// offset past top level dirs
+	
+	bool success = _send_receive_msg(&payload,	// FTP payload header
+					strlen(dir)+1,	// size in bytes of data
+					(uint8_t*)dir,	// Data to start into FTP message payload
+					&ftp_msg,	// Response from server
+					&reply);	// Payload inside FTP message response
+	if (!success) {
+		return false;
+	}
+
+	ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
+	ut_compare("Incorrect payload size", reply->size, 1);
+	ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrEOF);
+	
+	return true;
+}
+
+/// @brief Tests for correct reponse to an Open command on a file which does not exist.
+bool MavlinkFtpTest::_open_badfile_test(void)
+{
+	MavlinkFTP::PayloadHeader		payload;
+	mavlink_file_transfer_protocol_t	ftp_msg;
+	MavlinkFTP::PayloadHeader		*reply;
+	const char				*dir = "/foo";	// non-existent file
+	
+	payload.opcode = MavlinkFTP::kCmdOpenFile;
+	payload.offset = 0;
+	
+	bool success = _send_receive_msg(&payload,	// FTP payload header
+					strlen(dir)+1,	// size in bytes of data
+					(uint8_t*)dir,	// Data to start into FTP message payload
+					&ftp_msg,	// Response from server
+					&reply);	// Payload inside FTP message response
+	if (!success) {
+		return false;
+	}
+	
+	ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
+	ut_compare("Incorrect payload size", reply->size, 2);
+	ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
+	
+	return true;
+}
+
+/// @brief Tests for correct reponse to an Open command on a file, followed by Terminate
+bool MavlinkFtpTest::_open_terminate_test(void)
+{
+	MavlinkFTP::PayloadHeader		payload;
+	mavlink_file_transfer_protocol_t	ftp_msg;
+	MavlinkFTP::PayloadHeader		*reply;
+	
+	for (size_t i=0; i<sizeof(_rgReadTestCases)/sizeof(_rgReadTestCases[0]); i++) {
+		struct stat st;
+		const ReadTestCase *test = &_rgReadTestCases[i];
+		
+		payload.opcode = MavlinkFTP::kCmdOpenFile;
+		payload.offset = 0;
+		
+		bool success = _send_receive_msg(&payload,		// FTP payload header
+						 strlen(test->file)+1,	// size in bytes of data
+						 (uint8_t*)test->file,	// Data to start into FTP message payload
+						 &ftp_msg,		// Response from server
+						 &reply);		// Payload inside FTP message response
+		if (!success) {
+			return false;
+		}
+		
+		ut_compare("stat failed", stat(test->file, &st), 0);
+		
+		
+		ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+		ut_compare("Incorrect payload size", reply->size, sizeof(uint32_t));
+		ut_compare("File size incorrect", *((uint32_t*)&reply->data[0]), (uint32_t)st.st_size);
+
+		payload.opcode = MavlinkFTP::kCmdTerminateSession;
+		payload.session = reply->session;
+		payload.size = 0;
+		
+		success = _send_receive_msg(&payload,	// FTP payload header
+					    0,		// size in bytes of data
+					    nullptr,	// Data to start into FTP message payload
+					    &ftp_msg,	// Response from server
+					    &reply);	// Payload inside FTP message response
+		if (!success) {
+			return false;
+		}
+		
+		ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+		ut_compare("Incorrect payload size", reply->size, 0);
+	}
+
+	return true;
+}
+
+/// @brief Tests for correct reponse to a Terminate command on an invalid session.
+bool MavlinkFtpTest::_terminate_badsession_test(void)
+{
+	MavlinkFTP::PayloadHeader		payload;
+	mavlink_file_transfer_protocol_t	ftp_msg;
+	MavlinkFTP::PayloadHeader		*reply;
+	const char				*file = _rgReadTestCases[0].file;
+	
+	payload.opcode = MavlinkFTP::kCmdOpenFile;
+	payload.offset = 0;
+	
+	bool success = _send_receive_msg(&payload,	// FTP payload header
+					strlen(file)+1,	// size in bytes of data
+					(uint8_t*)file,	// Data to start into FTP message payload
+					&ftp_msg,	// Response from server
+					&reply);	// Payload inside FTP message response
+	if (!success) {
+		return false;
+	}
+	
+	ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+	
+	payload.opcode = MavlinkFTP::kCmdTerminateSession;
+	payload.session = reply->session + 1;
+	payload.size = 0;
+	
+	success = _send_receive_msg(&payload,	// FTP payload header
+				   0,		// size in bytes of data
+				   nullptr,	// Data to start into FTP message payload
+				   &ftp_msg,	// Response from server
+				   &reply);	// Payload inside FTP message response
+	if (!success) {
+		return false;
+	}
+	
+	ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
+	ut_compare("Incorrect payload size", reply->size, 1);
+	ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidSession);
+	
+	return true;
+}
+
+/// @brief Tests for correct reponse to a Read command on an open session.
+bool MavlinkFtpTest::_read_test(void)
+{
+	MavlinkFTP::PayloadHeader		payload;
+	mavlink_file_transfer_protocol_t	ftp_msg;
+	MavlinkFTP::PayloadHeader		*reply;
+	
+	for (size_t i=0; i<sizeof(_rgReadTestCases)/sizeof(_rgReadTestCases[0]); i++) {
+		struct stat st;
+		const ReadTestCase *test = &_rgReadTestCases[i];
+		
+		// Read in the file so we can compare it to what we get back
+		ut_compare("stat failed", stat(test->file, &st), 0);
+		uint8_t *bytes = new uint8_t[st.st_size];
+		ut_assert("new failed", bytes != nullptr);
+		int fd = ::open(test->file, O_RDONLY);
+		ut_assert("open failed", fd != -1);
+		int bytes_read = ::read(fd, bytes, st.st_size);
+		ut_compare("read failed", bytes_read, st.st_size);
+		::close(fd);
+		
+		// Test case data files are created for specific boundary conditions
+		ut_compare("Test case data files are out of date", test->length, st.st_size);
+		
+		payload.opcode = MavlinkFTP::kCmdOpenFile;
+		payload.offset = 0;
+		
+		bool success = _send_receive_msg(&payload,		// FTP payload header
+						 strlen(test->file)+1,	// size in bytes of data
+						 (uint8_t*)test->file,	// Data to start into FTP message payload
+						 &ftp_msg,		// Response from server
+						 &reply);		// Payload inside FTP message response
+		if (!success) {
+			return false;
+		}
+		
+		ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+		
+		payload.opcode = MavlinkFTP::kCmdReadFile;
+		payload.session = reply->session;
+		payload.offset = 0;
+		
+		success = _send_receive_msg(&payload,	// FTP payload header
+					    0,		// size in bytes of data
+					    nullptr,	// Data to start into FTP message payload
+					    &ftp_msg,	// Response from server
+					    &reply);	// Payload inside FTP message response
+		if (!success) {
+			return false;
+		}
+		
+		ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+		ut_compare("Offset incorrect", reply->offset, 0);
+		
+		if (test->length <= MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader)) {
+			ut_compare("Payload size incorrect", reply->size, (uint32_t)st.st_size);
+			ut_compare("File contents differ", memcmp(reply->data, bytes, st.st_size), 0);
+		}
+		
+		payload.opcode = MavlinkFTP::kCmdTerminateSession;
+		payload.session = reply->session;
+		payload.size = 0;
+		
+		success = _send_receive_msg(&payload,	// FTP payload header
+					    0,		// size in bytes of data
+					    nullptr,	// Data to start into FTP message payload
+					    &ftp_msg,	// Response from server
+					    &reply);	// Payload inside FTP message response
+		if (!success) {
+			return false;
+		}
+		
+		ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+		ut_compare("Incorrect payload size", reply->size, 0);
+	}
+	
+	return true;
+}
+
+/// @brief Tests for correct reponse to a Read command on an invalid session.
+bool MavlinkFtpTest::_read_badsession_test(void)
+{
+	MavlinkFTP::PayloadHeader		payload;
+	mavlink_file_transfer_protocol_t	ftp_msg;
+	MavlinkFTP::PayloadHeader		*reply;
+	const char				*file = _rgReadTestCases[0].file;
+	
+	payload.opcode = MavlinkFTP::kCmdOpenFile;
+	payload.offset = 0;
+	
+	bool success = _send_receive_msg(&payload,	// FTP payload header
+					 strlen(file)+1,	// size in bytes of data
+					 (uint8_t*)file,	// Data to start into FTP message payload
+					 &ftp_msg,	// Response from server
+					 &reply);	// Payload inside FTP message response
+	if (!success) {
+		return false;
+	}
+	
+	ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+	
+	payload.opcode = MavlinkFTP::kCmdReadFile;
+	payload.session = reply->session + 1;	// Invalid session
+	payload.offset = 0;
+	
+	success = _send_receive_msg(&payload,	// FTP payload header
+				    0,		// size in bytes of data
+				    nullptr,	// Data to start into FTP message payload
+				    &ftp_msg,	// Response from server
+				    &reply);	// Payload inside FTP message response
+	if (!success) {
+		return false;
+	}
+	
+	ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
+	ut_compare("Incorrect payload size", reply->size, 1);
+	ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidSession);
+	
+	return true;
+}
+
+bool MavlinkFtpTest::_removedirectory_test(void)
+{
+	MavlinkFTP::PayloadHeader		payload;
+	mavlink_file_transfer_protocol_t	ftp_msg;
+	MavlinkFTP::PayloadHeader		*reply;
+	int					fd;
+	
+	struct _testCase {
+		const char	*dir;
+		bool		success;
+		bool		deleteFile;
+	};
+	static const struct _testCase rgTestCases[] = {
+		{ "/bogus",						false,	false },
+		{ "/etc/unit_test_data/mavlink_tests/empty_dir",	false,	false },
+		{ _unittest_microsd_dir,				false,	false },
+		{ _unittest_microsd_file,				false,	false },
+		{ _unittest_microsd_dir,				true,	true },
+	};
+	
+	ut_compare("mkdir failed", ::mkdir(_unittest_microsd_dir, S_IRWXU | S_IRWXG | S_IRWXO), 0);
+	ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL)) != -1);
+	::close(fd);
+	
+	for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
+		const struct _testCase *test = &rgTestCases[i];
+		
+		if (test->deleteFile) {
+			ut_compare("unlink failed", ::unlink(_unittest_microsd_file), 0);
+		}
+		
+		payload.opcode = MavlinkFTP::kCmdRemoveDirectory;
+		payload.offset = 0;
+		
+		bool success = _send_receive_msg(&payload,		// FTP payload header
+						 strlen(test->dir)+1,	// size in bytes of data
+						 (uint8_t*)test->dir,	// Data to start into FTP message payload
+						 &ftp_msg,		// Response from server
+						 &reply);		// Payload inside FTP message response
+		if (!success) {
+			return false;
+		}
+		
+		if (test->success) {
+			ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+			ut_compare("Incorrect payload size", reply->size, 0);
+		} else {
+			ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
+			ut_compare("Incorrect payload size", reply->size, 2);
+			ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
+		}
+	}
+	
+	return true;
+}
+
+bool MavlinkFtpTest::_createdirectory_test(void)
+{
+	MavlinkFTP::PayloadHeader		payload;
+	mavlink_file_transfer_protocol_t	ftp_msg;
+	MavlinkFTP::PayloadHeader		*reply;
+	
+	struct _testCase {
+		const char	*dir;
+		bool		success;
+	};
+	static const struct _testCase rgTestCases[] = {
+		{ "/etc/bogus",			false },
+		{ _unittest_microsd_dir,	true },
+		{ _unittest_microsd_dir,	false },
+		{ "/fs/microsd/bogus/bogus",	false },
+	};
+	
+	for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
+		const struct _testCase *test = &rgTestCases[i];
+		
+		payload.opcode = MavlinkFTP::kCmdCreateDirectory;
+		payload.offset = 0;
+		
+		bool success = _send_receive_msg(&payload,		// FTP payload header
+						 strlen(test->dir)+1,	// size in bytes of data
+						 (uint8_t*)test->dir,	// Data to start into FTP message payload
+						 &ftp_msg,		// Response from server
+						 &reply);		// Payload inside FTP message response
+		if (!success) {
+			return false;
+		}
+		
+		if (test->success) {
+			ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+			ut_compare("Incorrect payload size", reply->size, 0);
+		} else {
+			ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
+			ut_compare("Incorrect payload size", reply->size, 2);
+			ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
+		}
+	}
+	
+	return true;
+}
+
+bool MavlinkFtpTest::_removefile_test(void)
+{
+	MavlinkFTP::PayloadHeader		payload;
+	mavlink_file_transfer_protocol_t	ftp_msg;
+	MavlinkFTP::PayloadHeader		*reply;
+	int					fd;
+	
+	struct _testCase {
+		const char	*file;
+		bool		success;
+	};
+	static const struct _testCase rgTestCases[] = {
+		{ "/bogus",			false },
+		{ _rgReadTestCases[0].file,	false },
+		{ _unittest_microsd_dir,	false },
+		{ _unittest_microsd_file,	true },
+		{ _unittest_microsd_file,	false },
+	};
+	
+	ut_compare("mkdir failed", ::mkdir(_unittest_microsd_dir, S_IRWXU | S_IRWXG | S_IRWXO), 0);
+	ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL)) != -1);
+	::close(fd);
+	
+	for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
+		const struct _testCase *test = &rgTestCases[i];
+		
+		payload.opcode = MavlinkFTP::kCmdRemoveFile;
+		payload.offset = 0;
+		
+		bool success = _send_receive_msg(&payload,		// FTP payload header
+						 strlen(test->file)+1,	// size in bytes of data
+						 (uint8_t*)test->file,	// Data to start into FTP message payload
+						 &ftp_msg,		// Response from server
+						 &reply);		// Payload inside FTP message response
+		if (!success) {
+			return false;
+		}
+		
+		if (test->success) {
+			ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+			ut_compare("Incorrect payload size", reply->size, 0);
+		} else {
+			ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
+			ut_compare("Incorrect payload size", reply->size, 2);
+			ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
+		}
+	}
+	
+	return true;
+}
+
+/// @brief Static method used as callback from MavlinkFTP. This method will be called by MavlinkFTP when
+/// it needs to send a message out on Mavlink.
+void MavlinkFtpTest::receive_message(const mavlink_message_t *msg, MavlinkFtpTest *ftp_test)
+{
+	ftp_test->_receive_message(msg);
+}
+
+/// @brief Non-Static version of receive_message
+void MavlinkFtpTest::_receive_message(const mavlink_message_t *msg)
+{
+	// Move the message into our own member variable
+	memcpy(&_reply_msg, msg, sizeof(mavlink_message_t));
+}
+
+/// @brief Decode and validate the incoming message
+bool MavlinkFtpTest::_decode_message(const mavlink_message_t		*msg,		///< Mavlink message to decode
+				     mavlink_file_transfer_protocol_t	*ftp_msg,	///< Decoded FTP message
+				     MavlinkFTP::PayloadHeader		**payload)	///< Payload inside FTP message response
+{
+	mavlink_msg_file_transfer_protocol_decode(msg, ftp_msg);
+	
+	// Make sure the targets are correct
+	ut_compare("Target network non-zero", ftp_msg->target_network, 0);
+	ut_compare("Target system id mismatch", ftp_msg->target_system, clientSystemId);
+	ut_compare("Target component id mismatch", ftp_msg->target_component, clientComponentId);
+	
+	*payload = reinterpret_cast<MavlinkFTP::PayloadHeader *>(ftp_msg->payload);
+	
+	// Make sure we have a good sequence number
+	ut_compare("Sequence number mismatch", (*payload)->seqNumber, _lastOutgoingSeqNumber + 1);
+	
+	// Bump sequence number for next outgoing message
+	_lastOutgoingSeqNumber++;
+	
+	return true;
+}
+
+/// @brief Initializes an FTP message into a mavlink message
+void MavlinkFtpTest::_setup_ftp_msg(MavlinkFTP::PayloadHeader	*payload_header,	///< FTP payload header
+				    uint8_t			size,			///< size in bytes of data
+				    const uint8_t		*data,			///< Data to start into FTP message payload
+				    mavlink_message_t		*msg)			///< Returned mavlink message
+{
+	uint8_t payload_bytes[MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN];
+	MavlinkFTP::PayloadHeader *payload = reinterpret_cast<MavlinkFTP::PayloadHeader *>(payload_bytes);
+			     
+	memcpy(payload, payload_header, sizeof(MavlinkFTP::PayloadHeader));
+	
+	payload->seqNumber = _lastOutgoingSeqNumber;
+	payload->size = size;
+	if (size != 0) {
+		memcpy(payload->data, data, size);
+	}
+    
+	payload->padding[0] = 0;
+	payload->padding[1] = 0;
+	
+	msg->checksum = 0;
+	mavlink_msg_file_transfer_protocol_pack(clientSystemId,		// Sender system id
+						clientComponentId,	// Sender component id
+						msg,			// Message to pack payload into
+						0,			// Target network
+						serverSystemId,		// Target system id
+						serverComponentId,	// Target component id
+						payload_bytes);		// Payload to pack into message
+}
+
+/// @brief Sends the specified FTP message to the server and returns response
+bool MavlinkFtpTest::_send_receive_msg(MavlinkFTP::PayloadHeader	*payload_header,	///< FTP payload header
+				      uint8_t				size,			///< size in bytes of data
+				      const uint8_t			*data,			///< Data to start into FTP message payload
+				      mavlink_file_transfer_protocol_t	*ftp_msg_reply,		///< Response from server
+				      MavlinkFTP::PayloadHeader		**payload_reply)	///< Payload inside FTP message response
+{
+	mavlink_message_t msg;
+
+	_setup_ftp_msg(payload_header, size, data, &msg);
+	_ftp_server->handle_message(nullptr /* mavlink */, &msg);
+	return _decode_message(&_reply_msg, ftp_msg_reply, payload_reply);
+}
+	
+/// @brief Cleans up an files created on microsd during testing
+void MavlinkFtpTest::_cleanup_microsd(void)
+{
+	::unlink(_unittest_microsd_file);
+	::rmdir(_unittest_microsd_dir);
+}
+
+/// @brief Runs all the unit tests
+void MavlinkFtpTest::runTests(void)
+{
+	ut_run_test(_ack_test);
+	ut_run_test(_bad_opcode_test);
+	ut_run_test(_bad_datasize_test);
+	ut_run_test(_list_test);
+	ut_run_test(_list_eof_test);
+	ut_run_test(_open_badfile_test);
+	ut_run_test(_open_terminate_test);
+	ut_run_test(_terminate_badsession_test);
+	ut_run_test(_read_test);
+	ut_run_test(_read_badsession_test);
+	ut_run_test(_removedirectory_test);
+	ut_run_test(_createdirectory_test);
+	ut_run_test(_removefile_test);
+}
+
diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h
new file mode 100644
index 000000000..babd909da
--- /dev/null
+++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h
@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/// @file mavlink_ftp_test.h
+///	@author Don Gagne <don@thegagnes.com>
+
+#pragma once
+
+#include <unit_test/unit_test.h>
+#include "../mavlink_bridge_header.h"
+#include "../mavlink_ftp.h"
+
+class MavlinkFtpTest : public UnitTest
+{
+public:
+	MavlinkFtpTest();
+	virtual ~MavlinkFtpTest();
+	
+	virtual void init(void);
+	virtual void cleanup(void);
+	
+	virtual void runTests(void);
+	
+	static void receive_message(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest);
+	
+	static const uint8_t serverSystemId = 50;	///< System ID for server
+	static const uint8_t serverComponentId = 1;	///< Component ID for server
+	static const uint8_t serverChannel = 0;		///< Channel to send to
+	
+	static const uint8_t clientSystemId = 1;	///< System ID for client
+	static const uint8_t clientComponentId = 0;	///< Component ID for client
+	
+	// We don't want any of these
+	MavlinkFtpTest(const MavlinkFtpTest&);
+	MavlinkFtpTest& operator=(const MavlinkFtpTest&);
+	
+private:
+	bool _ack_test(void);
+	bool _bad_opcode_test(void);
+	bool _bad_datasize_test(void);
+	bool _list_test(void);
+	bool _list_eof_test(void);
+	bool _open_badfile_test(void);
+	bool _open_terminate_test(void);
+	bool _terminate_badsession_test(void);
+	bool _read_test(void);
+	bool _read_badsession_test(void);
+	bool _removedirectory_test(void);
+	bool _createdirectory_test(void);
+	bool _removefile_test(void);
+	
+	void _receive_message(const mavlink_message_t *msg);
+	void _setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data, mavlink_message_t *msg);
+	bool _decode_message(const mavlink_message_t *msg, mavlink_file_transfer_protocol_t *ftp_msg, MavlinkFTP::PayloadHeader **payload);
+	bool _send_receive_msg(MavlinkFTP::PayloadHeader	*payload_header,
+                           uint8_t				size,
+                           const uint8_t			*data,
+                           mavlink_file_transfer_protocol_t	*ftp_msg_reply,
+                           MavlinkFTP::PayloadHeader		**payload_reply);
+	void _cleanup_microsd(void);
+	
+	MavlinkFTP *_ftp_server;
+    
+	mavlink_message_t _reply_msg;
+    
+	uint16_t _lastOutgoingSeqNumber;
+	
+	struct ReadTestCase {
+		const char	*file;
+		const uint16_t	length;
+	};
+	static const ReadTestCase _rgReadTestCases[];
+	
+	static const char _unittest_microsd_dir[];
+	static const char _unittest_microsd_file[];
+};
+
diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py
new file mode 100644
index 000000000..a7e68f2a3
--- /dev/null
+++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py
@@ -0,0 +1,7 @@
+import sys
+print 'Arguments: file - ' + sys.argv[1] + ', length - ' + sys.argv[2]
+file = open(sys.argv[1], 'w')
+for i in range(int(sys.argv[2])):
+	b = bytearray([i & 0xFF])
+	file.write(b)
+file.close()
+\ No newline at end of file
diff --git a/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp b/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp
new file mode 100644
index 000000000..d9c1e413a
--- /dev/null
+++ b/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp
@@ -0,0 +1,52 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_ftp_tests.cpp
+ */
+
+#include <systemlib/err.h>
+
+#include "mavlink_ftp_test.h"
+
+extern "C" __EXPORT int mavlink_tests_main(int argc, char *argv[]);
+
+int mavlink_tests_main(int argc, char *argv[])
+{
+	MavlinkFtpTest* test = new MavlinkFtpTest;
+	
+	test->runTests();
+	test->printResults();
+
+	return 0;
+}
diff --git a/src/modules/mavlink/mavlink_tests/module.mk b/src/modules/mavlink/mavlink_tests/module.mk
new file mode 100644
index 000000000..07cfce1dc
--- /dev/null
+++ b/src/modules/mavlink/mavlink_tests/module.mk
@@ -0,0 +1,48 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# System state machine tests.
+#
+
+MODULE_COMMAND		= mavlink_tests
+SRCS			= mavlink_tests.cpp \
+			mavlink_ftp_test.cpp \
+			../mavlink_ftp.cpp \
+			../mavlink.c
+
+INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
+
+MODULE_STACKSIZE = 5000
+
+EXTRACXXFLAGS	= -Weffc++ -DMAVLINK_FTP_UNIT_TEST
diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp
index 266215308..49aec21e0 100644
--- a/src/modules/navigator/geofence.cpp
+++ b/src/modules/navigator/geofence.cpp
@@ -76,11 +76,7 @@ Geofence::~Geofence()
 
 bool Geofence::inside(const struct vehicle_global_position_s *vehicle)
 {
-	double lat = vehicle->lat / 1e7d;
-	double lon = vehicle->lon / 1e7d;
-	//float	alt = vehicle->alt;
-
-	return inside(lat, lon, vehicle->alt);
+	return inside(vehicle->lat, vehicle->lon, vehicle->alt);
 }
 
 bool Geofence::inside(double lat, double lon, float altitude)
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 719301a0a..104065132 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -36,12 +36,14 @@
  * Helper class to access missions
  *
  * @author Julian Oes <julian@oes.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
 #include <sys/types.h>
 #include <string.h>
 #include <stdlib.h>
 #include <unistd.h>
+#include <float.h>
 
 #include <drivers/drv_hrt.h>
 
@@ -49,6 +51,7 @@
 #include <mavlink/mavlink_log.h>
 #include <systemlib/err.h>
 #include <geo/geo.h>
+#include <lib/mathlib/mathlib.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>
@@ -62,6 +65,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_param_onboard_enabled(this, "ONBOARD_EN"),
 	_param_takeoff_alt(this, "TAKEOFF_ALT"),
 	_param_dist_1wp(this, "DIST_1WP"),
+	_param_altmode(this, "ALTMODE"),
 	_onboard_mission({0}),
 	_offboard_mission({0}),
 	_current_onboard_mission_index(-1),
@@ -72,7 +76,11 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_mission_result({0}),
 	_mission_type(MISSION_TYPE_NONE),
 	_inited(false),
-	_dist_1wp_ok(false)
+	_dist_1wp_ok(false),
+	_missionFeasiblityChecker(),
+	_min_current_sp_distance_xy(FLT_MAX),
+	_mission_item_previous_alt(NAN),
+	_distance_current_previous(0.0f)
 {
 	/* load initial params */
 	updateParams();
@@ -145,6 +153,8 @@ Mission::on_active()
 		advance_mission();
 		set_mission_items();
 
+	} else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) {
+		altitude_sp_foh_update();
 	} else {
 		/* if waypoint position reached allow loiter on the setpoint */
 		if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) {
@@ -203,7 +213,7 @@ Mission::update_offboard_mission()
 		 * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
 		dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
 
-		missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing,
+		_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing,
 				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
 				_navigator->get_home_position()->alt);
 
@@ -314,11 +324,19 @@ Mission::set_mission_items()
 	/* make sure param is up to date */
 	updateParams();
 
+	/* reset the altitude foh logic, if altitude foh is enabled (param) a new foh element starts now */
+	altitude_sp_foh_reset();
+
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
 	/* set previous position setpoint to current */
 	set_previous_pos_setpoint();
 
+	/* Copy previous mission item altitude (can be extended to a copy of the full mission item if needed) */
+	if (pos_sp_triplet->previous.valid) {
+		_mission_item_previous_alt = _mission_item.altitude;
+	}
+
 	/* get home distance state */
 	bool home_dist_ok = check_dist_1wp();
 	/* the home dist check provides user feedback, so we initialize it to this */
@@ -470,9 +488,75 @@ Mission::set_mission_items()
 		pos_sp_triplet->next.valid = false;
 	}
 
+	/* Save the distance between the current sp and the previous one */
+	if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) {
+		_distance_current_previous = get_distance_to_next_waypoint(pos_sp_triplet->current.lat,
+				pos_sp_triplet->current.lon,
+				pos_sp_triplet->previous.lat,
+				pos_sp_triplet->previous.lon);
+	}
+
+	_navigator->set_position_setpoint_triplet_updated();
+}
+
+void
+Mission::altitude_sp_foh_update()
+{
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	/* Don't change setpoint if last and current waypoint are not valid */
+	if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid ||
+			!isfinite(_mission_item_previous_alt)) {
+		return;
+	}
+
+	/* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */
+	if (_distance_current_previous - _mission_item.acceptance_radius < 0.0f) {
+		return;
+	}
+
+	/* Don't do FOH for landing and takeoff waypoints, the ground may be near
+	 * and the FW controller has a custom landing logic */
+	if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
+		return;
+	}
+
+
+	/* Calculate distance to current waypoint */
+	float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
+			_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
+
+	/* Save distance to waypoint if it is the smallest ever achieved, however make sure that
+	 * _min_current_sp_distance_xy is never larger than the distance between the current and the previous wp */
+	_min_current_sp_distance_xy = math::min(math::min(d_current, _min_current_sp_distance_xy),
+			_distance_current_previous);
+
+	/* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt
+	 * navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */
+	if (_min_current_sp_distance_xy < _mission_item.acceptance_radius) {
+		pos_sp_triplet->current.alt = _mission_item.altitude;
+	} else {
+		/* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp
+		* of the mission item as it is used to check if the mission item is reached
+		* The setpoint is set linearly and such that the system reaches the current altitude at the acceptance
+		* radius around the current waypoint
+		**/
+		float delta_alt = (_mission_item.altitude - _mission_item_previous_alt);
+		float grad = -delta_alt/(_distance_current_previous - _mission_item.acceptance_radius);
+		float a = _mission_item_previous_alt - grad * _distance_current_previous;
+		pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy;
+
+	}
+
 	_navigator->set_position_setpoint_triplet_updated();
 }
 
+void
+Mission::altitude_sp_foh_reset()
+{
+	_min_current_sp_distance_xy = FLT_MAX;
+}
+
 bool
 Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item)
 {
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 1edaa5c8a..4737be391 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -36,6 +36,7 @@
  * Navigator mode to access missions
  *
  * @author Julian Oes <julian@oes.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
 #ifndef NAVIGATOR_MISSION_H
@@ -75,6 +76,11 @@ public:
 
 	virtual void on_active();
 
+	enum mission_altitude_mode {
+		MISSION_ALTMODE_ZOH = 0,
+		MISSION_ALTMODE_FOH = 1
+	};
+
 private:
 	/**
 	 * Update onboard mission topic
@@ -103,6 +109,16 @@ private:
 	void set_mission_items();
 
 	/**
+	 * Updates the altitude sp to follow a foh
+	 */
+	void altitude_sp_foh_update();
+
+	/**
+	 * Resets the altitude sp foh logic
+	 */
+	void altitude_sp_foh_reset();
+
+	/**
 	 * Read current or next mission item from the dataman and watch out for DO_JUMPS
 	 * @return true if successful
 	 */
@@ -136,6 +152,7 @@ private:
 	control::BlockParamInt _param_onboard_enabled;
 	control::BlockParamFloat _param_takeoff_alt;
 	control::BlockParamFloat _param_dist_1wp;
+	control::BlockParamInt _param_altmode;
 
 	struct mission_s _onboard_mission;
 	struct mission_s _offboard_mission;
@@ -157,7 +174,13 @@ private:
 	bool _inited;
 	bool _dist_1wp_ok;
 
-	MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */
+	MissionFeasibilityChecker _missionFeasiblityChecker; /**< class that checks if a mission is feasible */
+
+	float _min_current_sp_distance_xy; /**< minimum distance which was achieved to the current waypoint  */
+	float _mission_item_previous_alt; /**< holds the altitude of the previous mission item,
+					    can be replaced by a full copy of the previous mission item if needed*/
+	float _distance_current_previous; /**< distance from previous to current sp in pos_sp_triplet,
+					    only use if current and previous are valid */
 };
 
 #endif
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 4adf77dce..723caec7c 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -113,6 +113,19 @@ MissionBlock::is_mission_item_reached()
 			if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) {
 				_waypoint_position_reached = true;
 			}
+		} else if (!_navigator->get_vstatus()->is_rotary_wing &&
+			(_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
+			_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+			_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT)) {
+			/* Loiter mission item on a non rotary wing: the aircraft is going to circle the
+			 * coordinates with a radius equal to the loiter_radius field. It is not flying
+			 * through the waypoint center.
+			 * Therefore the item is marked as reached once the system reaches the loiter
+			 * radius (+ some margin). Time inside and turn count is handled elsewhere.
+			 */
+			if (dist >= 0.0f && dist <= _mission_item.loiter_radius * 1.2f) {
+				_waypoint_position_reached = true;
+			}
 		} else {
 			/* for normal mission items used their acceptance radius */
 			if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) {
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index 606521f20..7ae7f60cb 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -84,7 +84,12 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_
 bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
 {
 
-	return  (checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt));
+	/* Perform checks and issue feedback to the user for all checks */
+	bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
+	bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt);
+
+	/* Mission is only marked as feasible if all checks return true */
+	return (resGeofence && resHomeAltitude);
 }
 
 bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
@@ -93,7 +98,13 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre
 	updateNavigationCapabilities();
 //	warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement);
 
-	return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt));
+	/* Perform checks and issue feedback to the user for all checks */
+	bool resLanding = checkFixedWingLanding(dm_current, nMissionItems);
+	bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
+	bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt);
+
+	/* Mission is only marked as feasible if all checks return true */
+	return (resLanding && resGeofence && resHomeAltitude);
 }
 
 bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
@@ -216,9 +227,8 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
 		}
 	}
 
-
-//	float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt;
-	return false;
+	/* No landing waypoints or no waypoints */
+	return true;
 }
 
 void MissionFeasibilityChecker::updateNavigationCapabilities()
diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c
index d15e1e771..04c01fe51 100644
--- a/src/modules/navigator/mission_params.c
+++ b/src/modules/navigator/mission_params.c
@@ -82,3 +82,16 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1);
  * @group Mission
  */
 PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500);
+
+/**
+ * Altitude setpoint mode
+ *
+ * 0: the system will follow a zero order hold altitude setpoint
+ * 1: the system will follow a first order hold altitude setpoint
+ * values follow the definition in enum mission_altitude_mode
+ *
+ * @min 0
+ * @max 1
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(MIS_ALTMODE, 0);
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 331a9a728..042c46afd 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -353,6 +353,8 @@ Navigator::task_main()
 			case NAVIGATION_STATE_ACRO:
 			case NAVIGATION_STATE_ALTCTL:
 			case NAVIGATION_STATE_POSCTL:
+			case NAVIGATION_STATE_LAND:
+			case NAVIGATION_STATE_TERMINATION:
 				_navigation_mode = nullptr;
 				_can_loiter_at_sp = false;
 				break;
@@ -368,8 +370,6 @@ Navigator::task_main()
 			case NAVIGATION_STATE_AUTO_RTGS:
 				_navigation_mode = &_rtl; /* TODO: change this to something else */
 				break;
-			case NAVIGATION_STATE_LAND:
-			case NAVIGATION_STATE_TERMINATION:
 			case NAVIGATION_STATE_OFFBOARD:
 				_navigation_mode = &_offboard;
 				break;
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 606c639f9..3e19333d8 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -58,7 +58,7 @@ extern "C" {
 /*
  * Maximum interval in us before FMU signal is considered lost
  */
-#define FMU_INPUT_DROP_LIMIT_US		200000
+#define FMU_INPUT_DROP_LIMIT_US		500000
 
 /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
 #define ROLL     0
@@ -98,7 +98,8 @@ 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) {
+	if ((system_state.fmu_data_received_time == 0) ||
+		hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
 
 		/* too long without FMU input, time to go to failsafe */
 		if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
@@ -109,6 +110,9 @@ mixer_tick(void)
 
 	} else {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
+
+		/* this flag is never cleared once OK */
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED;
 	}
 
 	/* default to failsafe mixing - it will be forced below if flag is set */
@@ -139,7 +143,9 @@ mixer_tick(void)
 		     (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)) {
+		     !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
+		     /* do not enter manual override if we asked for termination failsafe and FMU is lost */
+		     !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) {
 
 		 	/* if allowed, mix from RC inputs directly */
 			source = MIX_OVERRIDE;
@@ -155,29 +161,11 @@ mixer_tick(void)
 	}
 
 	/*
-	 * Check if we should force failsafe - and do it if we have to
-	 */
-	if (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) {
-		source = MIX_FAILSAFE;
-	}
-
-	/*
-	 * 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)
@@ -195,6 +183,38 @@ mixer_tick(void)
 						&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
 
 	/*
+	 * Check if failsafe termination is set - if yes,
+	 * set the force failsafe flag once entering the first
+	 * failsafe condition.
+	 */
+	if (	/* if we have requested flight termination style failsafe (noreturn) */
+		(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) &&
+		/* and we ended up in a failsafe condition */
+		(source == MIX_FAILSAFE) &&
+		/* and we should be armed, so we intended to provide outputs */
+		should_arm &&
+		/* and FMU is initialized */
+		(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED)) {
+		r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
+	}
+
+	/*
+	 * Check if we should force failsafe - and do it if we have to
+	 */
+	if (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) {
+		source = MIX_FAILSAFE;
+	}
+
+	/*
+	 * 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);
+	}
+
+	/*
 	 * Run the mixers.
 	 */
 	if (source == MIX_FAILSAFE) {
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 050783687..4739f6e40 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -111,6 +111,7 @@
 #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_FLAGS_FMU_INITIALIZED	(1 << 13) /* FMU was initialized and OK once */
 
 #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) /* [1] VBatt is very close to regulator dropout */
@@ -180,6 +181,7 @@
 #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_ARMING_FORCE_FAILSAFE	(1 << 8) /* If set, the system will always output the failsafe values */
+#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE	(1 << 9) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */
 
 #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 */
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 43161aa70..7a5a5e484 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -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
@@ -190,7 +190,8 @@ volatile uint16_t	r_page_setup[] =
 					 PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
 					 PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
 					 PX4IO_P_SETUP_ARMING_LOCKDOWN | \
-					 PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE)
+					 PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \
+					 PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)
 #define PX4IO_P_SETUP_RATES_VALID	((1 << PX4IO_SERVO_COUNT) - 1)
 #define PX4IO_P_SETUP_RELAYS_VALID	((1 << PX4IO_RELAY_CHANNELS) - 1)
 
@@ -285,7 +286,9 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 		while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
 
 			/* XXX range-check value? */
-			r_page_servos[offset] = *values;
+			if (*values != PWM_IGNORE_THIS_CHANNEL) {
+				r_page_servos[offset] = *values;
+			}
 
 			offset++;
 			num_values--;
@@ -516,6 +519,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 				r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
 			}
 
+			/*
+			 * If the failsafe termination flag is set, do not allow the autopilot to unset it
+			 */
+			value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE);
+
+			/*
+			 * If failsafe termination is enabled and force failsafe bit is set, do not allow
+			 * the autopilot to clear it.
+			 */
+			if (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) {
+				value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE);
+			}
+
 			r_setup_arming = value;
 
 			break;
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 6c4b49452..e13593077 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -76,6 +76,7 @@
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/satellite_info.h>
 #include <uORB/topics/vehicle_vicon_position.h>
+#include <uORB/topics/vision_position_estimate.h>
 #include <uORB/topics/vehicle_global_velocity_setpoint.h>
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/battery_status.h>
@@ -934,6 +935,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct vehicle_global_position_s global_pos;
 		struct position_setpoint_triplet_s triplet;
 		struct vehicle_vicon_position_s vicon_pos;
+		struct vision_position_estimate vision_pos;
 		struct optical_flow_s flow;
 		struct rc_channels_s rc;
 		struct differential_pressure_s diff_pres;
@@ -984,6 +986,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_EST1_s log_EST1;
 			struct log_PWR_s log_PWR;
 			struct log_VICN_s log_VICN;
+			struct log_VISN_s log_VISN;
 			struct log_GS0A_s log_GS0A;
 			struct log_GS0B_s log_GS0B;
 			struct log_GS1A_s log_GS1A;
@@ -1013,6 +1016,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int gps_pos_sub;
 		int sat_info_sub;
 		int vicon_pos_sub;
+		int vision_pos_sub;
 		int flow_sub;
 		int rc_sub;
 		int airspeed_sub;
@@ -1043,6 +1047,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
 	subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
+	subs.vision_pos_sub = orb_subscribe(ORB_ID(vision_position_estimate));
 	subs.flow_sub = orb_subscribe(ORB_ID(optical_flow));
 	subs.rc_sub = orb_subscribe(ORB_ID(rc_channels));
 	subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
@@ -1427,6 +1432,11 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d;
 			log_msg.body.log_GPOS.eph = buf.global_pos.eph;
 			log_msg.body.log_GPOS.epv = buf.global_pos.epv;
+			if (buf.global_pos.terrain_alt_valid) {
+				log_msg.body.log_GPOS.terrain_alt = buf.global_pos.terrain_alt;
+			} else {
+				log_msg.body.log_GPOS.terrain_alt = -1.0f;
+			}
 			LOGBUFFER_WRITE_AND_COUNT(GPOS);
 		}
 
@@ -1460,6 +1470,22 @@ int sdlog2_thread_main(int argc, char *argv[])
 			LOGBUFFER_WRITE_AND_COUNT(VICN);
 		}
 
+		/* --- VISION POSITION --- */
+		if (copy_if_updated(ORB_ID(vision_position_estimate), subs.vision_pos_sub, &buf.vision_pos)) {
+			log_msg.msg_type = LOG_VISN_MSG;
+			log_msg.body.log_VISN.x = buf.vision_pos.x;
+			log_msg.body.log_VISN.y = buf.vision_pos.y;
+			log_msg.body.log_VISN.z = buf.vision_pos.z;
+			log_msg.body.log_VISN.vx = buf.vision_pos.vx;
+			log_msg.body.log_VISN.vy = buf.vision_pos.vy;
+			log_msg.body.log_VISN.vz = buf.vision_pos.vz;
+			log_msg.body.log_VISN.qx = buf.vision_pos.q[0];
+			log_msg.body.log_VISN.qy = buf.vision_pos.q[1];
+			log_msg.body.log_VISN.qz = buf.vision_pos.q[2];
+			log_msg.body.log_VISN.qw = buf.vision_pos.q[3];
+			LOGBUFFER_WRITE_AND_COUNT(VISN);
+		}
+
 		/* --- FLOW --- */
 		if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) {
 			log_msg.msg_type = LOG_FLOW_MSG;
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index fb7609435..d2845eb75 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -220,6 +220,7 @@ struct log_GPOS_s {
 	float vel_d;
 	float eph;
 	float epv;
+	float terrain_alt;
 };
 
 /* --- GPSP - GLOBAL POSITION SETPOINT --- */
@@ -391,6 +392,20 @@ struct log_TEL_s {
 	uint64_t heartbeat_time;
 };
 
+/* --- VISN - VISION POSITION --- */
+#define LOG_VISN_MSG 38
+struct log_VISN_s {
+	float x;
+	float y;
+	float z;
+	float vx;
+	float vy;
+	float vz;
+	float qx;
+	float qy;
+	float qz;
+	float qw;
+};
 
 /********** SYSTEM MESSAGES, ID > 0x80 **********/
 
@@ -435,7 +450,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(AIRS, "fff",			"IndSpeed,TrueSpeed,AirTemp"),
 	LOG_FORMAT(ARSP, "fff",			"RollRateSP,PitchRateSP,YawRateSP"),
 	LOG_FORMAT(FLOW, "hhfffBB",		"RawX,RawY,CompX,CompY,Dist,Q,SensID"),
-	LOG_FORMAT(GPOS, "LLffffff",		"Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV"),
+	LOG_FORMAT(GPOS, "LLfffffff",		"Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"),
 	LOG_FORMAT(GPSP, "BLLffBfbf",		"NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
 	LOG_FORMAT(ESC, "HBBBHHHHHHfH",		"count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 	LOG_FORMAT(GVSP, "fff",			"VX,VY,VZ"),
@@ -449,6 +464,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(EST1, "ffffffffffffffff",	"s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"),
 	LOG_FORMAT(PWR, "fffBBBBB",		"Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
 	LOG_FORMAT(VICN, "ffffff",		"X,Y,Z,Roll,Pitch,Yaw"),
+	LOG_FORMAT(VISN, "ffffffffff",		"X,Y,Z,VX,VY,VZ,QuatX,QuatY,QuatZ,QuatW"),
 	LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 7ce6ef5ef..229bfe3ce 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -214,6 +214,16 @@ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f);
  */
 PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0);
 
+/**
+ * QNH for barometer
+ *
+ * @min 500
+ * @max 1500
+ * @group Sensor Calibration
+ * @unit hPa
+ */
+PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f);
+
 
 /**
  * Board rotation
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 4e8a8c01d..cdcb428dd 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -143,6 +143,12 @@
 
 #define STICK_ON_OFF_LIMIT 0.75f
 
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
 /**
  * Sensor app start / stop handling function
  *
@@ -235,7 +241,7 @@ private:
 	math::Matrix<3, 3>	_board_rotation;		/**< rotation matrix for the orientation that the board is mounted */
 	math::Matrix<3, 3>	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
 	bool		_mag_is_external;		/**< true if the active mag is on an external board */
-	
+
 	uint64_t _battery_discharged;			/**< battery discharged current in mA*ms */
 	hrt_abstime _battery_current_timestamp;	/**< timestamp of last battery current reading */
 
@@ -258,7 +264,7 @@ private:
 
 		int board_rotation;
 		int external_mag_rotation;
-		
+
 		float board_offset[3];
 
 		int rc_map_roll;
@@ -301,6 +307,8 @@ private:
 		float battery_voltage_scaling;
 		float battery_current_scaling;
 
+		float baro_qnh;
+
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -354,9 +362,11 @@ private:
 
 		param_t board_rotation;
 		param_t external_mag_rotation;
-		
+
 		param_t board_offset[3];
 
+		param_t baro_qnh;
+
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -462,12 +472,6 @@ private:
 namespace sensors
 {
 
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
-
 Sensors	*g_sensors = nullptr;
 }
 
@@ -611,12 +615,15 @@ Sensors::Sensors() :
 	/* rotations */
 	_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
 	_parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
-	
+
 	/* rotation offsets */
 	_parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF");
 	_parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF");
 	_parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF");
 
+	/* Barometer QNH */
+	_parameter_handles.baro_qnh = param_find("SENS_BARO_QNH");
+
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -828,19 +835,37 @@ Sensors::parameters_update()
 
 	get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
 	get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
-	
+
 	param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0]));
 	param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1]));
 	param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2]));
-	
+
 	/** fine tune board offset on parameter update **/
-	math::Matrix<3, 3> board_rotation_offset; 
+	math::Matrix<3, 3> board_rotation_offset;
 	board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0],
 							 M_DEG_TO_RAD_F * _parameters.board_offset[1],
 							 M_DEG_TO_RAD_F * _parameters.board_offset[2]);
-	
+
 	_board_rotation = _board_rotation * board_rotation_offset;
 
+	/* update barometer qnh setting */
+	param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh));
+	int	fd;
+	fd = open(BARO_DEVICE_PATH, 0);
+	if (fd < 0) {
+		warn("%s", BARO_DEVICE_PATH);
+		errx(1, "FATAL: no barometer found");
+
+	} else {
+		int ret = ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100));
+		if (ret) {
+			warnx("qnh could not be set");
+			close(fd);
+			return ERROR;
+		}
+		close(fd);
+	}
+
 	return OK;
 }
 
@@ -1204,16 +1229,18 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 	if (updated) {
 		orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
 
-		raw.differential_pressure_pa = _diff_pres.differential_pressure_pa;
+		raw.differential_pressure_pa = _diff_pres.differential_pressure_raw_pa;
 		raw.differential_pressure_timestamp = _diff_pres.timestamp;
 		raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;
 
 		float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
 
 		_airspeed.timestamp = _diff_pres.timestamp;
-		_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa);
-		_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
-					      raw.baro_pres_mbar * 1e2f, air_temperature_celsius);
+
+		/* don't risk to feed negative airspeed into the system */
+		_airspeed.indicated_airspeed_m_s = math::max(0.0f, calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa));
+		_airspeed.true_airspeed_m_s = math::max(0.0f, calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
+					      raw.baro_pres_mbar * 1e2f, air_temperature_celsius));
 		_airspeed.air_temperature_celsius = air_temperature_celsius;
 
 		/* announce the airspeed if needed, just publish else */
@@ -1432,12 +1459,10 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 					if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) {
 
 						float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa;
-						float diff_pres_pa = (diff_pres_pa_raw > 0.0f) ? diff_pres_pa_raw : 0.0f;
 
 						_diff_pres.timestamp = t;
-						_diff_pres.differential_pressure_pa = diff_pres_pa;
 						_diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw;
-						_diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa * 0.1f);
+						_diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa_raw * 0.1f);
 						_diff_pres.temperature = -1000.0f;
 
 						/* announce the airspeed if needed, just publish else */
diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h
index cdf60febc..17989558e 100644
--- a/src/modules/systemlib/mixer/mixer.h
+++ b/src/modules/systemlib/mixer/mixer.h
@@ -130,6 +130,9 @@
 
 #include <nuttx/config.h>
 #include "drivers/drv_mixer.h"
+
+#include <uORB/topics/multirotor_motor_limits.h>
+
 #include "mixer_load.h"
 
 /**
@@ -531,6 +534,9 @@ private:
 	float				_yaw_scale;
 	float				_idle_speed;
 
+	orb_advert_t			_limits_pub;
+	multirotor_motor_limits_s 	_limits;
+
 	unsigned			_rotor_count;
 	const Rotor			*_rotors;
 
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index 4b22a46d0..57e17b67d 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -36,7 +36,8 @@
  *
  * Multi-rotor mixers.
  */
-
+#include <uORB/uORB.h>
+#include <uORB/topics/multirotor_motor_limits.h>
 #include <nuttx/config.h>
 
 #include <sys/types.h>
@@ -302,6 +303,11 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 	float		min_out = 0.0f;
 	float		max_out = 0.0f;
 
+	_limits.roll_pitch = false;
+	_limits.yaw = false;
+	_limits.throttle_upper = false;
+	_limits.throttle_lower = false;
+
 	/* perform initial mix pass yielding unbounded outputs, ignore yaw */
 	for (unsigned i = 0; i < _rotor_count; i++) {
 		float out = roll * _rotors[i].roll_scale +
@@ -311,6 +317,7 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 		/* limit yaw if it causes outputs clipping */
 		if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) {
 			yaw = -out / _rotors[i].yaw_scale;
+			_limits.yaw = true;
 		}
 
 		/* calculate min and max output values */
@@ -332,6 +339,7 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 		for (unsigned i = 0; i < _rotor_count; i++) {
 			outputs[i] = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust;
 		}
+		_limits.roll_pitch = true;
 
 	} else {
 		/* roll/pitch mixed without limiting, add yaw control */
@@ -344,6 +352,7 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 	float scale_out;
 	if (max_out > 1.0f) {
 		scale_out = 1.0f / max_out;
+		_limits.throttle_upper = true;
 
 	} else {
 		scale_out = 1.0f;
@@ -351,9 +360,20 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 
 	/* scale outputs to range _idle_speed..1, and do final limiting */
 	for (unsigned i = 0; i < _rotor_count; i++) {
+		if (outputs[i] < _idle_speed) {
+			_limits.throttle_lower = true;
+		}
 		outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out), _idle_speed, 1.0f);
 	}
 
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+        /* publish/advertise motor limits if running on FMU */
+        if (_limits_pub > 0) {
+            orb_publish(ORB_ID(multirotor_motor_limits), _limits_pub, &_limits);
+        } else {
+            _limits_pub = orb_advertise(ORB_ID(multirotor_motor_limits), &_limits);
+        }
+#endif
 	return _rotor_count;
 }
 
diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index e44e6cdb0..6b8d0e634 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -322,7 +322,8 @@ param_get_value_ptr(param_t param)
 			v = &param_info_base[param].val;
 		}
 
-		if (param_type(param) == PARAM_TYPE_STRUCT) {
+		if (param_type(param) >= PARAM_TYPE_STRUCT
+				&& param_type(param) <= PARAM_TYPE_STRUCT_MAX) {
 			result = v->p;
 
 		} else {
diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h
index 084cd931a..dc73b37e9 100644
--- a/src/modules/systemlib/param/param.h
+++ b/src/modules/systemlib/param/param.h
@@ -307,7 +307,7 @@ __EXPORT int 		param_load_default(void);
 	struct param_info_s __param__##_name = {	\
 		#_name,					\
 		PARAM_TYPE_STRUCT + sizeof(_default),	\
-		.val.p = &_default;			\
+		.val.p = &_default			\
 	}
 
 /**
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index f7d5f8737..b921865eb 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -192,6 +192,9 @@ 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/multirotor_motor_limits.h"
+ORB_DEFINE(multirotor_motor_limits, struct multirotor_motor_limits_s);
+
 #include "topics/telemetry_status.h"
 ORB_DEFINE(telemetry_status_0, struct telemetry_status_s);
 ORB_DEFINE(telemetry_status_1, struct telemetry_status_s);
diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h
index cd48d3cb2..7342fcf04 100644
--- a/src/modules/uORB/topics/differential_pressure.h
+++ b/src/modules/uORB/topics/differential_pressure.h
@@ -54,7 +54,6 @@
 struct differential_pressure_s {
 	uint64_t	timestamp;			/**< Microseconds since system boot, needed to integrate */
 	uint64_t	error_count;			/**< Number of errors detected by driver */
-	float	differential_pressure_pa;		/**< Differential pressure reading */
 	float	differential_pressure_raw_pa;		/**< Raw differential pressure reading (may be negative) */
 	float	differential_pressure_filtered_pa;	/**< Low pass filtered differential pressure reading */
 	float	max_differential_pressure_pa;		/**< Maximum differential pressure reading */
diff --git a/src/modules/uORB/topics/multirotor_motor_limits.h b/src/modules/uORB/topics/multirotor_motor_limits.h
new file mode 100644
index 000000000..44c51e4d9
--- /dev/null
+++ b/src/modules/uORB/topics/multirotor_motor_limits.h
@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ *   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 multirotor_motor_limits.h
+ *
+ * Definition of multirotor_motor_limits  topic
+ */
+
+#ifndef MULTIROTOR_MOTOR_LIMITS_H_
+#define MULTIROTOR_MOTOR_LIMITS_H_
+
+#include "../uORB.h"
+#include <stdint.h>
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Motor limits
+ */
+struct multirotor_motor_limits_s {
+        uint8_t roll_pitch	: 1; // roll/pitch limit reached
+        uint8_t yaw		: 1; // yaw limit reached
+        uint8_t throttle_lower	: 1; // lower throttle limit reached
+        uint8_t throttle_upper	: 1; // upper throttle limit reached
+        uint8_t reserved	: 4;
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(multirotor_motor_limits);
+
+#endif
diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h
index 7db33d98b..44aa50572 100644
--- a/src/modules/uORB/topics/vehicle_command.h
+++ b/src/modules/uORB/topics/vehicle_command.h
@@ -1,9 +1,6 @@
 /****************************************************************************
  *
- *   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 Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (C) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,6 +34,10 @@
 /**
  * @file vehicle_command.h
  * Definition of the vehicle command uORB topic.
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #ifndef TOPIC_VEHICLE_COMMAND_H_
@@ -52,6 +53,9 @@
  * but can contain additional ones.
  */
 enum VEHICLE_CMD {
+	VEHICLE_CMD_CUSTOM_0 = 0, /* test command */
+	VEHICLE_CMD_CUSTOM_1 = 1, /* test command */
+	VEHICLE_CMD_CUSTOM_2 = 2, /* test command */
 	VEHICLE_CMD_NAV_WAYPOINT = 16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude|  */
 	VEHICLE_CMD_NAV_LOITER_UNLIM = 17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude|  */
 	VEHICLE_CMD_NAV_LOITER_TURNS = 18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude|  */
@@ -87,7 +91,8 @@ enum VEHICLE_CMD {
 	VEHICLE_CMD_MISSION_START = 300, /* start running a mission |first_item: the first mission item to run| last_item:  the last mission item to run (after this item is run, the mission ends)|  */
 	VEHICLE_CMD_COMPONENT_ARM_DISARM = 400, /* Arms / Disarms a component |1 to arm, 0 to disarm|  */
 	VEHICLE_CMD_START_RX_PAIR = 500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX|  */
-	VEHICLE_CMD_ENUM_END = 501, /*  | */
+	VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001, /**< Prepare a payload deployment in the flight plan */
+	VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 /**< Control a pre-programmed payload deployment */
 };
 
 /**
@@ -115,8 +120,8 @@ struct vehicle_command_s {
 	float param2;			/**< Parameter 2, as defined by MAVLink VEHICLE_CMD enum.   */
 	float param3;			/**< Parameter 3, as defined by MAVLink VEHICLE_CMD enum.   */
 	float param4;			/**< Parameter 4, as defined by MAVLink VEHICLE_CMD enum.   */
-	float param5;			/**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum.   */
-	float param6;			/**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum.   */
+	double param5;			/**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum.   */
+	double param6;			/**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum.   */
 	float param7;			/**< Parameter 7, as defined by MAVLink 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  */
diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h
index 25137c1c6..c3bb3b893 100644
--- a/src/modules/uORB/topics/vehicle_global_position.h
+++ b/src/modules/uORB/topics/vehicle_global_position.h
@@ -72,6 +72,8 @@ struct vehicle_global_position_s {
 	float yaw; 			/**< Yaw in radians -PI..+PI.							   */
 	float eph;			/**< Standard deviation of position estimate horizontally */
 	float epv;			/**< Standard deviation of position vertically */
+	float terrain_alt;		/**< Terrain altitude in m, WGS84 */
+	bool terrain_alt_valid;		/**< Terrain altitude estimate is valid */
 };
 
 /**
diff --git a/src/modules/uavcan/esc_controller.cpp b/src/modules/uavcan/actuators/esc.cpp
index 406eba88c..223d94731 100644
--- a/src/modules/uavcan/esc_controller.cpp
+++ b/src/modules/uavcan/actuators/esc.cpp
@@ -32,12 +32,12 @@
  ****************************************************************************/
 
 /**
- * @file esc_controller.cpp
+ * @file esc.cpp
  *
  * @author Pavel Kirienko <pavel.kirienko@gmail.com>
  */
 
-#include "esc_controller.hpp"
+#include "esc.hpp"
 #include <systemlib/err.h>
 
 UavcanEscController::UavcanEscController(uavcan::INode &node) :
diff --git a/src/modules/uavcan/esc_controller.hpp b/src/modules/uavcan/actuators/esc.hpp
index 559ede561..cf0988210 100644
--- a/src/modules/uavcan/esc_controller.hpp
+++ b/src/modules/uavcan/actuators/esc.hpp
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file esc_controller.hpp
+ * @file esc.hpp
  *
  * UAVCAN <--> ORB bridge for ESC messages:
  *     uavcan.equipment.esc.RawCommand
diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk
index 3865f2468..f92bc754f 100644
--- a/src/modules/uavcan/module.mk
+++ b/src/modules/uavcan/module.mk
@@ -40,10 +40,19 @@ MODULE_COMMAND = uavcan
 
 MAXOPTIMIZATION = -Os
 
-SRCS += uavcan_main.cpp         \
-        uavcan_clock.cpp        \
-        esc_controller.cpp      \
-        gnss_receiver.cpp
+# Main
+SRCS += uavcan_main.cpp              \
+        uavcan_clock.cpp             \
+        uavcan_params.c
+
+# Actuators
+SRCS += actuators/esc.cpp
+
+# Sensors
+SRCS += sensors/sensor_bridge.cpp   \
+        sensors/gnss.cpp            \
+        sensors/mag.cpp             \
+        sensors/baro.cpp
 
 #
 # libuavcan
diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp
new file mode 100644
index 000000000..80c5e3828
--- /dev/null
+++ b/src/modules/uavcan/sensors/baro.cpp
@@ -0,0 +1,117 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include "baro.hpp"
+#include <cmath>
+
+static const orb_id_t BARO_TOPICS[2] = {
+	ORB_ID(sensor_baro0),
+	ORB_ID(sensor_baro1)
+};
+
+const char *const UavcanBarometerBridge::NAME = "baro";
+
+UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) :
+UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, BARO_TOPICS),
+_sub_air_data(node)
+{
+}
+
+int UavcanBarometerBridge::init()
+{
+	int res = device::CDev::init();
+	if (res < 0) {
+		return res;
+	}
+
+	res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb));
+	if (res < 0) {
+		log("failed to start uavcan sub: %d", res);
+		return res;
+	}
+	return 0;
+}
+
+int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+	case BAROIOCSMSLPRESSURE: {
+		if ((arg < 80000) || (arg > 120000)) {
+			return -EINVAL;
+		} else {
+			log("new msl pressure %u", _msl_pressure);
+			_msl_pressure = arg;
+			return OK;
+		}
+	}
+	case BAROIOCGMSLPRESSURE: {
+		return _msl_pressure;
+	}
+	default: {
+		return CDev::ioctl(filp, cmd, arg);
+	}
+	}
+}
+
+void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData> &msg)
+{
+	auto report = ::baro_report();
+
+	report.timestamp = msg.getUtcTimestamp().toUSec();
+	if (report.timestamp == 0) {
+		report.timestamp = msg.getMonotonicTimestamp().toUSec();
+	}
+
+	report.temperature = msg.static_temperature;
+	report.pressure    = msg.static_pressure / 100.0F;  // Convert to millibar
+
+	/*
+	 * Altitude computation
+	 * Refer to the MS5611 driver for details
+	 */
+	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
+	const double g  = 9.80665;       // gravity constant in m/s/s
+	const double R  = 287.05;        // ideal gas constant in J/kg/K
+
+	const double p1 = _msl_pressure / 1000.0;      // current pressure at MSL in kPa
+	const double p = double(msg.static_pressure) / 1000.0; // measured pressure in kPa
+
+	report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
+
+	publish(msg.getSrcNodeID().get(), &report);
+}
diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp
new file mode 100644
index 000000000..9d470219e
--- /dev/null
+++ b/src/modules/uavcan/sensors/baro.hpp
@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include "sensor_bridge.hpp"
+#include <drivers/drv_baro.h>
+
+#include <uavcan/equipment/air_data/StaticAirData.hpp>
+
+class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase
+{
+public:
+	static const char *const NAME;
+
+	UavcanBarometerBridge(uavcan::INode& node);
+
+	const char *get_name() const override { return NAME; }
+
+	int init() override;
+
+private:
+	int ioctl(struct file *filp, int cmd, unsigned long arg) override;
+
+	void air_data_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData> &msg);
+
+	typedef uavcan::MethodBinder<UavcanBarometerBridge*,
+		void (UavcanBarometerBridge::*)
+			(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData>&)>
+		AirDataCbBinder;
+
+	uavcan::Subscriber<uavcan::equipment::air_data::StaticAirData, AirDataCbBinder> _sub_air_data;
+	unsigned _msl_pressure = 101325;
+};
diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/sensors/gnss.cpp
index ba1fe5e49..24afe6aaf 100644
--- a/src/modules/uavcan/gnss_receiver.cpp
+++ b/src/modules/uavcan/sensors/gnss.cpp
@@ -32,52 +32,72 @@
  ****************************************************************************/
 
 /**
- * @file gnss_receiver.cpp
+ * @file gnss.cpp
  *
  * @author Pavel Kirienko <pavel.kirienko@gmail.com>
  * @author Andrew Chambers <achamber@gmail.com>
  *
  */
 
-#include "gnss_receiver.hpp"
+#include "gnss.hpp"
 #include <systemlib/err.h>
 #include <mathlib/mathlib.h>
 
-#define MM_PER_CM 			10	// Millimeters per centimeter
+const char *const UavcanGnssBridge::NAME = "gnss";
 
-UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) :
+UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
 _node(node),
-_uavcan_sub_status(node),
+_sub_fix(node),
 _report_pub(-1)
 {
 }
 
-int UavcanGnssReceiver::init()
+int UavcanGnssBridge::init()
 {
-	int res = -1;
-
-	// GNSS fix subscription
-	res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssReceiver::gnss_fix_sub_cb));
+	int res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb));
 	if (res < 0)
 	{
 		warnx("GNSS fix sub failed %i", res);
 		return res;
 	}
+	return res;
+}
 
-	// Clear the uORB GPS report
-	memset(&_report, 0, sizeof(_report));
+unsigned UavcanGnssBridge::get_num_redundant_channels() const
+{
+	return (_receiver_node_id < 0) ? 0 : 1;
+}
 
-	return res;
+void UavcanGnssBridge::print_status() const
+{
+	printf("RX errors: %d, receiver node id: ", _sub_fix.getFailureCount());
+	if (_receiver_node_id < 0) {
+		printf("N/A\n");
+	} else {
+		printf("%d\n", _receiver_node_id);
+	}
 }
 
-void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
+void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
 {
-	_report.timestamp_position = hrt_absolute_time();
-	_report.lat = msg.lat_1e7;
-	_report.lon = msg.lon_1e7;
-	_report.alt = msg.alt_1e2 * MM_PER_CM;	// Convert from centimeter (1e2) to millimeters (1e3)
+	// This bridge does not support redundant GNSS receivers yet.
+	if (_receiver_node_id < 0) {
+		_receiver_node_id = msg.getSrcNodeID().get();
+		warnx("GNSS receiver node ID: %d", _receiver_node_id);
+	} else {
+		if (_receiver_node_id != msg.getSrcNodeID().get()) {
+			return;  // This GNSS receiver is the redundant one, ignore it.
+		}
+	}
+
+	auto report = ::vehicle_gps_position_s();
+
+	report.timestamp_position = hrt_absolute_time();
+	report.lat = msg.latitude_deg_1e8 / 10;
+	report.lon = msg.longitude_deg_1e8 / 10;
+	report.alt = msg.height_msl_mm;
 
-	_report.timestamp_variance = _report.timestamp_position;
+	report.timestamp_variance = report.timestamp_position;
 
 
 	// Check if the msg contains valid covariance information
@@ -90,19 +110,19 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav
 
 		// Horizontal position uncertainty
 		const float horizontal_pos_variance = math::max(pos_cov[0], pos_cov[4]);
-		_report.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F;
+		report.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F;
 
 		// Vertical position uncertainty
-		_report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F;
+		report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F;
 	} else {
-		_report.eph = -1.0F;
-		_report.epv = -1.0F;
+		report.eph = -1.0F;
+		report.epv = -1.0F;
 	}
 
 	if (valid_velocity_covariance) {
 	    float vel_cov[9];
 	    msg.velocity_covariance.unpackSquareMatrix(vel_cov);
-		_report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]);
+		report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]);
 
 		/* There is a nonlinear relationship between the velocity vector and the heading.
 		 * Use Jacobian to transform velocity covariance to heading covariance
@@ -118,36 +138,36 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav
 		float vel_e = msg.ned_velocity[1];
 		float vel_n_sq = vel_n * vel_n;
 		float vel_e_sq = vel_e * vel_e;
-		_report.c_variance_rad =
+		report.c_variance_rad =
 				(vel_e_sq * vel_cov[0] +
 						-2 * vel_n * vel_e * vel_cov[1] +	// Covariance matrix is symmetric
 						vel_n_sq* vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq));
 
 	} else {
-		_report.s_variance_m_s = -1.0F;
-		_report.c_variance_rad = -1.0F;
+		report.s_variance_m_s = -1.0F;
+		report.c_variance_rad = -1.0F;
 	}
 
-	_report.fix_type = msg.status;
+	report.fix_type = msg.status;
 
-	_report.timestamp_velocity = _report.timestamp_position;
-	_report.vel_n_m_s = msg.ned_velocity[0];
-	_report.vel_e_m_s = msg.ned_velocity[1];
-	_report.vel_d_m_s = msg.ned_velocity[2];
-	_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 = atan2f(_report.vel_e_m_s, _report.vel_n_m_s);
-	_report.vel_ned_valid = true;
+	report.timestamp_velocity = report.timestamp_position;
+	report.vel_n_m_s = msg.ned_velocity[0];
+	report.vel_e_m_s = msg.ned_velocity[1];
+	report.vel_d_m_s = msg.ned_velocity[2];
+	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 = atan2f(report.vel_e_m_s, report.vel_n_m_s);
+	report.vel_ned_valid = true;
 
-	_report.timestamp_time = _report.timestamp_position;
-	_report.time_gps_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec();	// Convert to microseconds
+	report.timestamp_time = report.timestamp_position;
+	report.time_gps_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec();	// Convert to microseconds
 
-	_report.satellites_used = msg.sats_used;
+	report.satellites_used = msg.sats_used;
 
 	if (_report_pub > 0) {
-		orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
+		orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &report);
 
 	} else {
-		_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
+		_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &report);
 	}
 
 }
diff --git a/src/modules/uavcan/gnss_receiver.hpp b/src/modules/uavcan/sensors/gnss.hpp
index 18df8da2f..e8466b401 100644
--- a/src/modules/uavcan/gnss_receiver.hpp
+++ b/src/modules/uavcan/sensors/gnss.hpp
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file gnss_receiver.hpp
+ * @file gnss.hpp
  *
  * UAVCAN --> ORB bridge for GNSS messages:
  *     uavcan.equipment.gnss.Fix
@@ -51,12 +51,22 @@
 #include <uavcan/uavcan.hpp>
 #include <uavcan/equipment/gnss/Fix.hpp>
 
-class UavcanGnssReceiver
+#include "sensor_bridge.hpp"
+
+class UavcanGnssBridge : public IUavcanSensorBridge
 {
 public:
-	UavcanGnssReceiver(uavcan::INode& node);
+	static const char *const NAME;
+
+	UavcanGnssBridge(uavcan::INode& node);
+
+	const char *get_name() const override { return NAME; }
+
+	int init() override;
 
-	int init();
+	unsigned get_num_redundant_channels() const override;
+
+	void print_status() const override;
 
 private:
 	/**
@@ -64,21 +74,14 @@ private:
 	 */
 	void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg);
 
-
-	typedef uavcan::MethodBinder<UavcanGnssReceiver*,
-		void (UavcanGnssReceiver::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>&)>
+	typedef uavcan::MethodBinder<UavcanGnssBridge*,
+		void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>&)>
 		FixCbBinder;
 
-	/*
-	 * libuavcan related things
-	 */
-	uavcan::INode													&_node;
-	uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder>	_uavcan_sub_status;
+	uavcan::INode &_node;
+	uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _sub_fix;
+	int _receiver_node_id = -1;
 
-	/*
-	 * uORB
-	 */
-	struct vehicle_gps_position_s 	_report;					///< uORB topic for gnss position
-	orb_advert_t			_report_pub;					///< uORB pub for gnss position
+	orb_advert_t _report_pub;                ///< uORB pub for gnss position
 
 };
diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp
new file mode 100644
index 000000000..8e6a9a22f
--- /dev/null
+++ b/src/modules/uavcan/sensors/mag.cpp
@@ -0,0 +1,123 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include "mag.hpp"
+
+static const orb_id_t MAG_TOPICS[3] = {
+	ORB_ID(sensor_mag0),
+	ORB_ID(sensor_mag1),
+	ORB_ID(sensor_mag2)
+};
+
+const char *const UavcanMagnetometerBridge::NAME = "mag";
+
+UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) :
+UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS),
+_sub_mag(node)
+{
+	_scale.x_scale = 1.0F;
+	_scale.y_scale = 1.0F;
+	_scale.z_scale = 1.0F;
+}
+
+int UavcanMagnetometerBridge::init()
+{
+	int res = device::CDev::init();
+	if (res < 0) {
+		return res;
+	}
+
+	res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb));
+	if (res < 0) {
+		log("failed to start uavcan sub: %d", res);
+		return res;
+	}
+	return 0;
+}
+
+int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+	case MAGIOCSSCALE: {
+		std::memcpy(&_scale, reinterpret_cast<const void*>(arg), sizeof(_scale));
+		return 0;
+	}
+	case MAGIOCGSCALE: {
+		std::memcpy(reinterpret_cast<void*>(arg), &_scale, sizeof(_scale));
+		return 0;
+	}
+	case MAGIOCSELFTEST: {
+		return 0;           // Nothing to do
+	}
+	case MAGIOCGEXTERNAL: {
+		return 0;           // We don't want anyone to transform the coordinate frame, so we declare it onboard
+	}
+	case MAGIOCSSAMPLERATE: {
+		return 0;           // Pretend that this stuff is supported to keep the sensor app happy
+	}
+	case MAGIOCCALIBRATE:
+	case MAGIOCGSAMPLERATE:
+	case MAGIOCSRANGE:
+	case MAGIOCGRANGE:
+	case MAGIOCSLOWPASS:
+	case MAGIOCEXSTRAP:
+	case MAGIOCGLOWPASS: {
+		return -EINVAL;
+	}
+	default: {
+		return CDev::ioctl(filp, cmd, arg);
+	}
+	}
+}
+
+void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg)
+{
+	auto report = ::mag_report();
+
+	report.range_ga = 1.3F;   // Arbitrary number, doesn't really mean anything
+
+	report.timestamp = msg.getUtcTimestamp().toUSec();
+	if (report.timestamp == 0) {
+		report.timestamp = msg.getMonotonicTimestamp().toUSec();
+	}
+
+	report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale;
+	report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale;
+	report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale;
+
+	publish(msg.getSrcNodeID().get(), &report);
+}
diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp
new file mode 100644
index 000000000..6d413a8f7
--- /dev/null
+++ b/src/modules/uavcan/sensors/mag.hpp
@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include "sensor_bridge.hpp"
+#include <drivers/drv_mag.h>
+
+#include <uavcan/equipment/ahrs/Magnetometer.hpp>
+
+class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase
+{
+public:
+	static const char *const NAME;
+
+	UavcanMagnetometerBridge(uavcan::INode& node);
+
+	const char *get_name() const override { return NAME; }
+
+	int init() override;
+
+private:
+	int ioctl(struct file *filp, int cmd, unsigned long arg) override;
+
+	void mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg);
+
+	typedef uavcan::MethodBinder<UavcanMagnetometerBridge*,
+		void (UavcanMagnetometerBridge::*)
+			(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer>&)>
+		MagCbBinder;
+
+	uavcan::Subscriber<uavcan::equipment::ahrs::Magnetometer, MagCbBinder> _sub_mag;
+	mag_scale _scale = {};
+};
diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp
new file mode 100644
index 000000000..9608ce680
--- /dev/null
+++ b/src/modules/uavcan/sensors/sensor_bridge.cpp
@@ -0,0 +1,158 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include "sensor_bridge.hpp"
+#include <cassert>
+
+#include "gnss.hpp"
+#include "mag.hpp"
+#include "baro.hpp"
+
+/*
+ * IUavcanSensorBridge
+ */
+void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list)
+{
+	list.add(new UavcanBarometerBridge(node));
+	list.add(new UavcanMagnetometerBridge(node));
+	list.add(new UavcanGnssBridge(node));
+}
+
+/*
+ * UavcanCDevSensorBridgeBase
+ */
+UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase()
+{
+	for (unsigned i = 0; i < _max_channels; i++) {
+		if (_channels[i].node_id >= 0) {
+			(void)unregister_class_devname(_class_devname, _channels[i].class_instance);
+		}
+	}
+	delete [] _orb_topics;
+	delete [] _channels;
+}
+
+void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
+{
+	assert(report != nullptr);
+
+	Channel *channel = nullptr;
+
+	// Checking if such channel already exists
+	for (unsigned i = 0; i < _max_channels; i++) {
+		if (_channels[i].node_id == node_id) {
+			channel = _channels + i;
+			break;
+		}
+	}
+
+	// No such channel - try to create one
+	if (channel == nullptr) {
+		if (_out_of_channels) {
+			return;           // Give up immediately - saves some CPU time
+		}
+
+		log("adding channel %d...", node_id);
+
+		// Search for the first free channel
+		for (unsigned i = 0; i < _max_channels; i++) {
+			if (_channels[i].node_id < 0) {
+				channel = _channels + i;
+				break;
+			}
+		}
+
+		// No free channels left
+		if (channel == nullptr) {
+			_out_of_channels = true;
+			log("out of channels");
+			return;
+		}
+
+		// Ask the CDev helper which class instance we can take
+		const int class_instance = register_class_devname(_class_devname);
+		if (class_instance < 0 || class_instance >= int(_max_channels)) {
+			_out_of_channels = true;
+			log("out of class instances");
+			(void)unregister_class_devname(_class_devname, class_instance);
+			return;
+		}
+
+		// Publish to the appropriate topic, abort on failure
+		channel->orb_id         = _orb_topics[class_instance];
+		channel->node_id        = node_id;
+		channel->class_instance = class_instance;
+
+		channel->orb_advert = orb_advertise(channel->orb_id, report);
+		if (channel->orb_advert < 0) {
+			log("ADVERTISE FAILED");
+			(void)unregister_class_devname(_class_devname, class_instance);
+			*channel = Channel();
+			return;
+		}
+
+		log("channel %d class instance %d ok", channel->node_id, channel->class_instance);
+	}
+	assert(channel != nullptr);
+
+	(void)orb_publish(channel->orb_id, channel->orb_advert, report);
+}
+
+unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const
+{
+	unsigned out = 0;
+	for (unsigned i = 0; i < _max_channels; i++) {
+		if (_channels[i].node_id >= 0) {
+			out += 1;
+		}
+	}
+	return out;
+}
+
+void UavcanCDevSensorBridgeBase::print_status() const
+{
+	printf("devname: %s\n", _class_devname);
+
+	for (unsigned i = 0; i < _max_channels; i++) {
+		if (_channels[i].node_id >= 0) {
+			printf("channel %d: node id %d --> class instance %d\n",
+			       i, _channels[i].node_id, _channels[i].class_instance);
+		} else {
+			printf("channel %d: empty\n", i);
+		}
+	}
+}
diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp
new file mode 100644
index 000000000..1316f7ecc
--- /dev/null
+++ b/src/modules/uavcan/sensors/sensor_bridge.hpp
@@ -0,0 +1,131 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <containers/List.hpp>
+#include <uavcan/uavcan.hpp>
+#include <drivers/device/device.h>
+#include <drivers/drv_orb_dev.h>
+
+/**
+ * A sensor bridge class must implement this interface.
+ */
+class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode<IUavcanSensorBridge*>
+{
+public:
+	static constexpr unsigned MAX_NAME_LEN = 20;
+
+	virtual ~IUavcanSensorBridge() { }
+
+	/**
+	 * Returns ASCII name of the bridge.
+	 */
+	virtual const char *get_name() const = 0;
+
+	/**
+	 * Starts the bridge.
+	 * @return Non-negative value on success, negative on error.
+	 */
+	virtual int init() = 0;
+
+	/**
+	 * Returns number of active redundancy channels.
+	 */
+	virtual unsigned get_num_redundant_channels() const = 0;
+
+	/**
+	 * Prints current status in a human readable format to stdout.
+	 */
+	virtual void print_status() const = 0;
+
+	/**
+	 * Sensor bridge factory.
+	 * Creates a bridge object by its ASCII name, e.g. "gnss", "mag".
+	 * @return nullptr if such bridge can't be created.
+	 */
+	static void make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list);
+};
+
+/**
+ * This is the base class for redundant sensors with an independent ORB topic per each redundancy channel.
+ * For example, sensor_mag0, sensor_mag1, etc.
+ */
+class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CDev
+{
+	struct Channel
+	{
+		int node_id              = -1;
+		orb_id_t orb_id          = nullptr;
+		orb_advert_t orb_advert  = -1;
+		int class_instance       = -1;
+	};
+
+	const unsigned _max_channels;
+	const char *const _class_devname;
+	orb_id_t *const _orb_topics;
+	Channel *const _channels;
+	bool _out_of_channels = false;
+
+protected:
+	template <unsigned MaxChannels>
+	UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname,
+	                           const orb_id_t (&orb_topics)[MaxChannels]) :
+	device::CDev(name, devname),
+	_max_channels(MaxChannels),
+	_class_devname(class_devname),
+	_orb_topics(new orb_id_t[MaxChannels]),
+	_channels(new Channel[MaxChannels])
+	{
+		memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels);
+	}
+
+	/**
+	 * Sends one measurement into appropriate ORB topic.
+	 * New redundancy channels will be registered automatically.
+	 * @param node_id Sensor's Node ID
+	 * @param report  Pointer to ORB message object
+	 */
+	void publish(const int node_id, const void *report);
+
+public:
+	virtual ~UavcanCDevSensorBridgeBase();
+
+	unsigned get_num_redundant_channels() const override;
+
+	void print_status() const override;
+};
diff --git a/src/modules/uavcan/uavcan_clock.cpp b/src/modules/uavcan/uavcan_clock.cpp
index e41d5f953..fe8ba406a 100644
--- a/src/modules/uavcan/uavcan_clock.cpp
+++ b/src/modules/uavcan/uavcan_clock.cpp
@@ -62,6 +62,8 @@ void adjustUtc(uavcan::UtcDuration adjustment)
 	(void)adjustment;
 }
 
+uavcan::uint64_t getUtcUSecFromCanInterrupt();
+
 uavcan::uint64_t getUtcUSecFromCanInterrupt()
 {
 	return 0;
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 4535b6d6a..a8485ee44 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -38,8 +38,10 @@
 #include <fcntl.h>
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
 #include <systemlib/mixer/mixer.h>
 #include <systemlib/board_serial.h>
+#include <systemlib/scheduling_priorities.h>
 #include <version/version.h>
 #include <arch/board/board.h>
 #include <arch/chip/chip.h>
@@ -65,16 +67,18 @@ UavcanNode *UavcanNode::_instance;
 UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
 	CDev("uavcan", UAVCAN_DEVICE_PATH),
 	_node(can_driver, system_clock),
-	_esc_controller(_node),
-	_gnss_receiver(_node)
+	_node_mutex(),
+	_esc_controller(_node)
 {
 	_control_topics[0] = ORB_ID(actuator_controls_0);
 	_control_topics[1] = ORB_ID(actuator_controls_1);
 	_control_topics[2] = ORB_ID(actuator_controls_2);
 	_control_topics[3] = ORB_ID(actuator_controls_3);
 
-	// memset(_controls, 0, sizeof(_controls));
-	// memset(_poll_fds, 0, sizeof(_poll_fds));
+	const int res = pthread_mutex_init(&_node_mutex, nullptr);
+	if (res < 0) {
+		std::abort();
+	}
 }
 
 UavcanNode::~UavcanNode()
@@ -99,10 +103,18 @@ UavcanNode::~UavcanNode()
 	}
 
 	/* clean up the alternate device node */
-		// unregister_driver(PWM_OUTPUT_DEVICE_PATH);
+	// unregister_driver(PWM_OUTPUT_DEVICE_PATH);
 
 	::close(_armed_sub);
 
+	// Removing the sensor bridges
+	auto br = _sensor_bridges.getHead();
+	while (br != nullptr) {
+		auto next = br->getSibling();
+		delete br;
+		br = next;
+	}
+
 	_instance = nullptr;
 }
 
@@ -164,7 +176,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
 	 * Start the task. Normally it should never exit.
 	 */
 	static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();};
-	_instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize,
+	_instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize,
 			      static_cast<main_t>(run_trampoline), nullptr);
 
 	if (_instance->_task < 0) {
@@ -214,11 +226,12 @@ int UavcanNode::init(uavcan::NodeID node_id)
 {
 	int ret = -1;
 
-	/* do regular cdev init */
+	// Do regular cdev init
 	ret = CDev::init();
 
-	if (ret != OK)
+	if (ret != OK) {
 		return ret;
+	}
 
 	_node.setName("org.pixhawk.pixhawk");
 
@@ -226,14 +239,24 @@ int UavcanNode::init(uavcan::NodeID node_id)
 
 	fill_node_info();
 
-	/* initializing the bridges UAVCAN <--> uORB */
+	// Actuators
 	ret = _esc_controller.init();
-	if (ret < 0)
+	if (ret < 0) {
 		return ret;
+	}
 
-	ret = _gnss_receiver.init();
-	if (ret < 0)
-		return ret;
+	// Sensor bridges
+	IUavcanSensorBridge::make_all(_node, _sensor_bridges);
+	auto br = _sensor_bridges.getHead();
+	while (br != nullptr) {
+		ret = br->init();
+		if (ret < 0) {
+			warnx("cannot init sensor bridge '%s' (%d)", br->get_name(), ret);
+			return ret;
+		}
+		warnx("sensor bridge '%s' init ok", br->get_name());
+		br = br->getSibling();
+	}
 
 	return _node.start();
 }
@@ -248,6 +271,8 @@ void UavcanNode::node_spin_once()
 
 int UavcanNode::run()
 {
+	(void)pthread_mutex_lock(&_node_mutex);
+
 	const unsigned PollTimeoutMs = 50;
 
 	// XXX figure out the output count
@@ -291,8 +316,13 @@ int UavcanNode::run()
 			_groups_subscribed = _groups_required;
 		}
 
+		// Mutex is unlocked while the thread is blocked on IO multiplexing
+		(void)pthread_mutex_unlock(&_node_mutex);
+
 		const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
 
+		(void)pthread_mutex_lock(&_node_mutex);
+
 		node_spin_once();  // Non-blocking
 
 		// this would be bad...
@@ -352,7 +382,6 @@ int UavcanNode::run()
 				// Output to the bus
 				_esc_controller.update_outputs(outputs.output, outputs.noutputs);
 			}
-
 		}
 
 		// Check arming state
@@ -376,10 +405,7 @@ int UavcanNode::run()
 }
 
 int
-UavcanNode::control_callback(uintptr_t handle,
-			 uint8_t control_group,
-			 uint8_t control_index,
-			 float &input)
+UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input)
 {
 	const actuator_controls_s *controls = (actuator_controls_s *)handle;
 
@@ -520,8 +546,23 @@ UavcanNode::print_info()
 		warnx("not running, start first");
 	}
 
-	warnx("actuators control groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
-	warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK");
+	(void)pthread_mutex_lock(&_node_mutex);
+
+	// ESC mixer status
+	printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n",
+	       (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
+	printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK");
+
+	// Sensor bridges
+	auto br = _sensor_bridges.getHead();
+	while (br != nullptr) {
+		printf("Sensor '%s':\n", br->get_name());
+		br->print_status();
+		printf("\n");
+		br = br->getSibling();
+	}
+
+	(void)pthread_mutex_unlock(&_node_mutex);
 }
 
 /*
@@ -529,79 +570,57 @@ UavcanNode::print_info()
  */
 static void print_usage()
 {
-	warnx("usage: uavcan start <node_id> [can_bitrate]");
+	warnx("usage: \n"
+	      "\tuavcan {start|status|stop}");
 }
 
 extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
 
 int uavcan_main(int argc, char *argv[])
 {
-	constexpr unsigned DEFAULT_CAN_BITRATE = 1000000;
-
 	if (argc < 2) {
 		print_usage();
 		::exit(1);
 	}
 
 	if (!std::strcmp(argv[1], "start")) {
-		if (argc < 3) {
-			print_usage();
-			::exit(1);
+		if (UavcanNode::instance()) {
+			errx(1, "already started");
 		}
 
-		/*
-		 * Node ID
-		 */
-		const int node_id = atoi(argv[2]);
+		// Node ID
+		int32_t node_id = 0;
+		(void)param_get(param_find("UAVCAN_NODE_ID"), &node_id);
 
 		if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) {
 			warnx("Invalid Node ID %i", node_id);
 			::exit(1);
 		}
 
-		/*
-		 * CAN bitrate
-		 */
-		unsigned bitrate = 0;
-
-		if (argc > 3) {
-			bitrate = atol(argv[3]);
-		}
-
-		if (bitrate <= 0) {
-			bitrate = DEFAULT_CAN_BITRATE;
-		}
+		// CAN bitrate
+		int32_t bitrate = 0;
+		(void)param_get(param_find("UAVCAN_BITRATE"), &bitrate);
 
-		if (UavcanNode::instance()) {
-			errx(1, "already started");
-		}
-
-		/*
-		 * Start
-		 */
+		// Start
 		warnx("Node ID %u, bitrate %u", node_id, bitrate);
 		return UavcanNode::start(node_id, bitrate);
-
 	}
 
 	/* commands below require the app to be started */
-	UavcanNode *inst = UavcanNode::instance();
+	UavcanNode *const inst = UavcanNode::instance();
 
 	if (!inst) {
 		errx(1, "application not running");
 	}
 
 	if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) {
-		
-			inst->print_info();
-			return OK;
+		inst->print_info();
+		::exit(0);
 	}
 
 	if (!std::strcmp(argv[1], "stop")) {
-		
-			delete inst;
-			inst = nullptr;
-			return OK;
+		delete inst;
+		::exit(0);
 	}
 
 	print_usage();
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
index 05b66fd7b..be7db9741 100644
--- a/src/modules/uavcan/uavcan_main.hpp
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -42,8 +42,8 @@
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 
-#include "esc_controller.hpp"
-#include "gnss_receiver.hpp"
+#include "actuators/esc.hpp"
+#include "sensors/sensor_bridge.hpp"
 
 /**
  * @file uavcan_main.hpp
@@ -77,12 +77,10 @@ public:
 
 	static int	start(uavcan::NodeID node_id, uint32_t bitrate);
 
-	Node&		getNode() { return _node; }
+	Node&		get_node() { return _node; }
 
-	static int	control_callback(uintptr_t handle,
-					 uint8_t control_group,
-					 uint8_t control_index,
-					 float &input);
+	// TODO: move the actuator mixing stuff into the ESC controller class
+	static int	control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input);
 
 	void		subscribe();
 
@@ -109,8 +107,11 @@ private:
 
 	static UavcanNode	*_instance;			///< singleton pointer
 	Node			_node;				///< library instance
+	pthread_mutex_t		_node_mutex;
+
 	UavcanEscController	_esc_controller;
-	UavcanGnssReceiver 	_gnss_receiver;
+
+	List<IUavcanSensorBridge*> _sensor_bridges;		///< List of active sensor bridges
 
 	MixerGroup		*_mixers = nullptr;
 
diff --git a/src/modules/uavcan/uavcan_params.c b/src/modules/uavcan/uavcan_params.c
new file mode 100644
index 000000000..e6ea8a8fb
--- /dev/null
+++ b/src/modules/uavcan/uavcan_params.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.
+ *
+ ****************************************************************************/
+
+/**
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <nuttx/config.h>
+#include <systemlib/param/param.h>
+
+/**
+ * Enable UAVCAN.
+ *
+ * Enables support for UAVCAN-interfaced actuators and sensors.
+ *
+ * @min 0
+ * @max 1
+ * @group UAVCAN
+ */
+PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0);
+
+/**
+ * UAVCAN Node ID.
+ *
+ * Read the specs at http://uavcan.org to learn more about Node ID.
+ *
+ * @min 1
+ * @max 125
+ * @group UAVCAN
+ */
+PARAM_DEFINE_INT32(UAVCAN_NODE_ID, 1);
+
+/**
+ * UAVCAN CAN bus bitrate.
+ *
+ * @min 20000
+ * @max 1000000
+ * @group UAVCAN
+ */
+PARAM_DEFINE_INT32(UAVCAN_BITRATE, 1000000);
+
+
+
diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp
index 02d1af481..be3b9461a 100644
--- a/src/modules/unit_test/unit_test.cpp
+++ b/src/modules/unit_test/unit_test.cpp
@@ -56,3 +56,8 @@ void UnitTest::printAssert(const char* msg, const char* test, const char* file,
 {
     warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line);
 }
+
+void UnitTest::printCompare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line)
+{
+	warnx("Compare failed: %s - (%s:%d) (%s:%d) (%s:%d)", msg, v1_text, v1, v2_text, v2, file, line);
+}
diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h
index 32eb8c308..1a5489709 100644
--- a/src/modules/unit_test/unit_test.h
+++ b/src/modules/unit_test/unit_test.h
@@ -52,11 +52,15 @@ INLINE_GLOBAL(const char*, mu_last_test)
 
 	UnitTest();
     virtual ~UnitTest();
+	
+	virtual void init(void) { };
+	virtual void cleanup(void) { };
 
     virtual void runTests(void) = 0;
     void printResults(void);
     
     void printAssert(const char* msg, const char* test, const char* file, int line);
+	void printCompare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line);
     
 #define ut_assert(message, test)                                \
     do {                                                        \
@@ -68,10 +72,23 @@ INLINE_GLOBAL(const char*, mu_last_test)
         }                                                       \
     } while (0)
     
+#define ut_compare(message, v1, v2)							\
+	do {										\
+		int _v1 = v1;								\
+		int _v2 = v2;								\
+		if (_v1 != _v2) {							\
+			printCompare(message, #v1, _v1, #v2, _v2, __FILE__, __LINE__);	\
+			return false;							\
+		} else {								\
+			mu_assertion()++;						\
+		}									\
+	} while (0)
+    
 #define ut_run_test(test)                       \
     do {                                        \
         warnx("RUNNING TEST: %s", #test);       \
         mu_tests_run()++;                       \
+	init();					\
         if (!test()) {                          \
             warnx("TEST FAILED: %s", #test);    \
             mu_tests_failed()++;                \
@@ -79,6 +96,7 @@ INLINE_GLOBAL(const char*, mu_last_test)
             warnx("TEST PASSED: %s", #test);    \
             mu_tests_passed()++;                \
         }                                       \
+	cleanup();				\
     } while (0)
 
 };
diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index c8d698b86..478c2a772 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -654,9 +654,28 @@ pwm_main(int argc, char *argv[])
 			}
 		}
 		exit(0);
+	} else if (!strcmp(argv[1], "terminatefail")) {
+
+		if (argc < 3) {
+			errx(1, "arg missing [on|off]");
+		} else {
+
+			if (!strcmp(argv[2], "on")) {
+				/* force failsafe */
+				ret = ioctl(fd, PWM_SERVO_SET_TERMINATION_FAILSAFE, 1);
+			} else {
+				/* force failsafe */
+				ret = ioctl(fd, PWM_SERVO_SET_TERMINATION_FAILSAFE, 0);
+			}
+
+			if (ret != OK) {
+				warnx("FAILED setting termination failsafe %s", argv[2]);
+			}
+		}
+		exit(0);
 	}
 
-	usage("specify arm|disarm|rate|failsafe|disarmed|min|max|test|info|forcefail");
+	usage("specify arm|disarm|rate|failsafe\n\t\tdisarmed|min|max|test|info|forcefail|terminatefail");
 	return 0;
 }
 
diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c
index a4f17eebd..e005bf9c1 100644
--- a/src/systemcmds/tests/test_sensors.c
+++ b/src/systemcmds/tests/test_sensors.c
@@ -331,7 +331,7 @@ mag(int argc, char *argv[])
 
 	float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z);
 
-	if (len < 1.0f || len > 3.0f) {
+	if (len < 0.25f || len > 3.0f) {
 		warnx("MAG scale error!");
 		return ERROR;
 	}
diff --git a/uavcan b/uavcan
-Subproject 6980ee824074aa2f7a62221bf6040ee41111952
+Subproject 286adbcc56c4b093143b647ec7546abb149cd53
author	Anton Babushkin <anton.babushkin@me.com>	2014-09-28 13:41:21 +0400
committer	Anton Babushkin <anton.babushkin@me.com>	2014-09-28 13:41:21 +0400
commit	1cc5f3778dd6c18f3a14c64959d28f978b0305ef (patch)
tree	f2b8138e6f85b5f760fa22bbe3aecbce61539277
parent	cf4604f5c3614bc5cd1b82dfee693cbac36181ab (diff)
parent	8a18cfa3869555389e7e9ff8f104d83f9c54cb43 (diff)
download	px4-firmware-1cc5f3778dd6c18f3a14c64959d28f978b0305ef.tar.gz px4-firmware-1cc5f3778dd6c18f3a14c64959d28f978b0305ef.tar.bz2 px4-firmware-1cc5f3778dd6c18f3a14c64959d28f978b0305ef.zip