Merge remote-tracking branch 'upstream/master' into dev_ros

Conflicts: .gitignore src/lib/uavcan
2014-12-25 09:48:15 +01:00 · 2014-12-25 09:48:15 +01:00 · 25af4b266c
parent ad189cf7d6 9b535f6553
commit 25af4b266c
65 changed files with 21014 additions and 289 deletions
--- a/.gitignore
+++ b/.gitignore
@ -40,3 +40,4 @@ tags
 .ropeproject
 *.orig
 src/modules/uORB/topics/*
+Firmware.zip
--- a/.gitmodules
+++ b/.gitmodules
@ -5,7 +5,7 @@
 	path = NuttX
 	url = git://github.com/PX4/NuttX.git
 [submodule "uavcan"]
-	path = uavcan
+	path = src/lib/uavcan
 	url = git://github.com/pavel-kirienko/uavcan.git
 [submodule "Tools/genmsg"]
 	path = Tools/genmsg
@ -13,3 +13,6 @@
 [submodule "Tools/gencpp"]
 	path = Tools/gencpp
 	url = https://github.com/ros/gencpp.git
+[submodule "src/lib/uavcan"]
+	path = src/lib/uavcan
+	url = git://github.com/pavel-kirienko/uavcan.git
--- a/.travis.yml
+++ b/.travis.yml
@ -0,0 +1,95 @@
+# Build and autotest script for PX4 Firmware
+# http://travis-ci.org
+
+language: cpp
+
+before_script:
+  - sudo apt-get update -q
+# Travis specific tools
+  - sudo apt-get install s3cmd grep zip mailutils
+# General toolchain dependencies
+  - sudo apt-get install libc6:i386 libgcc1:i386 gcc-4.6-base:i386 libstdc++5:i386 libstdc++6:i386
+  - sudo apt-get install python-serial python-argparse
+  - sudo apt-get install flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget
+  - pushd .
+  - cd ~
+  - wget https://launchpadlibrarian.net/174121628/gcc-arm-none-eabi-4_7-2014q2-20140408-linux.tar.bz2
+  - tar -jxf gcc-arm-none-eabi-4_7-2014q2-20140408-linux.tar.bz2
+  - exportline="export PATH=$HOME/gcc-arm-none-eabi-4_7-2014q2/bin:\$PATH"
+  - if grep -Fxq "$exportline" ~/.profile; then echo nothing to do ; else echo $exportline >> ~/.profile; fi
+  - . ~/.profile
+  - popd
+
+git:
+  depth: 500
+
+env:
+  global:
+# AWS KEY: $PX4_AWS_KEY
+    - secure: "XknnZHWBbpHbN4f3fuAVwUztdLIu8ej4keC3aQSDofo3uw8AFEzojfsQsN9u77ShWSIV4iYJWh9C9ALkCx7TocJ+xYjiboo10YhM9lH/8u+EXjYWG6GHS8ua0wkir+cViSxoLNaMtmcb/rPTicJecAGANxLsIHyBAgTL3fkbLSA="
+# AWS SECRET: $PX4_AWS_SECRET
+    - secure: "h6oajlW68dWIr+wZhO58Dv6e68dZHrBLVA6lPXZmheFQBW6Xam1HuLGA0LOW6cL9TnrAsOZ8g4goB58eMQnMEijFZKi3mhRwZhd/Xjq/ZGJOWBUrLoQHZUw2dQk5ja5vmUlKEoQnFZjDuMjx8KfX5ZMNy8A3yssWZtJYHD8c+bk="
+    - PX4_AWS_BUCKET=px4-travis
+    - PX4_EMAIL_SUBJECT="Travis CI result"
+# Email address: $PX4_EMAIL
+    - secure: "ei3hKAw6Pk+vEkQBI5Y2Ak74BRAaXcK2UHVnVadviBHI4EVPwn1YGP6A4Y0wnLe4U7ETTl0UiijRoVxyDW0Mq896Pv0siw02amNpjSZZYu+RfN1+//MChB48OxsLDirUdHVrULhl/bOARM02h2Bg28jDE2g7IqmJwg3em3oMbjU="
+    - PX4_REPORT=report.txt
+
+script:
+  - arm-none-eabi-gcc --version
+  - echo 'Running Tests..' && echo -en 'travis_fold:start:script.1\\r'
+  - make tests
+  - echo -en 'travis_fold:end:script.1\\r'
+  - echo 'Building NuttX..' && echo -en 'travis_fold:start:script.2\\r'
+  - make archives
+  - echo -en 'travis_fold:end:script.2\\r'
+  - echo 'Building Firmware..' && echo -en 'travis_fold:start:script.3\\r'
+  - make -j6
+  - echo -en 'travis_fold:end:script.3\\r'
+  - zip Firmware.zip Images/*.px4
+
+# We use an encrypted env variable to ensure this only executes when artifacts are uploaded.
+after_script:
+  - echo "Branch $TRAVIS_BRANCH (pull request: $TRAVIS_PULL_REQUEST) ready for flight testing." >> $PX4_REPORT
+  - git log -n1 > $PX4_REPORT
+  - echo " " >> $PX4_REPORT
+  - echo "Files available at:" >> $PX4_REPORT
+  - echo "https://px4-travis.s3.amazonaws.com/PX4/Firmware/$TRAVIS_BUILD_NUMBER/$TRAVIS_BUILD_NUMBER.1/Firmware.zip" >> $PX4_REPORT
+  - echo "Description of desired tests is available at:" >> $PX4_REPORT
+  - echo "https://github.com/PX4/Firmware/pull/$TRAVIS_PULL_REQUEST" >> $PX4_REPORT
+  - echo " " >> $PX4_REPORT
+  - echo "Thanks for testing!" >> $PX4_REPORT
+  - echo " " >> $PX4_REPORT
+  - /usr/bin/mail -s "$SUBJECT ($TRAVIS_COMMIT)" "$PX4_EMAIL" < "$PX4_REPORT"
+  #- s3cmd put --acl-public --guess-mime-type --config=.s3cfg Firmware.zip s3://s3-website-us-east-1.amazonaws.com/#$TRAVIS_JOB_ID/
+
+deploy:
+  provider: releases
+  api_key:
+    secure: cdHWLRBxA5UlYpOS0Sp891QK7PFmMgQ5ZWs1aPt+sw0rIrowyWMHCwXNBEdUqaExHYNYgXCUDI0EzNgfB7ZcR63Qv1MQeoyamV4jsxlyAqDqmxNtWO82S6RhHGeMLk26VgFKzynVcEk1IYlQP2nqzMQLdu+jTrngERuAIrCdRuc=
+  file: "Firmware.zip"
+  skip_cleanup: true
+  on:
+    tags: true
+    all_branches: true
+    repo: PX4/Firmware
+
+addons:
+  artifacts:
+    paths:
+    - "Firmware.zip"
+    key:
+      secure: j4y9x9KXUiarGrnpFBLPIkEKIH8X6oSRUO61TwxTOamsE0eEKnIaCz1Xq83q7DoqzomHBD3qXAFPV9dhLr1zdKEPJDIyV45GVD4ClIQIzh/P3Uc7kDNxKzdmxY12SH6D0orMpC4tCf1sNK7ETepltWfcnjaDk1Rjs9+TVY7LuzM=
+    secret:
+      secure: CJC7VPGtEhJu8Pix85iPF8xUvMPZvTgnHyd9MrSlPKCFFMrlgz9eMT0WWW/TPQ+s4LPwJIfEQx2Q0BRT5tOXuvsTLuOG68mplVddhTWbHb0m0qTQErXFHEppvW4ayuSdeLJ4TjTWphBVainL0mcLLRwQfuAJJDDs/sGan3WrG+Y=
+    bucket: px4-travis
+    region: us-east-1
+    endpoint: s3-website-us-east-1.amazonaws.com
+
+notifications:
+  webhooks:
+    urls:
+      - https://webhooks.gitter.im/e/2b9c4a4cb2211f8befba
+    on_success: always  # options: [always|never|change] default: always
+    on_failure: always  # options: [always|never|change] default: always
+    on_start: false     # default: false
--- a/18
+++ b/18
@ -245,6 +245,14 @@ generateuorbtopicheaders:
 #
 testbuild:
 	$(Q) (cd $(PX4_BASE) && $(MAKE) distclean && $(MAKE) archives && $(MAKE) -j8)
+	$(Q) (zip -r Firmware.zip $(PX4_BASE)/Images)
+
+#
+# Unittest targets. Builds and runs the host-level
+# unit tests.
+.PHONY: tests
+tests:
+	$(Q) (cd $(PX4_BASE)/unittests && $(MAKE) unittests)

 #
 # Cleanup targets.  'clean' should remove all built products and force
@ -253,14 +261,14 @@ testbuild:
 #
 .PHONY:	clean
 clean:
-	$(Q) $(RMDIR) $(BUILD_DIR)*.build
-	$(Q) $(REMOVE) $(IMAGE_DIR)*.px4
+	$(Q) $(RMDIR) $(BUILD_DIR)*.build > /dev/null
+	$(Q) $(REMOVE) $(IMAGE_DIR)*.px4 > /dev/null

 .PHONY:	distclean
 distclean: clean
-	$(Q) $(REMOVE) $(ARCHIVE_DIR)*.export
-	$(Q) $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean
-	$(Q) (cd $(NUTTX_SRC)/configs && $(FIND) . -maxdepth 1 -type l -delete)
+	$(Q) $(REMOVE) $(ARCHIVE_DIR)*.export > /dev/null
+	$(Q) $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean > /dev/null
+	$(Q) (cd $(NUTTX_SRC)/configs && $(FIND) . -maxdepth 1 -type l -delete) > /dev/null

 #
 # Print some help text
--- a/2
+++ b/2
@ -1 +1 @@
-Subproject commit c7b06385926349e10b3739314d1d56eec7efb8be
+Subproject commit b66de6506941dc7628efcf65e5543c0e3cb05a8f
--- a/README.md
+++ b/README.md
@ -1,10 +1,33 @@
-## PX4 Aerial Middleware and Flight Control Stack ##
+## PX4 Flight Control Stack and Middleware ##
+
+[![Build Status](https://travis-ci.org/PX4/Firmware.svg?branch=master)](https://travis-ci.org/PX4/Firmware)
+
+[![Gitter](https://badges.gitter.im/Join%20Chat.svg)](https://gitter.im/PX4/Firmware?utm_source=badge&utm_medium=badge&utm_campaign=pr-badge&utm_content=badge)

 *   Official Website: http://px4.io
 *   License: BSD 3-clause (see LICENSE.md)
 *   Supported airframes:
-  * Multicopters
-  * Fixed wing
+  * [Multicopters](http://px4.io/platforms/multicopters/start)
+  * [Fixed wing](http://px4.io/platforms/planes/start)
+  * [VTOL](http://px4.io/platforms/vtol/start)
 *   Binaries (always up-to-date from master):
  * [Downloads](https://pixhawk.org/downloads)
 *   Mailing list: [Google Groups](http://groups.google.com/group/px4users)
+
+### Developers ###
+
+Contributing guide:
+http://px4.io/dev/contributing
+
+Developer guide:
+http://px4.io/dev/
+
+This repository contains code supporting these boards:
+  * PX4FMUv1.x
+  * PX4FMUv2.x
+  * AeroCore
+
+## NuttShell (NSH) ##
+
+NSH usage documentation:
+http://px4.io/users/serial_connection
--- a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol
+++ b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol
@ -9,8 +9,8 @@ sh /etc/init.d/rc.vtol_defaults

 set MIXER FMU_caipirinha_vtol

-set PWM_OUTPUTS 12
+set PWM_OUT 12
 set PWM_MAX 2000
 set PWM_RATE 400
-param set VTOL_MOT_COUNT 2
-param set IDLE_PWM_MC  1080
+param set VT_MOT_COUNT 2
+param set VT_IDLE_PWM_MC  1080
--- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults
+++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults
@ -2,7 +2,7 @@

 set VEHICLE_TYPE vtol

-if [ $DO_AUTOCONFIG == yes ]
+if [ $AUTOCNF == yes ]
 then
 	#
 	#Default controller parameters for MC
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@ -584,7 +584,7 @@ then
 		sh /etc/init.d/rc.interface

 		# Start standard vtol apps
-		if [ $LOAD_DEFAULT_APPS == yes ]
+		if [ $LOAD_DAPPS == yes ]
 		then
 			sh /etc/init.d/rc.vtol_apps
 		fi
--- a/Tools/px4params/srcparser.py
+++ b/Tools/px4params/srcparser.py
@ -103,7 +103,7 @@ class SourceParser(object):
        Returns list of supported file extensions that can be parsed by this
        parser.
        """
-        return ["cpp", "c"]
+        return [".cpp", ".c"]

    def Parse(self, contents):
        """
--- a/Tools/px4params/srcscanner.py
+++ b/Tools/px4params/srcscanner.py
@ -26,5 +26,10 @@ class SourceScanner(object):
        parser.Parse method.
        """
        with codecs.open(path, 'r', 'utf-8') as f:
-            contents = f.read()
+            try:
+                contents = f.read()
+            except:
+                contents = ''
+                print('Failed reading file: %s, skipping content.' % path)
+                pass
        parser.Parse(contents)
--- a/Tools/tests-host/Makefile
+++ b/Tools/tests-host/Makefile
@ -1,54 +0,0 @@
-
-CC=g++
-CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers \
-	-I../../src -I../../src/lib -D__EXPORT="" -Dnullptr="0" -lm
-
-all: mixer_test sbus2_test autodeclination_test st24_test sf0x_test
-
-MIXER_FILES=../../src/systemcmds/tests/test_mixer.cpp \
-		../../src/systemcmds/tests/test_conv.cpp \
-		../../src/modules/systemlib/mixer/mixer_simple.cpp \
-		../../src/modules/systemlib/mixer/mixer_multirotor.cpp \
-		../../src/modules/systemlib/mixer/mixer.cpp \
-		../../src/modules/systemlib/mixer/mixer_group.cpp \
-		../../src/modules/systemlib/mixer/mixer_load.c \
-		../../src/modules/systemlib/pwm_limit/pwm_limit.c \
-		hrt.cpp \
-		mixer_test.cpp
-
-SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \
-		hrt.cpp \
-		sbus2_test.cpp
-
-ST24_FILES=../../src/lib/rc/st24.c \
-		hrt.cpp \
-		st24_test.cpp
-
-SF0X_FILES= \
-		hrt.cpp \
-		sf0x_test.cpp \
-		../../src/drivers/sf0x/sf0x_parser.cpp
-
-AUTODECLINATION_FILES= ../../src/lib/geo_lookup/geo_mag_declination.c \
-		hrt.cpp \
-		autodeclination_test.cpp
-
-mixer_test: $(MIXER_FILES)
-	$(CC) -o mixer_test $(MIXER_FILES) $(CFLAGS)
-
-sbus2_test: $(SBUS2_FILES)
-	$(CC) -o sbus2_test $(SBUS2_FILES) $(CFLAGS)
-
-sf0x_test: $(SF0X_FILES)
-	$(CC) -o sf0x_test $(SF0X_FILES) $(CFLAGS)
-
-autodeclination_test: $(SBUS2_FILES)
-	$(CC) -o autodeclination_test $(AUTODECLINATION_FILES) $(CFLAGS)
-
-st24_test: $(ST24_FILES)
-	$(CC) -o st24_test $(ST24_FILES) $(CFLAGS)
-
-.PHONY: clean
-
-clean:
-	rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test st24_test sf0x_test
--- a/Tools/tests-host/autodeclination_test.cpp
+++ b/Tools/tests-host/autodeclination_test.cpp
@ -1,28 +0,0 @@
-
-#include <stdio.h>
-#include <stdlib.h> 
-#include <unistd.h>
-#include <string.h>
-#include <systemlib/mixer/mixer.h>
-#include <systemlib/err.h>
-#include <drivers/drv_hrt.h>
-#include <px4iofirmware/px4io.h>
-#include "../../src/systemcmds/tests/tests.h"
-#include <geo/geo.h>
-
-int main(int argc, char *argv[]) {
-	warnx("autodeclination test started");
-
-	if (argc < 3)
-		errx(1, "Need lat/lon!");
-
-	char* p_end;
-
-	float lat = strtod(argv[1], &p_end);
-	float lon = strtod(argv[2], &p_end);
-
-	float declination = get_mag_declination(lat, lon);
-
-	printf("lat: %f lon: %f, dec: %f\n", lat, lon, declination);
-
-}
--- a/makefiles/gumstix-aerocore.cfg
+++ b/makefiles/gumstix-aerocore.cfg
@ -0,0 +1,10 @@
+# JTAG for the STM32F4x chip used on the Gumstix AeroCore is available on
+# the first interface of a Quad FTDI chip.  nTRST is bit 4.
+interface ftdi
+ftdi_vid_pid 0x0403 0x6011
+
+ftdi_layout_init 0x0000 0x001b
+ftdi_layout_signal nTRST -data 0x0010
+
+source [find target/stm32f4x.cfg]
+reset_config trst_only
--- a/makefiles/upload.mk
+++ b/makefiles/upload.mk
@ -30,6 +30,11 @@ upload-serial-px4fmu-v1:	$(BUNDLE) $(UPLOADER)
 upload-serial-px4fmu-v2:	$(BUNDLE) $(UPLOADER)
 	$(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE)

+upload-serial-aerocore:
+	openocd -f $(PX4_BASE)/makefiles/gumstix-aerocore.cfg -c 'init; reset halt; flash write_image erase $(PX4_BASE)/../Bootloader/px4aerocore_bl.bin 0x08000000; flash write_image erase $(PX4_BASE)/Build/aerocore_default.build/firmware.bin 0x08004000; reset run; exit'
+
+
+
 #
 # JTAG firmware uploading with OpenOCD
 #
--- a/nuttx-configs/px4fmu-v1/nsh/Make.defs
+++ b/nuttx-configs/px4fmu-v1/nsh/Make.defs
@ -139,8 +139,7 @@ ARCHCWARNINGS		 = $(ARCHWARNINGS) \
 			   -Wold-style-declaration \
 			   -Wmissing-parameter-type \
 			   -Wmissing-prototypes \
-			   -Wnested-externs \
-			   -Wunsuffixed-float-constants
+			   -Wnested-externs
 ARCHWARNINGSXX		 = $(ARCHWARNINGS) \
 			   -Wno-psabi
 ARCHDEFINES		 =
--- a/nuttx-configs/px4fmu-v2/nsh/Make.defs
+++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs
@ -139,8 +139,7 @@ ARCHCWARNINGS		 = $(ARCHWARNINGS) \
 			   -Wold-style-declaration \
 			   -Wmissing-parameter-type \
 			   -Wmissing-prototypes \
-			   -Wnested-externs \
-			   -Wunsuffixed-float-constants
+			   -Wnested-externs
 ARCHWARNINGSXX		 = $(ARCHWARNINGS) \
 			   -Wno-psabi
 ARCHDEFINES		 =
--- a/nuttx-configs/px4io-v1/nsh/Make.defs
+++ b/nuttx-configs/px4io-v1/nsh/Make.defs
@ -128,8 +128,7 @@ ARCHCWARNINGS		 = $(ARCHWARNINGS) \
 			   -Wold-style-declaration \
 			   -Wmissing-parameter-type \
 			   -Wmissing-prototypes \
-			   -Wnested-externs \
-			   -Wunsuffixed-float-constants
+			   -Wnested-externs
 ARCHWARNINGSXX		 = $(ARCHWARNINGS)
 ARCHDEFINES		 = 
 ARCHPICFLAGS		 = -fpic -msingle-pic-base -mpic-register=r10
--- a/nuttx-configs/px4io-v2/nsh/Make.defs
+++ b/nuttx-configs/px4io-v2/nsh/Make.defs
@ -128,8 +128,7 @@ ARCHCWARNINGS		 = $(ARCHWARNINGS) \
 			   -Wold-style-declaration \
 			   -Wmissing-parameter-type \
 			   -Wmissing-prototypes \
-			   -Wnested-externs \
-			   -Wunsuffixed-float-constants
+			   -Wnested-externs
 ARCHWARNINGSXX		 = $(ARCHWARNINGS)
 ARCHDEFINES		 = 
 ARCHPICFLAGS		 = -fpic -msingle-pic-base -mpic-register=r10
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@ -194,6 +194,8 @@ public:
 	 */
 	void			print_info();

+	void			print_registers();
+
 protected:
 	virtual int		probe();

@ -1414,6 +1416,21 @@ MPU6000::print_info()
 	_gyro_reports->print_info("gyro queue");
 }

+void
+MPU6000::print_registers()
+{
+	printf("MPU6000 registers\n");
+	for (uint8_t reg=MPUREG_PRODUCT_ID; reg<=108; reg++) {
+		uint8_t v = read_reg(reg);
+		printf("%02x:%02x ",(unsigned)reg, (unsigned)v);
+		if ((reg - (MPUREG_PRODUCT_ID-1)) % 13 == 0) {
+			printf("\n");
+		}
+	}
+	printf("\n");
+}
+
+
 MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) :
 	CDev("MPU6000_gyro", path),
 	_parent(parent),
@ -1479,6 +1496,7 @@ void	start(bool, enum Rotation);
 void	test(bool);
 void	reset(bool);
 void	info(bool);
+void	regdump(bool);
 void	usage();

 /**
@ -1654,10 +1672,26 @@ info(bool external_bus)
 	exit(0);
 }

+/**
+ * Dump the register information
+ */
+void
+regdump(bool external_bus)
+{
+	MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int;
+	if (*g_dev_ptr == nullptr)
+		errx(1, "driver not running");
+
+	printf("regdump @ %p\n", *g_dev_ptr);
+	(*g_dev_ptr)->print_registers();
+
+	exit(0);
+}
+
 void
 usage()
 {
-	warnx("missing command: try 'start', 'info', 'test', 'reset'");
+	warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump'");
 	warnx("options:");
 	warnx("    -X    (external bus)");
 	warnx("    -R rotation");
@ -1714,5 +1748,11 @@ mpu6000_main(int argc, char *argv[])
 	if (!strcmp(verb, "info"))
 		mpu6000::info(external_bus);

-	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+	/*
+	 * Print register information.
+	 */
+	if (!strcmp(verb, "regdump"))
+		mpu6000::regdump(external_bus);
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'");
 }
--- a/src/drivers/stm32/drv_hrt.c
+++ b/src/drivers/stm32/drv_hrt.c
@ -253,9 +253,11 @@ static uint16_t			latency_baseline;
 static uint16_t			latency_actual;

 /* latency histogram */
-#define LATENCY_BUCKET_COUNT	8
-static const uint16_t		latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };
-static uint32_t			latency_counters[LATENCY_BUCKET_COUNT + 1];
+#define LATENCY_BUCKET_COUNT 8
+__EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT;
+__EXPORT const uint16_t	latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };
+__EXPORT uint32_t		latency_counters[LATENCY_BUCKET_COUNT + 1];
+

 /* timer-specific functions */
 static void		hrt_tim_init(void);
--- a/src/lib/uavcan
+++ b/src/lib/uavcan
@ -0,0 +1 @@
+Subproject commit 6a193648985adab9665e31a9460be04bc91d8965
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@ -81,6 +81,7 @@
 #include <uORB/topics/system_power.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
+#include <uORB/topics/geofence_result.h>
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/vtol_vehicle_status.h>

@ -926,6 +927,11 @@ int commander_thread_main(int argc, char *argv[])
 	struct mission_result_s mission_result;
 	memset(&mission_result, 0, sizeof(mission_result));

+	/* Subscribe to geofence result topic */
+	int geofence_result_sub = orb_subscribe(ORB_ID(geofence_result));
+	struct geofence_result_s geofence_result;
+	memset(&geofence_result, 0, sizeof(geofence_result));
+
 	/* Subscribe to manual control data */
 	int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	struct manual_control_setpoint_s sp_man;
@ -1545,27 +1551,34 @@ int commander_thread_main(int argc, char *argv[])

 		if (updated) {
 			orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
-
-			/* Check for geofence violation */
-			if (armed.armed && (mission_result.geofence_violated || mission_result.flight_termination)) {
-				//XXX: make this configurable to select different actions (e.g. navigation modes)
-				/* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */
-				armed.force_failsafe = true;
-				status_changed = true;
-				static bool flight_termination_printed = false;
-
-				if (!flight_termination_printed) {
-					warnx("Flight termination because of navigator request or geofence");
-					mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
-					flight_termination_printed = true;
-				}
-
-				if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
-					mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
-				}
-			} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
 		}

+		/* start geofence result check */
+		orb_check(geofence_result_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(geofence_result), geofence_result_sub, &geofence_result);
+		}
+
+		/* Check for geofence violation */
+		if (armed.armed && (geofence_result.geofence_violated || mission_result.flight_termination)) {
+			//XXX: make this configurable to select different actions (e.g. navigation modes)
+			/* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */
+			armed.force_failsafe = true;
+			status_changed = true;
+			static bool flight_termination_printed = false;
+
+			if (!flight_termination_printed) {
+				warnx("Flight termination because of navigator request or geofence");
+				mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
+				flight_termination_printed = true;
+			}
+
+			if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
+				mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
+			}
+		} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
+
 		/* RC input check */
 		if (!status.rc_input_blocked && sp_man.timestamp != 0 &&
 		    hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) {
--- 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
@ -202,9 +202,11 @@ private:
 	ECL_L1_Pos_Controller				_l1_control;
 	TECS						_tecs;
 	fwPosctrl::mTecs				_mTecs;
-	bool						_was_pos_control_mode;
-	bool						_was_velocity_control_mode;
-	bool						_was_alt_control_mode;
+	enum FW_POSCTRL_MODE {
+		FW_POSCTRL_MODE_AUTO,
+		FW_POSCTRL_MODE_POSITION,
+		FW_POSCTRL_MODE_OTHER
+	} _control_mode_current;			///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode.

 	struct {
 		float l1_period;
@ -473,7 +475,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_global_pos_valid(false),
 	_l1_control(),
 	_mTecs(),
-	_was_pos_control_mode(false)
+	_control_mode_current(FW_POSCTRL_MODE_OTHER)
 {
 	_nav_capabilities.turn_distance = 0.0f;

@ -910,22 +912,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	/* no throttle limit as default */
 	float throttle_max = 1.0f;

-	/* AUTONOMOUS FLIGHT */
+	if (_control_mode.flag_control_auto_enabled &&
+			pos_sp_triplet.current.valid) {
+		/* AUTONOMOUS FLIGHT */

-	// XXX this should only execute if auto AND safety off (actuators active),
-	// else integrators should be constantly reset.
-	if (pos_sp_triplet.current.valid) {
-
-		if (!_was_pos_control_mode) {
+		/* Reset integrators if switching to this mode from a other mode in which posctl was not active */
+		if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
 			/* reset integrators */
 			if (_mTecs.getEnabled()) {
 				_mTecs.resetIntegrators();
 				_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
 			}
 		}
-
-		_was_pos_control_mode = true;
-		_was_velocity_control_mode = false;
+		_control_mode_current = FW_POSCTRL_MODE_AUTO;

 		/* reset hold altitude */
 		_hold_alt = _global_pos.alt;
@ -1210,15 +1209,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
 							calculate_target_airspeed(_parameters.airspeed_trim),
 							eas2tas,
-								math::radians(_parameters.pitch_limit_min),
-								math::radians(_parameters.pitch_limit_max),
-								_parameters.throttle_min,
-								takeoff_throttle,
-								_parameters.throttle_cruise,
-								false,
-								math::radians(_parameters.pitch_limit_min),
-								_global_pos.alt,
-								ground_speed);
+							math::radians(_parameters.pitch_limit_min),
+							math::radians(_parameters.pitch_limit_max),
+							_parameters.throttle_min,
+							takeoff_throttle,
+							_parameters.throttle_cruise,
+							false,
+							math::radians(_parameters.pitch_limit_min),
+							_global_pos.alt,
+							ground_speed);
 				}
 			} else {
 				/* Tell the attitude controller to stop integrating while we are waiting
@ -1250,53 +1249,63 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			_att_sp.roll_reset_integral = true;
 		}

-	} else if (_control_mode.flag_control_velocity_enabled) {
+	} else if (_control_mode.flag_control_velocity_enabled &&
+			_control_mode.flag_control_altitude_enabled) {
+		/* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */
+
 		const float deadBand = (60.0f/1000.0f);
 		const float factor = 1.0f - deadBand;
-		if (!_was_velocity_control_mode) {
+		if (_control_mode_current != FW_POSCTRL_MODE_POSITION) {
+			/* Need to init because last loop iteration was in a different mode */
 			_hold_alt = _global_pos.alt;
-			_was_alt_control_mode = false;
 		}
-		_was_velocity_control_mode = true;
-		_was_pos_control_mode = false;
-		// Get demanded airspeed
+		/* Reset integrators if switching to this mode from a other mode in which posctl was not active */
+		if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
+			/* reset integrators */
+			if (_mTecs.getEnabled()) {
+				_mTecs.resetIntegrators();
+				_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
+			}
+		}
+		_control_mode_current = FW_POSCTRL_MODE_POSITION;
+
+		/* Get demanded airspeed */
 		float altctrl_airspeed = _parameters.airspeed_min +
 					  (_parameters.airspeed_max - _parameters.airspeed_min) *
 					  _manual.z;

-		// Get demanded vertical velocity from pitch control
-		float pitch = 0.0f;
+		/* Get demanded vertical velocity from pitch control */
+		static bool was_in_deadband = false;
 		if (_manual.x > deadBand) {
-			pitch = (_manual.x - deadBand) / factor;
-		} else if (_manual.x < - deadBand) {
-			pitch = (_manual.x + deadBand) / factor;
-		}
-		if (pitch > 0.0f) {
+			float pitch = (_manual.x - deadBand) / factor;
 			_hold_alt -= (_parameters.max_climb_rate * dt) * pitch;
-			_was_alt_control_mode = false;
-		} else if (pitch < 0.0f) {
+			was_in_deadband = false;
+		} else if (_manual.x < - deadBand) {
+			float pitch = (_manual.x + deadBand) / factor;
 			_hold_alt -= (_parameters.max_sink_rate * dt) * pitch;
-			_was_alt_control_mode = false;
-		} else if (!_was_alt_control_mode) {
+			was_in_deadband = false;
+		} else if (!was_in_deadband) {
+			 /* store altitude at which manual.x was inside deadBand
+			  * The aircraft should immediately try to fly at this altitude
+			  * as this is what the pilot expects when he moves the stick to the center */
 			_hold_alt = _global_pos.alt;
-			_was_alt_control_mode = true;
+			was_in_deadband = true;
 		}
 		tecs_update_pitch_throttle(_hold_alt,
-									altctrl_airspeed,
-									eas2tas,
-									math::radians(_parameters.pitch_limit_min),
-									math::radians(_parameters.pitch_limit_max),
-									_parameters.throttle_min,
-									_parameters.throttle_max,
-									_parameters.throttle_cruise,
-									false,
-									math::radians(_parameters.pitch_limit_min),
-									_global_pos.alt,
-									ground_speed,
-									TECS_MODE_NORMAL);
+				altctrl_airspeed,
+				eas2tas,
+				math::radians(_parameters.pitch_limit_min),
+				math::radians(_parameters.pitch_limit_max),
+				_parameters.throttle_min,
+				_parameters.throttle_max,
+				_parameters.throttle_cruise,
+				false,
+				math::radians(_parameters.pitch_limit_min),
+				_global_pos.alt,
+				ground_speed,
+				TECS_MODE_NORMAL);
 	} else {
-		_was_velocity_control_mode = false;
-		_was_pos_control_mode = false;
+		_control_mode_current = FW_POSCTRL_MODE_OTHER;

 		/** MANUAL FLIGHT **/

@ -1313,10 +1322,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		}
 	}

+	/* Copy thrust output for publication */
 	if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) {
 		/* Set thrust to 0 to minimize damage */
 		_att_sp.thrust = 0.0f;
-	} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
+	} else if (_control_mode_current ==  FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto
+			pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
 			launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
 		 /* making sure again that the correct thrust is used,
 		 * without depending on library calls for safety reasons */
@ -1329,8 +1340,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi

 	/* 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)) {
+	if (!(_control_mode_current ==  FW_POSCTRL_MODE_AUTO &&
+				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();
 	}

--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@ -292,9 +292,6 @@ MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
 void
 MavlinkMissionManager::send(const hrt_abstime now)
 {
-	/* update interval for slow rate limiter */
-	_slow_rate_limiter.set_interval(_interval * 10 / _mavlink->get_rate_mult());
-
 	bool updated = false;
 	orb_check(_mission_result_sub, &updated);

@ -312,6 +309,12 @@ MavlinkMissionManager::send(const hrt_abstime now)

 		send_mission_current(_current_seq);

+		if (mission_result.item_do_jump_changed) {
+			/* send a mission item again if the remaining DO_JUMPs has changed */
+			send_mission_item(_transfer_partner_sysid, _transfer_partner_compid,
+					  (uint16_t)mission_result.item_changed_index);
+		}
+
 	} else {
 		if (_slow_rate_limiter.check(now)) {
 			send_mission_current(_current_seq);
@ -811,7 +814,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *

 	case NAV_CMD_DO_JUMP:
 		mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
-		mavlink_mission_item->param2 = mission_item->do_jump_repeat_count;
+		mavlink_mission_item->param2 = mission_item->do_jump_repeat_count - mission_item->do_jump_current_count;
 		break;

 	default:
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@ -653,8 +653,6 @@ MulticopterPositionControl::control_offboard(float dt)
 			/* control position */
 			_pos_sp(0) = _pos_sp_triplet.current.x;
 			_pos_sp(1) = _pos_sp_triplet.current.y;
-			_pos_sp(2) = _pos_sp_triplet.current.z;
-
 		} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
 			/* control velocity */
 			/* reset position setpoint to current position if needed */
@ -671,7 +669,10 @@ MulticopterPositionControl::control_offboard(float dt)
 			_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
 		}

-		if (_control_mode.flag_control_altitude_enabled) {
+		if (_control_mode.flag_control_altitude_enabled && _pos_sp_triplet.current.position_valid) {
+			/* Control altitude */
+			_pos_sp(2) = _pos_sp_triplet.current.z;
+		} else if (_control_mode.flag_control_climb_rate_enabled && _pos_sp_triplet.current.velocity_valid) {
 			/* reset alt setpoint to current altitude if needed */
 			reset_alt_sp();

--- a/src/modules/navigator/datalinkloss.cpp
+++ b/src/modules/navigator/datalinkloss.cpp
@ -155,7 +155,7 @@ DataLinkLoss::set_dll_item()
 	case DLL_STATE_TERMINATE: {
 		/* Request flight termination from the commander */
 		_navigator->get_mission_result()->flight_termination = true;
-		_navigator->publish_mission_result();
+		_navigator->set_mission_result_updated();
 		reset_mission_item_reached();
 		warnx("not switched to manual: request flight termination");
 		pos_sp_triplet->previous.valid = false;
@ -188,7 +188,7 @@ DataLinkLoss::advance_dll()
 					_navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get());
 			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home");
 			_navigator->get_mission_result()->stay_in_failsafe = true;
-			_navigator->publish_mission_result();
+			_navigator->set_mission_result_updated();
 			reset_mission_item_reached();
 			_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
 		} else {
@ -209,7 +209,7 @@ DataLinkLoss::advance_dll()
 			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home");
 		_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
 		_navigator->get_mission_result()->stay_in_failsafe = true;
-		_navigator->publish_mission_result();
+		_navigator->set_mission_result_updated();
 		reset_mission_item_reached();
 		break;
 	case DLL_STATE_FLYTOAIRFIELDHOMEWP:
@ -217,7 +217,7 @@ DataLinkLoss::advance_dll()
 		warnx("time is up, state should have been changed manually by now");
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating");
 		_navigator->get_mission_result()->stay_in_failsafe = true;
-		_navigator->publish_mission_result();
+		_navigator->set_mission_result_updated();
 		reset_mission_item_reached();
 		break;
 	case DLL_STATE_TERMINATE:
--- a/src/modules/navigator/geofence.cpp
+++ b/src/modules/navigator/geofence.cpp
@ -279,8 +279,14 @@ Geofence::loadFromFile(const char *filename)
 		while((textStart < sizeof(line)/sizeof(char)) && isspace(line[textStart])) textStart++;

 		/* if the line starts with #, skip */
-		if (line[textStart] == commentChar)
+		if (line[textStart] == commentChar) {
 			continue;
+		}
+
+		/* if there is only a linefeed, skip it */
+		if (line[0] == '\n') {
+			continue;
+		}

 		if (gotVertical) {
 			/* Parse the line as a geofence point */
@ -291,8 +297,10 @@ Geofence::loadFromFile(const char *filename)
 				/* Handle degree minute second format */
 				float lat_d, lat_m, lat_s, lon_d, lon_m, lon_s;

-				if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6)
+				if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6) {
+					warnx("Scanf to parse DMS geofence vertex failed.");
 					return ERROR;
+				}

 //				warnx("Geofence DMS: %.5f %.5f %.5f ; %.5f %.5f %.5f", (double)lat_d, (double)lat_m, (double)lat_s, (double)lon_d, (double)lon_m, (double)lon_s);

@ -301,9 +309,10 @@ Geofence::loadFromFile(const char *filename)

 			} else {
 				/* Handle decimal degree format */
-
-				if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2)
+				if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) {
+					warnx("Scanf to parse geofence vertex failed.");
 					return ERROR;
+				}
 			}

 			if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex))
--- a/src/modules/navigator/gpsfailure.cpp
+++ b/src/modules/navigator/gpsfailure.cpp
@ -141,7 +141,7 @@ GpsFailure::set_gpsf_item()
 	case GPSF_STATE_TERMINATE: {
 		/* Request flight termination from the commander */
 		_navigator->get_mission_result()->flight_termination = true;
-		_navigator->publish_mission_result();
+		_navigator->set_mission_result_updated();
 		warnx("gps fail: request flight termination");
 	}
 	default:
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@ -38,6 +38,7 @@
 * @author Julian Oes <julian@oes.ch>
 * @author Thomas Gubler <thomasgubler@gmail.com>
 * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Ban Siesta <bansiesta@gmail.com>
 */

 #include <sys/types.h>
@ -149,18 +150,12 @@ Mission::on_active()

 	/* lets check if we reached the current mission item */
 	if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
+		set_mission_item_reached();
 		if (_mission_item.autocontinue) {
 			/* switch to next waypoint if 'autocontinue' flag set */
 			advance_mission();
 			set_mission_items();

-		} else {
-			/* else just report that item reached */
-			if (_mission_type == MISSION_TYPE_OFFBOARD) {
-				if (!(_navigator->get_mission_result()->seq_reached == _current_offboard_mission_index && _navigator->get_mission_result()->reached)) {
-					set_mission_item_reached();
-				}
-			}
 		}

 	} else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) {
@ -395,7 +390,6 @@ Mission::set_mission_items()
 		/* reuse setpoint for LOITER only if it's not IDLE */
 		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);

-		reset_mission_item_reached();
 		set_mission_finished();

 		_navigator->set_position_setpoint_triplet_updated();
@ -636,6 +630,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 								     "ERROR DO JUMP waypoint could not be written");
 						return false;
 					}
+					report_do_jump_mission_changed(*mission_index_ptr,
+								       mission_item_tmp.do_jump_repeat_count);
 				}
 				/* set new mission item index and repeat
 				* we don't have to validate here, if it's invalid, we should realize this later .*/
@ -706,23 +702,32 @@ Mission::save_offboard_mission_state()
 	dm_unlock(DM_KEY_MISSION_STATE);
 }

+void
+Mission::report_do_jump_mission_changed(int index, int do_jumps_remaining)
+{
+	/* inform about the change */
+	_navigator->get_mission_result()->item_do_jump_changed = true;
+	_navigator->get_mission_result()->item_changed_index = index;
+	_navigator->get_mission_result()->item_do_jump_remaining = do_jumps_remaining;
+	_navigator->set_mission_result_updated();
+}
+
 void
 Mission::set_mission_item_reached()
 {
 	_navigator->get_mission_result()->reached = true;
 	_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
-	_navigator->publish_mission_result();
+	_navigator->set_mission_result_updated();
 	reset_mission_item_reached();
 }

 void
 Mission::set_current_offboard_mission_item()
 {
-	warnx("current offboard mission index: %d", _current_offboard_mission_index);
 	_navigator->get_mission_result()->reached = false;
 	_navigator->get_mission_result()->finished = false;
 	_navigator->get_mission_result()->seq_current = _current_offboard_mission_index;
-	_navigator->publish_mission_result();
+	_navigator->set_mission_result_updated();

 	save_offboard_mission_state();
 }
@ -731,5 +736,5 @@ void
 Mission::set_mission_finished()
 {
 	_navigator->get_mission_result()->finished = true;
-	_navigator->publish_mission_result();
+	_navigator->set_mission_result_updated();
 }
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@ -38,6 +38,7 @@
 * @author Julian Oes <julian@oes.ch>
 * @author Thomas Gubler <thomasgubler@gmail.com>
 * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Ban Siesta <bansiesta@gmail.com>
 */

 #ifndef NAVIGATOR_MISSION_H
@ -130,6 +131,11 @@ private:
 	 */
 	void save_offboard_mission_state();

+	/**
+	 * Inform about a changed mission item after a DO_JUMP
+	 */
+	void report_do_jump_mission_changed(int index, int do_jumps_remaining);
+
 	/**
 	 * Set a mission item as reached
 	 */
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@ -54,6 +54,7 @@
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/mission_result.h>
+#include <uORB/topics/geofence_result.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>

 #include "navigator_mode.h"
@ -107,9 +108,9 @@ public:
 	void		load_fence_from_file(const char *filename);

 	/**
-	 * Publish the mission result so commander and mavlink know what is going on
+	 * Publish the geofence result
 	 */
-	void publish_mission_result();
+	void publish_geofence_result();

 	/**
 	 * Publish the attitude sp, only to be used in very special modes when position control is deactivated
@ -122,6 +123,7 @@ public:
 	 */
 	void		set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
 	void		set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; }
+	void		set_mission_result_updated() { _mission_result_updated = true; }

 	/**
 	 * Getters
@ -134,6 +136,7 @@ public:
 	struct home_position_s*		    get_home_position() { return &_home_pos; }
 	struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
 	struct mission_result_s*	    get_mission_result() { return &_mission_result; }
+	struct geofence_result_s*		    get_geofence_result() { return &_geofence_result; }
 	struct vehicle_attitude_setpoint_s* get_att_sp() { return &_att_sp; }

 	int		get_onboard_mission_sub() { return _onboard_mission_sub; }
@ -164,6 +167,7 @@ private:

 	orb_advert_t	_pos_sp_triplet_pub;		/**< publish position setpoint triplet */
 	orb_advert_t	_mission_result_pub;
+	orb_advert_t	_geofence_result_pub;
 	orb_advert_t	_att_sp_pub;			/**< publish att sp
 							  used only in very special failsafe modes
 							  when pos control is deactivated */
@ -179,7 +183,8 @@ private:
 	position_setpoint_triplet_s			_pos_sp_triplet;	/**< triplet of position setpoints */

 	mission_result_s				_mission_result;
-	vehicle_attitude_setpoint_s					_att_sp;
+	geofence_result_s				_geofence_result;
+	vehicle_attitude_setpoint_s			_att_sp;

 	bool 		_mission_item_valid;		/**< flags if the current mission item is valid */

@ -206,6 +211,7 @@ private:
 	bool		_can_loiter_at_sp;			/**< flags if current position SP can be used to loiter */
 	bool		_pos_sp_triplet_updated;		/**< flags if position SP triplet needs to be published */
 	bool 		_pos_sp_triplet_published_invalid_once;	/**< flags if position SP triplet has been published once to UORB */
+	bool		_mission_result_updated;		/**< flags if mission result has seen an update */

 	control::BlockParamFloat _param_loiter_radius;	/**< loiter radius for fixedwing */
 	control::BlockParamFloat _param_acceptance_radius;	/**< acceptance for takeoff */
@ -271,6 +277,12 @@ private:
 	 */
 	void		publish_position_setpoint_triplet();

+
+	/**
+	 * Publish the mission result so commander and mavlink know what is going on
+	 */
+	void		publish_mission_result();
+
 	/* this class has ptr data members, so it should not be copied,
 	 * consequently the copy constructors are private.
 	 */
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@ -110,6 +110,7 @@ Navigator::Navigator() :
 	_param_update_sub(-1),
 	_pos_sp_triplet_pub(-1),
 	_mission_result_pub(-1),
+	_geofence_result_pub(-1),
 	_att_sp_pub(-1),
 	_vstatus{},
 	_control_mode{},
@ -138,6 +139,7 @@ Navigator::Navigator() :
 	_can_loiter_at_sp(false),
 	_pos_sp_triplet_updated(false),
 	_pos_sp_triplet_published_invalid_once(false),
+	_mission_result_updated(false),
 	_param_loiter_radius(this, "LOITER_RAD"),
 	_param_acceptance_radius(this, "ACC_RAD"),
 	_param_datalinkloss_obc(this, "DLL_OBC"),
@ -398,8 +400,8 @@ Navigator::task_main()
 			have_geofence_position_data = false;
 			if (!inside) {
 				/* inform other apps via the mission result */
-				_mission_result.geofence_violated = true;
-				publish_mission_result();
+				_geofence_result.geofence_violated = true;
+				publish_geofence_result();

 				/* Issue a warning about the geofence violation once */
 				if (!_geofence_violation_warning_sent) {
@ -408,8 +410,8 @@ Navigator::task_main()
 				}
 			} else {
 				/* inform other apps via the mission result */
-				_mission_result.geofence_violated = false;
-				publish_mission_result();
+				_geofence_result.geofence_violated = false;
+				publish_geofence_result();
 				/* Reset the _geofence_violation_warning_sent field */
 				_geofence_violation_warning_sent = false;
 			}
@ -490,6 +492,11 @@ Navigator::task_main()
 			_pos_sp_triplet_updated = false;
 		}

+		if (_mission_result_updated) {
+			publish_mission_result();
+			_mission_result_updated = false;
+		}
+
 		perf_end(_loop_perf);
 	}
 	warnx("exiting.");
@ -639,6 +646,28 @@ Navigator::publish_mission_result()
 		/* advertise and publish */
 		_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
 	}
+
+	/* reset some of the flags */
+	_mission_result.seq_reached = false;
+	_mission_result.seq_current = 0;
+	_mission_result.item_do_jump_changed = false;
+	_mission_result.item_changed_index = 0;
+	_mission_result.item_do_jump_remaining = 0;
+}
+
+void
+Navigator::publish_geofence_result()
+{
+
+	/* lazily publish the geofence result only once available */
+	if (_geofence_result_pub > 0) {
+		/* publish mission result */
+		orb_publish(ORB_ID(geofence_result), _geofence_result_pub, &_geofence_result);
+
+	} else {
+		/* advertise and publish */
+		_geofence_result_pub = orb_advertise(ORB_ID(geofence_result), &_geofence_result);
+	}
 }

 void
--- a/src/modules/navigator/navigator_mode.cpp
+++ b/src/modules/navigator/navigator_mode.cpp
@ -65,7 +65,7 @@ NavigatorMode::run(bool active) {
 			_first_run = false;
 			/* Reset stay in failsafe flag */
 			_navigator->get_mission_result()->stay_in_failsafe = false;
-			_navigator->publish_mission_result();
+			_navigator->set_mission_result_updated();
 			on_activation();

 		} else {
--- a/src/modules/navigator/rcloss.cpp
+++ b/src/modules/navigator/rcloss.cpp
@ -128,7 +128,7 @@ RCLoss::set_rcl_item()
 	case RCL_STATE_TERMINATE: {
 		/* Request flight termination from the commander */
 		_navigator->get_mission_result()->flight_termination = true;
-		_navigator->publish_mission_result();
+		_navigator->set_mission_result_updated();
 		warnx("rc not recovered: request flight termination");
 		pos_sp_triplet->previous.valid = false;
 		pos_sp_triplet->current.valid = false;
@ -162,7 +162,7 @@ RCLoss::advance_rcl()
 			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, terminating");
 			_rcl_state = RCL_STATE_TERMINATE;
 			_navigator->get_mission_result()->stay_in_failsafe = true;
-			_navigator->publish_mission_result();
+			_navigator->set_mission_result_updated();
 			reset_mission_item_reached();
 		}
 		break;
@ -171,7 +171,7 @@ RCLoss::advance_rcl()
 		warnx("time is up, no RC regain, terminating");
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating");
 		_navigator->get_mission_result()->stay_in_failsafe = true;
-		_navigator->publish_mission_result();
+		_navigator->set_mission_result_updated();
 		reset_mission_item_reached();
 		break;
 	case RCL_STATE_TERMINATE:
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@ -1053,10 +1053,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				landed = false;
 				landed_time = 0;
 			}
-			/* reset xy velocity estimates when landed */
-			x_est[1] = 0.0f;
-			y_est[1] = 0.0f;
-
 		} else {
 			if (alt_disp2 < land_disp2 && thrust < params.land_thr) {
 				if (landed_time == 0) {
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@ -33,6 +33,8 @@

 #pragma once

+#include <inttypes.h>
+
 /**
 * @file protocol.h
 *
--- a/src/modules/systemlib/perf_counter.c
+++ b/src/modules/systemlib/perf_counter.c
@ -41,7 +41,7 @@
 #include <stdio.h>
 #include <sys/queue.h>
 #include <drivers/drv_hrt.h>
-
+#include <math.h>
 #include "perf_counter.h"

 /**
@ -84,7 +84,8 @@ struct perf_ctr_interval {
 	uint64_t		time_last;
 	uint64_t		time_least;
 	uint64_t		time_most;
-
+	float			mean;
+	float			M2;
 };

 /**
@ -109,6 +110,7 @@ perf_alloc(enum perf_counter_type type, const char *name)

 	case PC_INTERVAL:
 		ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_interval), 1);
+
 		break;

 	default:
@ -156,15 +158,23 @@ perf_count(perf_counter_t handle)
 		case 1:
 			pci->time_least = now - pci->time_last;
 			pci->time_most = now - pci->time_last;
+			pci->mean = pci->time_least / 1e6f;
+			pci->M2 = 0;
 			break;
 		default: {
-			hrt_abstime interval = now - pci->time_last;
-			if (interval < pci->time_least)
-				pci->time_least = interval;
-			if (interval > pci->time_most)
-				pci->time_most = interval;
-			break;
-		}
+				hrt_abstime interval = now - pci->time_last;
+				if (interval < pci->time_least)
+					pci->time_least = interval;
+				if (interval > pci->time_most)
+					pci->time_most = interval;
+				// maintain mean and variance of interval in seconds
+				// Knuth/Welford recursive mean and variance of update intervals (via Wikipedia)
+				float dt = interval / 1e6f;
+				float delta_intvl = dt - pci->mean;
+				pci->mean += delta_intvl / pci->event_count;
+				pci->M2 += delta_intvl * (dt - pci->mean);
+				break;
+			}
 		}
 		pci->time_last = now;
 		pci->event_count++;
@ -313,13 +323,16 @@ perf_print_counter_fd(int fd, perf_counter_t handle)

 	case PC_INTERVAL: {
 		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
+		float rms = sqrtf(pci->M2 / (pci->event_count-1));

-		dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus\n",
+		dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus %5.3f msec mean %5.3f msec rms\n",
 		       handle->name,
 		       pci->event_count,
 		       (pci->time_last - pci->time_first) / pci->event_count,
 		       pci->time_least,
-		       pci->time_most);
+		       pci->time_most,
+			   (double)(1000 * pci->mean),
+			   (double)(1000 * rms));
 		break;
 	}

@ -365,6 +378,21 @@ perf_print_all(int fd)
 	}
 }

+extern const uint16_t latency_bucket_count;
+extern uint32_t latency_counters[];
+extern const uint16_t latency_buckets[];
+
+void
+perf_print_latency(int fd)
+{
+	dprintf(fd, "bucket : events\n");
+	for (int i = 0; i < latency_bucket_count; i++) {
+		printf("  %4i : %i\n", latency_buckets[i], latency_counters[i]);
+	}
+	// print the overflow bucket value
+	dprintf(fd, " >%4i : %i\n", latency_buckets[latency_bucket_count-1], latency_counters[latency_bucket_count]);
+}
+
 void
 perf_reset_all(void)
 {
@ -374,4 +402,7 @@ perf_reset_all(void)
 		perf_reset(handle);
 		handle = (perf_counter_t)sq_next(&handle->link);
 	}
+	for (int i = 0; i <= latency_bucket_count; i++) {
+		latency_counters[i] = 0;
+	}
 }
--- a/src/modules/systemlib/perf_counter.h
+++ b/src/modules/systemlib/perf_counter.h
@ -95,7 +95,7 @@ __EXPORT extern void		perf_begin(perf_counter_t handle);
 * End a performance event.
 *
 * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
- * If a call is made without a corresopnding perf_begin call, or if perf_cancel
+ * If a call is made without a corresponding perf_begin call, or if perf_cancel
 * has been called subsequently, no change is made to the counter.
 *
 * @param handle		The handle returned from perf_alloc.
@ -143,6 +143,13 @@ __EXPORT extern void		perf_print_counter_fd(int fd, perf_counter_t handle);
 */
 __EXPORT extern void		perf_print_all(int fd);

+/**
+ * Print hrt latency counters.
+ *
+ * @param fd			File descriptor to print to - e.g. 0 for stdout
+ */
+__EXPORT extern void		perf_print_latency(int fd);
+
 /**
 * Reset all of the performance counters.
 */
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012-2014 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@ -148,6 +148,9 @@ ORB_DEFINE(onboard_mission, struct mission_s);
 #include "topics/mission_result.h"
 ORB_DEFINE(mission_result, struct mission_result_s);

+#include "topics/geofence_result.h"
+ORB_DEFINE(geofence_result, struct geofence_result_s);
+
 #include "topics/fence.h"
 ORB_DEFINE(fence, unsigned);

--- a/src/modules/uORB/topics/geofence_result.h
+++ b/src/modules/uORB/topics/geofence_result.h
@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file geofence_result.h
+ * Status of the plance concerning the geofence
+ *
+ * @author Ban Siesta <bansiesta@gmail.com>
+ */
+
+#ifndef TOPIC_GEOFENCE_RESULT_H
+#define TOPIC_GEOFENCE_RESULT_H
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+struct geofence_result_s
+{
+	bool geofence_violated;		/**< true if the geofence is violated */
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(geofence_result);
+
+#endif
--- a/src/modules/uORB/topics/mission_result.h
+++ b/src/modules/uORB/topics/mission_result.h
@ -1,9 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (C) 2012-2014 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@ -37,6 +34,11 @@
 /**
 * @file mission_result.h
 * Mission results that navigator needs to pass on to commander and mavlink.
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Ban Siesta <bansiesta@gmail.com>
 */

 #ifndef TOPIC_MISSION_RESULT_H
@ -58,8 +60,10 @@ struct mission_result_s
 	bool reached;			/**< true if mission has been reached					 */
 	bool finished;			/**< true if mission has been completed					 */
 	bool stay_in_failsafe;		/**< true if the commander should not switch out of the failsafe mode*/
-	bool geofence_violated;		/**< true if the geofence is violated */
 	bool flight_termination;	/**< true if the navigator demands a flight termination from the commander app */
+	bool item_do_jump_changed;	/**< true if the number of do jumps remaining has changed */
+	unsigned item_changed_index;	/**< indicate which item has changed */
+	unsigned item_do_jump_remaining;/**< set to the number of do jumps remaining for that item */
 };

 /**
--- a/src/modules/uavcan/module.mk
+++ b/src/modules/uavcan/module.mk
@ -57,7 +57,7 @@ SRCS += sensors/sensor_bridge.cpp   \
 #
 # libuavcan
 #
-include $(UAVCAN_DIR)/libuavcan/include.mk
+include $(PX4_LIB_DIR)/uavcan/libuavcan/include.mk
 SRCS += $(LIBUAVCAN_SRC)
 INCLUDE_DIRS += $(LIBUAVCAN_INC)
 # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile
@ -67,7 +67,7 @@ override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAV
 #
 # libuavcan drivers for STM32
 #
-include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk
+include $(PX4_LIB_DIR)/uavcan/libuavcan_drivers/stm32/driver/include.mk
 SRCS += $(LIBUAVCAN_STM32_SRC)
 INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC)
 override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@ -74,11 +74,8 @@
 #include <lib/geo/geo.h>

 #include "drivers/drv_pwm_output.h"
-#include <nuttx/fs/nxffs.h>
 #include <nuttx/fs/ioctl.h>

-#include <nuttx/mtd.h>
-
 #include <fcntl.h>


@ -238,11 +235,11 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	_params.idle_pwm_mc = PWM_LOWEST_MIN;
 	_params.vtol_motor_count = 0;

-	_params_handles.idle_pwm_mc = param_find("IDLE_PWM_MC");
-	_params_handles.vtol_motor_count = param_find("VTOL_MOT_COUNT");
-	_params_handles.mc_airspeed_min = param_find("VTOL_MC_AIRSPEED_MIN");
-	_params_handles.mc_airspeed_max = param_find("VTOL_MC_AIRSPEED_MAX");
-	_params_handles.mc_airspeed_trim = param_find("VTOL_MC_AIRSPEED_TRIM");
+	_params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC");
+	_params_handles.vtol_motor_count = param_find("VT_MOT_COUNT");
+	_params_handles.mc_airspeed_min = param_find("VT_MC_ARSPD_MIN");
+	_params_handles.mc_airspeed_max = param_find("VT_MC_ARSPD_MAX");
+	_params_handles.mc_airspeed_trim = param_find("VT_MC_ARSPD_TRIM");

 	/* fetch initial parameter values */
 	parameters_update();
--- a/src/modules/vtol_att_control/vtol_att_control_params.c
+++ b/src/modules/vtol_att_control/vtol_att_control_params.c
@ -1,13 +1,88 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vtol_att_control_params.c
+ * Parameters for vtol attitude controller.
+ *
+ * @author Roman Bapst <bapstr@ethz.ch>
+ */
+
 #include <systemlib/param/param.h>

-// number of engines
-PARAM_DEFINE_INT32(VTOL_MOT_COUNT,0);
-// idle pwm in multicopter mode
-PARAM_DEFINE_INT32(IDLE_PWM_MC,900);
-// min airspeed in multicopter mode
-PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MIN,2);
-// max airspeed in multicopter mode
-PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MAX,30);
-// trim airspeed in multicopter mode
-PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_TRIM,10);
+/**
+ * VTOL number of engines
+ *
+ * @min 1.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_INT32(VT_MOT_COUNT,0);
+
+/**
+ * Idle speed of VTOL when in multicopter mode
+ *
+ * @min 900
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_INT32(VT_IDLE_PWM_MC,900);
+
+/**
+ * Minimum airspeed in multicopter mode
+ *
+ * This is the minimum speed of the air flowing over the control surfaces.
+ *
+ * @min 0.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN,2.0f);
+
+/**
+ * Maximum airspeed in multicopter mode
+ *
+ * This is the maximum speed of the air flowing over the control surfaces.
+ *
+ * @min 0.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MAX,30.0f);
+
+/**
+ * Trim airspeed when in multicopter mode
+ *
+ * This is the airflow over the control surfaces for which no airspeed scaling is applied in multicopter mode.
+ *
+ * @min 0.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM,10.0f);

--- a/src/systemcmds/perf/perf.c
+++ b/src/systemcmds/perf/perf.c
@ -68,8 +68,12 @@ int perf_main(int argc, char *argv[])
 		if (strcmp(argv[1], "reset") == 0) {
 			perf_reset_all();
 			return 0;
+		} else if (strcmp(argv[1], "latency") == 0) {
+			perf_print_latency(0 /* stdout */);
+			fflush(stdout);
+			return 0;
 		}
-		printf("Usage: perf <reset>\n");
+		printf("Usage: perf [reset | latency]\n");
 		return -1;
 	}

--- a/1
+++ b/1
@ -1 +0,0 @@
-Subproject commit 1efd24427539fa332a15151143466ec760fa5fff
--- a/Tools/tests-host/.gitignore
+++ b/Tools/tests-host/.gitignore
--- a/unittests/Makefile
+++ b/unittests/Makefile
@ -0,0 +1,60 @@
+
+CC=g++
+CFLAGS=-I. -I../src/modules -I ../src/include -I../src/drivers \
+	-I../src -I../src/lib -D__EXPORT="" -Dnullptr="0" -lm
+
+all: mixer_test sbus2_test autodeclination_test st24_test sf0x_test
+
+MIXER_FILES=../src/systemcmds/tests/test_mixer.cpp \
+		../src/systemcmds/tests/test_conv.cpp \
+		../src/modules/systemlib/mixer/mixer_simple.cpp \
+		../src/modules/systemlib/mixer/mixer_multirotor.cpp \
+		../src/modules/systemlib/mixer/mixer.cpp \
+		../src/modules/systemlib/mixer/mixer_group.cpp \
+		../src/modules/systemlib/mixer/mixer_load.c \
+		../src/modules/systemlib/pwm_limit/pwm_limit.c \
+		hrt.cpp \
+		mixer_test.cpp
+
+SBUS2_FILES=../src/modules/px4iofirmware/sbus.c \
+		hrt.cpp \
+		sbus2_test.cpp
+
+ST24_FILES=../src/lib/rc/st24.c \
+		hrt.cpp \
+		st24_test.cpp
+
+SF0X_FILES= \
+		hrt.cpp \
+		sf0x_test.cpp \
+		../src/drivers/sf0x/sf0x_parser.cpp
+
+AUTODECLINATION_FILES= ../src/lib/geo_lookup/geo_mag_declination.c \
+		hrt.cpp \
+		autodeclination_test.cpp
+
+mixer_test: $(MIXER_FILES)
+	$(CC) -o mixer_test $(MIXER_FILES) $(CFLAGS)
+
+sbus2_test: $(SBUS2_FILES)
+	$(CC) -o sbus2_test $(SBUS2_FILES) $(CFLAGS)
+
+sf0x_test: $(SF0X_FILES)
+	$(CC) -o sf0x_test $(SF0X_FILES) $(CFLAGS)
+
+autodeclination_test: $(SBUS2_FILES)
+	$(CC) -o autodeclination_test $(AUTODECLINATION_FILES) $(CFLAGS)
+
+st24_test: $(ST24_FILES)
+	$(CC) -o st24_test $(ST24_FILES) $(CFLAGS)
+
+unittests: clean mixer_test sbus2_test sf0x_test autodeclination_test st24_test
+	./mixer_test
+	./sbus2_test
+	./sf0x_test
+	./autodeclination_test
+	./st24_test
+
+.PHONY: clean
+clean:
+	rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test st24_test sf0x_test
--- a/Tools/tests-host/arch/board/board.h
+++ b/Tools/tests-host/arch/board/board.h
--- a/unittests/autodeclination_test.cpp
+++ b/unittests/autodeclination_test.cpp
@ -0,0 +1,52 @@
+
+#include <stdio.h>
+#include <stdlib.h> 
+#include <unistd.h>
+#include <string.h>
+#include <math.h>
+#include <systemlib/mixer/mixer.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+#include <px4iofirmware/px4io.h>
+// #include "../../src/systemcmds/tests/tests.h"
+#include <geo/geo.h>
+
+int main(int argc, char *argv[]) {
+	warnx("autodeclination test started");
+
+	char* latstr = 0;
+	char* lonstr = 0;
+	char* declstr = 0;
+
+	if (argc < 4) {
+		warnx("Too few arguments. Using default lat / lon and declination");
+		latstr = "47.0";
+		lonstr = "8.0";
+		declstr = "0.6";
+	} else {
+		latstr = argv[1];
+		lonstr = argv[2];
+		declstr = argv[3];
+	}
+
+	char* p_end;
+
+	float lat = strtod(latstr, &p_end);
+	float lon = strtod(lonstr, &p_end);
+	float decl_truth = strtod(declstr, &p_end);
+
+	float declination = get_mag_declination(lat, lon);
+
+	printf("lat: %f lon: %f, expected dec: %f, estimated dec: %f\n", lat, lon, declination, decl_truth);
+
+	int ret = 0;
+
+	// Fail if the declination differs by more than one degree
+	float decldiff = fabs(decl_truth - declination);
+	if (decldiff > 0.5f) {
+		warnx("declination differs more than 1 degree: difference: %12.8f", decldiff);
+		ret = 1;
+	}
+
+	return ret;
+}
--- a/Tools/tests-host/board_config.h
+++ b/Tools/tests-host/board_config.h
--- a/Tools/tests-host/data/fit_linear_voltage.m
+++ b/Tools/tests-host/data/fit_linear_voltage.m
--- a/Tools/tests-host/data/px4io_v1.3.csv
+++ b/Tools/tests-host/data/px4io_v1.3.csv
--- a/Tools/tests-host/debug.h
+++ b/Tools/tests-host/debug.h
@ -2,4 +2,5 @@
 #pragma once

 #include <systemlib/err.h>
-#define lowsyslog warnx
+#define lowsyslog warnx
+#define dbg warnx
--- a/Tools/tests-host/hrt.cpp
+++ b/Tools/tests-host/hrt.cpp
--- a/Tools/tests-host/mixer_test.cpp
+++ b/Tools/tests-host/mixer_test.cpp
@ -3,12 +3,17 @@
 #include "../../src/systemcmds/tests/tests.h"

 int main(int argc, char *argv[]) {
+
+	int ret;
+
 	warnx("Host execution started");

-	char* args[] = {argv[0], "../../ROMFS/px4fmu_common/mixers/IO_pass.mix",
-				 "../../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"};
+	char* args[] = {argv[0], "../ROMFS/px4fmu_common/mixers/IO_pass.mix",
+				 "../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"};

-	test_mixer(3, args);
+	if (ret = test_mixer(3, args));

 	test_conv(1, args);
+
+	return 0;
 }
--- a/Tools/tests-host/nuttx/config.h
+++ b/Tools/tests-host/nuttx/config.h
--- a/Tools/tests-host/queue.h
+++ b/Tools/tests-host/queue.h
--- a/Tools/tests-host/run_tests.sh
+++ b/Tools/tests-host/run_tests.sh
--- a/Tools/tests-host/sbus2_test.cpp
+++ b/Tools/tests-host/sbus2_test.cpp
@ -11,14 +11,20 @@
 int main(int argc, char *argv[]) {
 	warnx("SBUS2 test started");

-	if (argc < 2)
-		errx(1, "Need a filename for the input file");
+	char *filepath = 0;

-	warnx("loading data from: %s", argv[1]);
+	if (argc < 2) {
+		warnx("Using default input file");
+		filepath = "testdata/sbus2_r7008SB.txt";
+	} else {
+		filepath = argv[1];
+	}
+
+	warnx("loading data from: %s", filepath);

 	FILE *fp;
 	
-	fp = fopen(argv[1],"rt");
+	fp = fopen(filepath,"rt");

 	if (!fp)
 		errx(1, "failed opening file");
@ -47,7 +53,7 @@ int main(int argc, char *argv[]) {
 	while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
 		if (((f - last_time) * 1000 * 1000) > 3000) {
 			partial_frame_count = 0;
-			warnx("FRAME RESET\n\n");
+			//warnx("FRAME RESET\n\n");
 		}

 		frame[partial_frame_count] = x;
@ -69,8 +75,10 @@ int main(int argc, char *argv[]) {

 	if (ret == EOF) {
 		warnx("Test finished, reached end of file");
+		ret = 0;
 	} else {
 		warnx("Test aborted, errno: %d", ret);
 	}

+	return ret;
 }
--- a/Tools/tests-host/sf0x_test.cpp
+++ b/Tools/tests-host/sf0x_test.cpp
@ -43,7 +43,7 @@ int main(int argc, char *argv[])

 	for (unsigned l = 0; l < sizeof(lines) / sizeof(lines[0]); l++) {

-		printf("\n%s", _linebuf);
+		//printf("\n%s", _linebuf);

 		int parse_ret;

@ -51,11 +51,11 @@ int main(int argc, char *argv[])
 			parse_ret = sf0x_parser(lines[l][i], _parserbuf, &_parsebuf_index, &state, &dist_m);

 			if (parse_ret == 0) {
-				printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : "");
+				//printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : "");
 			}
 		}

-		printf("%s", lines[l]);
+		//printf("%s", lines[l]);

 	}

--- a/Tools/tests-host/st24_test.cpp
+++ b/Tools/tests-host/st24_test.cpp
@ -11,15 +11,22 @@ int main(int argc, char *argv[])
 {
 	warnx("ST24 test started");

+	char* defaultfile = "testdata/st24_data.txt";
+
+	char* filepath = 0;
+
 	if (argc < 2) {
-		errx(1, "Need a filename for the input file");
+		warnx("Too few arguments. Using default file: %s", defaultfile);
+		filepath = defaultfile;
+	} else {
+		filepath = argv[1];
 	}

-	warnx("loading data from: %s", argv[1]);
+	warnx("loading data from: %s", filepath);

 	FILE *fp;

-	fp = fopen(argv[1], "rt");
+	fp = fopen(filepath, "rt");

 	if (!fp) {
 		errx(1, "failed opening file");
@ -56,21 +63,22 @@ int main(int argc, char *argv[])


 		if (!st24_decode(b, &rssi, &rx_count, &channel_count, channels, sizeof(channels) / sizeof(channels[0]))) {
-			warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count);
+			//warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count);

 			for (unsigned i = 0; i < channel_count; i++) {

 				int16_t val = channels[i];
-				warnx("channel %u: %d 0x%03X", i, static_cast<int>(val), static_cast<int>(val));
+				//warnx("channel %u: %d 0x%03X", i, static_cast<int>(val), static_cast<int>(val));
 			}
 		}
 	}

 	if (ret == EOF) {
 		warnx("Test finished, reached end of file");
-
+		ret = 0;
 	} else {
 		warnx("Test aborted, errno: %d", ret);
 	}

+	return ret;
 }
--- a/unittests/testdata/sbus2_r7008SB.txt
+++ b/unittests/testdata/sbus2_r7008SB.txt
--- a/unittests/testdata/st24_data.txt
+++ b/unittests/testdata/st24_data.txt
				`@ -0,0 +1 @@`
				`Subproject commit 6a193648985adab9665e31a9460be04bc91d8965`
				`@ -1 +0,0 @@`
				`Subproject commit 1efd24427539fa332a15151143466ec760fa5fff`