Merge branch 'master' into quad_vtol

155 files changed, 4688 insertions, 1511 deletions

diff --git a/.travis.yml b/.travis.yml
index 3e97545e1..0405ba560 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -3,16 +3,36 @@
 
 language: cpp
 
+# use travis-ci docker based infrastructure
+sudo: false
+
+cache:
+  directories:
+    - $HOME/.ccache
+
+addons:
+  apt:
+    sources:
+    - ubuntu-toolchain-r-test
+    packages:
+    - build-essential
+    - ccache
+    - cmake
+    - g++-4.8
+    - gcc-4.8
+    - genromfs
+    - libc6-i386
+    - libncurses5-dev
+    - python-argparse
+    - python-empy
+    - python-serial
+    - s3cmd
+    - texinfo
+    - zlib1g-dev
+
 before_script:
-  - sudo add-apt-repository --yes ppa:ubuntu-toolchain-r/test
-  - sudo apt-get update -qq
-  - if [ "$CXX" = "g++" ]; then sudo apt-get install -qq g++-4.8 gcc-4.8 libstdc++-4.8-dev; fi
   - if [ "$CXX" = "g++" ]; then export CXX="g++-4.8" CC="gcc-4.8"; fi
-# Travis specific tools
-  - sudo apt-get install -qq s3cmd grep zip
 # General toolchain dependencies
-  - sudo apt-get install -qq libc6-i386 gcc-4.7-base:i386 python-serial python-argparse python-empy
-  - sudo apt-get install -qq flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget cmake
   - pushd .
   - cd ~
   - wget https://launchpadlibrarian.net/186124160/gcc-arm-none-eabi-4_8-2014q3-20140805-linux.tar.bz2
@@ -21,9 +41,13 @@ before_script:
   - if grep -Fxq "$exportline" ~/.profile; then echo nothing to do ; else echo $exportline >> ~/.profile; fi
   - . ~/.profile
   - popd
-
-git:
-  depth: 500
+# setup ccache
+  - mkdir -p ~/bin
+  - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-g++
+  - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-gcc
+  - ln -s /usr/bin/ccache ~/bin/g++-4.8
+  - ln -s /usr/bin/ccache ~/bin/gcc-4.8
+  - export PATH=~/bin:$PATH
 
 env:
   global:
@@ -32,13 +56,9 @@ env:
 # 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
-    - BUILD_URI=https://px4-travis.s3.amazonaws.com/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/Firmware.zip
 
 script:
+  - ccache -z
   - arm-none-eabi-gcc --version
   - echo 'Running Tests..' && echo -en 'travis_fold:start:script.1\\r'
   - make tests
@@ -46,9 +66,11 @@ script:
   - echo -en 'travis_fold:end:script.1\\r'
   - echo 'Building NuttX..' && echo -en 'travis_fold:start:script.2\\r'
   - make archives
+  - ccache -s
   - echo -en 'travis_fold:end:script.2\\r'
   - echo 'Building Firmware..' && echo -en 'travis_fold:start:script.3\\r'
-  - make -j6
+  - make -j4
+  - ccache -s
   - echo -en 'travis_fold:end:script.3\\r'
   - zip Firmware.zip Images/*.px4
 
@@ -84,4 +106,3 @@ notifications:
       - 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
diff --git a/Documentation/rc_mode_switch.odg b/Documentation/rc_mode_switch.odg
index 682c63e47..0a4f532eb 100644
--- a/Documentation/rc_mode_switch.odg
+++ b/Documentation/rc_mode_switch.odg
diff --git a/Documentation/rc_mode_switch.pdf b/Documentation/rc_mode_switch.pdf
index e795f4870..cbcc59f72 100644
--- a/Documentation/rc_mode_switch.pdf
+++ b/Documentation/rc_mode_switch.pdf
diff --git a/Images/px4-stm32f4discovery.prototype b/Images/px4-stm32f4discovery.prototype
new file mode 100644
index 000000000..9250691f7
--- /dev/null
+++ b/Images/px4-stm32f4discovery.prototype
@@ -0,0 +1,12 @@
+{
+    "board_id": 99, 
+    "magic": "PX4FWv1", 
+    "description": "Firmware for the STM32F4Discovery board", 
+    "image": "", 
+    "build_time": 0, 
+    "summary": "PX4/STM32F4Discovery",
+    "version": "0.1",
+    "image_size": 0,
+    "git_identity": "",
+    "board_revision": 0
+}
diff --git a/NuttX b/NuttX
-Subproject 94818ce16f63c4728a1d9316628a3651287f16d
+Subproject 678eea3d2c157c686439597abb0367b0f1e643f
diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6
index 551a19928..191c58da4 100644
--- a/ROMFS/px4fmu_common/init.d/13002_firefly6
+++ b/ROMFS/px4fmu_common/init.d/13002_firefly6
@@ -8,7 +8,16 @@
 sh /etc/init.d/rc.vtol_defaults
 
 set MIXER firefly6
-
 set PWM_OUT 12345678
+
+set MIXER_AUX firefly6
+set PWM_AUX_RATE 50
+set PWM_AUX_OUT 1234
+set PWM_AUX_DISARMED 1000
+set PWM_AUX_MIN 1000
+set PWM_AUX_MAX 2000
+
+set MAV_TYPE 21
+
 param set VT_MOT_COUNT 6
 param set VT_IDLE_PWM_MC 1080
diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface
index 5101acc07..7a424970f 100644
--- a/ROMFS/px4fmu_common/init.d/rc.interface
+++ b/ROMFS/px4fmu_common/init.d/rc.interface
@@ -45,7 +45,7 @@ then
 
 	if mixer load $OUTPUT_DEV $MIXER_FILE
 	then
-		echo "[i] Mixer: $MIXER_FILE"
+		echo "[i] Mixer: $MIXER_FILE on $OUTPUT_DEV"
 	else
 		echo "[i] Error loading mixer: $MIXER_FILE"
 		tone_alarm $TUNE_ERR
@@ -105,14 +105,14 @@ then
 	set MIXER_AUX_FILE none
 	set OUTPUT_AUX_DEV /dev/pwm_output1
 
-	if [ -f $SDCARD_MIXERS_PATH/$MIXER_AUX.mix ]
+	if [ -f $SDCARD_MIXERS_PATH/$MIXER_AUX.aux.mix ]
 	then
-		set MIXER_AUX_FILE $SDCARD_MIXERS_PATH/$MIXER_AUX.mix
+		set MIXER_AUX_FILE $SDCARD_MIXERS_PATH/$MIXER_AUX.aux.mix
 	else
 
-		if [ -f /etc/mixers/$MIXER_AUX.mix ]
+		if [ -f /etc/mixers/$MIXER_AUX.aux.mix ]
 		then
-			set MIXER_AUX_FILE /etc/mixers/$MIXER_AUX.mix
+			set MIXER_AUX_FILE /etc/mixers/$MIXER_AUX.aux.mix
 		fi
 	fi
 
@@ -120,7 +120,12 @@ then
 	then
 		if fmu mode_pwm
 		then
-			mixer load $OUTPUT_AUX_DEV $MIXER_AUX_FILE
+			if mixer load $OUTPUT_AUX_DEV $MIXER_AUX_FILE
+			then
+				echo "[i] Mixer: $MIXER_AUX_FILE on $OUTPUT_AUX_DEV"
+			else
+				echo "[i] Error loading mixer: $MIXER_AUX_FILE"
+			fi
 		else
 			tone_alarm $TUNE_ERR
 		fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io
index e957626ce..86042220e 100644
--- a/ROMFS/px4fmu_common/init.d/rc.io
+++ b/ROMFS/px4fmu_common/init.d/rc.io
@@ -16,5 +16,8 @@ then
 	set PX4IO_LIMIT 200
 fi
 
-echo "[i] Set PX4IO update rate limit: $PX4IO_LIMIT Hz"
-px4io limit $PX4IO_LIMIT
+if px4io limit $PX4IO_LIMIT
+then
+else
+	echo "[i] Set PX4IO update rate to $PX4IO_LIMIT Hz failed!"
+fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging
index a14eb5247..454af8da7 100644
--- a/ROMFS/px4fmu_common/init.d/rc.logging
+++ b/ROMFS/px4fmu_common/init.d/rc.logging
@@ -7,10 +7,12 @@ if [ -d /fs/microsd ]
 then
 	if ver hwcmp PX4FMU_V1
 	then
-		echo "Start sdlog2 at 50Hz"
-		sdlog2 start -r 40 -a -b 3 -t
+		if sdlog2 start -r 40 -a -b 3 -t
+		then
+		fi
 	else
-		echo "Start sdlog2 at 200Hz"
-		sdlog2 start -r 200 -a -b 22 -t
+		if sdlog2 start -r 200 -a -b 22 -t
+		then
+		fi
 	fi
 fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index ffded7d6b..b8a704b90 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -115,9 +115,7 @@ then
 fi
 
 #
-# Start sensors -> preflight_check
+# Start sensors
 #
-if sensors start
-then
-	preflight_check &
-fi
+sensors start
+
diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb
index fddcf5b3e..e456eff7f 100644
--- a/ROMFS/px4fmu_common/init.d/rc.usb
+++ b/ROMFS/px4fmu_common/init.d/rc.usb
@@ -3,7 +3,7 @@
 # USB MAVLink start
 #
 
-mavlink start -r 20000 -d /dev/ttyACM0 -x
+mavlink start -r 30000 -d /dev/ttyACM0 -x
 # Enable a number of interesting streams we want via USB
 mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 200
 mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 0a8774b30..1237d9bb1 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -24,7 +24,6 @@ set LOG_FILE /fs/microsd/bootlog.txt
 # Try to mount the microSD card.
 #
 # REBOOTWORK this needs to start after the flight control loop
-echo "[i] Looking for microSD..."
 if mount -t vfat /dev/mmcsd0 /fs/microsd
 then
 	echo "[i] microSD mounted: /fs/microsd"
@@ -42,6 +41,8 @@ else
 			tone_alarm MNBG
 			set LOG_FILE /dev/null
 		fi
+	else
+		set LOG_FILE /dev/null
 	fi
 fi
 
@@ -51,11 +52,9 @@ fi
 #
 if [ -f $FRC ]
 then
-	echo "[i] Executing init script: $FRC"
+	echo "[i] Executing script: $FRC"
 	sh $FRC
 	set MODE custom
-else
-	echo "[i] Init script not found: $FRC"
 fi
 unset FRC
 
@@ -124,6 +123,7 @@ then
 	set PWM_AUX_DISARMED none
 	set PWM_AUX_MIN none
 	set PWM_AUX_MAX none
+	set FAILSAFE_AUX none
 	set MK_MODE none
 	set FMU_MODE pwm
 	set MAVLINK_F default
@@ -172,10 +172,8 @@ then
 	#
 	if [ -f $FCONFIG ]
 	then
-		echo "[i] Config: $FCONFIG"
+		echo "[i] Custom config: $FCONFIG"
 		sh $FCONFIG
-	else
-		echo "[i] Config not found: $FCONFIG"
 	fi
 	unset FCONFIG
 
@@ -299,6 +297,11 @@ then
 	then
 	fi
 
+	#
+	# Sensors System (start before Commander so Preflight checks are properly run)
+	#
+	sh /etc/init.d/rc.sensors
+
 	# Needs to be this early for in-air-restarts
 	commander start
 
@@ -323,10 +326,8 @@ then
 		then
 			if px4io start
 			then
-				echo "[i] PX4IO started"
 				sh /etc/init.d/rc.io
 			else
-				echo "[i] ERROR: PX4IO start failed"
 				tone_alarm $TUNE_ERR
 			fi
 		fi
@@ -454,10 +455,16 @@ then
 	#
 	if ver hwcmp PX4FMU_V2
 	then
+		# XXX We need a better way for runtime eval of shell variables,
+		# but this works for now
 		if param compare SYS_COMPANION 921600
 		then
 			mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 20000
 		fi
+		if param compare SYS_COMPANION 57600
+		then
+			mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 1000
+		fi
 	fi
 
 	# UAVCAN
@@ -465,9 +472,8 @@ then
 	sh /etc/init.d/rc.uavcan
 
 	#
-	# Sensors, Logging, GPS
+	# Logging, GPS
 	#
-	sh /etc/init.d/rc.sensors
 	sh /etc/init.d/rc.logging
 
 	if [ $GPS == yes ]
@@ -603,7 +609,7 @@ then
 			then
 				set MAV_TYPE 19
 			fi
-			if [ $MIXER == firefly6_rotors ]
+			if [ $MIXER == firefly6 ]
 			then
 				set MAV_TYPE 21
 			fi
@@ -669,8 +675,6 @@ then
 	then
 		echo "[i] Addons script: $FEXTRAS"
 		sh $FEXTRAS
-	else
-		echo "[i] No addons script: $FEXTRAS"
 	fi
 	unset FEXTRAS
 
diff --git a/ROMFS/px4fmu_common/mixers/firefly6.aux.mix b/ROMFS/px4fmu_common/mixers/firefly6.aux.mix
index 9ed6eeed9..fda841640 100644
--- a/ROMFS/px4fmu_common/mixers/firefly6.aux.mix
+++ b/ROMFS/px4fmu_common/mixers/firefly6.aux.mix
@@ -1,4 +1,14 @@
-# mixer for the FireFly6 elevons
+# mixer for the FireFly6 tilt mechansim servo, elevons and landing gear
+=======================================================================
+
+Tilt mechanism servo mixer
+---------------------------
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 1 4  10000  10000      0 -10000  10000
+
+Elevon mixers
+-------------
 M: 2
 O:      10000  10000      0 -10000  10000
 S: 1 0   7500   7500      0 -10000  10000
@@ -8,3 +18,9 @@ M: 2
 O:       10000   10000    0 -10000  10000
 S: 1 0    7500    7500    0 -10000  10000
 S: 1 1   -8000   -8000    0 -10000  10000
+
+Landing gear mixer
+------------------
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 3 6  10000  10000      0 -10000  10000
diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/dokuwikiout.py
index 77e0ef53d..28e487ea6 100644
--- a/Tools/px4params/dokuwikiout.py
+++ b/Tools/px4params/dokuwikiout.py
@@ -12,11 +12,11 @@ class DokuWikiTablesOutput():
             result += "^ Name ^ Description ^  Min ^  Max ^  Default ^\n"
             result += "^ :::  ^ Comment ^^^^\n"
             for param in group.GetParams():
-                code = param.GetFieldValue("code")
+                code = param.GetName()
+                def_val = param.GetDefault()
                 name = param.GetFieldValue("short_desc")
                 min_val = param.GetFieldValue("min")
                 max_val = param.GetFieldValue("max")
-                def_val = param.GetFieldValue("default")
                 long_desc = param.GetFieldValue("long_desc")
 
                 if name == code:
diff --git a/Tools/px4params/srcparser.py b/Tools/px4params/srcparser.py
index 8e6092195..0d2413a75 100644
--- a/Tools/px4params/srcparser.py
+++ b/Tools/px4params/srcparser.py
@@ -37,20 +37,30 @@ class Parameter(object):
 
     # Define sorting order of the fields
     priority = {
-        "code": 10,
-        "type": 9,
+		"board": 9,
         "short_desc": 8,
         "long_desc": 7,
-        "default": 6,
         "min": 5,
         "max": 4,
         "unit": 3,
         # all others == 0 (sorted alphabetically)
     }
 
-    def __init__(self):
+    def __init__(self, name, type, default = ""):
         self.fields = {}
+        self.name = name
+        self.type = type
+        self.default = default
 
+    def GetName(self):
+        return self.name
+
+    def GetType(self):
+        return self.type
+    
+    def GetDefault(self):
+        return self.default
+    
     def SetField(self, code, value):
         """
         Set named field value
@@ -83,11 +93,12 @@ class SourceParser(object):
     re_comment_tag = re.compile(r'@([a-zA-Z][a-zA-Z0-9_]*)\s*(.*)')
     re_comment_end = re.compile(r'(.*?)\s*\*\/')
     re_parameter_definition = re.compile(r'PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*,\s*([^ ,\)]+)\s*\)\s*;')
+    re_px4_parameter_definition = re.compile(r'PX4_PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*\)\s*;')
     re_cut_type_specifier = re.compile(r'[a-z]+$')
     re_is_a_number = re.compile(r'^-?[0-9\.]')
     re_remove_dots = re.compile(r'\.+$')
 
-    valid_tags = set(["group", "min", "max", "unit"])
+    valid_tags = set(["group", "board", "min", "max", "unit"])
 
     # Order of parameter groups
     priority = {
@@ -176,15 +187,12 @@ class SourceParser(object):
                 # Non-empty line outside the comment
                 m = self.re_parameter_definition.match(line)
                 if m:
-                    tp, code, defval = m.group(1, 2, 3)
+                    tp, name, defval = m.group(1, 2, 3)
                     # Remove trailing type specifier from numbers: 0.1f => 0.1
                     if self.re_is_a_number.match(defval):
                         defval = self.re_cut_type_specifier.sub('', defval)
-                    param = Parameter()
-                    param.SetField("code", code)
-                    param.SetField("short_desc", code)
-                    param.SetField("type", tp)
-                    param.SetField("default", defval)
+                    param = Parameter(name, tp, defval)
+                    param.SetField("short_desc", name)
                     # If comment was found before the parameter declaration,
                     # inject its data into the newly created parameter.
                     group = "Miscellaneous"
@@ -206,8 +214,36 @@ class SourceParser(object):
                     if group not in self.param_groups:
                         self.param_groups[group] = ParameterGroup(group)
                     self.param_groups[group].AddParameter(param)
-                # Reset parsed comment.
-                state = None
+                else:
+                    # Nasty code dup, but this will all go away soon, so quick and dirty (DonLakeFlyer)
+                    m = self.re_px4_parameter_definition.match(line)
+                    if m:
+                        tp, name = m.group(1, 2)
+                        param = Parameter(name, tp)
+                        param.SetField("short_desc", name)
+                        # If comment was found before the parameter declaration,
+                        # inject its data into the newly created parameter.
+                        group = "Miscellaneous"
+                        if state == "comment-processed":
+                            if short_desc is not None:
+                                param.SetField("short_desc",
+                                   self.re_remove_dots.sub('', short_desc))
+                            if long_desc is not None:
+                                param.SetField("long_desc", long_desc)
+                            for tag in tags:
+                                if tag == "group":
+                                    group = tags[tag]
+                                elif tag not in self.valid_tags:
+                                    sys.stderr.write("Skipping invalid "
+                                             "documentation tag: '%s'\n" % tag)
+                                else:
+                                    param.SetField(tag, tags[tag])
+                        # Store the parameter
+                        if group not in self.param_groups:
+                            self.param_groups[group] = ParameterGroup(group)
+                        self.param_groups[group].AddParameter(param)
+                    # Reset parsed comment.
+                    state = None
 
     def GetParamGroups(self):
         """
diff --git a/Tools/px4params/xmlout.py b/Tools/px4params/xmlout.py
index e845cd1b1..07cced478 100644
--- a/Tools/px4params/xmlout.py
+++ b/Tools/px4params/xmlout.py
@@ -1,26 +1,56 @@
-from xml.dom.minidom import getDOMImplementation
+import xml.etree.ElementTree as ET
 import codecs
 
+def indent(elem, level=0):
+    i = "\n" + level*"  "
+    if len(elem):
+        if not elem.text or not elem.text.strip():
+            elem.text = i + "  "
+        if not elem.tail or not elem.tail.strip():
+            elem.tail = i
+        for elem in elem:
+            indent(elem, level+1)
+        if not elem.tail or not elem.tail.strip():
+            elem.tail = i
+    else:
+        if level and (not elem.tail or not elem.tail.strip()):
+            elem.tail = i
+
 class XMLOutput():
-    def __init__(self, groups):
-        impl = getDOMImplementation()
-        xml_document = impl.createDocument(None, "parameters", None)
-        xml_parameters = xml_document.documentElement
+
+    def __init__(self, groups, board):
+        xml_parameters = ET.Element("parameters")
+        xml_version = ET.SubElement(xml_parameters, "version")
+        xml_version.text = "3"
+        last_param_name = ""
+        board_specific_param_set = False
         for group in groups:
-            xml_group = xml_document.createElement("group")
-            xml_group.setAttribute("name", group.GetName())
-            xml_parameters.appendChild(xml_group)
+            xml_group = ET.SubElement(xml_parameters, "group")
+            xml_group.attrib["name"] = group.GetName()
             for param in group.GetParams():
-                xml_param = xml_document.createElement("parameter")
-                xml_group.appendChild(xml_param)
-                for code in param.GetFieldCodes():
-                    value = param.GetFieldValue(code)
-                    xml_field = xml_document.createElement(code)
-                    xml_param.appendChild(xml_field)
-                    xml_value = xml_document.createTextNode(value)
-                    xml_field.appendChild(xml_value)
-        self.xml_document = xml_document
+                if (last_param_name == param.GetName() and not board_specific_param_set) or last_param_name != param.GetName():
+                    xml_param = ET.SubElement(xml_group, "parameter")
+                    xml_param.attrib["name"] = param.GetName()
+                    xml_param.attrib["default"] = param.GetDefault()
+                    xml_param.attrib["type"] = param.GetType()
+                    last_param_name = param.GetName()
+                    for code in param.GetFieldCodes():
+                        value = param.GetFieldValue(code)
+                        if code == "board":
+                            if value == board:
+                                board_specific_param_set = True
+                                xml_field = ET.SubElement(xml_param, code)
+                                xml_field.text = value
+                            else:
+                                xml_group.remove(xml_param)
+                        else:
+                            xml_field = ET.SubElement(xml_param, code)
+                            xml_field.text = value
+                if last_param_name != param.GetName():
+                    board_specific_param_set = False
+        indent(xml_parameters)
+        self.xml_document = ET.ElementTree(xml_parameters)
 
     def Save(self, filename):
         with codecs.open(filename, 'w', 'utf-8') as f:
-            self.xml_document.writexml(f, indent="    ", addindent="    ", newl="\n")
+            self.xml_document.write(f)
diff --git a/Tools/px_mkfw.py b/Tools/px_mkfw.py
index c2da8a203..152444f84 100755
--- a/Tools/px_mkfw.py
+++ b/Tools/px_mkfw.py
@@ -105,6 +105,7 @@ if args.git_identity != None:
 if args.parameter_xml != None:
 	f = open(args.parameter_xml, "rb")
 	bytes = f.read()
+	desc['parameter_xml_size'] = len(bytes)
 	desc['parameter_xml'] = base64.b64encode(zlib.compress(bytes,9)).decode('utf-8')
 if args.image != None:
 	f = open(args.image, "rb")
diff --git a/Tools/px_process_params.py b/Tools/px_process_params.py
index 12128a997..cb2202d52 100644
--- a/Tools/px_process_params.py
+++ b/Tools/px_process_params.py
@@ -65,6 +65,11 @@ def main():
                         metavar="FILENAME",
                         help="Create XML file"
                              " (default FILENAME: parameters.xml)")
+    parser.add_argument("-b", "--board",
+                         nargs='?',
+                         const="",
+                         metavar="BOARD",
+                         help="Board to create xml parameter xml for")
     parser.add_argument("-w", "--wiki",
                         nargs='?',
                         const="parameters.wiki",
@@ -116,7 +121,7 @@ def main():
     # Output to XML file
     if args.xml:
         print("Creating XML file " + args.xml)
-        out = xmlout.XMLOutput(param_groups)
+        out = xmlout.XMLOutput(param_groups, args.board)
         out.Save(args.xml)
 
     # Output to DokuWiki tables
diff --git a/makefiles/board_px4-stm32f4discovery.mk b/makefiles/board_px4-stm32f4discovery.mk
new file mode 100644
index 000000000..fe761ba44
--- /dev/null
+++ b/makefiles/board_px4-stm32f4discovery.mk
@@ -0,0 +1,11 @@
+#
+# Board-specific definitions for the PX4_STM32F4DISCOVERY
+#
+
+#
+# Configure the toolchain
+#
+CONFIG_ARCH			 = CORTEXM4F
+CONFIG_BOARD			 = PX4_STM32F4DISCOVERY
+
+include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk
diff --git a/makefiles/config_aerocore_default.mk b/makefiles/config_aerocore_default.mk
index 9ab63a973..c906d5418 100644
--- a/makefiles/config_aerocore_default.mk
+++ b/makefiles/config_aerocore_default.mk
@@ -31,7 +31,6 @@ MODULES		+= systemcmds/ver
 MODULES		+= systemcmds/mixer
 MODULES		+= systemcmds/param
 MODULES		+= systemcmds/perf
-MODULES		+= systemcmds/preflight_check
 MODULES		+= systemcmds/pwm
 MODULES		+= systemcmds/esc_calib
 MODULES		+= systemcmds/reboot
diff --git a/makefiles/config_px4-stm32f4discovery_default.mk b/makefiles/config_px4-stm32f4discovery_default.mk
new file mode 100644
index 000000000..8f73a7f04
--- /dev/null
+++ b/makefiles/config_px4-stm32f4discovery_default.mk
@@ -0,0 +1,92 @@
+#
+# Makefile for the px4fmu_default configuration
+#
+
+#
+# Use the configuration's ROMFS, copy the PX4_STM32F4DISCOVERY firmware into
+# the ROMFS if it's available
+#
+ROMFS_ROOT	 = $(PX4_BASE)/ROMFS/px4fmu_common
+ROMFS_OPTIONAL_FILES = 
+
+#
+# Board support modules
+#
+MODULES		+= drivers/device
+MODULES		+= drivers/stm32
+MODULES		+= drivers/led
+MODULES		+= drivers/boards/px4-stm32f4discovery
+
+#
+# System commands
+#
+MODULES		+= systemcmds/bl_update
+MODULES		+= systemcmds/mixer
+MODULES		+= systemcmds/param
+MODULES		+= systemcmds/perf
+MODULES		+= systemcmds/reboot
+MODULES		+= systemcmds/top
+MODULES		+= systemcmds/tests
+MODULES		+= systemcmds/config
+MODULES		+= systemcmds/nshterm
+MODULES		+= systemcmds/ver
+
+#
+# Library modules
+#
+MODULES		+= modules/systemlib
+MODULES		+= modules/systemlib/mixer
+MODULES		+= modules/controllib
+MODULES		+= modules/uORB
+
+#
+# Libraries
+#
+LIBRARIES	+= lib/mathlib/CMSIS
+MODULES		+= lib/mathlib
+MODULES		+= lib/mathlib/math/filter
+MODULES		+= lib/ecl
+MODULES		+= lib/external_lgpl
+MODULES		+= lib/geo
+MODULES		+= lib/conversion
+MODULES		+= platforms/nuttx
+
+#
+# Demo apps
+#
+#MODULES		+= examples/math_demo
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/hello_sky
+MODULES		+= examples/px4_simple_app
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/daemon
+#MODULES		+= examples/px4_daemon_app
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/debug_values
+#MODULES		+= examples/px4_mavlink_debug
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control
+#MODULES			+= examples/fixedwing_control
+
+# Hardware test
+#MODULES			+= examples/hwtest
+
+#
+# Transitional support - add commands from the NuttX export archive.
+#
+# In general, these should move to modules over time.
+#
+# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro
+# to make the table a bit more readable.
+#
+define _B
+	$(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4)
+endef
+
+#                  command                 priority                   stack  entrypoint
+BUILTIN_COMMANDS := \
+	$(call _B, sercon,                 ,                          2048,  sercon_main                ) \
+	$(call _B, serdis,                 ,                          2048,  serdis_main                )
diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index e3515a927..55b73ffde 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -45,7 +45,6 @@ MODULES		+= systemcmds/mtd
 MODULES		+= systemcmds/mixer
 MODULES		+= systemcmds/param
 MODULES		+= systemcmds/perf
-MODULES		+= systemcmds/preflight_check
 MODULES		+= systemcmds/pwm
 MODULES		+= systemcmds/esc_calib
 MODULES		+= systemcmds/reboot
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index 324e12696..cbfda9735 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -53,7 +53,6 @@ MODULES		+= systemcmds/bl_update
 MODULES		+= systemcmds/mixer
 MODULES		+= systemcmds/param
 MODULES		+= systemcmds/perf
-MODULES		+= systemcmds/preflight_check
 MODULES		+= systemcmds/pwm
 MODULES		+= systemcmds/esc_calib
 MODULES		+= systemcmds/reboot
diff --git a/makefiles/config_px4fmu-v2_multiplatform.mk b/makefiles/config_px4fmu-v2_multiplatform.mk
index 568f940ee..b98532f71 100644
--- a/makefiles/config_px4fmu-v2_multiplatform.mk
+++ b/makefiles/config_px4fmu-v2_multiplatform.mk
@@ -51,7 +51,6 @@ MODULES		+= systemcmds/bl_update
 MODULES		+= systemcmds/mixer
 MODULES		+= systemcmds/param
 MODULES		+= systemcmds/perf
-MODULES		+= systemcmds/preflight_check
 MODULES		+= systemcmds/pwm
 MODULES		+= systemcmds/esc_calib
 MODULES		+= systemcmds/reboot
diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk
index 0d04a1f19..154f7e5ba 100644
--- a/makefiles/config_px4fmu-v2_test.mk
+++ b/makefiles/config_px4fmu-v2_test.mk
@@ -45,7 +45,6 @@ MODULES		+= systemcmds/bl_update
 MODULES		+= systemcmds/mixer
 MODULES		+= systemcmds/param
 MODULES		+= systemcmds/perf
-MODULES		+= systemcmds/preflight_check
 MODULES		+= systemcmds/pwm
 MODULES		+= systemcmds/esc_calib
 MODULES		+= systemcmds/reboot
diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk
index 21e8739aa..af3ca249e 100644
--- a/makefiles/firmware.mk
+++ b/makefiles/firmware.mk
@@ -181,11 +181,6 @@ EXTRA_CLEANS		 =
 
 
 #
-# Extra defines for compilation
-#
-export EXTRADEFINES := -DGIT_VERSION=$(GIT_DESC)
-
-#
 # Append the per-board driver directory to the header search path.
 #
 INCLUDE_DIRS		+= $(PX4_MODULE_SRC)drivers/boards/$(BOARD)
@@ -499,7 +494,7 @@ $(filter %.S.o,$(OBJS)): $(WORK_DIR)%.S.o: %.S $(GLOBAL_DEPS)
 $(PRODUCT_BUNDLE):	$(PRODUCT_BIN)
 	@$(ECHO) %% Generating $@
 ifdef GEN_PARAM_XML
-	python $(PX4_BASE)/Tools/px_process_params.py --src-path $(PX4_BASE)/src --xml
+	python $(PX4_BASE)/Tools/px_process_params.py --src-path $(PX4_BASE)/src --board CONFIG_ARCH_BOARD_$(CONFIG_BOARD) --xml
 	$(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \
 		--git_identity $(PX4_BASE) \
 		--parameter_xml $(PRODUCT_PARAMXML) \
diff --git a/makefiles/upload.mk b/makefiles/upload.mk
index 29b415688..dd7710bf7 100644
--- a/makefiles/upload.mk
+++ b/makefiles/upload.mk
@@ -33,7 +33,8 @@ upload-serial-px4fmu-v2:	$(BUNDLE) $(UPLOADER)
 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'
 
-
+upload-serial-px4-stm32f4discovery:	$(BUNDLE) $(UPLOADER)
+	$(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE)
 
 #
 # JTAG firmware uploading with OpenOCD
diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0
-Subproject 7ae438b86ea983e95af5f092e45a5d0f9d093c7
+Subproject 475e7cc4348b207ac3efed45eb61160d23ac7a2
diff --git a/msg/mc_att_ctrl_status.msg b/msg/mc_att_ctrl_status.msg
new file mode 100644
index 000000000..114e843b0
--- /dev/null
+++ b/msg/mc_att_ctrl_status.msg
@@ -0,0 +1,5 @@
+uint64 timestamp    # in microseconds since system start
+
+float32 roll_rate_integ	    # roll rate inegrator
+float32 pitch_rate_integ	# pitch rate integrator
+float32 yaw_rate_integ	    # yaw rate integrator
diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg
index 2e001cac7..2a7e493a3 100644
--- a/msg/vehicle_status.msg
+++ b/msg/vehicle_status.msg
@@ -72,8 +72,10 @@ uint8 VEHICLE_TYPE_FLAPPING_WING = 16	    # Flapping wing
 uint8 VEHICLE_TYPE_KITE = 17		    # Kite
 uint8 VEHICLE_TYPE_ONBOARD_CONTROLLER=18    # Onboard companion controller
 uint8 VEHICLE_TYPE_VTOL_DUOROTOR = 19	    # Vtol with two engines
-uint8 VEHICLE_TYPE_VTOL_QUADROTOR = 20	    # Vtol with four engines*/
-uint8 VEHICLE_TYPE_ENUM_END = 21
+uint8 VEHICLE_TYPE_VTOL_QUADROTOR = 20	    # Vtol with four engines
+uint8 VEHICLE_TYPE_VTOL_HEXAROTOR = 21 		# Vtol with six engines
+uint8 VEHICLE_TYPE_VTOL_OCTOROTOR = 22		# Vtol with eight engines
+uint8 VEHICLE_TYPE_ENUM_END = 23
 
 uint8 VEHICLE_BATTERY_WARNING_NONE = 0	    # no battery low voltage warning active
 uint8 VEHICLE_BATTERY_WARNING_LOW = 1	    # warning of low voltage
diff --git a/nuttx-configs/aerocore/nsh/defconfig b/nuttx-configs/aerocore/nsh/defconfig
index 08ff4a988..e314bf1d8 100644
--- a/nuttx-configs/aerocore/nsh/defconfig
+++ b/nuttx-configs/aerocore/nsh/defconfig
@@ -403,7 +403,7 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=36
+CONFIG_NFILE_DESCRIPTORS=42
 CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4
diff --git a/nuttx-configs/px4-stm32f4discovery/include/board.h b/nuttx-configs/px4-stm32f4discovery/include/board.h
new file mode 100644
index 000000000..0412c3aa6
--- /dev/null
+++ b/nuttx-configs/px4-stm32f4discovery/include/board.h
@@ -0,0 +1,273 @@
+/************************************************************************************
+ * configs/stm32f4discovery/include/board.h
+ * include/arch/board/board.h
+ *
+ *   Copyright (C) 2012 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ************************************************************************************/
+
+#ifndef __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H
+#define __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#ifndef __ASSEMBLY__
+# include <stdint.h>
+#endif
+
+#include "stm32_rcc.h"
+#include "stm32_sdio.h"
+#include "stm32.h"
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/* Clocking *************************************************************************/
+/* The STM32F4 Discovery board features a single 8MHz crystal.  Space is provided
+ * for a 32kHz RTC backup crystal, but it is not stuffed.
+ *
+ * This is the canonical configuration:
+ *   System Clock source           : PLL (HSE)
+ *   SYSCLK(Hz)                    : 168000000    Determined by PLL configuration
+ *   HCLK(Hz)                      : 168000000    (STM32_RCC_CFGR_HPRE)
+ *   AHB Prescaler                 : 1            (STM32_RCC_CFGR_HPRE)
+ *   APB1 Prescaler                : 4            (STM32_RCC_CFGR_PPRE1)
+ *   APB2 Prescaler                : 2            (STM32_RCC_CFGR_PPRE2)
+ *   HSE Frequency(Hz)             : 8000000      (STM32_BOARD_XTAL)
+ *   PLLM                          : 8            (STM32_PLLCFG_PLLM)
+ *   PLLN                          : 336          (STM32_PLLCFG_PLLN)
+ *   PLLP                          : 2            (STM32_PLLCFG_PLLP)
+ *   PLLQ                          : 7            (STM32_PLLCFG_PLLQ)
+ *   Main regulator output voltage : Scale1 mode  Needed for high speed SYSCLK
+ *   Flash Latency(WS)             : 5
+ *   Prefetch Buffer               : OFF
+ *   Instruction cache             : ON
+ *   Data cache                    : ON
+ *   Require 48MHz for USB OTG FS, : Enabled
+ *   SDIO and RNG clock
+ */
+
+/* HSI - 16 MHz RC factory-trimmed
+ * LSI - 32 KHz RC
+ * HSE - On-board crystal frequency is 8MHz
+ * LSE - 32.768 kHz
+ */
+
+#define STM32_BOARD_XTAL        8000000ul
+
+#define STM32_HSI_FREQUENCY     16000000ul
+#define STM32_LSI_FREQUENCY     32000
+#define STM32_HSE_FREQUENCY     STM32_BOARD_XTAL
+#define STM32_LSE_FREQUENCY     32768
+
+/* Main PLL Configuration.
+ *
+ * PLL source is HSE
+ * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN
+ *         = (8,000,000 / 8) * 336
+ *         = 336,000,000
+ * SYSCLK  = PLL_VCO / PLLP
+ *         = 336,000,000 / 2 = 168,000,000
+ * USB OTG FS, SDIO and RNG Clock
+ *         =  PLL_VCO / PLLQ
+ *         = 48,000,000
+ */
+
+#define STM32_PLLCFG_PLLM       RCC_PLLCFG_PLLM(8)
+#define STM32_PLLCFG_PLLN       RCC_PLLCFG_PLLN(336)
+#define STM32_PLLCFG_PLLP       RCC_PLLCFG_PLLP_2
+#define STM32_PLLCFG_PLLQ       RCC_PLLCFG_PLLQ(7)
+
+#define STM32_SYSCLK_FREQUENCY  168000000ul
+
+/* AHB clock (HCLK) is SYSCLK (168MHz) */
+
+#define STM32_RCC_CFGR_HPRE     RCC_CFGR_HPRE_SYSCLK  /* HCLK  = SYSCLK / 1 */
+#define STM32_HCLK_FREQUENCY    STM32_SYSCLK_FREQUENCY
+#define STM32_BOARD_HCLK        STM32_HCLK_FREQUENCY  /* same as above, to satisfy compiler */
+
+/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */
+
+#define STM32_RCC_CFGR_PPRE1    RCC_CFGR_PPRE1_HCLKd4     /* PCLK1 = HCLK / 4 */
+#define STM32_PCLK1_FREQUENCY   (STM32_HCLK_FREQUENCY/4)
+
+/* Timers driven from APB1 will be twice PCLK1 */
+
+#define STM32_APB1_TIM2_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM3_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM4_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM5_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM6_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM7_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM12_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM13_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM14_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+
+/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */
+
+#define STM32_RCC_CFGR_PPRE2    RCC_CFGR_PPRE2_HCLKd2     /* PCLK2 = HCLK / 2 */
+#define STM32_PCLK2_FREQUENCY   (STM32_HCLK_FREQUENCY/2)
+
+/* Timers driven from APB2 will be twice PCLK2 */
+
+#define STM32_APB2_TIM1_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM8_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM9_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM10_CLKIN  (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM11_CLKIN  (2*STM32_PCLK2_FREQUENCY)
+
+/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx
+ * otherwise frequency is 2xAPBx.
+ * Note: TIM1,8 are on APB2, others on APB1
+ */
+
+#define STM32_TIM18_FREQUENCY   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_TIM27_FREQUENCY   (2*STM32_PCLK1_FREQUENCY)
+
+/* LED definitions ******************************************************************/
+/* If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any
+ * way.  The following definitions are used to access individual LEDs.
+ */
+
+/* LED index values for use with stm32_setled() */
+
+#define BOARD_LED1        0
+#define BOARD_LED2        1
+#define BOARD_NLEDS       2
+
+#define BOARD_LED_BLUE    BOARD_LED1
+#define BOARD_LED_RED     BOARD_LED2
+
+/* LED bits for use with stm32_setleds() */
+
+#define BOARD_LED1_BIT    (1 << BOARD_LED1)
+#define BOARD_LED2_BIT    (1 << BOARD_LED2)
+
+/* If CONFIG_ARCH_LEDs is defined, then NuttX will control the 2 LEDs on board the
+ * px4fmu-v1.  The following definitions describe how NuttX controls the LEDs:
+ */
+
+#define LED_STARTED       0  /* LED1 */
+#define LED_HEAPALLOCATE  1  /* LED2 */
+#define LED_IRQSENABLED   2  /* LED1 */
+#define LED_STACKCREATED  3  /* LED1 + LED2 */
+#define LED_INIRQ         4  /* LED1 */
+#define LED_SIGNAL        5  /* LED2 */
+#define LED_ASSERTION     6  /* LED1 + LED2 */
+#define LED_PANIC         7  /* LED1 + LED2 */
+
+/* Alternate function pin selections ************************************************/
+
+/* UART2:
+ *
+ * The STM32F4 Discovery has no on-board serial devices, but the console is
+ * brought out to PA2 (TX) and PA3 (RX) for connection to an external serial device.
+ * (See the README.txt file for other options)
+ */
+
+#define GPIO_USART2_RX GPIO_USART2_RX_1
+#define GPIO_USART2_TX GPIO_USART2_TX_1
+ 
+/* SPI - There is a MEMS device on SPI1 using these pins: */
+
+#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1
+#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1
+#define GPIO_SPI1_SCK  GPIO_SPI1_SCK_1
+
+/*
+ * I2C
+ *
+ * The optional _GPIO configurations allow the I2C driver to manually
+ * reset the bus to clear stuck slaves.  They match the pin configuration,
+ * but are normally-high GPIOs.
+ */
+#define GPIO_I2C1_SCL		GPIO_I2C1_SCL_1
+#define GPIO_I2C1_SDA		GPIO_I2C1_SDA_2
+#define GPIO_I2C1_SCL_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN6)
+#define GPIO_I2C1_SDA_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9)
+
+/************************************************************************************
+ * Public Data
+ ************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+#undef EXTERN
+#if defined(__cplusplus)
+#define EXTERN extern "C"
+extern "C" {
+#else
+#define EXTERN extern
+#endif
+
+/************************************************************************************
+ * Public Function Prototypes
+ ************************************************************************************/
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+EXTERN void stm32_boardinitialize(void);
+
+/************************************************************************************
+ * Name:  stm32_ledinit, stm32_setled, and stm32_setleds
+ *
+ * Description:
+ *   If CONFIG_ARCH_LEDS is defined, then NuttX will control the on-board LEDs.  If
+ *   CONFIG_ARCH_LEDS is not defined, then the following interfacesare available to
+ *   control the LEDs from user applications.
+ *
+ ************************************************************************************/
+
+#ifndef CONFIG_ARCH_LEDS
+EXTERN void stm32_ledinit(void);
+EXTERN void stm32_setled(int led, bool ledon);
+EXTERN void stm32_setleds(uint8_t ledset);
+#endif
+
+#undef EXTERN
+#if defined(__cplusplus)
+}
+#endif
+
+#endif /* __ASSEMBLY__ */
+#endif  /* __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H */
diff --git a/nuttx-configs/px4-stm32f4discovery/include/nsh_romfsimg.h b/nuttx-configs/px4-stm32f4discovery/include/nsh_romfsimg.h
new file mode 100644
index 000000000..15e4e7a8d
--- /dev/null
+++ b/nuttx-configs/px4-stm32f4discovery/include/nsh_romfsimg.h
@@ -0,0 +1,42 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * nsh_romfsetc.h
+ *
+ * This file is a stub for 'make export' purposes; the actual ROMFS
+ * must be supplied by the library client.
+ */
+
+extern unsigned char romfs_img[];
+extern unsigned int romfs_img_len;
diff --git a/nuttx-configs/px4-stm32f4discovery/nsh/Make.defs b/nuttx-configs/px4-stm32f4discovery/nsh/Make.defs
new file mode 100644
index 000000000..e70320aaa
--- /dev/null
+++ b/nuttx-configs/px4-stm32f4discovery/nsh/Make.defs
@@ -0,0 +1,179 @@
+############################################################################
+# configs/px4fmu-v2/nsh/Make.defs
+#
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name NuttX nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+include ${TOPDIR}/.config
+include ${TOPDIR}/tools/Config.mk
+
+#
+# We only support building with the ARM bare-metal toolchain from
+# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS.
+#
+CONFIG_ARMV7M_TOOLCHAIN	:= GNU_EABI
+
+include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs
+
+CC			 = $(CROSSDEV)gcc
+CXX			 = $(CROSSDEV)g++
+CPP			 = $(CROSSDEV)gcc -E
+LD			 = $(CROSSDEV)ld
+AR			 = $(CROSSDEV)ar rcs
+NM			 = $(CROSSDEV)nm
+OBJCOPY			 = $(CROSSDEV)objcopy
+OBJDUMP			 = $(CROSSDEV)objdump
+
+MAXOPTIMIZATION		 = -O3
+ARCHCPUFLAGS		 = -mcpu=cortex-m4 \
+			   -mthumb \
+			   -march=armv7e-m \
+			   -mfpu=fpv4-sp-d16 \
+			   -mfloat-abi=hard
+
+
+# enable precise stack overflow tracking
+INSTRUMENTATIONDEFINES	 = -finstrument-functions \
+			   -ffixed-r10
+
+# pull in *just* libm from the toolchain ... this is grody
+LIBM			 = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}"
+EXTRA_LIBS		+= $(LIBM)
+
+# use our linker script
+LDSCRIPT		 = ld.script
+
+ifeq ($(WINTOOL),y)
+  # Windows-native toolchains
+  DIRLINK		 = $(TOPDIR)/tools/copydir.sh
+  DIRUNLINK		 = $(TOPDIR)/tools/unlink.sh
+  MKDEP			 = $(TOPDIR)/tools/mknulldeps.sh
+  ARCHINCLUDES		 = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}"
+  ARCHXXINCLUDES	 = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}"
+  ARCHSCRIPT		 = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}"
+else
+  ifeq ($(PX4_WINTOOL),y)
+    # Windows-native toolchains (MSYS)
+    DIRLINK		 = $(TOPDIR)/tools/copydir.sh
+    DIRUNLINK		 = $(TOPDIR)/tools/unlink.sh
+    MKDEP		 = $(TOPDIR)/tools/mknulldeps.sh
+    ARCHINCLUDES	 = -I. -isystem $(TOPDIR)/include
+    ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+    ARCHSCRIPT		 = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)
+  else
+    # Linux/Cygwin-native toolchain 
+    MKDEP		 = $(TOPDIR)/tools/mkdeps.sh
+    ARCHINCLUDES	 = -I. -isystem $(TOPDIR)/include
+    ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+    ARCHSCRIPT		 = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)
+  endif
+endif
+
+# tool versions
+ARCHCCVERSION		 = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'}
+ARCHCCMAJOR		 = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1}
+
+# optimisation flags
+ARCHOPTIMIZATION	 = $(MAXOPTIMIZATION) \
+			   -fno-strict-aliasing \
+			   -fno-strength-reduce \
+			   -fomit-frame-pointer \
+   			   -funsafe-math-optimizations \
+   			   -fno-builtin-printf \
+   			   -ffunction-sections \
+   			   -fdata-sections
+
+ifeq ("${CONFIG_DEBUG_SYMBOLS}","y")
+ARCHOPTIMIZATION	+= -g
+endif
+
+ARCHCFLAGS		 = -std=gnu99
+ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x
+ARCHWARNINGS		 = -Wall \
+			   -Wextra \
+			   -Wdouble-promotion \
+			   -Wshadow \
+			   -Wfloat-equal \
+			   -Wframe-larger-than=1024 \
+			   -Wpointer-arith \
+			   -Wlogical-op \
+			   -Wmissing-declarations \
+			   -Wpacked \
+			   -Wno-unused-parameter
+#   -Wcast-qual  - generates spurious noreturn attribute warnings, try again later
+#   -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code
+#   -Wcast-align - would help catch bad casts in some cases, but generates too many false positives
+
+ARCHCWARNINGS		 = $(ARCHWARNINGS) \
+			   -Wbad-function-cast \
+			   -Wstrict-prototypes \
+			   -Wold-style-declaration \
+			   -Wmissing-parameter-type \
+			   -Wmissing-prototypes \
+			   -Wnested-externs \
+			   -Wunsuffixed-float-constants
+ARCHWARNINGSXX		 = $(ARCHWARNINGS) \
+			   -Wno-psabi
+ARCHDEFINES		 =
+ARCHPICFLAGS		 = -fpic -msingle-pic-base -mpic-register=r10
+
+# this seems to be the only way to add linker flags
+EXTRA_LIBS		+= --warn-common \
+			   --gc-sections
+
+CFLAGS			 = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common
+CPICFLAGS		 = $(ARCHPICFLAGS) $(CFLAGS)
+CXXFLAGS		 = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe
+CXXPICFLAGS		 = $(ARCHPICFLAGS) $(CXXFLAGS)
+CPPFLAGS		 = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES)
+AFLAGS			 = $(CFLAGS) -D__ASSEMBLY__
+
+NXFLATLDFLAGS1		 = -r -d -warn-common
+NXFLATLDFLAGS2		 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections
+LDNXFLATFLAGS		 = -e main -s 2048
+
+OBJEXT			 = .o
+LIBEXT			 = .a
+EXEEXT			 =
+
+
+# produce partially-linked $1 from files in $2
+define PRELINK
+	@echo "PRELINK: $1"
+	$(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1
+endef
+
+HOSTCC			 = gcc
+HOSTINCLUDES		 = -I.
+HOSTCFLAGS		 = -Wall -Wstrict-prototypes -Wshadow -g -pipe
+HOSTLDFLAGS		 =
+
diff --git a/src/systemcmds/preflight_check/module.mk b/nuttx-configs/px4-stm32f4discovery/nsh/appconfig
index 0cb2a4cd0..f14ab4044 100644
--- a/src/systemcmds/preflight_check/module.mk
+++ b/nuttx-configs/px4-stm32f4discovery/nsh/appconfig
@@ -1,6 +1,8 @@
 ############################################################################
+# configs/px4fmu/nsh/appconfig
 #
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -12,7 +14,7 @@
 #    notice, this list of conditions and the following disclaimer in
 #    the documentation and/or other materials provided with the
 #    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
+# 3. Neither the name NuttX nor the names of its contributors may be
 #    used to endorse or promote products derived from this software
 #    without specific prior written permission.
 #
@@ -31,14 +33,16 @@
 #
 ############################################################################
 
-#
-# Pre-flight check. Locks down system for a few systems with blinking leds
-# and buzzer if the sensors do not report an OK status.
-#
+# Path to example in apps/examples containing the user_start entry point
 
-MODULE_COMMAND	 = preflight_check
-SRCS		 = preflight_check.c
+CONFIGURED_APPS	+= examples/nsh
 
-MAXOPTIMIZATION	 = -Os
+# The NSH application library
+CONFIGURED_APPS += nshlib
+CONFIGURED_APPS += system/readline
 
-MODULE_STACKSIZE = 1800
+#ifeq ($(CONFIG_USBDEV),y)
+#ifeq ($(CONFIG_CDCACM),y)
+CONFIGURED_APPS += examples/cdcacm
+#endif
+#endif
diff --git a/nuttx-configs/px4-stm32f4discovery/nsh/defconfig b/nuttx-configs/px4-stm32f4discovery/nsh/defconfig
new file mode 100644
index 000000000..6a2470bea
--- /dev/null
+++ b/nuttx-configs/px4-stm32f4discovery/nsh/defconfig
@@ -0,0 +1,897 @@
+#
+# Automatically generated file; DO NOT EDIT.
+# Nuttx/ Configuration
+#
+CONFIG_NUTTX_NEWCONFIG=y
+
+#
+# Build Setup
+#
+# CONFIG_EXPERIMENTAL is not set
+# CONFIG_HOST_LINUX is not set
+CONFIG_HOST_OSX=y
+# CONFIG_HOST_WINDOWS is not set
+# CONFIG_HOST_OTHER is not set
+
+#
+# Build Configuration
+#
+CONFIG_APPS_DIR="../apps"
+# CONFIG_BUILD_2PASS is not set
+
+#
+# Binary Output Formats
+#
+# CONFIG_RRLOAD_BINARY is not set
+# CONFIG_INTELHEX_BINARY is not set
+# CONFIG_MOTOROLA_SREC is not set
+CONFIG_RAW_BINARY=y
+
+#
+# Customize Header Files
+#
+# CONFIG_ARCH_STDBOOL_H is not set
+CONFIG_ARCH_MATH_H=y
+# CONFIG_ARCH_FLOAT_H is not set
+# CONFIG_ARCH_STDARG_H is not set
+
+#
+# Debug Options
+#
+# CONFIG_DEBUG is not set
+CONFIG_DEBUG_SYMBOLS=y
+
+#
+# System Type
+#
+# CONFIG_ARCH_8051 is not set
+CONFIG_ARCH_ARM=y
+# CONFIG_ARCH_AVR is not set
+# CONFIG_ARCH_HC is not set
+# CONFIG_ARCH_MIPS is not set
+# CONFIG_ARCH_RGMP is not set
+# CONFIG_ARCH_SH is not set
+# CONFIG_ARCH_SIM is not set
+# CONFIG_ARCH_X86 is not set
+# CONFIG_ARCH_Z16 is not set
+# CONFIG_ARCH_Z80 is not set
+CONFIG_ARCH="arm"
+
+#
+# ARM Options
+#
+# CONFIG_ARCH_CHIP_C5471 is not set
+# CONFIG_ARCH_CHIP_CALYPSO is not set
+# CONFIG_ARCH_CHIP_DM320 is not set
+# CONFIG_ARCH_CHIP_IMX is not set
+# CONFIG_ARCH_CHIP_KINETIS is not set
+# CONFIG_ARCH_CHIP_KL is not set
+# CONFIG_ARCH_CHIP_LM is not set
+# CONFIG_ARCH_CHIP_LPC17XX is not set
+# CONFIG_ARCH_CHIP_LPC214X is not set
+# CONFIG_ARCH_CHIP_LPC2378 is not set
+# CONFIG_ARCH_CHIP_LPC31XX is not set
+# CONFIG_ARCH_CHIP_LPC43XX is not set
+# CONFIG_ARCH_CHIP_NUC1XX is not set
+# CONFIG_ARCH_CHIP_SAM34 is not set
+CONFIG_ARCH_CHIP_STM32=y
+# CONFIG_ARCH_CHIP_STR71X is not set
+CONFIG_ARCH_CORTEXM4=y
+CONFIG_ARCH_FAMILY="armv7-m"
+CONFIG_ARCH_CHIP="stm32"
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_ARCH_HAVE_CMNVECTOR=y
+CONFIG_ARMV7M_CMNVECTOR=y
+CONFIG_ARCH_HAVE_FPU=y
+CONFIG_ARCH_FPU=y
+CONFIG_ARCH_HAVE_MPU=y
+# CONFIG_ARMV7M_MPU is not set
+
+#
+# ARMV7M Configuration Options
+#
+# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set
+CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y
+CONFIG_ARMV7M_STACKCHECK=y
+CONFIG_SERIAL_TERMIOS=y
+
+#
+# STM32 Configuration Options
+#
+# CONFIG_ARCH_CHIP_STM32L151C6 is not set
+# CONFIG_ARCH_CHIP_STM32L151C8 is not set
+# CONFIG_ARCH_CHIP_STM32L151CB is not set
+# CONFIG_ARCH_CHIP_STM32L151R6 is not set
+# CONFIG_ARCH_CHIP_STM32L151R8 is not set
+# CONFIG_ARCH_CHIP_STM32L151RB is not set
+# CONFIG_ARCH_CHIP_STM32L151V6 is not set
+# CONFIG_ARCH_CHIP_STM32L151V8 is not set
+# CONFIG_ARCH_CHIP_STM32L151VB is not set
+# CONFIG_ARCH_CHIP_STM32L152C6 is not set
+# CONFIG_ARCH_CHIP_STM32L152C8 is not set
+# CONFIG_ARCH_CHIP_STM32L152CB is not set
+# CONFIG_ARCH_CHIP_STM32L152R6 is not set
+# CONFIG_ARCH_CHIP_STM32L152R8 is not set
+# CONFIG_ARCH_CHIP_STM32L152RB is not set
+# CONFIG_ARCH_CHIP_STM32L152V6 is not set
+# CONFIG_ARCH_CHIP_STM32L152V8 is not set
+# CONFIG_ARCH_CHIP_STM32L152VB is not set
+# CONFIG_ARCH_CHIP_STM32F100C8 is not set
+# CONFIG_ARCH_CHIP_STM32F100CB is not set
+# CONFIG_ARCH_CHIP_STM32F100R8 is not set
+# CONFIG_ARCH_CHIP_STM32F100RB is not set
+# CONFIG_ARCH_CHIP_STM32F100RC is not set
+# CONFIG_ARCH_CHIP_STM32F100RD is not set
+# CONFIG_ARCH_CHIP_STM32F100RE is not set
+# CONFIG_ARCH_CHIP_STM32F100V8 is not set
+# CONFIG_ARCH_CHIP_STM32F100VB is not set
+# CONFIG_ARCH_CHIP_STM32F100VC is not set
+# CONFIG_ARCH_CHIP_STM32F100VD is not set
+# CONFIG_ARCH_CHIP_STM32F100VE is not set
+# CONFIG_ARCH_CHIP_STM32F103C4 is not set
+# CONFIG_ARCH_CHIP_STM32F103C8 is not set
+# CONFIG_ARCH_CHIP_STM32F103RET6 is not set
+# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set
+# CONFIG_ARCH_CHIP_STM32F103VET6 is not set
+# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set
+# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set
+# CONFIG_ARCH_CHIP_STM32F107VC is not set
+# CONFIG_ARCH_CHIP_STM32F207IG is not set
+# CONFIG_ARCH_CHIP_STM32F302CB is not set
+# CONFIG_ARCH_CHIP_STM32F302CC is not set
+# CONFIG_ARCH_CHIP_STM32F302RB is not set
+# CONFIG_ARCH_CHIP_STM32F302RC is not set
+# CONFIG_ARCH_CHIP_STM32F302VB is not set
+# CONFIG_ARCH_CHIP_STM32F302VC is not set
+# CONFIG_ARCH_CHIP_STM32F303CB is not set
+# CONFIG_ARCH_CHIP_STM32F303CC is not set
+# CONFIG_ARCH_CHIP_STM32F303RB is not set
+# CONFIG_ARCH_CHIP_STM32F303RC is not set
+# CONFIG_ARCH_CHIP_STM32F303VB is not set
+# CONFIG_ARCH_CHIP_STM32F303VC is not set
+# CONFIG_ARCH_CHIP_STM32F405RG is not set
+# CONFIG_ARCH_CHIP_STM32F405VG is not set
+# CONFIG_ARCH_CHIP_STM32F405ZG is not set
+# CONFIG_ARCH_CHIP_STM32F407VE is not set
+CONFIG_ARCH_CHIP_STM32F407VG=y
+# CONFIG_ARCH_CHIP_STM32F407ZE is not set
+# CONFIG_ARCH_CHIP_STM32F407ZG is not set
+# CONFIG_ARCH_CHIP_STM32F407IE is not set
+# CONFIG_ARCH_CHIP_STM32F407IG is not set
+# CONFIG_ARCH_CHIP_STM32F427V is not set
+# CONFIG_ARCH_CHIP_STM32F427Z is not set
+# CONFIG_ARCH_CHIP_STM32F427I is not set
+# CONFIG_STM32_STM32L15XX is not set
+# CONFIG_STM32_ENERGYLITE is not set
+# CONFIG_STM32_STM32F10XX is not set
+# CONFIG_STM32_VALUELINE is not set
+# CONFIG_STM32_CONNECTIVITYLINE is not set
+# CONFIG_STM32_PERFORMANCELINE is not set
+# CONFIG_STM32_HIGHDENSITY is not set
+# CONFIG_STM32_MEDIUMDENSITY is not set
+# CONFIG_STM32_LOWDENSITY is not set
+# CONFIG_STM32_STM32F20XX is not set
+# CONFIG_STM32_STM32F30XX is not set
+CONFIG_STM32_STM32F40XX=y
+# CONFIG_STM32_STM32F427 is not set
+# CONFIG_STM32_DFU is not set
+
+#
+# STM32 Peripheral Support
+#
+CONFIG_STM32_ADC1=y
+# CONFIG_STM32_ADC2 is not set
+# CONFIG_STM32_ADC3 is not set
+CONFIG_STM32_BKPSRAM=y
+# CONFIG_STM32_CAN1 is not set
+# CONFIG_STM32_CAN2 is not set
+CONFIG_STM32_CCMDATARAM=y
+# CONFIG_STM32_CRC is not set
+# CONFIG_STM32_CRYP is not set
+CONFIG_STM32_DMA1=y
+CONFIG_STM32_DMA2=y
+# CONFIG_STM32_DAC1 is not set
+# CONFIG_STM32_DAC2 is not set
+# CONFIG_STM32_DCMI is not set
+# CONFIG_STM32_ETHMAC is not set
+# CONFIG_STM32_FSMC is not set
+# CONFIG_STM32_HASH is not set
+CONFIG_STM32_I2C1=y
+# CONFIG_STM32_I2C2 is not set
+# CONFIG_STM32_I2C3 is not set
+CONFIG_STM32_OTGFS=y
+# CONFIG_STM32_OTGHS is not set
+CONFIG_STM32_PWR=y
+# CONFIG_STM32_RNG is not set
+# CONFIG_STM32_SDIO is not set
+CONFIG_STM32_SPI1=y
+# CONFIG_STM32_SPI2 is not set
+# CONFIG_STM32_SPI3 is not set
+# CONFIG_STM32_SPI4 is not set
+# CONFIG_STM32_SPI5 is not set
+# CONFIG_STM32_SPI6 is not set
+CONFIG_STM32_SYSCFG=y
+CONFIG_STM32_TIM1=y
+# CONFIG_STM32_TIM2 is not set
+CONFIG_STM32_TIM3=y
+CONFIG_STM32_TIM4=y
+# CONFIG_STM32_TIM5 is not set
+# CONFIG_STM32_TIM6 is not set
+# CONFIG_STM32_TIM7 is not set
+# CONFIG_STM32_TIM8 is not set
+CONFIG_STM32_TIM9=y
+CONFIG_STM32_TIM10=y
+CONFIG_STM32_TIM11=y
+# CONFIG_STM32_TIM12 is not set
+# CONFIG_STM32_TIM13 is not set
+# CONFIG_STM32_TIM14 is not set
+# CONFIG_STM32_USART1 is not set
+CONFIG_STM32_USART2=y
+# CONFIG_STM32_USART3 is not set
+# CONFIG_STM32_UART4 is not set
+# CONFIG_STM32_UART5 is not set
+# CONFIG_STM32_USART6 is not set
+# CONFIG_STM32_UART7 is not set
+# CONFIG_STM32_UART8 is not set
+# CONFIG_STM32_IWDG is not set
+# CONFIG_STM32_WWDG is not set
+CONFIG_STM32_ADC=y
+CONFIG_STM32_SPI=y
+CONFIG_STM32_I2C=y
+
+#
+# Alternate Pin Mapping
+#
+CONFIG_STM32_FLASH_PREFETCH=y
+# CONFIG_STM32_JTAG_DISABLE is not set
+# CONFIG_STM32_JTAG_FULL_ENABLE is not set
+# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set
+CONFIG_STM32_JTAG_SW_ENABLE=y
+CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y
+# CONFIG_STM32_FORCEPOWER is not set
+# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set
+# CONFIG_STM32_CCMEXCLUDE is not set
+CONFIG_STM32_DMACAPABLE=y
+# CONFIG_STM32_TIM1_PWM is not set
+# CONFIG_STM32_TIM3_PWM is not set
+# CONFIG_STM32_TIM4_PWM is not set
+# CONFIG_STM32_TIM9_PWM is not set
+# CONFIG_STM32_TIM10_PWM is not set
+# CONFIG_STM32_TIM11_PWM is not set
+# CONFIG_STM32_TIM1_ADC is not set
+# CONFIG_STM32_TIM3_ADC is not set
+# CONFIG_STM32_TIM4_ADC is not set
+CONFIG_STM32_USART=y
+
+#
+# U[S]ART Configuration
+#
+# CONFIG_USART2_RS485 is not set
+CONFIG_USART2_RXDMA=y
+CONFIG_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32_USART_SINGLEWIRE=y
+
+#
+# SPI Configuration
+#
+# CONFIG_STM32_SPI_INTERRUPTS is not set
+# CONFIG_STM32_SPI_DMA is not set
+
+#
+# I2C Configuration
+#
+# CONFIG_STM32_I2C_DYNTIMEO is not set
+CONFIG_STM32_I2CTIMEOSEC=0
+CONFIG_STM32_I2CTIMEOMS=500
+CONFIG_STM32_I2CTIMEOTICKS=500
+# CONFIG_STM32_I2C_DUTY16_9 is not set
+
+#
+# USB Host Configuration
+#
+
+#
+# USB Device Configuration
+#
+
+#
+# External Memory Configuration
+#
+
+#
+# Architecture Options
+#
+# CONFIG_ARCH_NOINTC is not set
+# CONFIG_ARCH_VECNOTIRQ is not set
+CONFIG_ARCH_DMA=y
+CONFIG_ARCH_IRQPRIO=y
+# CONFIG_CUSTOM_STACK is not set
+# CONFIG_ADDRENV is not set
+CONFIG_ARCH_HAVE_VFORK=y
+CONFIG_ARCH_STACKDUMP=y
+# CONFIG_ENDIAN_BIG is not set
+# CONFIG_ARCH_HAVE_RAMFUNCS is not set
+CONFIG_ARCH_HAVE_RAMVECTORS=y
+# CONFIG_ARCH_RAMVECTORS is not set
+
+#
+# Board Settings
+#
+CONFIG_BOARD_LOOPSPERMSEC=16717
+# CONFIG_ARCH_CALIBRATION is not set
+CONFIG_DRAM_START=0x20000000
+CONFIG_DRAM_SIZE=196608
+CONFIG_ARCH_HAVE_INTERRUPTSTACK=y
+CONFIG_ARCH_INTERRUPTSTACK=4096
+
+#
+# Boot options
+#
+# CONFIG_BOOT_RUNFROMEXTSRAM is not set
+CONFIG_BOOT_RUNFROMFLASH=y
+# CONFIG_BOOT_RUNFROMISRAM is not set
+# CONFIG_BOOT_RUNFROMSDRAM is not set
+# CONFIG_BOOT_COPYTORAM is not set
+
+#
+# Board Selection
+#
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD=""
+
+#
+# Common Board Options
+#
+CONFIG_NSH_MMCSDMINOR=0
+
+#
+# Board-Specific Options
+#
+
+#
+# RTOS Features
+#
+# CONFIG_BOARD_INITIALIZE is not set
+CONFIG_MSEC_PER_TICK=1
+CONFIG_RR_INTERVAL=0
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_TASK_NAME_SIZE=24
+# CONFIG_SCHED_HAVE_PARENT is not set
+# CONFIG_JULIAN_TIME is not set
+CONFIG_START_YEAR=1970
+CONFIG_START_MONTH=1
+CONFIG_START_DAY=1
+CONFIG_DEV_CONSOLE=y
+# CONFIG_MUTEX_TYPES is not set
+CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_SEM_PREALLOCHOLDERS=0
+CONFIG_SEM_NNESTPRIO=8
+# CONFIG_FDCLONE_DISABLE is not set
+CONFIG_FDCLONE_STDIO=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SCHED_WAITPID=y
+# CONFIG_SCHED_STARTHOOK is not set
+CONFIG_SCHED_ATEXIT=y
+CONFIG_SCHED_ATEXIT_MAX=1
+# CONFIG_SCHED_ONEXIT is not set
+CONFIG_USER_ENTRYPOINT="nsh_main"
+# CONFIG_DISABLE_OS_API is not set
+
+#
+# Signal Numbers
+#
+CONFIG_SIG_SIGUSR1=1
+CONFIG_SIG_SIGUSR2=2
+CONFIG_SIG_SIGALARM=3
+CONFIG_SIG_SIGCONDTIMEDOUT=16
+CONFIG_SIG_SIGWORK=4
+
+#
+# Sizes of configurable things (0 disables)
+#
+CONFIG_MAX_TASKS=32
+CONFIG_MAX_TASK_ARGS=10
+CONFIG_NPTHREAD_KEYS=4
+CONFIG_NFILE_DESCRIPTORS=44
+CONFIG_NFILE_STREAMS=8
+CONFIG_NAME_MAX=32
+CONFIG_PREALLOC_MQ_MSGS=4
+CONFIG_MQ_MAXMSGSIZE=32
+CONFIG_MAX_WDOGPARMS=2
+CONFIG_PREALLOC_WDOGS=50
+CONFIG_PREALLOC_TIMERS=50
+
+#
+# Stack and heap information
+#
+CONFIG_IDLETHREAD_STACKSIZE=6000
+CONFIG_USERMAIN_STACKSIZE=4096
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_PTHREAD_STACK_DEFAULT=2048
+
+#
+# Device Drivers
+#
+# CONFIG_DISABLE_POLL is not set
+CONFIG_DEV_NULL=y
+# CONFIG_DEV_ZERO is not set
+# CONFIG_LOOP is not set
+# CONFIG_RAMDISK is not set
+# CONFIG_CAN is not set
+# CONFIG_PWM is not set
+CONFIG_I2C=y
+# CONFIG_I2C_SLAVE is not set
+CONFIG_I2C_TRANSFER=y
+# CONFIG_I2C_WRITEREAD is not set
+# CONFIG_I2C_POLLED is not set
+# CONFIG_I2C_TRACE is not set
+CONFIG_ARCH_HAVE_I2CRESET=y
+CONFIG_I2C_RESET=y
+CONFIG_SPI=y
+# CONFIG_SPI_OWNBUS is not set
+CONFIG_SPI_EXCHANGE=y
+# CONFIG_SPI_CMDDATA is not set
+# CONFIG_RTC is not set
+CONFIG_WATCHDOG=y
+# CONFIG_ANALOG is not set
+# CONFIG_AUDIO_DEVICES is not set
+# CONFIG_BCH is not set
+# CONFIG_INPUT is not set
+# CONFIG_LCD is not set
+# CONFIG_MMCSD is not set
+# CONFIG_MTD is not set
+CONFIG_PIPES=y
+# CONFIG_PM is not set
+# CONFIG_POWER is not set
+# CONFIG_SENSORS is not set
+CONFIG_SERIAL=y
+# CONFIG_DEV_LOWCONSOLE is not set
+CONFIG_SERIAL_REMOVABLE=y
+# CONFIG_16550_UART is not set
+CONFIG_ARCH_HAVE_USART2=y
+CONFIG_MCU_SERIAL=y
+CONFIG_STANDARD_SERIAL=y
+CONFIG_SERIAL_NPOLLWAITERS=2
+CONFIG_USART2_SERIAL_CONSOLE=y
+# CONFIG_NO_SERIAL_CONSOLE is not set
+
+#
+# USART2 Configuration
+#
+CONFIG_USART2_RXBUFSIZE=512
+CONFIG_USART2_TXBUFSIZE=512
+CONFIG_USART2_BAUD=57600
+CONFIG_USART2_BITS=8
+CONFIG_USART2_PARITY=0
+CONFIG_USART2_2STOP=0
+# CONFIG_USART2_IFLOWCONTROL is not set
+# CONFIG_USART2_OFLOWCONTROL is not set
+# CONFIG_SERIAL_IFLOWCONTROL is not set
+# CONFIG_SERIAL_OFLOWCONTROL is not set
+CONFIG_USBDEV=y
+
+#
+# USB Device Controller Driver Options
+#
+# CONFIG_USBDEV_ISOCHRONOUS is not set
+# CONFIG_USBDEV_DUALSPEED is not set
+# CONFIG_USBDEV_SELFPOWERED is not set
+CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_MAXPOWER=500
+# CONFIG_USBDEV_DMA is not set
+# CONFIG_USBDEV_TRACE is not set
+
+#
+# USB Device Class Driver Options
+#
+# CONFIG_USBDEV_COMPOSITE is not set
+# CONFIG_PL2303 is not set
+CONFIG_CDCACM=y
+# CONFIG_CDCACM_CONSOLE is not set
+CONFIG_CDCACM_EP0MAXPACKET=64
+CONFIG_CDCACM_EPINTIN=1
+CONFIG_CDCACM_EPINTIN_FSSIZE=64
+CONFIG_CDCACM_EPINTIN_HSSIZE=64
+CONFIG_CDCACM_EPBULKOUT=3
+CONFIG_CDCACM_EPBULKOUT_FSSIZE=64
+CONFIG_CDCACM_EPBULKOUT_HSSIZE=512
+CONFIG_CDCACM_EPBULKIN=2
+CONFIG_CDCACM_EPBULKIN_FSSIZE=64
+CONFIG_CDCACM_EPBULKIN_HSSIZE=512
+CONFIG_CDCACM_NWRREQS=4
+CONFIG_CDCACM_NRDREQS=4
+CONFIG_CDCACM_BULKIN_REQLEN=96
+CONFIG_CDCACM_RXBUFSIZE=512
+CONFIG_CDCACM_TXBUFSIZE=2048
+CONFIG_CDCACM_VENDORID=0x26ac
+CONFIG_CDCACM_PRODUCTID=0x0011
+CONFIG_CDCACM_VENDORSTR="3D Robotics"
+CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2.x"
+# CONFIG_USBMSC is not set
+# CONFIG_USBHOST is not set
+# CONFIG_WIRELESS is not set
+
+#
+# System Logging Device Options
+#
+
+#
+# System Logging
+#
+# CONFIG_RAMLOG is not set
+
+#
+# Networking Support
+#
+# CONFIG_NET is not set
+
+#
+# File Systems
+#
+
+#
+# File system configuration
+#
+# CONFIG_DISABLE_MOUNTPOINT is not set
+# CONFIG_FS_RAMMAP is not set
+CONFIG_FS_FAT=y
+# CONFIG_FAT_LCNAMES is not set
+# CONFIG_FAT_LFN is not set
+# CONFIG_FS_FATTIME is not set
+# CONFIG_FAT_DMAMEMORY is not set
+CONFIG_FS_NXFFS=y
+CONFIG_NXFFS_PREALLOCATED=y
+CONFIG_NXFFS_ERASEDSTATE=0xff
+CONFIG_NXFFS_PACKTHRESHOLD=32
+CONFIG_NXFFS_MAXNAMLEN=32
+CONFIG_NXFFS_TAILTHRESHOLD=2048
+CONFIG_FS_ROMFS=y
+# CONFIG_FS_SMARTFS is not set
+CONFIG_FS_BINFS=y
+
+#
+# System Logging
+#
+# CONFIG_SYSLOG_ENABLE is not set
+# CONFIG_SYSLOG is not set
+
+#
+# Graphics Support
+#
+# CONFIG_NX is not set
+
+#
+# Memory Management
+#
+# CONFIG_MM_MULTIHEAP is not set
+# CONFIG_MM_SMALL is not set
+CONFIG_MM_REGIONS=2
+CONFIG_GRAN=y
+# CONFIG_GRAN_SINGLE is not set
+# CONFIG_GRAN_INTR is not set
+
+#
+# Audio Support
+#
+# CONFIG_AUDIO is not set
+
+#
+# Binary Formats
+#
+# CONFIG_BINFMT_DISABLE is not set
+# CONFIG_BINFMT_EXEPATH is not set
+# CONFIG_NXFLAT is not set
+# CONFIG_ELF is not set
+CONFIG_BUILTIN=y
+# CONFIG_PIC is not set
+# CONFIG_SYMTAB_ORDEREDBYNAME is not set
+
+#
+# Library Routines
+#
+
+#
+# Standard C Library Options
+#
+CONFIG_STDIO_BUFFER_SIZE=32
+CONFIG_STDIO_LINEBUFFER=y
+CONFIG_NUNGET_CHARS=2
+CONFIG_LIB_HOMEDIR="/"
+# CONFIG_NOPRINTF_FIELDWIDTH is not set
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIB_RAND_ORDER=1
+# CONFIG_EOL_IS_CR is not set
+# CONFIG_EOL_IS_LF is not set
+# CONFIG_EOL_IS_BOTH_CRLF is not set
+CONFIG_EOL_IS_EITHER_CRLF=y
+# CONFIG_LIBC_EXECFUNCS is not set
+CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024
+CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048
+CONFIG_LIBC_STRERROR=y
+# CONFIG_LIBC_STRERROR_SHORT is not set
+# CONFIG_LIBC_PERROR_STDOUT is not set
+CONFIG_ARCH_LOWPUTC=y
+CONFIG_LIB_SENDFILE_BUFSIZE=512
+# CONFIG_ARCH_ROMGETC is not set
+CONFIG_ARCH_OPTIMIZED_FUNCTIONS=y
+CONFIG_ARCH_MEMCPY=y
+# CONFIG_ARCH_MEMCMP is not set
+# CONFIG_ARCH_MEMMOVE is not set
+# CONFIG_ARCH_MEMSET is not set
+# CONFIG_MEMSET_OPTSPEED is not set
+# CONFIG_ARCH_STRCHR is not set
+# CONFIG_ARCH_STRCMP is not set
+# CONFIG_ARCH_STRCPY is not set
+# CONFIG_ARCH_STRNCPY is not set
+# CONFIG_ARCH_STRLEN is not set
+# CONFIG_ARCH_STRNLEN is not set
+# CONFIG_ARCH_BZERO is not set
+
+#
+# Non-standard Library Support
+#
+CONFIG_SCHED_WORKQUEUE=y
+CONFIG_SCHED_HPWORK=y
+CONFIG_SCHED_WORKPRIORITY=192
+CONFIG_SCHED_WORKPERIOD=5000
+CONFIG_SCHED_WORKSTACKSIZE=4000
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKPERIOD=50000
+CONFIG_SCHED_LPWORKSTACKSIZE=4000
+# CONFIG_LIB_KBDCODEC is not set
+# CONFIG_LIB_SLCDCODEC is not set
+
+#
+# Basic CXX Support
+#
+CONFIG_C99_BOOL8=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+# CONFIG_CXX_NEWLONG is not set
+
+#
+# uClibc++ Standard C++ Library
+#
+# CONFIG_UCLIBCXX is not set
+
+#
+# Application Configuration
+#
+
+#
+# Built-In Applications
+#
+CONFIG_BUILTIN_PROXY_STACKSIZE=1024
+
+#
+# Examples
+#
+# CONFIG_EXAMPLES_BUTTONS is not set
+# CONFIG_EXAMPLES_CAN is not set
+CONFIG_EXAMPLES_CDCACM=y
+# CONFIG_EXAMPLES_COMPOSITE is not set
+# CONFIG_EXAMPLES_CXXTEST is not set
+# CONFIG_EXAMPLES_DHCPD is not set
+# CONFIG_EXAMPLES_ELF is not set
+# CONFIG_EXAMPLES_FTPC is not set
+# CONFIG_EXAMPLES_FTPD is not set
+# CONFIG_EXAMPLES_HELLO is not set
+# CONFIG_EXAMPLES_HELLOXX is not set
+# CONFIG_EXAMPLES_JSON is not set
+# CONFIG_EXAMPLES_HIDKBD is not set
+# CONFIG_EXAMPLES_KEYPADTEST is not set
+# CONFIG_EXAMPLES_IGMP is not set
+# CONFIG_EXAMPLES_LCDRW is not set
+# CONFIG_EXAMPLES_MM is not set
+# CONFIG_EXAMPLES_MODBUS is not set
+CONFIG_EXAMPLES_MOUNT=y
+# CONFIG_EXAMPLES_NRF24L01TERM is not set
+CONFIG_EXAMPLES_NSH=y
+# CONFIG_EXAMPLES_NULL is not set
+# CONFIG_EXAMPLES_NX is not set
+# CONFIG_EXAMPLES_NXCONSOLE is not set
+# CONFIG_EXAMPLES_NXFFS is not set
+# CONFIG_EXAMPLES_NXFLAT is not set
+# CONFIG_EXAMPLES_NXHELLO is not set
+# CONFIG_EXAMPLES_NXIMAGE is not set
+# CONFIG_EXAMPLES_NXLINES is not set
+# CONFIG_EXAMPLES_NXTEXT is not set
+# CONFIG_EXAMPLES_OSTEST is not set
+# CONFIG_EXAMPLES_PASHELLO is not set
+# CONFIG_EXAMPLES_PIPE is not set
+# CONFIG_EXAMPLES_POSIXSPAWN is not set
+# CONFIG_EXAMPLES_QENCODER is not set
+# CONFIG_EXAMPLES_RGMP is not set
+# CONFIG_EXAMPLES_ROMFS is not set
+# CONFIG_EXAMPLES_SENDMAIL is not set
+# CONFIG_EXAMPLES_SERLOOP is not set
+# CONFIG_EXAMPLES_SLCD is not set
+# CONFIG_EXAMPLES_SMART_TEST is not set
+# CONFIG_EXAMPLES_SMART is not set
+# CONFIG_EXAMPLES_TCPECHO is not set
+# CONFIG_EXAMPLES_TELNETD is not set
+# CONFIG_EXAMPLES_THTTPD is not set
+# CONFIG_EXAMPLES_TIFF is not set
+# CONFIG_EXAMPLES_TOUCHSCREEN is not set
+# CONFIG_EXAMPLES_UDP is not set
+# CONFIG_EXAMPLES_UIP is not set
+# CONFIG_EXAMPLES_USBSERIAL is not set
+# CONFIG_EXAMPLES_USBMSC is not set
+# CONFIG_EXAMPLES_USBTERM is not set
+# CONFIG_EXAMPLES_WATCHDOG is not set
+
+#
+# Graphics Support
+#
+# CONFIG_TIFF is not set
+
+#
+# Interpreters
+#
+# CONFIG_INTERPRETERS_FICL is not set
+# CONFIG_INTERPRETERS_PCODE is not set
+
+#
+# Network Utilities
+#
+
+#
+# Networking Utilities
+#
+# CONFIG_NETUTILS_CODECS is not set
+# CONFIG_NETUTILS_DHCPC is not set
+# CONFIG_NETUTILS_DHCPD is not set
+# CONFIG_NETUTILS_FTPC is not set
+# CONFIG_NETUTILS_FTPD is not set
+# CONFIG_NETUTILS_JSON is not set
+# CONFIG_NETUTILS_RESOLV is not set
+# CONFIG_NETUTILS_SMTP is not set
+# CONFIG_NETUTILS_TELNETD is not set
+# CONFIG_NETUTILS_TFTPC is not set
+# CONFIG_NETUTILS_THTTPD is not set
+# CONFIG_NETUTILS_UIPLIB is not set
+# CONFIG_NETUTILS_WEBCLIENT is not set
+
+#
+# FreeModBus
+#
+# CONFIG_MODBUS is not set
+
+#
+# NSH Library
+#
+CONFIG_NSH_LIBRARY=y
+CONFIG_NSH_BUILTIN_APPS=y
+
+#
+# Disable Individual commands
+#
+# CONFIG_NSH_DISABLE_CAT is not set
+# CONFIG_NSH_DISABLE_CD is not set
+# CONFIG_NSH_DISABLE_CP is not set
+# CONFIG_NSH_DISABLE_DD is not set
+# CONFIG_NSH_DISABLE_ECHO is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXIT is not set
+# CONFIG_NSH_DISABLE_FREE is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_HELP is not set
+# CONFIG_NSH_DISABLE_HEXDUMP is not set
+# CONFIG_NSH_DISABLE_IFCONFIG is not set
+# CONFIG_NSH_DISABLE_KILL is not set
+# CONFIG_NSH_DISABLE_LOSETUP is not set
+# CONFIG_NSH_DISABLE_LS is not set
+# CONFIG_NSH_DISABLE_MB is not set
+# CONFIG_NSH_DISABLE_MKDIR is not set
+# CONFIG_NSH_DISABLE_MKFATFS is not set
+# CONFIG_NSH_DISABLE_MKFIFO is not set
+# CONFIG_NSH_DISABLE_MKRD is not set
+# CONFIG_NSH_DISABLE_MH is not set
+# CONFIG_NSH_DISABLE_MOUNT is not set
+# CONFIG_NSH_DISABLE_MW is not set
+# CONFIG_NSH_DISABLE_NSFMOUNT is not set
+# CONFIG_NSH_DISABLE_PS is not set
+# CONFIG_NSH_DISABLE_PING is not set
+# CONFIG_NSH_DISABLE_PUT is not set
+# CONFIG_NSH_DISABLE_PWD is not set
+# CONFIG_NSH_DISABLE_RM is not set
+# CONFIG_NSH_DISABLE_RMDIR is not set
+# CONFIG_NSH_DISABLE_SET is not set
+# CONFIG_NSH_DISABLE_SH is not set
+# CONFIG_NSH_DISABLE_SLEEP is not set
+# CONFIG_NSH_DISABLE_TEST is not set
+# CONFIG_NSH_DISABLE_UMOUNT is not set
+# CONFIG_NSH_DISABLE_UNSET is not set
+# CONFIG_NSH_DISABLE_USLEEP is not set
+# CONFIG_NSH_DISABLE_WGET is not set
+# CONFIG_NSH_DISABLE_XD is not set
+
+#
+# Configure Command Options
+#
+# CONFIG_NSH_CMDOPT_DF_H is not set
+CONFIG_NSH_CODECS_BUFSIZE=128
+CONFIG_NSH_FILEIOSIZE=512
+CONFIG_NSH_STRERROR=y
+CONFIG_NSH_LINELEN=128
+CONFIG_NSH_MAXARGUMENTS=12
+CONFIG_NSH_NESTDEPTH=8
+# CONFIG_NSH_DISABLESCRIPT is not set
+# CONFIG_NSH_DISABLEBG is not set
+CONFIG_NSH_ROMFSETC=y
+# CONFIG_NSH_ROMFSRC is not set
+CONFIG_NSH_ROMFSMOUNTPT="/etc"
+CONFIG_NSH_INITSCRIPT="init.d/rcS"
+CONFIG_NSH_ROMFSDEVNO=0
+CONFIG_NSH_ROMFSSECTSIZE=128
+CONFIG_NSH_ARCHROMFS=y
+CONFIG_NSH_FATDEVNO=0
+CONFIG_NSH_FATSECTSIZE=512
+CONFIG_NSH_FATNSECTORS=1024
+CONFIG_NSH_FATMOUNTPT="/tmp"
+CONFIG_NSH_CONSOLE=y
+# CONFIG_NSH_USBCONSOLE is not set
+
+#
+# USB Trace Support
+#
+# CONFIG_NSH_CONDEV is not set
+CONFIG_NSH_ARCHINIT=y
+
+#
+# NxWidgets/NxWM
+#
+
+#
+# System NSH Add-Ons
+#
+
+#
+# Custom Free Memory Command
+#
+# CONFIG_SYSTEM_FREE is not set
+
+#
+# I2C tool
+#
+# CONFIG_SYSTEM_I2CTOOL is not set
+
+#
+# FLASH Program Installation
+#
+# CONFIG_SYSTEM_INSTALL is not set
+
+#
+# FLASH Erase-all Command
+#
+
+#
+# readline()
+#
+CONFIG_SYSTEM_READLINE=y
+CONFIG_READLINE_ECHO=y
+
+#
+# Power Off
+#
+# CONFIG_SYSTEM_POWEROFF is not set
+
+#
+# RAMTRON
+#
+# CONFIG_SYSTEM_RAMTRON is not set
+
+#
+# SD Card
+#
+# CONFIG_SYSTEM_SDCARD is not set
+
+#
+# Sysinfo
+#
+CONFIG_SYSTEM_SYSINFO=y
+
+#
+# USB Monitor
+#
diff --git a/nuttx-configs/px4-stm32f4discovery/nsh/setenv.sh b/nuttx-configs/px4-stm32f4discovery/nsh/setenv.sh
new file mode 100755
index 000000000..265520997
--- /dev/null
+++ b/nuttx-configs/px4-stm32f4discovery/nsh/setenv.sh
@@ -0,0 +1,67 @@
+#!/bin/bash
+# configs/stm3240g-eval/nsh/setenv.sh
+#
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name NuttX nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+
+if [ "$_" = "$0" ] ; then
+  echo "You must source this script, not run it!" 1>&2
+  exit 1
+fi
+
+WD=`pwd`
+if [ ! -x "setenv.sh" ]; then
+  echo "This script must be executed from the top-level NuttX build directory"
+  exit 1
+fi
+
+if [ -z "${PATH_ORIG}" ]; then
+  export PATH_ORIG="${PATH}"
+fi
+
+# This the Cygwin path to the location where I installed the RIDE
+# toolchain under windows.  You will also have to edit this if you install
+# the RIDE toolchain in any other location
+#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin"
+
+# This the Cygwin path to the location where I installed the CodeSourcery
+# toolchain under windows.  You will also have to edit this if you install
+# the CodeSourcery toolchain in any other location
+export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin"
+
+# This the Cygwin path to the location where I build the buildroot
+# toolchain.
+#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin"
+
+# Add the path to the toolchain to the PATH varialble
+export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}"
+
+echo "PATH : ${PATH}"
diff --git a/nuttx-configs/px4-stm32f4discovery/scripts/ld.script b/nuttx-configs/px4-stm32f4discovery/scripts/ld.script
new file mode 100644
index 000000000..f39a95c45
--- /dev/null
+++ b/nuttx-configs/px4-stm32f4discovery/scripts/ld.script
@@ -0,0 +1,149 @@
+/****************************************************************************
+ * configs/px4fmu-v1/scripts/ld.script
+ *
+ *   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* The STM32F40VGG has 1024Kb of FLASH beginning at address 0x0800:0000 and
+ * 192Kb of SRAM. SRAM is split up into three blocks:
+ *
+ * 1) 112Kb of SRAM beginning at address 0x2000:0000
+ * 2)  16Kb of SRAM beginning at address 0x2001:c000
+ * 3)  64Kb of CCM SRAM beginning at address 0x1000:0000
+ *
+ * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000
+ * where the code expects to begin execution by jumping to the entry point in
+ * the 0x0800:0000 address range.
+ *
+ * The first 0x4000 of flash is reserved for the bootloader.
+ */
+
+MEMORY
+{
+    flash (rx)   : ORIGIN = 0x08004000, LENGTH = 1008K
+    sram (rwx)   : ORIGIN = 0x20000000, LENGTH = 128K
+    ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
+}
+
+OUTPUT_ARCH(arm)
+
+ENTRY(__start)		/* treat __start as the anchor for dead code stripping */
+EXTERN(_vectors)	/* force the vectors to be included in the output */
+
+/* 
+ * Ensure that abort() is present in the final object.  The exception handling
+ * code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
+ */
+EXTERN(abort)
+
+SECTIONS
+{
+	.text : {
+		_stext = ABSOLUTE(.);
+		*(.vectors)
+		*(.text .text.*)        
+		*(.fixup)
+		*(.gnu.warning)
+		*(.rodata .rodata.*)        
+		*(.gnu.linkonce.t.*)
+		*(.got)
+		*(.gcc_except_table)
+		*(.gnu.linkonce.r.*)
+		_etext = ABSOLUTE(.);
+
+		/* 
+		 * This is a hack to make the newlib libm __errno() call
+		 * use the NuttX get_errno_ptr() function.
+		 */
+		__errno = get_errno_ptr;
+	} > flash
+
+	/*
+	 * Init functions (static constructors and the like)
+	 */
+        .init_section : {
+                _sinit = ABSOLUTE(.);
+                KEEP(*(.init_array .init_array.*))
+                _einit = ABSOLUTE(.);
+        } > flash
+
+	/*
+	 * Construction data for parameters.
+	 */
+	__param ALIGN(4): {
+		__param_start = ABSOLUTE(.);
+		KEEP(*(__param*))
+		__param_end = ABSOLUTE(.);
+	} > flash
+
+	.ARM.extab : {
+		*(.ARM.extab*)
+	} > flash
+
+	__exidx_start = ABSOLUTE(.);
+	.ARM.exidx : {
+		*(.ARM.exidx*)
+	} > flash
+	__exidx_end = ABSOLUTE(.);
+
+	_eronly = ABSOLUTE(.);
+
+	.data : {
+		_sdata = ABSOLUTE(.);
+		*(.data .data.*)
+		*(.gnu.linkonce.d.*)
+		CONSTRUCTORS
+		_edata = ABSOLUTE(.);
+	} > sram AT > flash
+
+	.bss : {
+		_sbss = ABSOLUTE(.);
+		*(.bss .bss.*)
+		*(.gnu.linkonce.b.*)
+		*(COMMON)
+		_ebss = ABSOLUTE(.);
+	} > sram
+
+	/* Stabs debugging sections. */
+	.stab 0 : { *(.stab) }
+	.stabstr 0 : { *(.stabstr) }
+	.stab.excl 0 : { *(.stab.excl) }
+	.stab.exclstr 0 : { *(.stab.exclstr) }
+	.stab.index 0 : { *(.stab.index) }
+	.stab.indexstr 0 : { *(.stab.indexstr) }
+	.comment 0 : { *(.comment) }
+	.debug_abbrev 0 : { *(.debug_abbrev) }
+	.debug_info 0 : { *(.debug_info) }
+	.debug_line 0 : { *(.debug_line) }
+	.debug_pubnames 0 : { *(.debug_pubnames) }
+	.debug_aranges 0 : { *(.debug_aranges) }
+}
diff --git a/nuttx-configs/px4-stm32f4discovery/src/Makefile b/nuttx-configs/px4-stm32f4discovery/src/Makefile
new file mode 100644
index 000000000..d4276f7fc
--- /dev/null
+++ b/nuttx-configs/px4-stm32f4discovery/src/Makefile
@@ -0,0 +1,84 @@
+############################################################################
+# configs/px4fmu/src/Makefile
+#
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name NuttX nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+-include $(TOPDIR)/Make.defs
+
+CFLAGS		+= -I$(TOPDIR)/sched
+
+ASRCS		= 
+AOBJS		= $(ASRCS:.S=$(OBJEXT))
+
+CSRCS		= empty.c
+COBJS		= $(CSRCS:.c=$(OBJEXT))
+
+SRCS		= $(ASRCS) $(CSRCS)
+OBJS		= $(AOBJS) $(COBJS)
+
+ARCH_SRCDIR	= $(TOPDIR)/arch/$(CONFIG_ARCH)/src
+ifeq ($(WINTOOL),y)
+  CFLAGS	+= -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \
+  		   -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \
+  		   -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}"
+else
+  CFLAGS	+= -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m
+endif
+
+all: libboard$(LIBEXT)
+
+$(AOBJS): %$(OBJEXT): %.S
+	$(call ASSEMBLE, $<, $@)
+
+$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c
+	$(call COMPILE, $<, $@)
+
+libboard$(LIBEXT): $(OBJS)
+	$(call ARCHIVE, $@, $(OBJS))
+
+.depend: Makefile $(SRCS)
+	$(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
+	$(Q) touch $@
+
+depend: .depend
+
+clean:
+	$(call DELFILE, libboard$(LIBEXT))
+	$(call CLEAN)
+
+distclean: clean
+	$(call DELFILE, Make.dep)
+	$(call DELFILE, .depend)
+
+-include Make.dep
+
diff --git a/nuttx-configs/px4-stm32f4discovery/src/empty.c b/nuttx-configs/px4-stm32f4discovery/src/empty.c
new file mode 100644
index 000000000..ace900866
--- /dev/null
+++ b/nuttx-configs/px4-stm32f4discovery/src/empty.c
@@ -0,0 +1,4 @@
+/*
+ * There are no source files here, but libboard.a can't be empty, so 
+ * we have this empty source file to keep it company.
+ */
diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig
index cd410051c..2c9a21f21 100644
--- a/nuttx-configs/px4fmu-v1/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v1/nsh/defconfig
@@ -326,7 +326,7 @@ CONFIG_DRAM_START=0x20000000
 CONFIG_DRAM_SIZE=196608
 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y
 # The actual usage is 420 bytes
-CONFIG_ARCH_INTERRUPTSTACK=1500
+CONFIG_ARCH_INTERRUPTSTACK=750
 
 #
 # Boot options
diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c
index 7d4b7d880..53d98ba9a 100644
--- a/src/drivers/ardrone_interface/ardrone_interface.c
+++ b/src/drivers/ardrone_interface/ardrone_interface.c
@@ -108,8 +108,9 @@ usage(const char *reason)
  */
 int ardrone_interface_main(int argc, char *argv[])
 {
-		if (argc < 1)
+	if (argc < 2) {
 		usage("missing command");
+	}
 
 	if (!strcmp(argv[1], "start")) {
 
diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp
index d749a0d7b..cfa59eb3a 100644
--- a/src/drivers/blinkm/blinkm.cpp
+++ b/src/drivers/blinkm/blinkm.cpp
@@ -316,7 +316,6 @@ BlinkM::init()
 	ret = I2C::init();
 
 	if (ret != OK) {
-		warnx("I2C init failed");
 		return ret;
 	}
 
@@ -970,7 +969,7 @@ blinkm_main(int argc, char *argv[])
 		if (OK != g_blinkm->init()) {
 			delete g_blinkm;
 			g_blinkm = nullptr;
-			errx(1, "init failed");
+			errx(1, "no BlinkM found");
 		}
 
 		exit(0);
diff --git a/src/drivers/boards/px4-stm32f4discovery/board_config.h b/src/drivers/boards/px4-stm32f4discovery/board_config.h
new file mode 100644
index 000000000..dd66abc19
--- /dev/null
+++ b/src/drivers/boards/px4-stm32f4discovery/board_config.h
@@ -0,0 +1,136 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file board_config.h
+ *
+ * PX4-STM32F4Discovery internal definitions
+ */
+
+#pragma once
+
+/****************************************************************************************************
+ * Included Files
+ ****************************************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+/* these headers are not C++ safe */
+#include <stm32.h>
+#include <arch/board/board.h>
+
+#define UDID_START		0x1FFF7A10
+ 
+/****************************************************************************************************
+ * Definitions
+ ****************************************************************************************************/
+/* Configuration ************************************************************************************/
+
+/* PX4FMU GPIOs ***********************************************************************************/
+/* LEDs */
+/* LEDs */
+
+#define GPIO_LED1       (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\
+                         GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN12)
+#define GPIO_LED2       (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\
+                         GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_LED3       (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\
+                         GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14)
+#define GPIO_LED4       (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\
+                         GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15)
+
+/* SPI chip selects */
+#define GPIO_SPI_CS_MEMS    (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3)
+
+/* USB OTG FS
+ *
+ * PA9  OTG_FS_VBUS VBUS sensing
+ */
+#define GPIO_OTGFS_VBUS		(GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
+
+/* High-resolution timer */
+#define HRT_TIMER		8	/* use timer8 for the HRT */
+#define HRT_TIMER_CHANNEL	1	/* use capture/compare channel */
+
+/****************************************************************************************************
+ * Public Types
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Public data
+ ****************************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+/****************************************************************************************************
+ * Public Functions
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ****************************************************************************************************/
+
+extern void stm32_spiinitialize(void);
+
+extern void stm32_usbinitialize(void);
+
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ *   Perform architecture specific initialization for NSH.
+ *
+ *   CONFIG_NSH_ARCHINIT=y :
+ *     Called from the NSH library
+ *
+ *   CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, &&
+ *   CONFIG_NSH_ARCHINIT=n :
+ *     Called from board_initialize().
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_NSH_LIBRARY
+int nsh_archinitialize(void);
+#endif
+
+#endif /* __ASSEMBLY__ */
+
+__END_DECLS
diff --git a/src/drivers/boards/px4-stm32f4discovery/module.mk b/src/drivers/boards/px4-stm32f4discovery/module.mk
new file mode 100644
index 000000000..6b75c08e9
--- /dev/null
+++ b/src/drivers/boards/px4-stm32f4discovery/module.mk
@@ -0,0 +1,7 @@
+#
+# Board-specific startup code for the PX4-STM32F4Discovery
+#
+
+SRCS		 = px4discovery_init.c \
+		   px4discovery_usb.c \
+		   px4discovery_led.c
diff --git a/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c b/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c
new file mode 100644
index 000000000..fb1b28236
--- /dev/null
+++ b/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c
@@ -0,0 +1,173 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4discovery_init.c
+ *
+ * PX4-stm32f4discovery specific early startup code.  This file implements the
+ * nsh_archinitialize() function that is called early by nsh during startup.
+ *
+ * Code here is run before the rcS script is invoked; it should start required
+ * subsystems and perform board-specific initialisation.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/spi.h>
+#include <nuttx/i2c.h>
+#include <nuttx/sdio.h>
+#include <nuttx/mmcsd.h>
+#include <nuttx/analog/adc.h>
+#include <nuttx/gran.h>
+
+#include <stm32.h>
+#include "board_config.h"
+#include <stm32_uart.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_led.h>
+
+#include <systemlib/cpuload.h>
+#include <systemlib/perf_counter.h>
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+/* Configuration ************************************************************/
+
+/* Debug ********************************************************************/
+
+#ifdef CONFIG_CPP_HAVE_VARARGS
+#  ifdef CONFIG_DEBUG
+#    define message(...) lowsyslog(__VA_ARGS__)
+#  else
+#    define message(...) printf(__VA_ARGS__)
+#  endif
+#else
+#  ifdef CONFIG_DEBUG
+#    define message lowsyslog
+#  else
+#    define message printf
+#  endif
+#endif
+
+/****************************************************************************
+ * Protected Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void
+stm32_boardinitialize(void)
+{
+	/* configure LEDs */
+	up_ledinit();
+}
+
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ *   Perform architecture specific initialization
+ *
+ ****************************************************************************/
+
+#include <math.h>
+
+#ifdef __cplusplus
+__EXPORT int matherr(struct __exception *e)
+{
+	return 1;
+}
+#else
+__EXPORT int matherr(struct exception *e)
+{
+	return 1;
+}
+#endif
+
+__EXPORT int nsh_archinitialize(void)
+{
+
+	/* configure the high-resolution time/callout interface */
+	hrt_init();
+
+	/* configure CPU load estimation */
+#ifdef CONFIG_SCHED_INSTRUMENTATION
+	cpuload_initialize_once();
+#endif
+
+	/* set up the serial DMA polling */
+	static struct hrt_call serial_dma_call;
+	struct timespec ts;
+
+	/*
+	 * Poll at 1ms intervals for received bytes that have not triggered
+	 * a DMA event.
+	 */
+	ts.tv_sec = 0;
+	ts.tv_nsec = 1000000;
+
+	hrt_call_every(&serial_dma_call,
+		       ts_to_abstime(&ts),
+		       ts_to_abstime(&ts),
+		       (hrt_callout)stm32_serial_dma_poll,
+		       NULL);
+
+	return OK;
+}
diff --git a/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c b/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c
new file mode 100644
index 000000000..1f42843ed
--- /dev/null
+++ b/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c
@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4discovery_led.c
+ *
+ * PX4-stm32f4discovery LED backend.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+
+#include "stm32.h"
+#include "board_config.h"
+
+#include <arch/board/board.h>
+
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init(void);
+extern void led_on(int led);
+extern void led_off(int led);
+extern void led_toggle(int led);
+__END_DECLS
+
+__EXPORT void led_init()
+{
+	/* Configure LED1 GPIO for output */
+
+	stm32_configgpio(GPIO_LED1);
+}
+
+__EXPORT void led_on(int led)
+{
+	if (led == 1)
+	{
+		/* Pull down to switch on */
+		stm32_gpiowrite(GPIO_LED1, false);
+	}
+}
+
+__EXPORT void led_off(int led)
+{
+	if (led == 1)
+	{
+		/* Pull up to switch off */
+		stm32_gpiowrite(GPIO_LED1, true);
+	}
+}
+
+__EXPORT void led_toggle(int led)
+{
+	if (led == 1)
+	{
+		if (stm32_gpioread(GPIO_LED1))
+			stm32_gpiowrite(GPIO_LED1, false);
+		else
+			stm32_gpiowrite(GPIO_LED1, true);
+	}
+}
diff --git a/src/drivers/boards/px4-stm32f4discovery/px4discovery_usb.c b/src/drivers/boards/px4-stm32f4discovery/px4discovery_usb.c
new file mode 100644
index 000000000..887ca67ef
--- /dev/null
+++ b/src/drivers/boards/px4-stm32f4discovery/px4discovery_usb.c
@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4discovery_usb.c
+ *
+ * Board-specific USB functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/usb/usbdev.h>
+#include <nuttx/usb/usbdev_trace.h>
+
+#include <up_arch.h>
+#include <stm32.h>
+#include "board_config.h"
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_usbinitialize
+ *
+ * Description:
+ *   Called to setup USB-related GPIO pins for the PX4-STM32F4Discovery board.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbinitialize(void)
+{
+	/* The OTG FS has an internal soft pull-up */
+
+	/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
+
+#ifdef CONFIG_STM32_OTGFS
+	stm32_configgpio(GPIO_OTGFS_VBUS);
+	/* XXX We only support device mode
+	stm32_configgpio(GPIO_OTGFS_PWRON);
+	stm32_configgpio(GPIO_OTGFS_OVER);
+	*/
+#endif
+}
+
+/************************************************************************************
+ * Name:  stm32_usbsuspend
+ *
+ * Description:
+ *   Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is
+ *   used.  This function is called whenever the USB enters or leaves suspend mode.
+ *   This is an opportunity for the board logic to shutdown clocks, power, etc.
+ *   while the USB is suspended.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
+{
+	ulldbg("resume: %d\n", resume);
+}
+
diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c
index 293021f8b..c7d4f1ce4 100644
--- a/src/drivers/boards/px4fmu-v1/px4fmu_init.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c
@@ -216,8 +216,6 @@ __EXPORT int nsh_archinitialize(void)
 	SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
 	up_udelay(20);
 
-	message("[boot] Successfully initialized SPI port 1\r\n");
-
 	/*
 	 * If SPI2 is enabled in the defconfig, we loose some ADC pins as chip selects.
 	 * Keep the SPI2 init optional and conditionally initialize the ADC pins
@@ -243,7 +241,6 @@ __EXPORT int nsh_archinitialize(void)
 
 	/* Get the SPI port for the microSD slot */
 
-	message("[boot] Initializing SPI port 3\n");
 	spi3 = up_spiinitialize(3);
 
 	if (!spi3) {
@@ -252,8 +249,6 @@ __EXPORT int nsh_archinitialize(void)
 		return -ENODEV;
 	}
 
-	message("[boot] Successfully initialized SPI port 3\n");
-
 	/* Now bind the SPI interface to the MMCSD driver */
 	result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3);
 
@@ -263,7 +258,5 @@ __EXPORT int nsh_archinitialize(void)
 		return -ENODEV;
 	}
 
-	message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
-
 	return OK;
 }
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
index 9b25c574a..266642cbb 100644
--- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
+++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
@@ -296,8 +296,6 @@ __EXPORT int nsh_archinitialize(void)
 	SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
 	up_udelay(20);
 
-	message("[boot] Initialized SPI port 1 (SENSORS)\n");
-
 	/* Get the SPI port for the FRAM */
 
 	spi2 = up_spiinitialize(2);
@@ -317,8 +315,6 @@ __EXPORT int nsh_archinitialize(void)
 	SPI_SETMODE(spi2, SPIDEV_MODE3);
 	SPI_SELECT(spi2, SPIDEV_FLASH, false);
 
-	message("[boot] Initialized SPI port 2 (RAMTRON FRAM)\n");
-
 	spi4 = up_spiinitialize(4);
 
 	/* Default SPI4 to 1MHz and de-assert the known chip selects. */
@@ -328,8 +324,6 @@ __EXPORT int nsh_archinitialize(void)
 	SPI_SELECT(spi4, PX4_SPIDEV_EXT0, false);
 	SPI_SELECT(spi4, PX4_SPIDEV_EXT1, false);
 
-	message("[boot] Initialized SPI port 4\n");
-
 	#ifdef CONFIG_MMCSD
 	/* First, get an instance of the SDIO interface */
 
@@ -350,7 +344,6 @@ __EXPORT int nsh_archinitialize(void)
 	/* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */
 	sdio_mediachange(sdio, true);
 
-	message("[boot] Initialized SDIO\n");
 	#endif
 
 	return OK;
diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h
index be9604b6e..f18c8162d 100644
--- a/src/drivers/drv_gpio.h
+++ b/src/drivers/drv_gpio.h
@@ -110,9 +110,13 @@
 /* no GPIO driver on the PX4IOv2 board */
 #endif
 
+#ifdef CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY
+/* no GPIO driver on the PX4_STM32F4DISCOVERY board */
+#endif
+
 #if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2)  && \
 	!defined(CONFIG_ARCH_BOARD_PX4FMU_V1) && !defined(CONFIG_ARCH_BOARD_PX4FMU_V2) && \
-	!defined(CONFIG_ARCH_BOARD_AEROCORE)
+	!defined(CONFIG_ARCH_BOARD_AEROCORE) && !defined(CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY)
 # error No CONFIG_ARCH_BOARD_xxxx set
 #endif
 /*
diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h
index d44728a71..a24d8814f 100644
--- a/src/drivers/drv_rc_input.h
+++ b/src/drivers/drv_rc_input.h
@@ -65,7 +65,7 @@
 /**
  * Maximum RSSI value
  */
-#define RC_INPUT_RSSI_MAX	255
+#define RC_INPUT_RSSI_MAX	100
 
 /**
  * @addtogroup topics
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 3a3848446..5bf88da3d 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -1490,8 +1490,9 @@ start(enum HMC5883_BUS busid, enum Rotation rotation)
 		started |= start_bus(bus_options[i], rotation);
 	}
 
-	if (!started)
-		errx(1, "driver start failed");
+	if (!started) {
+		exit(1);
+	}
 }
 
 /**
diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp
index 1aade450b..4ac7e4bdf 100644
--- a/src/drivers/hott/hott_sensors/hott_sensors.cpp
+++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp
@@ -197,7 +197,7 @@ hott_sensors_thread_main(int argc, char *argv[])
 int
 hott_sensors_main(int argc, char *argv[])
 {
-	if (argc < 1) {
+	if (argc < 2) {
 		errx(1, "missing command\n%s", commandline_usage);
 	}
 
diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp
index 17a24d104..43df0cb8c 100644
--- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp
+++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp
@@ -223,7 +223,7 @@ hott_telemetry_thread_main(int argc, char *argv[])
 int
 hott_telemetry_main(int argc, char *argv[])
 {
-	if (argc < 1) {
+	if (argc < 2) {
 		errx(1, "missing command\n%s", commandline_usage);
 	}
 
diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp
index 7e5904d05..d25d16fa1 100644
--- a/src/drivers/md25/md25_main.cpp
+++ b/src/drivers/md25/md25_main.cpp
@@ -97,8 +97,9 @@ usage(const char *reason)
 int md25_main(int argc, char *argv[])
 {
 
-	if (argc < 1)
+	if (argc < 2) {
 		usage("missing command");
+	}
 
 	if (!strcmp(argv[1], "start")) {
 
diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp
index 8ea7a6170..b343a3e30 100644
--- a/src/drivers/px4flow/px4flow.cpp
+++ b/src/drivers/px4flow/px4flow.cpp
@@ -674,7 +674,7 @@ fail:
 		g_dev = nullptr;
 	}
 
-	errx(1, "driver start failed");
+	errx(1, "no PX4 FLOW connected");
 }
 
 /**
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 33125699f..e5636ff0f 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -303,6 +303,10 @@ private:
 	uint64_t		_battery_last_timestamp;///< last amp hour calculation timestamp
 	bool			_cb_flighttermination;	///< true if the flight termination circuit breaker is enabled
 
+	int32_t			_rssi_pwm_chan; ///< RSSI PWM input channel
+	int32_t			_rssi_pwm_max; ///< max RSSI input on PWM channel
+	int32_t			_rssi_pwm_min; ///< min RSSI input on PWM channel
+
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	bool			_dsm_vcc_ctl;		///< true if relay 1 controls DSM satellite RX power
 #endif
@@ -524,7 +528,10 @@ PX4IO::PX4IO(device::Device *interface) :
 	_battery_amp_bias(0),
 	_battery_mamphour_total(0),
 	_battery_last_timestamp(0),
-	_cb_flighttermination(true)
+	_cb_flighttermination(true),
+	_rssi_pwm_chan(0),
+	_rssi_pwm_max(0),
+	_rssi_pwm_min(0)
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	, _dsm_vcc_ctl(false)
 #endif
@@ -664,6 +671,10 @@ PX4IO::init()
 	if (_max_rc_input > RC_INPUT_MAX_CHANNELS)
 		_max_rc_input = RC_INPUT_MAX_CHANNELS;
 
+	param_get(param_find("RC_RSSI_PWM_CHAN"), &_rssi_pwm_chan);
+	param_get(param_find("RC_RSSI_PWM_MAX"), &_rssi_pwm_max);
+	param_get(param_find("RC_RSSI_PWM_MIN"), &_rssi_pwm_min);
+
 	/*
 	 * Check for IO flight state - if FMU was flagged to be in
 	 * armed state, FMU is recovering from an in-air reset.
@@ -1069,6 +1080,10 @@ PX4IO::task_main()
 				/* Update Circuit breakers */
 				_cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY);
 
+				param_get(param_find("RC_RSSI_PWM_CHAN"), &_rssi_pwm_chan);
+				param_get(param_find("RC_RSSI_PWM_MAX"), &_rssi_pwm_max);
+				param_get(param_find("RC_RSSI_PWM_MIN"), &_rssi_pwm_min);
+
 			}
 
 		}
@@ -1633,6 +1648,15 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 		input_rc.values[i] = regs[prolog + i];
 	}
 
+	/* get RSSI from input channel */
+	if (_rssi_pwm_chan > 0 && _rssi_pwm_chan <= RC_INPUT_MAX_CHANNELS && _rssi_pwm_max - _rssi_pwm_min != 0) {
+		int rssi = (input_rc.values[_rssi_pwm_chan - 1] - _rssi_pwm_min) /
+			((_rssi_pwm_max - _rssi_pwm_min) / 100);
+		rssi = rssi > 100 ? 100 : rssi;
+		rssi = rssi < 0 ? 0 : rssi;
+		input_rc.rssi = rssi;
+	}
+
 	return ret;
 }
 
diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp
index 1e38a766e..512d6318d 100644
--- a/src/drivers/rgbled/rgbled.cpp
+++ b/src/drivers/rgbled/rgbled.cpp
@@ -625,7 +625,7 @@ rgbled_main(int argc, char *argv[])
 			if (g_rgbled == nullptr) {
 				// fall back to default bus
 				if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) {
-					errx(1, "init failed");
+					errx(1, "no RGB led on bus #%d", i2cdevice);
 				}
 				i2cdevice = PX4_I2C_BUS_LED;
 			}
@@ -640,7 +640,7 @@ rgbled_main(int argc, char *argv[])
 			if (OK != g_rgbled->init()) {
 				delete g_rgbled;
 				g_rgbled = nullptr;
-				errx(1, "init failed");
+				errx(1, "no RGB led on bus #%d", i2cdevice);
 			}
 		}
 
diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp
index 83086fd7c..ea7178076 100644
--- a/src/drivers/roboclaw/roboclaw_main.cpp
+++ b/src/drivers/roboclaw/roboclaw_main.cpp
@@ -92,8 +92,9 @@ static void usage()
 int roboclaw_main(int argc, char *argv[])
 {
 
-	if (argc < 1)
+	if (argc < 2) {
 		usage();
+	}
 
 	if (!strcmp(argv[1], "start")) {
 
diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp
index d6599c036..66641d640 100644
--- a/src/drivers/sf0x/sf0x.cpp
+++ b/src/drivers/sf0x/sf0x.cpp
@@ -114,6 +114,7 @@ protected:
 	virtual int			probe();
 
 private:
+	char 				_port[20];
 	float				_min_distance;
 	float				_max_distance;
 	work_s				_work;
@@ -199,8 +200,13 @@ SF0X::SF0X(const char *port) :
 	_comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "sf0x_buffer_overflows"))
 {
+	/* store port name */
+	strncpy(_port, port, sizeof(_port));
+	/* enforce null termination */
+	_port[sizeof(_port) - 1] = '\0';
+
 	/* open fd */
-	_fd = ::open(port, O_RDWR | O_NOCTTY | O_NONBLOCK);
+	_fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK);
 
 	if (_fd < 0) {
 		warnx("FAIL: laser fd");
@@ -633,7 +639,7 @@ SF0X::cycle()
 	/* fds initialized? */
 	if (_fd < 0) {
 		/* open fd */
-		_fd = ::open(SF0X_DEFAULT_PORT, O_RDWR | O_NOCTTY | O_NONBLOCK);
+		_fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK);
 	}
 
 	/* collection phase? */
diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index 8b6847348..a18b54981 100644
--- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -890,8 +890,9 @@ tone_alarm_main(int argc, char *argv[])
 	if (argc > 1) {
 		const char *argv1 = argv[1];
 
-		if (!strcmp(argv1, "start"))
-			play_tune(TONE_STARTUP_TUNE);
+		if (!strcmp(argv1, "start")) {
+			play_tune(TONE_STOP_TUNE);
+		}
 
 		if (!strcmp(argv1, "stop"))
 			play_tune(TONE_STOP_TUNE);
diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c
index 17b27290c..1d590ae61 100644
--- a/src/examples/fixedwing_control/main.c
+++ b/src/examples/fixedwing_control/main.c
@@ -418,7 +418,7 @@ usage(const char *reason)
  */
 int ex_fixedwing_control_main(int argc, char *argv[])
 {
-	if (argc < 1) {
+	if (argc < 2) {
 		usage("missing command");
 	}
 
diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c
index f8e3f2dc5..ab67bd253 100644
--- a/src/examples/flow_position_estimator/flow_position_estimator_main.c
+++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c
@@ -99,7 +99,7 @@ static void usage(const char *reason)
  */
 int flow_position_estimator_main(int argc, char *argv[])
 {
-	if (argc < 1) {
+	if (argc < 2) {
 		usage("missing command");
 	}
 
diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c
index 8e439bdac..085ef1fed 100644
--- a/src/examples/matlab_csv_serial/matlab_csv_serial.c
+++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c
@@ -91,7 +91,7 @@ static void usage(const char *reason)
  */
 int matlab_csv_serial_main(int argc, char *argv[])
 {
-	if (argc < 1) {
+	if (argc < 2) {
 		usage("missing command");
 	}
 
diff --git a/src/examples/publisher/publisher_start_nuttx.cpp b/src/examples/publisher/publisher_start_nuttx.cpp
index 405b3c987..4ff2cebfb 100644
--- a/src/examples/publisher/publisher_start_nuttx.cpp
+++ b/src/examples/publisher/publisher_start_nuttx.cpp
@@ -54,7 +54,7 @@ extern int main(int argc, char **argv);
 extern "C" __EXPORT int publisher_main(int argc, char *argv[]);
 int publisher_main(int argc, char *argv[])
 {
-	if (argc < 1) {
+	if (argc < 2) {
 		errx(1, "usage: publisher {start|stop|status}");
 	}
 
diff --git a/src/examples/px4_daemon_app/px4_daemon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c
index 860b1af78..6e99939d1 100644
--- a/src/examples/px4_daemon_app/px4_daemon_app.c
+++ b/src/examples/px4_daemon_app/px4_daemon_app.c
@@ -88,7 +88,7 @@ usage(const char *reason)
  */
 int px4_daemon_app_main(int argc, char *argv[])
 {
-	if (argc < 1) {
+	if (argc < 2) {
 		usage("missing command");
 	}
 
diff --git a/src/examples/rover_steering_control/main.cpp b/src/examples/rover_steering_control/main.cpp
index 96b40b23f..41df96775 100644
--- a/src/examples/rover_steering_control/main.cpp
+++ b/src/examples/rover_steering_control/main.cpp
@@ -412,7 +412,7 @@ usage(const char *reason)
  */
 int rover_steering_control_main(int argc, char *argv[])
 {
-	if (argc < 1) {
+	if (argc < 2) {
 		usage("missing command");
 	}
 
diff --git a/src/examples/subscriber/subscriber_start_nuttx.cpp b/src/examples/subscriber/subscriber_start_nuttx.cpp
index b450230c1..757e8ec52 100644
--- a/src/examples/subscriber/subscriber_start_nuttx.cpp
+++ b/src/examples/subscriber/subscriber_start_nuttx.cpp
@@ -54,7 +54,7 @@ extern int main(int argc, char **argv);
 extern "C" __EXPORT int subscriber_main(int argc, char *argv[]);
 int subscriber_main(int argc, char *argv[])
 {
-	if (argc < 1) {
+	if (argc < 2) {
 		errx(1, "usage: subscriber {start|stop|status}");
 	}
 
diff --git a/src/lib/rc/sumd.c b/src/lib/rc/sumd.c
index a98c986bb..cea7790ec 100644
--- a/src/lib/rc/sumd.c
+++ b/src/lib/rc/sumd.c
@@ -269,7 +269,7 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe
 			uint8_t _cnt = *rx_count + 1;
 			*rx_count = _cnt;
 		
-			*rssi = 255;
+			*rssi = 100;
 
 			/* received Channels */
 			if ((uint16_t)_rxpacket.length > max_chan_count) {
diff --git a/src/lib/version/version.h b/src/lib/version/version.h
index d8ccb6774..b79fee182 100644
--- a/src/lib/version/version.h
+++ b/src/lib/version/version.h
@@ -43,14 +43,6 @@
 #ifndef VERSION_H_
 #define VERSION_H_
 
-/*
- GIT_VERSION is defined at build time via a Makefile call to the
- git command line.
- */
-#define FREEZE_STR(s) #s
-#define STRINGIFY(s) FREEZE_STR(s)
-#define FW_GIT STRINGIFY(GIT_VERSION)
-
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 #define	HW_ARCH "PX4FMU_V1"
 #endif
@@ -63,4 +55,8 @@
 #define	HW_ARCH "AEROCORE"
 #endif
 
+#ifdef CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY
+#define HW_ARCH "PX4_STM32F4DISCOVERY"
+#endif
+
 #endif /* VERSION_H_ */
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index c60d70b9f..3faf63a27 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -118,13 +118,14 @@ usage(const char *reason)
  */
 int attitude_estimator_ekf_main(int argc, char *argv[])
 {
-	if (argc < 1)
+	if (argc < 2) {
 		usage("missing command");
+	}
 
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			printf("attitude_estimator_ekf already running\n");
+			warnx("already running\n");
 			/* this is not an error */
 			exit(0);
 		}
@@ -176,8 +177,6 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
 int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 {
 
-const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
-
 	float dt = 0.005f;
 /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
 	float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f};					/**< Measurement vector */
@@ -208,14 +207,10 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 			     };		/**< init: identity matrix */
 
 	float debugOutput[4] = { 0.0f };
-	int overloadcounter = 19;
 
 	/* Initialize filter */
 	AttitudeEKF_initialize();
 
-	/* store start time to guard against too slow update rates */
-	uint64_t last_run = hrt_absolute_time();
-
 	struct sensor_combined_s raw;
 	memset(&raw, 0, sizeof(raw));
 	struct vehicle_gps_position_s gps;
@@ -317,7 +312,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 			orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);
 
 			if (!control_mode.flag_system_hil_enabled) {
-				warnx("WARNING: Not getting sensors - sensor app running?");
+				warnx("WARNING: Not getting sensor data - sensor app running?");
 			}
 
 		} else {
@@ -446,7 +441,11 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					z_k[5] = raw.accelerometer_m_s2[2] - acc(2);
 
 					/* update magnetometer measurements */
-					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
+					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp &&
+						/* check that the mag vector is > 0 */
+						fabsf(sqrtf(raw.magnetometer_ga[0] * raw.magnetometer_ga[0] +
+							raw.magnetometer_ga[1] * raw.magnetometer_ga[1] +
+							raw.magnetometer_ga[2] * raw.magnetometer_ga[2])) > 0.1f) {
 						update_vect[2] = 1;
 						// sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
 						sensor_last_timestamp[2] = raw.magnetometer_timestamp;
@@ -466,8 +465,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 						math::Vector<3> v(1.0f, 0.0f, 0.4f);
 
-						math::Vector<3> vn = Rvis * v;
-
+						math::Vector<3> vn = Rvis.transposed() * v; //Rvis is Rwr (robot respect to world) while v is respect to world. Hence Rvis must be transposed having (Rwr)' * Vw
+											    // Rrw * Vw = vn. This way we have consistency
 						z_k[6] = vn(0);
 						z_k[7] = vn(1);
 						z_k[8] = vn(2);
@@ -477,20 +476,6 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						z_k[8] = raw.magnetometer_ga[2];
 					}
 
-					uint64_t now = hrt_absolute_time();
-					unsigned int time_elapsed = now - last_run;
-					last_run = now;
-
-					if (time_elapsed > loop_interval_alarm) {
-						//TODO: add warning, cpu overload here
-						// if (overloadcounter == 20) {
-						// 	printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
-						// 	overloadcounter = 0;
-						// }
-
-						overloadcounter++;
-					}
-
 					static bool const_initialized = false;
 
 					/* initialize with good values once we have a reasonable dt estimate */
@@ -560,8 +545,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						continue;
 					}
 
-					if (last_data > 0 && raw.timestamp - last_data > 30000)
-						printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
+					if (last_data > 0 && raw.timestamp - last_data > 30000) {
+						warnx("sensor data missed! (%llu)\n", raw.timestamp - last_data);
+					}
 
 					last_data = raw.timestamp;
 
@@ -597,12 +583,13 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					memcpy(&att.q[0],&q.data[0],sizeof(att.q));
 					att.R_valid = true;
 
-					if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
+					if (isfinite(att.q[0]) && isfinite(att.q[1])
+						&& isfinite(att.q[2]) && isfinite(att.q[3])) {
 						// Broadcast
 						orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
 
 					} else {
-						warnx("NaN in roll/pitch/yaw estimate!");
+						warnx("ERR: NaN estimate!");
 					}
 
 					perf_end(ekf_loop_perf);
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
index e143f37b9..fe480e12b 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
@@ -48,49 +48,49 @@
 /**
  * Body angular rate process noise
  *
- * @group attitude_ekf
+ * @group Attitude EKF estimator
  */
 PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f);
 
 /**
  * Body angular acceleration process noise
  *
- * @group attitude_ekf
+ * @group Attitude EKF estimator
  */
 PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f);
 
 /**
  * Acceleration process noise
  *
- * @group attitude_ekf
+ * @group Attitude EKF estimator
  */
 PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f);
 
 /**
  * Magnet field vector process noise
  *
- * @group attitude_ekf
+ * @group Attitude EKF estimator
  */
 PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f);
 
 /**
  * Gyro measurement noise
  *
- * @group attitude_ekf
+ * @group Attitude EKF estimator
  */
 PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f);
 
 /**
  * Accel measurement noise
  *
- * @group attitude_ekf
+ * @group Attitude EKF estimator
  */
 PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f);
 
 /**
  * Mag measurement noise
  *
- * @group attitude_ekf
+ * @group Attitude EKF estimator
  */
 PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
 
@@ -102,7 +102,7 @@ PARAM_DEFINE_INT32(ATT_ACC_COMP, 2);
 /**
  * Moment of inertia matrix diagonal entry (1, 1)
  *
- * @group attitude_ekf
+ * @group Attitude EKF estimator
  * @unit kg*m^2
  */
 PARAM_DEFINE_FLOAT(ATT_J11, 0.0018);
@@ -110,7 +110,7 @@ PARAM_DEFINE_FLOAT(ATT_J11, 0.0018);
 /**
  * Moment of inertia matrix diagonal entry (2, 2)
  *
- * @group attitude_ekf
+ * @group Attitude EKF estimator
  * @unit kg*m^2
  */
 PARAM_DEFINE_FLOAT(ATT_J22, 0.0018);
@@ -118,7 +118,7 @@ PARAM_DEFINE_FLOAT(ATT_J22, 0.0018);
 /**
  * Moment of inertia matrix diagonal entry (3, 3)
  *
- * @group attitude_ekf
+ * @group Attitude EKF estimator
  * @unit kg*m^2
  */
 PARAM_DEFINE_FLOAT(ATT_J33, 0.0037);
@@ -128,7 +128,7 @@ PARAM_DEFINE_FLOAT(ATT_J33, 0.0037);
  *
  * If set to != 0 the moment of inertia will be used in the estimator
  *
- * @group attitude_ekf
+ * @group Attitude EKF estimator
  * @min 0
  * @max 1
  */
diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
index 9414225ca..82bcbc66f 100755
--- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
+++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
@@ -136,8 +136,9 @@ usage(const char *reason)
  */
 int attitude_estimator_so3_main(int argc, char *argv[])
 {
-	if (argc < 1)
+	if (argc < 2) {
 		usage("missing command");
+	}
 
 	if (!strcmp(argv[1], "start")) {
 
diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp
new file mode 100644
index 000000000..4d9bd8ae0
--- /dev/null
+++ b/src/modules/commander/PreflightCheck.cpp
@@ -0,0 +1,343 @@
+/****************************************************************************
+*
+*   Copyright (c) 2012-2015 PX4 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 PreflightCheck.cpp
+*
+* Preflight check for main system components
+*
+* @author Lorenz Meier <lorenz@px4.io>
+* @author Johan Jansen <jnsn.johan@gmail.com>
+*/
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+
+#include <systemlib/err.h>
+#include <systemlib/param/param.h>
+#include <systemlib/rc_check.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_mag.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_baro.h>
+#include <drivers/drv_airspeed.h>
+
+#include <uORB/topics/airspeed.h>
+
+#include <mavlink/mavlink_log.h>
+
+#include "PreflightCheck.h"
+
+namespace Commander
+{
+static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
+{
+	bool success = true;
+
+	char s[30];
+	sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance);
+	int fd = open(s, 0);
+
+	if (fd < 0) {
+		if (!optional) {
+			mavlink_and_console_log_critical(mavlink_fd,
+							 "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance);
+		}
+
+		return false;
+	}
+
+	int calibration_devid;
+	int ret;
+	int devid = ioctl(fd, DEVIOCGDEVICEID, 0);
+	sprintf(s, "CAL_MAG%u_ID", instance);
+	param_get(param_find(s), &(calibration_devid));
+
+	if (devid != calibration_devid) {
+		mavlink_and_console_log_critical(mavlink_fd,
+						 "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
+		success = false;
+		goto out;
+	}
+
+	ret = ioctl(fd, MAGIOCSELFTEST, 0);
+
+	if (ret != OK) {
+		mavlink_and_console_log_critical(mavlink_fd,
+						 "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance);
+		success = false;
+		goto out;
+	}
+
+out:
+	close(fd);
+	return success;
+}
+
+static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic)
+{
+	bool success = true;
+
+	char s[30];
+	sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance);
+	int fd = open(s, O_RDONLY);
+
+	if (fd < 0) {
+		if (!optional) {
+			mavlink_and_console_log_critical(mavlink_fd,
+							 "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
+		}
+
+		return false;
+	}
+
+	int calibration_devid;
+	int ret;
+	int devid = ioctl(fd, DEVIOCGDEVICEID, 0);
+	sprintf(s, "CAL_ACC%u_ID", instance);
+	param_get(param_find(s), &(calibration_devid));
+
+	if (devid != calibration_devid) {
+		mavlink_and_console_log_critical(mavlink_fd,
+						 "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
+		success = false;
+		goto out;
+	}
+
+	ret = ioctl(fd, ACCELIOCSELFTEST, 0);
+
+	if (ret != OK) {
+		mavlink_and_console_log_critical(mavlink_fd,
+						 "PREFLIGHT FAIL: ACCEL #%u SELFTEST FAILED", instance);
+		success = false;
+		goto out;
+	}
+
+	if (dynamic) {
+		/* check measurement result range */
+		struct accel_report acc;
+		ret = read(fd, &acc, sizeof(acc));
+
+		if (ret == sizeof(acc)) {
+			/* evaluate values */
+			float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
+
+			if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
+				mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
+				/* this is frickin' fatal */
+				success = false;
+				goto out;
+			}
+		} else {
+			mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ");
+			/* this is frickin' fatal */
+			success = false;
+			goto out;
+		}
+	}
+
+out:
+	close(fd);
+	return success;
+}
+
+static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
+{
+	bool success = true;
+
+	char s[30];
+	sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance);
+	int fd = open(s, 0);
+
+	if (fd < 0) {
+		if (!optional) {
+			mavlink_and_console_log_critical(mavlink_fd,
+							 "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance);
+		}
+
+		return false;
+	}
+
+	int calibration_devid;
+	int ret;
+	int devid = ioctl(fd, DEVIOCGDEVICEID, 0);
+	sprintf(s, "CAL_GYRO%u_ID", instance);
+	param_get(param_find(s), &(calibration_devid));
+
+	if (devid != calibration_devid) {
+		mavlink_and_console_log_critical(mavlink_fd,
+						 "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
+		success = false;
+		goto out;
+	}
+
+	ret = ioctl(fd, GYROIOCSELFTEST, 0);
+
+	if (ret != OK) {
+		mavlink_and_console_log_critical(mavlink_fd,
+						 "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance);
+		success = false;
+		goto out;
+	}
+
+out:
+	close(fd);
+	return success;
+}
+
+static bool baroCheck(int mavlink_fd, unsigned instance, bool optional)
+{
+	bool success = true;
+
+	char s[30];
+	sprintf(s, "%s%u", BARO_BASE_DEVICE_PATH, instance);
+	int fd = open(s, 0);
+
+	if (fd < 0) {
+		if (!optional) {
+			mavlink_and_console_log_critical(mavlink_fd,
+							 "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
+		}
+
+		return false;
+	}
+
+	close(fd);
+	return success;
+}
+
+static bool airspeedCheck(int mavlink_fd, bool optional)
+{
+	bool success = true;
+	int ret;
+	int fd = orb_subscribe(ORB_ID(airspeed));
+
+	struct airspeed_s airspeed;
+
+	if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
+	    (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) {
+		mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
+		success = false;
+		goto out;
+	}
+
+	if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) {
+		mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
+		// XXX do not make this fatal yet
+	}
+
+out:
+	close(fd);
+	return success;
+}
+
+bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro,
+		    bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic)
+{
+	bool failed = false;
+
+	/* ---- MAG ---- */
+	if (checkMag) {
+		/* check all sensors, but fail only for mandatory ones */
+		for (unsigned i = 0; i < max_optional_mag_count; i++) {
+			bool required = (i < max_mandatory_mag_count);
+
+			if (!magnometerCheck(mavlink_fd, i, !required) && required) {
+				failed = true;
+			}
+		}
+	}
+
+	/* ---- ACCEL ---- */
+	if (checkAcc) {
+		/* check all sensors, but fail only for mandatory ones */
+		for (unsigned i = 0; i < max_optional_accel_count; i++) {
+			bool required = (i < max_mandatory_accel_count);
+
+			if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic) && required) {
+				failed = true;
+			}
+		}
+	}
+
+	/* ---- GYRO ---- */
+	if (checkGyro) {
+		/* check all sensors, but fail only for mandatory ones */
+		for (unsigned i = 0; i < max_optional_gyro_count; i++) {
+			bool required = (i < max_mandatory_gyro_count);
+
+			if (!gyroCheck(mavlink_fd, i, !required) && required) {
+				failed = true;
+			}
+		}
+	}
+
+	/* ---- BARO ---- */
+	if (checkBaro) {
+		/* check all sensors, but fail only for mandatory ones */
+		for (unsigned i = 0; i < max_optional_baro_count; i++) {
+			bool required = (i < max_mandatory_baro_count);
+
+			if (!baroCheck(mavlink_fd, i, !required) && required) {
+				failed = true;
+			}
+		}
+	}
+
+	/* ---- AIRSPEED ---- */
+	if (checkAirspeed) {
+		if (!airspeedCheck(mavlink_fd, true)) {
+			failed = true;
+		}
+	}
+
+	/* ---- RC CALIBRATION ---- */
+	if (checkRC) {
+		if (rc_calibration_check(mavlink_fd) != OK) {
+			failed = true;
+		}
+	}
+
+	/* Report status */
+	return !failed;
+}
+
+}
diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h
new file mode 100644
index 000000000..66947a347
--- /dev/null
+++ b/src/modules/commander/PreflightCheck.h
@@ -0,0 +1,80 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2015 PX4 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 PreflightCheck.h
+ *
+ * Preflight check for main system components
+ *
+ * @author Johan Jansen <jnsn.johan@gmail.com>
+ */
+
+#pragma once
+
+namespace Commander
+{
+/**
+* Runs a preflight check on all sensors to see if they are properly calibrated and healthy
+*
+* The function won't fail the test if optional sensors are not found, however,
+* it will fail the test if optional sensors are found but not in working condition.
+*
+* @param mavlink_fd
+*   Mavlink output file descriptor for feedback when a sensor fails
+* @param checkMag
+*   true if the magneteometer should be checked
+* @param checkAcc
+*   true if the accelerometers should be checked
+* @param checkGyro
+*   true if the gyroscopes should be checked
+* @param checkBaro
+*   true if the barometer should be checked
+* @param checkRC
+*   true if the Remote Controller should be checked
+**/
+bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc,
+	bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic = false);
+
+const unsigned max_mandatory_gyro_count = 1;
+const unsigned max_optional_gyro_count = 3;
+
+const unsigned max_mandatory_accel_count = 1;
+const unsigned max_optional_accel_count = 3;
+
+const unsigned max_mandatory_mag_count = 1;
+const unsigned max_optional_mag_count = 3;
+
+const unsigned max_mandatory_baro_count = 1;
+const unsigned max_optional_baro_count = 1;
+
+}
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 409c2ea00..f83640d28 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -152,11 +152,10 @@ static const int ERROR = -1;
 
 static const char *sensor_name = "accel";
 
-int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors);
-int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num);
+calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors);
+calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num);
 int mat_invert3(float src[3][3], float dst[3][3]);
-int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g);
-int accel_calibration_worker(detect_orientation_return orientation, void* worker_data);
+calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g);
 
 /// Data passed to calibration worker routine
 typedef struct  {
@@ -171,7 +170,7 @@ int do_accel_calibration(int mavlink_fd)
 	int fd;
 	int32_t device_id[max_accel_sens];
 
-	mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
+	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
 
 	struct accel_scale accel_scale = {
 		0.0f,
@@ -202,7 +201,7 @@ int do_accel_calibration(int mavlink_fd)
 		close(fd);
 
 		if (res != OK) {
-			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s);
+			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s);
 		}
 	}
 
@@ -210,13 +209,23 @@ int do_accel_calibration(int mavlink_fd)
 	float accel_T[max_accel_sens][3][3];
 	unsigned active_sensors;
 
+	/* measure and calculate offsets & scales */
 	if (res == OK) {
-		/* measure and calculate offsets & scales */
-		res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T, &active_sensors);
+		calibrate_return cal_return = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T, &active_sensors);
+		if (cal_return == calibrate_return_cancelled) {
+			// Cancel message already displayed, nothing left to do
+			return ERROR;
+		} else if (cal_return == calibrate_return_ok) {
+			res = OK;
+		} else {
+			res = ERROR;
+		}
 	}
 
-	if (res != OK || active_sensors == 0) {
-		mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
+	if (res != OK) {
+		if (active_sensors == 0) {
+			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SENSOR_MSG);
+		}
 		return ERROR;
 	}
 
@@ -263,7 +272,7 @@ int do_accel_calibration(int mavlink_fd)
 		failed |= (OK != param_set_no_notification(param_find(str), &(device_id[i])));
 		
 		if (failed) {
-			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG, i);
+			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, i);
 			return ERROR;
 		}
 
@@ -271,7 +280,7 @@ int do_accel_calibration(int mavlink_fd)
 		fd = open(str, 0);
 
 		if (fd < 0) {
-			mavlink_and_console_log_critical(mavlink_fd, "sensor does not exist");
+			mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "sensor does not exist");
 			res = ERROR;
 		} else {
 			res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
@@ -279,7 +288,7 @@ int do_accel_calibration(int mavlink_fd)
 		}
 
 		if (res != OK) {
-			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG, i);
+			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, i);
 		}
 	}
 
@@ -288,41 +297,47 @@ int do_accel_calibration(int mavlink_fd)
 		res = param_save_default();
 
 		if (res != OK) {
-			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
+			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
 		}
 
-		mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
+		/* if there is a any preflight-check system response, let the barrage of messages through */
+		usleep(200000);
+
+		mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
 
 	} else {
-		mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+		mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
 	}
+	
+	/* give this message enough time to propagate */
+	usleep(600000);
 
 	return res;
 }
 
-int accel_calibration_worker(detect_orientation_return orientation, void* data)
+static calibrate_return accel_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
 {
 	const unsigned samples_num = 3000;
 	accel_worker_data_t* worker_data = (accel_worker_data_t*)(data);
 	
-	mavlink_and_console_log_info(worker_data->mavlink_fd, "Hold still, starting to measure %s side", detect_orientation_str(orientation));
+	mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Hold still, measuring %s side", detect_orientation_str(orientation));
 	
 	read_accelerometer_avg(worker_data->subs, worker_data->accel_ref, orientation, samples_num);
 	
-	mavlink_and_console_log_info(worker_data->mavlink_fd, "%s side result: [ %8.4f %8.4f %8.4f ]", detect_orientation_str(orientation),
+	mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side result: [%8.4f %8.4f %8.4f]", detect_orientation_str(orientation),
 				     (double)worker_data->accel_ref[0][orientation][0],
 				     (double)worker_data->accel_ref[0][orientation][1],
 				     (double)worker_data->accel_ref[0][orientation][2]);
 	
 	worker_data->done_count++;
-	mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * worker_data->done_count);
+	mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 17 * worker_data->done_count);
 	
-	return OK;
+	return calibrate_return_ok;
 }
 
-int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors)
+calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors)
 {
-	int result = OK;
+	calibrate_return result = calibrate_return_ok;
 	
 	*active_sensors = 0;
 	
@@ -343,7 +358,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac
 	for (unsigned i = 0; i < max_accel_sens; i++) {
 		worker_data.subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
 		if (worker_data.subs[i] < 0) {
-			result = ERROR;
+			result = calibrate_return_error;
 			break;
 		}
 		
@@ -353,8 +368,10 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac
 		timestamps[i] = arp.timestamp;
 	}
 
-	if (result == OK) {
-		result = calibrate_from_orientation(mavlink_fd, data_collected, accel_calibration_worker, &worker_data);
+	if (result == calibrate_return_ok) {
+		int cancel_sub = calibrate_cancel_subscribe();
+		result = calibrate_from_orientation(mavlink_fd, cancel_sub, data_collected, accel_calibration_worker, &worker_data, false /* normal still */);
+		calibrate_cancel_unsubscribe(cancel_sub);
 	}
 
 	/* close all subscriptions */
@@ -370,13 +387,13 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac
 		}
 	}
 
-	if (result == OK) {
+	if (result == calibrate_return_ok) {
 		/* calculate offsets and transform matrix */
 		for (unsigned i = 0; i < (*active_sensors); i++) {
 			result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
 
-			if (result != OK) {
-				mavlink_and_console_log_critical(mavlink_fd, "ERROR: calibration values calculation error");
+			if (result != calibrate_return_ok) {
+				mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: calibration calculation error");
 				break;
 			}
 		}
@@ -388,7 +405,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac
 /*
  * Read specified number of accelerometer samples, calculate average and dispersion.
  */
-int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num)
+calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num)
 {
 	struct pollfd fds[max_accel_sens];
 
@@ -432,7 +449,7 @@ int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_a
 		}
 
 		if (errcount > samples_num / 10) {
-			return ERROR;
+			return calibrate_return_error;
 		}
 	}
 
@@ -442,7 +459,7 @@ int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_a
 		}
 	}
 
-	return OK;
+	return calibrate_return_ok;
 }
 
 int mat_invert3(float src[3][3], float dst[3][3])
@@ -468,7 +485,7 @@ int mat_invert3(float src[3][3], float dst[3][3])
 	return OK;
 }
 
-int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g)
+calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g)
 {
 	/* calculate offsets */
 	for (unsigned i = 0; i < 3; i++) {
@@ -490,7 +507,7 @@ int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_s
 	float mat_A_inv[3][3];
 
 	if (mat_invert3(mat_A, mat_A_inv) != OK) {
-		return ERROR;
+		return calibrate_return_error;
 	}
 
 	/* copy results to accel_T */
@@ -501,5 +518,5 @@ int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_s
 		}
 	}
 
-	return OK;
+	return calibrate_return_ok;
 }
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
index 747d915ff..837a3f1e8 100644
--- a/src/modules/commander/airspeed_calibration.cpp
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -38,6 +38,7 @@
 
 #include "airspeed_calibration.h"
 #include "calibration_messages.h"
+#include "calibration_routines.h"
 #include "commander_helper.h"
 
 #include <stdio.h>
@@ -61,19 +62,20 @@ static const int ERROR = -1;
 
 static const char *sensor_name = "dpress";
 
-#define HUMAN_ASPD_CAL_FAILED_MSG "Calibration failed, see http://px4.io/help/aspd"
-
 static void feedback_calibration_failed(int mavlink_fd)
 {
 	sleep(5);
-	mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
-	mavlink_log_critical(mavlink_fd, HUMAN_ASPD_CAL_FAILED_MSG);
+	mavlink_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
 }
 
 int do_airspeed_calibration(int mavlink_fd)
 {
+	int result = OK;
+	unsigned calibration_counter = 0;
+	const unsigned maxcount = 3000;
+
 	/* give directions */
-	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
+	mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
 
 	const unsigned calibration_count = 2000;
 
@@ -96,11 +98,13 @@ int do_airspeed_calibration(int mavlink_fd)
 			paramreset_successful = true;
 
 		} else {
-			mavlink_log_critical(mavlink_fd, "airspeed offset zero failed");
+			mavlink_log_critical(mavlink_fd, "[cal] airspeed offset zero failed");
 		}
 
 		close(fd);
 	}
+    
+	int cancel_sub = calibrate_cancel_subscribe();
 
 	if (!paramreset_successful) {
 
@@ -108,26 +112,26 @@ int do_airspeed_calibration(int mavlink_fd)
 		float analog_scaling = 0.0f;
 		param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
 		if (fabsf(analog_scaling) < 0.1f) {
-			mavlink_log_critical(mavlink_fd, "No airspeed sensor, see http://px4.io/help/aspd");
-			close(diff_pres_sub);
-			return ERROR;
+			mavlink_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd");
+			goto error_return;
 		}
 
 		/* set scaling offset parameter */
 		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
-			mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
-			close(diff_pres_sub);
-			return ERROR;
+			mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
+			goto error_return;
 		}
 	}
 
-	unsigned calibration_counter = 0;
-
-	mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind");
+	mavlink_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind");
 	usleep(500 * 1000);
 
 	while (calibration_counter < calibration_count) {
 
+		if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
+			goto error_return;
+		}
+        
 		/* wait blocking for new data */
 		struct pollfd fds[1];
 		fds[0].fd = diff_pres_sub;
@@ -142,14 +146,13 @@ int do_airspeed_calibration(int mavlink_fd)
 			calibration_counter++;
 
 			if (calibration_counter % (calibration_count / 20) == 0) {
-				mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 80) / calibration_count);
+				mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
 			}
 
 		} else if (poll_ret == 0) {
 			/* any poll failure for 1s is a reason to abort */
 			feedback_calibration_failed(mavlink_fd);
-			close(diff_pres_sub);
-			return ERROR;
+			goto error_return;
 		}
 	}
 
@@ -161,16 +164,15 @@ int do_airspeed_calibration(int mavlink_fd)
 		airscale.offset_pa = diff_pres_offset;
 		if (fd_scale > 0) {
 			if (OK != ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
-				mavlink_log_critical(mavlink_fd, "airspeed offset update failed");
+				mavlink_log_critical(mavlink_fd, "[cal] airspeed offset update failed");
 			}
 
 			close(fd_scale);
 		}
 
 		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
-			mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
-			close(diff_pres_sub);
-			return ERROR;
+			mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
+			goto error_return;
 		}
 
 		/* auto-save to EEPROM */
@@ -178,30 +180,31 @@ int do_airspeed_calibration(int mavlink_fd)
 
 		if (save_ret != 0) {
 			warn("WARNING: auto-save of params to storage failed");
-			mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
-			close(diff_pres_sub);
-			return ERROR;
+			mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
+			goto error_return;
 		}
 
 	} else {
 		feedback_calibration_failed(mavlink_fd);
-		close(diff_pres_sub);
-		return ERROR;
+		goto error_return;
 	}
 
-	mavlink_log_critical(mavlink_fd, "Offset of %d Pascal", (int)diff_pres_offset);
+	mavlink_log_critical(mavlink_fd, "[cal] 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");
+	mavlink_log_critical(mavlink_fd, "[cal] Create airflow now");
 
 	calibration_counter = 0;
-	const unsigned maxcount = 3000;
 
 	/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
 	while (calibration_counter < maxcount) {
 
+        if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
+		goto error_return;
+        }
+        
 		/* wait blocking for new data */
 		struct pollfd fds[1];
 		fds[0].fd = diff_pres_sub;
@@ -216,7 +219,7 @@ int do_airspeed_calibration(int mavlink_fd)
 
 			if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
 				if (calibration_counter % 500 == 0) {
-					mavlink_log_info(mavlink_fd, "Create air pressure! (got %d, wanted: 50 Pa)",
+					mavlink_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
 						(int)diff_pres.differential_pressure_raw_pa);
 				}
 				continue;
@@ -224,30 +227,26 @@ int do_airspeed_calibration(int mavlink_fd)
 
 			/* do not allow negative values */
 			if (diff_pres.differential_pressure_raw_pa < 0.0f) {
-				mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)",
+				mavlink_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)",
 						(int)diff_pres.differential_pressure_raw_pa);
-				mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!");
-				close(diff_pres_sub);
+				mavlink_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!");
 
 				/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
 
 				diff_pres_offset = 0.0f;
 				if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
-					mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
-					close(diff_pres_sub);
-					return ERROR;
+					mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
+					goto error_return;
 				}
 
 				/* save */
-				mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 0);
+				mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0);
 				(void)param_save_default();
 
-				close(diff_pres_sub);
-
 				feedback_calibration_failed(mavlink_fd);
-				return ERROR;
+				goto error_return;
 			} else {
-				mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)",
+				mavlink_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)",
 					(int)diff_pres.differential_pressure_raw_pa);
 				break;
 			}
@@ -255,21 +254,30 @@ int do_airspeed_calibration(int mavlink_fd)
 		} else if (poll_ret == 0) {
 			/* any poll failure for 1s is a reason to abort */
 			feedback_calibration_failed(mavlink_fd);
-			close(diff_pres_sub);
-			return ERROR;
+			goto error_return;
 		}
 	}
 
 	if (calibration_counter == maxcount) {
 		feedback_calibration_failed(mavlink_fd);
-		close(diff_pres_sub);
-		return ERROR;
+		goto error_return;
 	}
 
-	mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100);
+	mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
 
-	mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
+	mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
 	tune_neutral(true);
+
+normal_return:
+	calibrate_cancel_unsubscribe(cancel_sub);
 	close(diff_pres_sub);
-	return OK;
+	
+	// This give a chance for the log messages to go out of the queue before someone else stomps on then
+	sleep(1);
+	
+	return result;
+    
+error_return:
+	result = ERROR;
+	goto normal_return;
 }
diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h
index 29f44dc36..7b4baae62 100644
--- a/src/modules/commander/calibration_messages.h
+++ b/src/modules/commander/calibration_messages.h
@@ -42,17 +42,25 @@
 #ifndef CALIBRATION_MESSAGES_H_
 #define CALIBRATION_MESSAGES_H_
 
-#define CAL_STARTED_MSG	"%s calibration: started"
-#define CAL_DONE_MSG	"%s calibration: done"
-#define CAL_FAILED_MSG	"%s calibration: failed"
-#define CAL_PROGRESS_MSG	"%s calibration: progress <%u>"
+// The calibration message defines which begin with CAL_QGC_ are used by QGroundControl to run a state
+// machine to provide visual feedback for calibration. As such, the text for them or semantics of when
+// they are displayed cannot be modified without causing QGC to break. If modifications are made, make
+// sure to bump the calibration version number which will cause QGC to perform log based calibration
+// instead of visual calibration until such a time as QGC is update to the new version.
 
-#define	CAL_FAILED_UNKNOWN_ERROR	"ERROR: unknown error"
-#define CAL_FAILED_SENSOR_MSG		"ERROR: failed reading sensor"
-#define CAL_FAILED_RESET_CAL_MSG	"ERROR: failed to reset calibration, sensor %u"
-#define CAL_FAILED_APPLY_CAL_MSG	"ERROR: failed to apply calibration, sensor %u"
-#define CAL_FAILED_SET_PARAMS_MSG	"ERROR: failed to set parameters, sensor %u"
-#define CAL_FAILED_SAVE_PARAMS_MSG	"ERROR: failed to save parameters"
-#define CAL_FAILED_ORIENTATION_TIMEOUT	"ERROR: timed out waiting for correct orientation"
+// The number in the cal started message is used to indicate the version stamp for the current calibration code.
+#define CAL_QGC_STARTED_MSG			"[cal] calibration started: 1 %s"
+#define CAL_QGC_DONE_MSG			"[cal] calibration done: %s"
+#define CAL_QGC_FAILED_MSG			"[cal] calibration failed: %s"
+#define CAL_QGC_CANCELLED_MSG			"[cal] calibration cancelled"
+#define CAL_QGC_PROGRESS_MSG			"[cal] progress <%u>"
+#define CAL_QGC_ORIENTATION_DETECTED_MSG	"[cal] %s orientation detected"
+#define CAL_QGC_SIDE_DONE_MSG			"[cal] %s side done, rotate to a different side"
+
+#define CAL_ERROR_SENSOR_MSG		"[cal] calibration failed: reading sensor"
+#define CAL_ERROR_RESET_CAL_MSG		"[cal] calibration failed: to reset, sensor %u"
+#define CAL_ERROR_APPLY_CAL_MSG		"[cal] calibration failed: to apply calibration, sensor %u"
+#define CAL_ERROR_SET_PARAMS_MSG	"[cal] calibration failed: to set parameters, sensor %u"
+#define CAL_ERROR_SAVE_PARAMS_MSG	"[cal] calibration failed: failed to save parameters"
 
 #endif /* CALIBRATION_MESSAGES_H_ */
diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp
index 192b8c727..7e8c0fa52 100644
--- a/src/modules/commander/calibration_routines.cpp
+++ b/src/modules/commander/calibration_routines.cpp
@@ -48,6 +48,8 @@
 #include <geo/geo.h>
 #include <string.h>
 
+#include <uORB/topics/vehicle_command.h>
+
 #include "calibration_routines.h"
 #include "calibration_messages.h"
 #include "commander_helper.h"
@@ -230,23 +232,19 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
 	return 0;
 }
 
-enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub)
+enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub, int accel_sub, bool lenient_still_position)
 {
 	const unsigned ndim = 3;
 	
 	struct accel_report sensor;
-	/* exponential moving average of accel */
-	float accel_ema[ndim] = { 0.0f };
-	/* max-hold dispersion of accel */
-	float accel_disp[3] = { 0.0f, 0.0f, 0.0f };
-	/* EMA time constant in seconds*/
-	float ema_len = 0.5f;
-	/* set "still" threshold to 0.25 m/s^2 */
-	float still_thr2 = powf(0.25f, 2);
-	/* set accel error threshold to 5m/s^2 */
-	float accel_err_thr = 5.0f;
-	/* still time required in us */
-	hrt_abstime still_time = 2000000;
+	float		accel_ema[ndim] = { 0.0f };		// exponential moving average of accel
+	float		accel_disp[3] = { 0.0f, 0.0f, 0.0f };	// max-hold dispersion of accel
+	float		ema_len = 0.5f;				// EMA time constant in seconds
+	const float	normal_still_thr = 0.25;		// normal still threshold
+	float		still_thr2 = powf(lenient_still_position ? (normal_still_thr * 3) : normal_still_thr, 2);
+	float		accel_err_thr = 5.0f;			// set accel error threshold to 5m/s^2
+	hrt_abstime	still_time = lenient_still_position ? 1000000 : 1500000;	// still time required in us
+    
 	struct pollfd fds[1];
 	fds[0].fd = accel_sub;
 	fds[0].events = POLLIN;
@@ -308,7 +306,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub)
 				/* is still now */
 				if (t_still == 0) {
 					/* first time */
-					mavlink_and_console_log_info(mavlink_fd, "detected rest position, hold still...");
+					mavlink_and_console_log_info(mavlink_fd, "[cal] detected rest position, hold still...");
 					t_still = t;
 					t_timeout = t + timeout;
 					
@@ -325,8 +323,8 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub)
 				   accel_disp[2] > still_thr2 * 4.0f) {
 				/* not still, reset still start time */
 				if (t_still != 0) {
-					mavlink_and_console_log_info(mavlink_fd, "detected motion, hold still...");
-					sleep(3);
+					mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still...");
+					usleep(500000);
 					t_still = 0;
 				}
 			}
@@ -340,7 +338,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub)
 		}
 		
 		if (poll_errcount > 1000) {
-			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
+			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SENSOR_MSG);
 			return DETECT_ORIENTATION_ERROR;
 		}
 	}
@@ -381,7 +379,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub)
 		return DETECT_ORIENTATION_RIGHTSIDE_UP;        // [ 0, 0, -g ]
 	}
 	
-	mavlink_and_console_log_critical(mavlink_fd, "ERROR: invalid orientation");
+	mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: invalid orientation");
 	
 	return DETECT_ORIENTATION_ERROR;	// Can't detect orientation
 }
@@ -401,28 +399,35 @@ const char* detect_orientation_str(enum detect_orientation_return orientation)
 	return rgOrientationStrs[orientation];
 }
 
-int calibrate_from_orientation(int	mavlink_fd,
-			       bool	side_data_collected[detect_orientation_side_count],
-			       calibration_from_orientation_worker_t calibration_worker,
-			       void*	worker_data)
+calibrate_return calibrate_from_orientation(int		mavlink_fd,
+					    int		cancel_sub,
+					    bool	side_data_collected[detect_orientation_side_count],
+					    calibration_from_orientation_worker_t calibration_worker,
+					    void*	worker_data,
+					    bool	lenient_still_position)
 {
-	int result = OK;
+	calibrate_return result = calibrate_return_ok;
 	
 	// Setup subscriptions to onboard accel sensor
 	
 	int sub_accel = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
 	if (sub_accel < 0) {
-		mavlink_and_console_log_critical(mavlink_fd, "No onboard accel found, abort");
-		return ERROR;
+		mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "No onboard accel");
+		return calibrate_return_error;
 	}
 	
 	unsigned orientation_failures = 0;
 	
-	// Rotate through all three main positions
+	// Rotate through all requested orientation
 	while (true) {
-		if (orientation_failures > 10) {
-			result = ERROR;
-			mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_ORIENTATION_TIMEOUT);
+		if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
+			result = calibrate_return_cancelled;
+			break;
+		}
+		
+		if (orientation_failures > 4) {
+			result = calibrate_return_error;
+			mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "timeout: no motion");
 			break;
 		}
 		
@@ -450,44 +455,102 @@ int calibrate_from_orientation(int	mavlink_fd,
 				strcat(pendingStr, detect_orientation_str((enum detect_orientation_return)cur_orientation));
 			}
 		}
-		mavlink_and_console_log_info(mavlink_fd, "pending:%s", pendingStr);
+		mavlink_and_console_log_info(mavlink_fd, "[cal] pending:%s", pendingStr);
 		
-		mavlink_and_console_log_info(mavlink_fd, "hold the vehicle still on one of the pending sides");
-		enum detect_orientation_return orient = detect_orientation(mavlink_fd, sub_accel);
+		mavlink_and_console_log_info(mavlink_fd, "[cal] hold vehicle still on a pending side");
+		enum detect_orientation_return orient = detect_orientation(mavlink_fd, cancel_sub, sub_accel, lenient_still_position);
 		
 		if (orient == DETECT_ORIENTATION_ERROR) {
 			orientation_failures++;
-			mavlink_and_console_log_info(mavlink_fd, "detected motion, hold still...");
+			mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still...");
 			continue;
 		}
 		
 		/* inform user about already handled side */
 		if (side_data_collected[orient]) {
 			orientation_failures++;
-			mavlink_and_console_log_info(mavlink_fd, "%s side already completed or not needed", detect_orientation_str(orient));
-			mavlink_and_console_log_info(mavlink_fd, "rotate to a pending side");
+			mavlink_and_console_log_info(mavlink_fd, "[cal] %s side completed or not needed", detect_orientation_str(orient));
+			mavlink_and_console_log_info(mavlink_fd, "[cal] rotate to a pending side");
 			continue;
 		}
 		
-		mavlink_and_console_log_info(mavlink_fd, "%s orientation detected", detect_orientation_str(orient));
+		mavlink_and_console_log_info(mavlink_fd, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient));
 		orientation_failures = 0;
 		
 		// Call worker routine
-		calibration_worker(orient, worker_data);
+		result = calibration_worker(orient, cancel_sub, worker_data);
+		if (result != calibrate_return_ok ) {
+			break;
+		}
 		
-		mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", detect_orientation_str(orient));
+		mavlink_and_console_log_info(mavlink_fd, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient));
 		
 		// Note that this side is complete
 		side_data_collected[orient] = true;
 		tune_neutral(true);
-		sleep(1);
+		usleep(500000);
 	}
 	
 	if (sub_accel >= 0) {
 		close(sub_accel);
 	}
 	
-	// FIXME: Do we need an orientation complete routine?
-	
 	return result;
 }
+
+int calibrate_cancel_subscribe(void)
+{
+	return orb_subscribe(ORB_ID(vehicle_command));
+}
+
+void calibrate_cancel_unsubscribe(int cmd_sub)
+{
+	orb_unsubscribe(cmd_sub);
+}
+
+static void calibrate_answer_command(int mavlink_fd, struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result)
+{
+	switch (result) {
+		case VEHICLE_CMD_RESULT_ACCEPTED:
+			tune_positive(true);
+			break;
+			
+		case VEHICLE_CMD_RESULT_DENIED:
+			mavlink_log_critical(mavlink_fd, "command denied during calibration: %u", cmd.command);
+			tune_negative(true);
+			break;
+			
+		default:
+			break;
+	}
+}
+
+bool calibrate_cancel_check(int mavlink_fd, int cancel_sub)
+{
+	struct pollfd fds[1];
+	fds[0].fd = cancel_sub;
+	fds[0].events = POLLIN;
+
+	if (poll(&fds[0], 1, 0) > 0) {
+		struct vehicle_command_s cmd;
+		memset(&cmd, 0, sizeof(cmd));
+		
+		orb_copy(ORB_ID(vehicle_command), cancel_sub, &cmd);
+		
+		if (cmd.command == VEHICLE_CMD_PREFLIGHT_CALIBRATION &&
+		    (int)cmd.param1 == 0 &&
+		    (int)cmd.param2 == 0 &&
+		    (int)cmd.param3 == 0 &&
+		    (int)cmd.param4 == 0 &&
+		    (int)cmd.param5 == 0 &&
+		    (int)cmd.param6 == 0) {
+			calibrate_answer_command(mavlink_fd, cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+			mavlink_log_critical(mavlink_fd, CAL_QGC_CANCELLED_MSG);
+			return true;
+		} else {
+			calibrate_answer_command(mavlink_fd, cmd, VEHICLE_CMD_RESULT_DENIED);
+		}
+	}
+	
+	return false;
+}
\ No newline at end of file
diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h
index 8f34f0204..b8232730a 100644
--- a/src/modules/commander/calibration_routines.h
+++ b/src/modules/commander/calibration_routines.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -31,12 +31,8 @@
  *
  ****************************************************************************/
 
-/**
- * @file calibration_routines.h
- * Calibration routines definitions.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
+/// @file calibration_routines.h
+///	@authot Don Gagne <don@thegagnes.com>
 
 /**
  * Least-squares fit of a sphere to a set of points.
@@ -75,30 +71,45 @@ enum detect_orientation_return {
 };
 static const unsigned detect_orientation_side_count = 6;
 
-/**
- * Wait for vehicle to become still and detect it's orientation.
- *
- * @param mavlink_fd the MAVLink file descriptor to print output to
- * @param accel_sub Subscription to onboard accel
- *
- * @return detect_orientation)_return according to orientation when vehicle is still and ready for measurements,
- * DETECT_ORIENTATION_ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2
- */
-enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub);
+/// Wait for vehicle to become still and detect it's orientation
+///	@return Returns detect_orientation_return according to orientation when vehicle
+///		and ready for measurements
+enum detect_orientation_return detect_orientation(int	mavlink_fd,			///< Mavlink fd to write output to
+						  int	cancel_sub,			///< Cancel subscription from calibration_cancel_subscribe
+						  int	accel_sub,			///< Orb subcription to accel sensor
+						  bool	lenient_still_detection);	///< true: Use more lenient still position detection
 
-
-/**
- * Returns the human readable string representation of the orientation
- *
- * @param orientation Orientation to return string for, "error" if buffer is too small
- *
- * @return str Returned orientation string
- */
+/// Returns the human readable string representation of the orientation
+///	@param orientation Orientation to return string for, "error" if buffer is too small
 const char* detect_orientation_str(enum detect_orientation_return orientation);
 
-typedef int (*calibration_from_orientation_worker_t)(detect_orientation_return orientation, void* worker_data);
+enum calibrate_return {
+	calibrate_return_ok,
+	calibrate_return_error,
+	calibrate_return_cancelled
+};
+
+typedef calibrate_return (*calibration_from_orientation_worker_t)(detect_orientation_return	orientation,	///< Orientation which was detected
+								  int				cancel_sub,	///< Cancel subscription from calibration_cancel_subscribe
+								  void*				worker_data);	///< Opaque worker data
+
+/// Perform calibration sequence which require a rest orientation detection prior to calibration.
+///	@return OK: Calibration succeeded, ERROR: Calibration failed
+calibrate_return calibrate_from_orientation(int		mavlink_fd,						///< Mavlink fd to write output to
+					    int		cancel_sub,						///< Cancel subscription from calibration_cancel_subscribe
+					    bool	side_data_collected[detect_orientation_side_count],	///< Sides for which data still needs calibration
+					    calibration_from_orientation_worker_t calibration_worker,		///< Worker routine which performs the actual calibration
+					    void*	worker_data,						///< Opaque data passed to worker routine
+					    bool	lenient_still_detection);				///< true: Use more lenient still position detection
+
+/// Called at the beginning of calibration in order to subscribe to the cancel command
+///	@return Handle to vehicle_command subscription
+int calibrate_cancel_subscribe(void);
+
+/// Called to cancel the subscription to the cancel command
+///	@param cancel_sub Cancel subcription from calibration_cancel_subscribe
+void calibrate_cancel_unsubscribe(int cancel_sub);
 
-int calibrate_from_orientation(int mavlink_fd,
-			       bool side_data_collected[detect_orientation_side_count],
-			       calibration_from_orientation_worker_t calibration_worker,
-			       void* worker_data);
+/// Used to periodically check for a cancel command
+bool calibrate_cancel_check(int mavlink_fd,	///< Mavlink fd for output
+			    int cancel_sub);	///< Cancel subcription fromcalibration_cancel_subscribe
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 453d5f921..7aaf5e5cd 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -114,6 +114,7 @@
 #include "baro_calibration.h"
 #include "rc_calibration.h"
 #include "airspeed_calibration.h"
+#include "PreflightCheck.h"
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -181,21 +182,6 @@ static struct safety_s safety;
 static struct vehicle_control_mode_s control_mode;
 static struct offboard_control_mode_s offboard_control_mode;
 
-/* tasks waiting for low prio thread */
-typedef enum {
-	LOW_PRIO_TASK_NONE = 0,
-	LOW_PRIO_TASK_PARAM_SAVE,
-	LOW_PRIO_TASK_PARAM_LOAD,
-	LOW_PRIO_TASK_GYRO_CALIBRATION,
-	LOW_PRIO_TASK_MAG_CALIBRATION,
-	LOW_PRIO_TASK_ALTITUDE_CALIBRATION,
-	LOW_PRIO_TASK_RC_CALIBRATION,
-	LOW_PRIO_TASK_ACCEL_CALIBRATION,
-	LOW_PRIO_TASK_AIRSPEED_CALIBRATION
-} low_prio_task_t;
-
-static low_prio_task_t low_prio_task = LOW_PRIO_TASK_NONE;
-
 /**
  * The daemon app only briefly exists to start
  * the background job. The stack size assigned in the
@@ -227,6 +213,8 @@ int commander_thread_main(int argc, char *argv[]);
 
 void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed);
 
+void get_circuit_breaker_params();
+
 void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
 
 transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man);
@@ -261,7 +249,7 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
 
 int commander_main(int argc, char *argv[])
 {
-	if (argc < 1) {
+	if (argc < 2) {
 		usage("missing command");
 	}
 
@@ -535,9 +523,9 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 		break;
 
 	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
+
 			// Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints
 			// for logic state parameters
-
 			if (static_cast<int>(cmd->param1 + 0.5f) != 0 && static_cast<int>(cmd->param1 + 0.5f) != 1) {
 				mavlink_log_critical(mavlink_fd, "Unsupported ARM_DISARM param: %.3f", (double)cmd->param1);
 
@@ -549,6 +537,16 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 				if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) {
 					status_local->arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE;
 				}
+				else {
+
+					// Refuse to arm if preflight checks have failed
+					if (!status.hil_state != vehicle_status_s::HIL_STATE_ON && !status.condition_system_sensors_initialized) {
+						mavlink_log_critical(mavlink_fd, "Arming DENIED. Preflight checks have failed.");
+						cmd_result = VEHICLE_CMD_RESULT_DENIED;			
+						break;
+					}
+					
+				}
 
 				transition_result_t arming_res = arm_disarm(cmd_arms, mavlink_fd, "arm/disarm component command");
 
@@ -949,10 +947,10 @@ int commander_thread_main(int argc, char *argv[])
 
 	if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
 		if (mission.dataman_id >= 0 && mission.dataman_id <= 1) {
-			warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count,
-			      mission.current_seq);
-			mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d",
-					 mission.dataman_id, mission.count, mission.current_seq);
+			if (mission.count > 0) {
+				mavlink_log_info(mavlink_fd, "[cmd] Mission #%d loaded, %u WPs, curr: %d",
+						 mission.dataman_id, mission.count, mission.current_seq);
+			}
 
 		} else {
 			const char *missionfail = "reading mission state failed";
@@ -1021,7 +1019,7 @@ int commander_thread_main(int argc, char *argv[])
 	bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM];
 
 	for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
-		telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
+		telemetry_subs[i] = -1;
 		telemetry_last_heartbeat[i] = 0;
 		telemetry_last_dl_loss[i] = 0;
 		telemetry_lost[i] = true;
@@ -1114,6 +1112,28 @@ int commander_thread_main(int argc, char *argv[])
 	commander_initialized = true;
 	thread_running = true;
 
+	/* update vehicle status to find out vehicle type (required for preflight checks) */
+	param_get(_param_sys_type, &(status.system_type)); // get system type
+	status.is_rotary_wing = is_rotary_wing(&status) || is_vtol(&status);
+
+	get_circuit_breaker_params();
+
+	bool checkAirspeed = false;
+	/* Perform airspeed check only if circuit breaker is not
+	 * engaged and it's not a rotary wing */
+	if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
+		checkAirspeed = true;
+	}
+
+	// Run preflight check
+	status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
+	if (!status.condition_system_sensors_initialized) {
+		set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune
+	}
+	else {
+		set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
+	}
+
 	const hrt_abstime commander_boot_timestamp = hrt_absolute_time();
 
 	transition_result_t arming_ret;
@@ -1139,7 +1159,7 @@ int commander_thread_main(int argc, char *argv[])
 	/* initialize low priority thread */
 	pthread_attr_t commander_low_prio_attr;
 	pthread_attr_init(&commander_low_prio_attr);
-	pthread_attr_setstacksize(&commander_low_prio_attr, 2100);
+	pthread_attr_setstacksize(&commander_low_prio_attr, 2000);
 
 	struct sched_param param;
 	(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
@@ -1177,15 +1197,7 @@ int commander_thread_main(int argc, char *argv[])
 				}
 
 				/* disable manual override for all systems that rely on electronic stabilization */
-				if (status.system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL ||
-				    status.system_type == vehicle_status_s::VEHICLE_TYPE_HELICOPTER ||
-				    status.system_type == vehicle_status_s::VEHICLE_TYPE_TRICOPTER ||
-				    status.system_type == vehicle_status_s::VEHICLE_TYPE_QUADROTOR ||
-				    status.system_type == vehicle_status_s::VEHICLE_TYPE_HEXAROTOR ||
-				    status.system_type == vehicle_status_s::VEHICLE_TYPE_OCTOROTOR ||
-				    (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) ||
-				    (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) {
-
+				if (is_rotary_wing(&status) || (is_vtol(&status) && vtol_status.vtol_in_rw_mode)) {
 					status.is_rotary_wing = true;
 
 				} else {
@@ -1193,21 +1205,13 @@ int commander_thread_main(int argc, char *argv[])
 				}
 
 				/* set vehicle_status.is_vtol flag */
-				status.is_vtol = (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR) ||
-						 (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR);
+				status.is_vtol = is_vtol(&status);
 
 				/* check and update system / component ID */
 				param_get(_param_system_id, &(status.system_id));
 				param_get(_param_component_id, &(status.component_id));
 
-				status.circuit_breaker_engaged_power_check =
-					circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
-				status.circuit_breaker_engaged_airspd_check =
-					circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
-				status.circuit_breaker_engaged_enginefailure_check =
-					circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY);
-				status.circuit_breaker_engaged_gpsfailure_check =
-					circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY);
+				get_circuit_breaker_params();
 
 				status_changed = true;
 
@@ -1268,6 +1272,11 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
+
+			if (telemetry_subs[i] < 0 && (OK == orb_exists(telemetry_status_orb_id[i], 0))) {
+				telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
+			}
+
 			orb_check(telemetry_subs[i], &updated);
 
 			if (updated) {
@@ -1282,7 +1291,15 @@ int commander_thread_main(int argc, char *argv[])
 				    telemetry.heartbeat_time > 0 &&
 				    hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) {
 
-					(void)rc_calibration_check(mavlink_fd);
+				    	bool chAirspeed = false;
+					/* Perform airspeed check only if circuit breaker is not
+					 * engaged and it's not a rotary wing */
+					if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
+						chAirspeed = true;
+					}
+
+					/* provide RC and sensor status feedback to the user */
+					(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, true);
 				}
 
 				telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
@@ -1387,8 +1404,8 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status);
 			status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab;
 
-			/* Make sure that this is only adjusted if vehicle realy is of type vtol*/
-			if ((status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR)) {
+			/* Make sure that this is only adjusted if vehicle really is of type vtol*/
+			if (is_vtol(&status)) {
 				status.is_rotary_wing = vtol_status.vtol_in_rw_mode;
 			}
 		}
@@ -1549,7 +1566,9 @@ int commander_thread_main(int argc, char *argv[])
 		/* if battery voltage is getting lower, warn using buzzer, etc. */
 		if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) {
 			low_battery_voltage_actions_done = true;
-			mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
+			if (armed.armed) {
+				mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
+			}
 			status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW;
 			status_changed = true;
 
@@ -1557,7 +1576,6 @@ int commander_thread_main(int argc, char *argv[])
 			   && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
 			/* critical battery voltage, this is rather an emergency, change state machine */
 			critical_battery_voltage_actions_done = true;
-			mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
 			status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL;
 
 			if (!armed.armed) {
@@ -1567,7 +1585,11 @@ int commander_thread_main(int argc, char *argv[])
 
 				if (arming_ret == TRANSITION_CHANGED) {
 					arming_state_changed = true;
+					mavlink_and_console_log_critical(mavlink_fd, "LOW BATTERY, LOCKING ARMING DOWN");
 				}
+
+			} else {
+				mavlink_and_console_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
 			}
 
 			status_changed = true;
@@ -1576,8 +1598,7 @@ int commander_thread_main(int argc, char *argv[])
 		/* End battery voltage check */
 
 		/* If in INIT state, try to proceed to STANDBY state */
-		if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
-			/* TODO: check for sensors */
+		if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
 			arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */,
 							     mavlink_fd);
 
@@ -1585,8 +1606,6 @@ int commander_thread_main(int argc, char *argv[])
 				arming_state_changed = true;
 			}
 
-		} else {
-			/* TODO: Add emergency stuff if sensors are lost */
 		}
 
 
@@ -1793,13 +1812,17 @@ int commander_thread_main(int argc, char *argv[])
 		for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
 			if (telemetry_last_heartbeat[i] != 0 &&
 			    hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) {
-				/* handle the case where data link was regained,
+				/* handle the case where data link was gained first time or regained,
 				 * accept datalink as healthy only after datalink_regain_timeout seconds
 				 * */
 				if (telemetry_lost[i] &&
 				    hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) {
 
-					mavlink_log_info(mavlink_fd, "data link %i regained", i);
+					/* only report a regain */
+					if (telemetry_last_dl_loss[i] > 0) {
+						mavlink_and_console_log_critical(mavlink_fd, "data link #%i regained", i);
+					}
+
 					telemetry_lost[i] = false;
 					have_link = true;
 
@@ -1810,10 +1833,12 @@ int commander_thread_main(int argc, char *argv[])
 				}
 
 			} else {
-				telemetry_last_dl_loss[i]  = hrt_absolute_time();
 
 				if (!telemetry_lost[i]) {
-					mavlink_log_info(mavlink_fd, "data link %i lost", i);
+					/* only reset the timestamp to a different time on state change */
+					telemetry_last_dl_loss[i]  = hrt_absolute_time();
+
+					mavlink_and_console_log_critical(mavlink_fd, "data link #%i lost", i);
 					telemetry_lost[i] = true;
 				}
 			}
@@ -1828,7 +1853,7 @@ int commander_thread_main(int argc, char *argv[])
 
 		} else {
 			if (!status.data_link_lost) {
-				mavlink_log_info(mavlink_fd, "ALL DATA LINKS LOST");
+				mavlink_and_console_log_critical(mavlink_fd, "ALL DATA LINKS LOST");
 				status.data_link_lost = true;
 				status.data_link_lost_counter++;
 				status_changed = true;
@@ -2080,6 +2105,19 @@ int commander_thread_main(int argc, char *argv[])
 }
 
 void
+get_circuit_breaker_params()
+{
+	status.circuit_breaker_engaged_power_check =
+		circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
+	status.circuit_breaker_engaged_airspd_check =
+		circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
+	status.circuit_breaker_engaged_enginefailure_check =
+		circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY);
+	status.circuit_breaker_engaged_gpsfailure_check =
+		circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY);
+}
+
+void
 check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed)
 {
 	hrt_abstime t = hrt_absolute_time();
@@ -2103,7 +2141,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
 			rgbled_set_mode(RGBLED_MODE_ON);
 			set_normal_color = true;
 
-		} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) {
+		} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR || !status.condition_system_sensors_initialized) {
 			rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
 			rgbled_set_color(RGBLED_COLOR_RED);
 
@@ -2185,13 +2223,34 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 
 		if (res == TRANSITION_DENIED) {
 			print_reject_mode(status_local, "OFFBOARD");
+			/* mode rejected, continue to evaluate the main system mode */
 
 		} else {
+			/* changed successfully or already in this state */
+			return res;
+		}
+	}
+
+	/* RTL switch overrides main switch */
+	if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
+		res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_RTL);
+
+		if (res == TRANSITION_DENIED) {
+			print_reject_mode(status_local, "AUTO_RTL");
+
+			/* fallback to LOITER if home position not set */
+			res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER);
+		}
+
+		if (res != TRANSITION_DENIED) {
+			/* changed successfully or already in this state */
 			return res;
 		}
+
+		/* if we get here mode was rejected, continue to evaluate the main system mode */
 	}
 
-	/* offboard switched off or denied, check main mode switch */
+	/* offboard and RTL switches off or denied, check main mode switch */
 	switch (sp_man->mode_switch) {
 	case manual_control_setpoint_s::SWITCH_POS_NONE:
 		res = TRANSITION_NOT_CHANGED;
@@ -2236,23 +2295,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 		break;
 
 	case manual_control_setpoint_s::SWITCH_POS_ON:			// AUTO
-		if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
-			res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_RTL);
-
-			if (res != TRANSITION_DENIED) {
-				break;	// changed successfully or already in this state
-			}
-
-			print_reject_mode(status_local, "AUTO_RTL");
-
-			// fallback to LOITER if home position not set
-			res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER);
-
-			if (res != TRANSITION_DENIED) {
-				break;  // changed successfully or already in this state
-			}
-
-		} else if (sp_man->loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
+		if (sp_man->loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
 			res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER);
 
 			if (res != TRANSITION_DENIED) {
@@ -2623,7 +2666,7 @@ void *commander_low_prio_loop(void *arg)
 
 					/* try to go to INIT/PREFLIGHT arming state */
 					if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed,
-							true /* fRunPreArmChecks */, mavlink_fd)) {
+							false /* fRunPreArmChecks */, mavlink_fd)) {
 						answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
 						break;
 					}
@@ -2672,22 +2715,33 @@ void *commander_low_prio_loop(void *arg)
 							/* enable RC control input */
 							status.rc_input_blocked = false;
 							mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN");
+                            calib_ret = OK;
 						}
-
-						/* this always succeeds */
-						calib_ret = OK;
-
 					}
 
 					if (calib_ret == OK) {
 						tune_positive(true);
 
+						// Update preflight check status
+						// we do not set the calibration return value based on it because the calibration
+						// might have worked just fine, but the preflight check fails for a different reason,
+						// so this would be prone to false negatives.
+
+						bool checkAirspeed = false;
+						/* Perform airspeed check only if circuit breaker is not
+						 * engaged and it's not a rotary wing */
+						if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
+							checkAirspeed = true;
+						}
+
+						status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
+
+						arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
+
 					} else {
 						tune_negative(true);
 					}
 
-					arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
-
 					break;
 				}
 
@@ -2716,9 +2770,15 @@ void *commander_low_prio_loop(void *arg)
 						}
 
 					} else if (((int)(cmd.param1)) == 1) {
+
 						int ret = param_save_default();
 
 						if (ret == OK) {
+							if (need_param_autosave) {
+								need_param_autosave = false;
+								need_param_autosave_timeout = 0;
+							}
+
 							mavlink_log_info(mavlink_fd, "settings saved");
 							answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index a5e4d1972..99b926be4 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -84,6 +84,13 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status)
 	       || (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL);
 }
 
+bool is_vtol(const struct vehicle_status_s * current_status) {
+	return (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR ||
+		current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR ||
+		current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_HEXAROTOR ||
+		current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_OCTOROTOR);
+}
+
 static int buzzer = -1;
 static hrt_abstime blink_msg_end = 0;	// end time for currently blinking LED message, 0 if no blink message
 static hrt_abstime tune_end = 0;		// end time of currently played tune, 0 for repeating tunes or silence
diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h
index 0cefedba7..bf0c0505d 100644
--- a/src/modules/commander/commander_helper.h
+++ b/src/modules/commander/commander_helper.h
@@ -51,6 +51,7 @@
 
 bool is_multirotor(const struct vehicle_status_s *current_status);
 bool is_rotary_wing(const struct vehicle_status_s *current_status);
+bool is_vtol(const struct vehicle_status_s *current_status);
 
 int buzzer_init(void);
 void buzzer_deinit(void);
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index a55012243..6663525cc 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -105,7 +105,7 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
  *
  * Set to 1 to enable actions triggered when the datalink is lost.
  *
- * @group commander
+ * @group Commander
  * @min 0
  * @max 1
  */
@@ -115,7 +115,7 @@ PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0);
  *
  * After this amount of seconds without datalink the data link lost mode triggers
  *
- * @group commander
+ * @group Commander
  * @unit second
  * @min 0
  * @max 30
@@ -127,7 +127,7 @@ PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10);
  * After a data link loss: after this this amount of seconds with a healthy datalink the 'datalink loss'
  * flag is set back to false
  *
- * @group commander
+ * @group Commander
  * @unit second
  * @min 0
  * @max 30
@@ -138,9 +138,9 @@ PARAM_DEFINE_INT32(COM_DL_REG_T, 0);
  *
  * Engine failure triggers only above this throttle value
  *
- * @group commander
- * @min 0.0f
- * @max 1.0f
+ * @group Commander
+ * @min 0.0
+ * @max 1.0
  */
 PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f);
 
@@ -148,9 +148,9 @@ PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f);
  *
  * Engine failure triggers only below this current/throttle value
  *
- * @group commander
- * @min 0.0f
- * @max 7.0f
+ * @group Commander
+ * @min 0.0
+ * @max 7.0
  */
 PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f);
 
@@ -159,10 +159,10 @@ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f);
  * Engine failure triggers only if the throttle threshold and the
  * current to throttle threshold are violated for this time
  *
- * @group commander
+ * @group Commander
  * @unit second
- * @min 0.0f
- * @max 7.0f
+ * @min 0.0
+ * @max 7.0
  */
 PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f);
 
@@ -170,7 +170,7 @@ PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f);
  *
  * After this amount of seconds without RC connection the rc lost flag is set to true
  *
- * @group commander
+ * @group Commander
  * @unit second
  * @min 0
  * @max 35
@@ -183,7 +183,7 @@ PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5);
  * Default is on, as the interoperability with currently deployed GCS solutions depends on parameters
  * being sticky. Developers can default it to off.
  *
- * @group commander
+ * @group Commander
  * @min 0
  * @max 1
  */
diff --git a/src/modules/commander/commander_tests/module.mk b/src/modules/commander/commander_tests/module.mk
index 4d10275d1..64afa86c4 100644
--- a/src/modules/commander/commander_tests/module.mk
+++ b/src/modules/commander/commander_tests/module.mk
@@ -38,4 +38,5 @@
 MODULE_COMMAND		= commander_tests
 SRCS			= commander_tests.cpp \
 			state_machine_helper_test.cpp \
-			../state_machine_helper.cpp
+			../state_machine_helper.cpp \
+			../PreflightCheck.cpp
diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
index 4ddb4e0fb..967304cb4 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
@@ -255,10 +255,10 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
 
         // Sensor tests
 
-        { "no transition: init to standby - sensors not initialized",
+        { "transition to standby error: init to standby - sensors not initialized",
             { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
             vehicle_status_s::ARMING_STATE_STANDBY,
-            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
 
         // Safety switch arming tests
 
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index e2a7ef743..bdef8771e 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -39,6 +39,7 @@
 
 #include "gyro_calibration.h"
 #include "calibration_messages.h"
+#include "calibration_routines.h"
 #include "commander_helper.h"
 
 #include <stdio.h>
@@ -62,149 +63,180 @@ static const int ERROR = -1;
 
 static const char *sensor_name = "gyro";
 
-int do_gyro_calibration(int mavlink_fd)
-{
-	const unsigned max_gyros = 3;
-
-	int32_t device_id[3];
-	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
-	mavlink_log_info(mavlink_fd, "HOLD STILL");
-
-	/* wait for the user to respond */
-	sleep(2);
-
-	struct gyro_scale gyro_scale_zero = {
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-	};
-
-	struct gyro_scale gyro_scale[max_gyros] = {};
-
-	int res = OK;
+static const unsigned max_gyros = 3;
 
-	/* store board ID */
-	uint32_t mcu_id[3];
-	mcu_unique_id(&mcu_id[0]);
-
-	/* store last 32bit number - not unique, but unique in a given set */
-	(void)param_set(param_find("CAL_BOARD_ID"), &mcu_id[2]);
-
-	char str[30];
+/// Data passed to calibration worker routine
+typedef struct  {
+	int			mavlink_fd;
+	int32_t			device_id[max_gyros];
+	int			gyro_sensor_sub[max_gyros];
+	struct gyro_scale	gyro_scale[max_gyros];
+	struct gyro_report	gyro_report_0;
+} gyro_worker_data_t;
 
+static calibrate_return gyro_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
+{
+	gyro_worker_data_t*	worker_data = (gyro_worker_data_t*)(data);
+	unsigned		calibration_counter[max_gyros] = { 0 };
+	const unsigned		calibration_count = 5000;
+	struct gyro_report	gyro_report;
+	unsigned		poll_errcount = 0;
+	
+	struct pollfd fds[max_gyros];
 	for (unsigned s = 0; s < max_gyros; s++) {
-
-		/* ensure all scale fields are initialized tha same as the first struct */
-		(void)memcpy(&gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale[0]));
-
-		sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
-		/* reset all offsets to zero and all scales to one */
-		int fd = open(str, 0);
-
-		if (fd < 0) {
-			continue;
+		fds[s].fd = worker_data->gyro_sensor_sub[s];
+		fds[s].events = POLLIN;
+	}
+	
+	memset(&worker_data->gyro_report_0, 0, sizeof(worker_data->gyro_report_0));
+	
+	/* use first gyro to pace, but count correctly per-gyro for statistics */
+	while (calibration_counter[0] < calibration_count) {
+		if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) {
+			return calibrate_return_cancelled;
 		}
-
-		device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0);
-
-		res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero);
-		close(fd);
-
-		if (res != OK) {
-			mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s);
+		
+		int poll_ret = poll(&fds[0], max_gyros, 1000);
+		
+		if (poll_ret > 0) {
+			
+			for (unsigned s = 0; s < max_gyros; s++) {
+				bool changed;
+				orb_check(worker_data->gyro_sensor_sub[s], &changed);
+				
+				if (changed) {
+					orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &gyro_report);
+					
+					if (s == 0) {
+						orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &worker_data->gyro_report_0);
+					}
+					
+					worker_data->gyro_scale[s].x_offset += gyro_report.x;
+					worker_data->gyro_scale[s].y_offset += gyro_report.y;
+					worker_data->gyro_scale[s].z_offset += gyro_report.z;
+					calibration_counter[s]++;
+				}
+				
+				if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) {
+					mavlink_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter[0] * 100) / calibration_count);
+				}
+			}
+			
+		} else {
+			poll_errcount++;
+		}
+		
+		if (poll_errcount > 1000) {
+			mavlink_log_critical(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG);
+			return calibrate_return_error;
 		}
 	}
-
-	unsigned calibration_counter[max_gyros] = { 0 };
-	const unsigned calibration_count = 5000;
-
-	struct gyro_report gyro_report_0 = {};
-
-	if (res == OK) {
-		/* determine gyro mean values */
-		unsigned poll_errcount = 0;
-
-		/* subscribe to gyro sensor topic */
-		int sub_sensor_gyro[max_gyros];
-		struct pollfd fds[max_gyros];
-
-		for (unsigned s = 0; s < max_gyros; s++) {
-			sub_sensor_gyro[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
-			fds[s].fd = sub_sensor_gyro[s];
-			fds[s].events = POLLIN;
+	
+	for (unsigned s = 0; s < max_gyros; s++) {
+		if (worker_data->device_id[s] != 0 && calibration_counter[s] < calibration_count / 2) {
+			mavlink_log_critical(worker_data->mavlink_fd, "[cal] ERROR: missing data, sensor %d", s)
+			return calibrate_return_error;
 		}
 
-		struct gyro_report gyro_report;
-
-		/* use first gyro to pace, but count correctly per-gyro for statistics */
-		while (calibration_counter[0] < calibration_count) {
-			/* wait blocking for new data */
-
-			int poll_ret = poll(&fds[0], max_gyros, 1000);
-
-			if (poll_ret > 0) {
-
-				for (unsigned s = 0; s < max_gyros; s++) {
-					bool changed;
-					orb_check(sub_sensor_gyro[s], &changed);
-
-					if (changed) {
-						orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report);
-
-						if (s == 0) {
-							orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report_0);
-						}
+		worker_data->gyro_scale[s].x_offset /= calibration_counter[s];
+		worker_data->gyro_scale[s].y_offset /= calibration_counter[s];
+		worker_data->gyro_scale[s].z_offset /= calibration_counter[s];
+	}
 
-						gyro_scale[s].x_offset += gyro_report.x;
-						gyro_scale[s].y_offset += gyro_report.y;
-						gyro_scale[s].z_offset += gyro_report.z;
-						calibration_counter[s]++;
-					}
+	return calibrate_return_ok;
+}
 
-					if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) {
-						mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter[0] * 100) / calibration_count);
-					}
-				}
+int do_gyro_calibration(int mavlink_fd)
+{
+	int			res = OK;
+	gyro_worker_data_t	worker_data;
 
-			} else {
-				poll_errcount++;
-			}
+	mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
 
-			if (poll_errcount > 1000) {
-				mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
-				res = ERROR;
-				break;
-			}
+	worker_data.mavlink_fd = mavlink_fd;
+	
+	struct gyro_scale gyro_scale_zero = {
+		0.0f,	// x offset
+		1.0f,	// x scale
+		0.0f,	// y offset
+		1.0f,	// y scale
+		0.0f,	// z offset
+		1.0f,	// z scale
+	};
+	
+	for (unsigned s = 0; s < max_gyros; s++) {
+		char str[30];
+		
+		// Reset gyro ids to unavailable
+		worker_data.device_id[s] = 0;
+		(void)sprintf(str, "CAL_GYRO%u_ID", s);
+		res = param_set_no_notification(param_find(str), &(worker_data.device_id[s]));
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, "[cal] Unable to reset CAL_GYRO%u_ID", s);
+			return ERROR;
 		}
+		
+		// Reset all offsets to 0 and scales to 1
+		(void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale));
+		sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
+		int fd = open(str, 0);
+		if (fd >= 0) {
+			worker_data.device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0);
+			res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero);
+			close(fd);
 
-		for (unsigned s = 0; s < max_gyros; s++) {
-			close(sub_sensor_gyro[s]);
-
-			gyro_scale[s].x_offset /= calibration_counter[s];
-			gyro_scale[s].y_offset /= calibration_counter[s];
-			gyro_scale[s].z_offset /= calibration_counter[s];
+			if (res != OK) {
+				mavlink_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s);
+				return ERROR;
+			}
 		}
+		
+	}
+	
+	for (unsigned s = 0; s < max_gyros; s++) {
+		worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
+	}
+	
+	// Calibrate right-side up
+	
+	bool side_collected[detect_orientation_side_count] = { true, true, true, true, true, false };
+	
+	int cancel_sub  = calibrate_cancel_subscribe();	
+	calibrate_return cal_return = calibrate_from_orientation(mavlink_fd,                 // Mavlink fd to write output
+								cancel_sub,                 // Subscription to vehicle_command for cancel support
+								side_collected,		// Sides to calibrate
+								gyro_calibration_worker,     // Calibration worker
+								&worker_data,		// Opaque data for calibration worked
+								true);			// true: lenient still detection
+	calibrate_cancel_unsubscribe(cancel_sub);
+	
+	for (unsigned s = 0; s < max_gyros; s++) {
+		close(worker_data.gyro_sensor_sub[s]);
 	}
 
+	if (cal_return == calibrate_return_cancelled) {
+		// Cancel message already sent, we are done here
+		return ERROR;
+	} else if (cal_return == calibrate_return_error) {
+		res = ERROR;
+	}
+		
 	if (res == OK) {
 		/* check offsets */
-		float xdiff = gyro_report_0.x - gyro_scale[0].x_offset;
-		float ydiff = gyro_report_0.y - gyro_scale[0].y_offset;
-		float zdiff = gyro_report_0.z - gyro_scale[0].z_offset;
+		float xdiff = worker_data.gyro_report_0.x - worker_data.gyro_scale[0].x_offset;
+		float ydiff = worker_data.gyro_report_0.y - worker_data.gyro_scale[0].y_offset;
+		float zdiff = worker_data.gyro_report_0.z - worker_data.gyro_scale[0].z_offset;
 
 		/* maximum allowable calibration error in radians */
-		const float maxoff = 0.002f;
+		const float maxoff = 0.0055f;
 
-		if (!isfinite(gyro_scale[0].x_offset) ||
-		    !isfinite(gyro_scale[0].y_offset) ||
-		    !isfinite(gyro_scale[0].z_offset) ||
+		if (!isfinite(worker_data.gyro_scale[0].x_offset) ||
+		    !isfinite(worker_data.gyro_scale[0].y_offset) ||
+		    !isfinite(worker_data.gyro_scale[0].z_offset) ||
 		    fabsf(xdiff) > maxoff ||
 		    fabsf(ydiff) > maxoff ||
 		    fabsf(zdiff) > maxoff) {
-			mavlink_log_critical(mavlink_fd, "ERROR: Motion during calibration");
+			mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: Motion during calibration");
 			res = ERROR;
 		}
 	}
@@ -214,59 +246,69 @@ int do_gyro_calibration(int mavlink_fd)
 		bool failed = false;
 
 		for (unsigned s = 0; s < max_gyros; s++) {
+			if (worker_data.device_id[s] != 0) {
+				char str[30];
+				
+				(void)sprintf(str, "CAL_GYRO%u_XOFF", s);
+				failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].x_offset)));
+				(void)sprintf(str, "CAL_GYRO%u_YOFF", s);
+				failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].y_offset)));
+				(void)sprintf(str, "CAL_GYRO%u_ZOFF", s);
+				failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].z_offset)));
+				(void)sprintf(str, "CAL_GYRO%u_ID", s);
+				failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.device_id[s])));
+
+				/* apply new scaling and offsets */
+				(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
+				int fd = open(str, 0);
+
+				if (fd < 0) {
+					failed = true;
+					continue;
+				}
 
-			/* if any reasonable amount of data is missing, skip */
-			if (calibration_counter[s] < calibration_count / 2) {
-				continue;
-			}
-
-			(void)sprintf(str, "CAL_GYRO%u_XOFF", s);
-			failed |= (OK != param_set(param_find(str), &(gyro_scale[s].x_offset)));
-			(void)sprintf(str, "CAL_GYRO%u_YOFF", s);
-			failed |= (OK != param_set(param_find(str), &(gyro_scale[s].y_offset)));
-			(void)sprintf(str, "CAL_GYRO%u_ZOFF", s);
-			failed |= (OK != param_set(param_find(str), &(gyro_scale[s].z_offset)));
-			(void)sprintf(str, "CAL_GYRO%u_ID", s);
-			failed |= (OK != param_set(param_find(str), &(device_id[s])));
-
-			/* apply new scaling and offsets */
-			(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
-			int fd = open(str, 0);
-
-			if (fd < 0) {
-				failed = true;
-				continue;
-			}
-
-			res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale[s]);
-			close(fd);
+				res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&worker_data.gyro_scale[s]);
+				close(fd);
 
-			if (res != OK) {
-				mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
+				if (res != OK) {
+					mavlink_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG);
+				}
 			}
 		}
 
 		if (failed) {
-			mavlink_and_console_log_critical(mavlink_fd, "ERROR: failed to set offset params");
+			mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: failed to set offset params");
 			res = ERROR;
 		}
 	}
 
+	/* store board ID */
+	uint32_t mcu_id[3];
+	mcu_unique_id(&mcu_id[0]);
+
+	/* store last 32bit number - not unique, but unique in a given set */
+	(void)param_set(param_find("CAL_BOARD_ID"), &mcu_id[2]);
+
 	if (res == OK) {
 		/* auto-save to EEPROM */
 		res = param_save_default();
 
 		if (res != OK) {
-			mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
+			mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
 		}
 	}
 
-	if (res == OK) {
-		mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
+	/* if there is a any preflight-check system response, let the barrage of messages through */
+	usleep(200000);
 
+	if (res == OK) {
+		mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
 	} else {
-		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+		mavlink_log_info(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
 	}
 
+	/* give this message enough time to propagate */
+	usleep(600000);
+	
 	return res;
 }
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 198acb027..8a8d85818 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -65,8 +65,7 @@ static const int ERROR = -1;
 static const char *sensor_name = "mag";
 static const unsigned max_mags = 3;
 
-int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
-int mag_calibration_worker(detect_orientation_return orientation, void* worker_data);
+calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
 
 /// Data passed to calibration worker routine
 typedef struct  {
@@ -86,7 +85,7 @@ typedef struct  {
 
 int do_mag_calibration(int mavlink_fd)
 {
-	mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
+	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
 
 	struct mag_scale mscale_null = {
 		0.0f,
@@ -113,7 +112,7 @@ int do_mag_calibration(int mavlink_fd)
 		(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
 		result = param_set_no_notification(param_find(str), &(device_ids[cur_mag]));
 		if (result != OK) {
-			mavlink_and_console_log_info(mavlink_fd, "Unabled to reset CAL_MAG%u_ID", cur_mag);
+			mavlink_and_console_log_info(mavlink_fd, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag);
 			break;
 		}
 
@@ -131,15 +130,15 @@ int do_mag_calibration(int mavlink_fd)
 		result = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
 
 		if (result != OK) {
-			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, cur_mag);
+			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, cur_mag);
 		}
 
+		/* calibrate range */
 		if (result == OK) {
-			/* calibrate range */
 			result = ioctl(fd, MAGIOCCALIBRATE, fd);
 
 			if (result != OK) {
-				mavlink_and_console_log_info(mavlink_fd, "Skipped scale calibration, sensor %u", cur_mag);
+				mavlink_and_console_log_info(mavlink_fd, "[cal] Skipped scale calibration, sensor %u", cur_mag);
 				/* this is non-fatal - mark it accordingly */
 				result = OK;
 			}
@@ -148,39 +147,52 @@ int do_mag_calibration(int mavlink_fd)
 		close(fd);
 	}
 
+	// Calibrate all mags at the same time
 	if (result == OK) {
-		// Calibrate all mags at the same time
-		result = mag_calibrate_all(mavlink_fd, device_ids);
-	}
-	
-	if (result == OK) {
-		/* auto-save to EEPROM */
-		result = param_save_default();
-		if (result != OK) {
-			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
+		switch (mag_calibrate_all(mavlink_fd, device_ids)) {
+			case calibrate_return_cancelled:
+				// Cancel message already displayed, we're done here
+				result = ERROR;
+				break;
+				
+			case calibrate_return_ok:
+				/* auto-save to EEPROM */
+				result = param_save_default();
+
+				/* if there is a any preflight-check system response, let the barrage of messages through */
+				usleep(200000);
+
+				if (result == OK) {
+					mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
+					mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
+					break;
+				} else {
+					mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
+				}
+				// Fall through
+				
+			default:
+				mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
+				break;
 		}
 	}
-	
-	if (result == OK) {
-		mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100);
-		mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
-	} else {
-		mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
-	}
 
+	/* give this message enough time to propagate */
+	usleep(600000);
+	
 	return result;
 }
 
-int mag_calibration_worker(detect_orientation_return orientation, void* data)
+static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
 {
-	int result = OK;
+	calibrate_return result = calibrate_return_ok;
 	
 	unsigned int calibration_counter_side;
 
 	mag_worker_data_t* worker_data = (mag_worker_data_t*)(data);
 	
-	mavlink_and_console_log_info(worker_data->mavlink_fd, "Rotate vehicle around the detected orientation");
-	mavlink_and_console_log_info(worker_data->mavlink_fd, "Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds);
+	mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Rotate vehicle around the detected orientation");
+	mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds);
 	sleep(2);
 	
 	uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds;
@@ -191,6 +203,11 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data)
 	while (hrt_absolute_time() < calibration_deadline &&
 	       calibration_counter_side < worker_data->calibration_points_perside) {
 		
+		if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) {
+			result = calibrate_return_cancelled;
+			break;
+		}
+		
 		// Wait clocking for new data on all mags
 		struct pollfd fds[max_mags];
 		size_t fd_count = 0;
@@ -222,8 +239,7 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data)
 			
 			// Progress indicator for side
 			mavlink_and_console_log_info(worker_data->mavlink_fd,
-						     "%s %s side calibration: progress <%u>",
-						     sensor_name,
+						     "[cal] %s side calibration: progress <%u>",
 						     detect_orientation_str(orientation),
 						     (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
 		} else {
@@ -231,50 +247,25 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data)
 		}
 		
 		if (poll_errcount > worker_data->calibration_points_perside * 3) {
-			result = ERROR;
-			mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_FAILED_SENSOR_MSG);
+			result = calibrate_return_error;
+			mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG);
 			break;
 		}
 	}
 	
-	// Mark the opposite side as collected as well. No need to collect opposite side since it
-	// would generate similar points.
-	detect_orientation_return alternateOrientation = orientation;
-	switch (orientation) {
-		case DETECT_ORIENTATION_TAIL_DOWN:
-			alternateOrientation = DETECT_ORIENTATION_NOSE_DOWN;
-			break;
-		case DETECT_ORIENTATION_NOSE_DOWN:
-			alternateOrientation = DETECT_ORIENTATION_TAIL_DOWN;
-			break;
-		case DETECT_ORIENTATION_LEFT:
-			alternateOrientation = DETECT_ORIENTATION_RIGHT;
-			break;
-		case DETECT_ORIENTATION_RIGHT:
-			alternateOrientation = DETECT_ORIENTATION_LEFT;
-			break;
-		case DETECT_ORIENTATION_UPSIDE_DOWN:
-			alternateOrientation = DETECT_ORIENTATION_RIGHTSIDE_UP;
-			break;
-		case DETECT_ORIENTATION_RIGHTSIDE_UP:
-			alternateOrientation = DETECT_ORIENTATION_UPSIDE_DOWN;
-			break;
-		case DETECT_ORIENTATION_ERROR:
-			warnx("Invalid orientation in mag_calibration_worker");
-			break;
+	if (result == calibrate_return_ok) {
+		mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side done, rotate to a different side", detect_orientation_str(orientation));
+		
+		worker_data->done_count++;
+		mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 34 * worker_data->done_count);
 	}
-	worker_data->side_data_collected[alternateOrientation] = true;
-	mavlink_and_console_log_info(worker_data->mavlink_fd, "%s side done, rotate to a different side", detect_orientation_str(alternateOrientation));
-	
-	worker_data->done_count++;
-	mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 34 * worker_data->done_count);
 	
 	return result;
 }
 
-int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
+calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
 {
-	int result = OK;
+	calibrate_return result = calibrate_return_ok;
 
 	mag_worker_data_t worker_data;
 	
@@ -285,10 +276,13 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
 	worker_data.calibration_interval_perside_seconds = 20;
 	worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;
 
-	// Initialize to collect all sides
-	for (size_t cur_side=0; cur_side<6; cur_side++) {
-		worker_data.side_data_collected[cur_side] = false;
-	}
+	// Collect: Right-side up, Left Side, Nose down
+	worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false;
+	worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false;
+	worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false;
+	worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = true;
+	worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = true;
+	worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = true;
 	
 	for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
 		// Initialize to no subscription
@@ -310,21 +304,21 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
 		worker_data.y[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
 		worker_data.z[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
 		if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) {
-			mavlink_and_console_log_critical(mavlink_fd, "ERROR: out of memory");
-			result = ERROR;
+			mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: out of memory");
+			result = calibrate_return_error;
 		}
 	}
 
 	
 	// Setup subscriptions to mag sensors
-	if (result == OK) {
+	if (result == calibrate_return_ok) {
 		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
 			if (device_ids[cur_mag] != 0) {
 				// Mag in this slot is available
 				worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag);
 				if (worker_data.sub_mag[cur_mag] < 0) {
-					mavlink_and_console_log_critical(mavlink_fd, "Mag #%u not found, abort", cur_mag);
-					result = ERROR;
+					mavlink_and_console_log_critical(mavlink_fd, "[cal] Mag #%u not found, abort", cur_mag);
+					result = calibrate_return_error;
 					break;
 				}
 			}
@@ -332,7 +326,7 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
 	}
 	
 	// Limit update rate to get equally spaced measurements over time (in ms)
-	if (result == OK) {
+	if (result == calibrate_return_ok) {
 		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
 			if (device_ids[cur_mag] != 0) {
 				// Mag in this slot is available
@@ -344,8 +338,18 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
 		}
 		
 	}
-
-	result = calibrate_from_orientation(mavlink_fd, worker_data.side_data_collected, mag_calibration_worker, &worker_data);
+    
+	if (result == calibrate_return_ok) {
+		int cancel_sub  = calibrate_cancel_subscribe();
+
+		result = calibrate_from_orientation(mavlink_fd,                         // Mavlink fd to write output
+						    cancel_sub,                         // Subscription to vehicle_command for cancel support
+						    worker_data.side_data_collected,    // Sides to calibrate
+						    mag_calibration_worker,             // Calibration worker
+						    &worker_data,			// Opaque data for calibration worked
+						    true);				// true: lenient still detection
+		calibrate_cancel_unsubscribe(cancel_sub);
+	}
 	
 	// Close subscriptions
 	for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
@@ -363,7 +367,7 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
 	float sphere_radius[max_mags];
 	
 	// Sphere fit the data to get calibration values
-	if (result == OK) {
+	if (result == calibrate_return_ok) {
 		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
 			if (device_ids[cur_mag] != 0) {
 				// Mag in this slot is available and we should have values for it to calibrate
@@ -375,8 +379,8 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
 							 &sphere_radius[cur_mag]);
 				
 				if (!isfinite(sphere_x[cur_mag]) || !isfinite(sphere_y[cur_mag]) || !isfinite(sphere_z[cur_mag])) {
-					mavlink_and_console_log_info(mavlink_fd, "ERROR: NaN in sphere fit for mag #%u", cur_mag);
-					result = ERROR;
+					mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: NaN in sphere fit for mag #%u", cur_mag);
+					result = calibrate_return_error;
 				}
 			}
 		}
@@ -389,7 +393,7 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
 		free(worker_data.z[cur_mag]);
 	}
 	
-	if (result == OK) {
+	if (result == calibrate_return_ok) {
 		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
 			if (device_ids[cur_mag] != 0) {
 				int fd_mag = -1;
@@ -400,27 +404,25 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
 				(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
 				fd_mag = open(str, 0);
 				if (fd_mag < 0) {
-					mavlink_and_console_log_info(mavlink_fd, "ERROR: unable to open mag device #%u", cur_mag);
-					result = ERROR;
+					mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: unable to open mag device #%u", cur_mag);
+					result = calibrate_return_error;
 				}
 				
-				if (result == OK) {
-					result = ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale);
-					if (result != OK) {
-						mavlink_and_console_log_info(mavlink_fd, "ERROR: failed to get current calibration #%u", cur_mag);
-						result = ERROR;
+				if (result == calibrate_return_ok) {
+					if (ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != OK) {
+						mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: failed to get current calibration #%u", cur_mag);
+						result = calibrate_return_error;
 					}
 				}
 
-				if (result == OK) {
+				if (result == calibrate_return_ok) {
 					mscale.x_offset = sphere_x[cur_mag];
 					mscale.y_offset = sphere_y[cur_mag];
 					mscale.z_offset = sphere_z[cur_mag];
 
-					result = ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale);
-					if (result != OK) {
-						mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG, cur_mag);
-						result = ERROR;
+					if (ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) {
+						mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, cur_mag);
+						result = calibrate_return_error;
 					}
 				}
 				
@@ -429,7 +431,7 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
 					close(fd_mag);
 				}
 
-				if (result == OK) {
+				if (result == calibrate_return_ok) {
 					bool failed = false;
 					
 					/* set parameters */
@@ -449,13 +451,13 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
 					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale)));
 
 					if (failed) {
-						mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG, cur_mag);
-						result = ERROR;
+						mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, cur_mag);
+						result = calibrate_return_error;
 					} else {
-						mavlink_and_console_log_info(mavlink_fd, "mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
+						mavlink_and_console_log_info(mavlink_fd, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
 									     cur_mag,
 									     (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset);
-						mavlink_and_console_log_info(mavlink_fd, "mag #%u scale: x:%.2f y:%.2f z:%.2f",
+						mavlink_and_console_log_info(mavlink_fd, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f",
 									     cur_mag,
 									     (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale);
 					}
diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk
index 4ee8732fc..4437041e2 100644
--- a/src/modules/commander/module.mk
+++ b/src/modules/commander/module.mk
@@ -46,7 +46,8 @@ SRCS		 	= commander.cpp \
 			mag_calibration.cpp \
 			baro_calibration.cpp \
 			rc_calibration.cpp \
-			airspeed_calibration.cpp
+			airspeed_calibration.cpp \
+			PreflightCheck.cpp
 
 MODULE_STACKSIZE = 5000
 
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index e1d2d72d1..73fdb0940 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -52,18 +52,17 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/differential_pressure.h>
-#include <uORB/topics/airspeed.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_accel.h>
-#include <drivers/drv_airspeed.h>
 #include <drivers/drv_device.h>
 #include <mavlink/mavlink_log.h>
 
 #include "state_machine_helper.h"
 #include "commander_helper.h"
+#include "PreflightCheck.h"
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -77,12 +76,12 @@ static const int ERROR = -1;
 // though the transition is marked as true additional checks must be made. See arming_state_transition
 // code for those checks.
 static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = {
-	//                                  INIT,  STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
-	{ /* vehicle_status_s::ARMING_STATE_INIT */           true,  true,    false, false,       false,         false,  false },
+	//                                                    INIT,  STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
+	{ /* vehicle_status_s::ARMING_STATE_INIT */           true,  true,    false, false,       true,          false,  false },
 	{ /* vehicle_status_s::ARMING_STATE_STANDBY */        true,  true,    true,  true,        false,         false,  false },
 	{ /* vehicle_status_s::ARMING_STATE_ARMED */          false, true,    true,  false,       false,         false,  true },
 	{ /* vehicle_status_s::ARMING_STATE_ARMED_ERROR */    false, false,   true,  true,        false,         false,  false },
-	{ /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */  true,  true,    false, true,        true,          false,  false },
+	{ /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */  true,  true,    true,  true,        true,          false,  false },
 	{ /* vehicle_status_s::ARMING_STATE_REBOOT */         true,  true,    false, false,       true,          true,   true },
 	{ /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false,   false, false,       false,         false,  false }, // NYI
 };
@@ -138,6 +137,13 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 		/* enforce lockdown in HIL */
 		if (status->hil_state == vehicle_status_s::HIL_STATE_ON) {
 			armed->lockdown = true;
+			prearm_ret = OK;
+			status->condition_system_sensors_initialized = true;
+
+			/* recover from a prearm fail */
+			if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
+				status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY;
+			}
 
 		} else {
 			armed->lockdown = false;
@@ -175,7 +181,7 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 						// Fail transition if power is not good
 						if (!status->condition_power_input_valid) {
 
-							mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
+							mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
 							feedback_provided = true;
 							valid_transition = false;
 						}
@@ -185,7 +191,7 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 						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);
+								mavlink_and_console_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) {
@@ -210,11 +216,35 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 			valid_transition = true;
 		}
 
-		/* Sensors need to be initialized for STANDBY state */
-		if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
-			mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational.");
+		// Sensors need to be initialized for STANDBY state, except for HIL
+		if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) &&
+			(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
+			(!status->condition_system_sensors_initialized)) {
+			mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection");
 			feedback_provided = true;
 			valid_transition = false;
+			status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
+		}
+
+		// Check if we are trying to arm, checks look good but we are in STANDBY_ERROR
+		if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
+
+			if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+
+				if (status->condition_system_sensors_initialized) {
+					mavlink_and_console_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming");
+				} else {
+					mavlink_and_console_log_critical(mavlink_fd, "Preflight check failed, refusing to arm");
+				}
+				feedback_provided = true;
+
+			} else if ((new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
+					status->condition_system_sensors_initialized) {
+				mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, power cycle to complete");
+				feedback_provided = true;
+			} else {
+
+			}
 		}
 
 		// Finish up the state transition
@@ -649,69 +679,15 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 
 int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
 {
-	int ret;
-	bool failed = false;
-
-	int fd = open(ACCEL0_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0) {
-		mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL SENSOR MISSING");
-		failed = true;
-		goto system_eval;
-	}
-
-	ret = ioctl(fd, ACCELIOCSELFTEST, 0);
-
-	if (ret != OK) {
-		mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL CALIBRATION");
-		failed = true;
-		goto system_eval;
-	}
-
-	/* check measurement result range */
-	struct accel_report acc;
-	ret = read(fd, &acc, sizeof(acc));
-
-	if (ret == sizeof(acc)) {
-		/* evaluate values */
-		float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
-
-		if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
-			mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE, hold still");
-			/* this is frickin' fatal */
-			failed = true;
-			goto system_eval;
-		}
-	} else {
-		mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL READ");
-		/* this is frickin' fatal */
-		failed = true;
-		goto system_eval;
-	}
-
+	/* at this point this equals the preflight check, but might add additional
+	 * quantities later.
+	 */
+	bool checkAirspeed = false;
 	/* Perform airspeed check only if circuit breaker is not
 	 * engaged and it's not a rotary wing */
 	if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) {
-		/* accel done, close it */
-		close(fd);
-		fd = orb_subscribe(ORB_ID(airspeed));
-
-		struct airspeed_s airspeed;
-
-		if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
-			(hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) {
-			mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING");
-			failed = true;
-			goto system_eval;
-		}
-
-		if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) {
-			mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
-			// XXX do not make this fatal yet
-		}
+		checkAirspeed = true;
 	}
 
-system_eval:
-	close(fd);
-	return (failed);
+	return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, true);
 }
diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
index 68bf12024..b442b7430 100644
--- a/src/modules/dataman/dataman.c
+++ b/src/modules/dataman/dataman.c
@@ -685,20 +685,21 @@ task_main(int argc, char *argv[])
 
 	fsync(g_task_fd);
 
+	printf("dataman: ");
 	/* see if we need to erase any items based on restart type */
 	int sys_restart_val;
 	if (param_get(param_find("SYS_RESTART_TYPE"), &sys_restart_val) == OK) {
 		if (sys_restart_val == DM_INIT_REASON_POWER_ON) {
-			warnx("Power on restart");
+			printf("Power on restart");
 			_restart(DM_INIT_REASON_POWER_ON);
 		} else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) {
-			warnx("In flight restart");
+			printf("In flight restart");
 			_restart(DM_INIT_REASON_IN_FLIGHT);
 		} else {
-			warnx("Unknown restart");
+			printf("Unknown restart");
 		}
 	} else {
-		warnx("Unknown restart");
+		printf("Unknown restart");
 	}
 
 	/* We use two file descriptors, one for the caller context and one for the worker thread */
@@ -706,7 +707,7 @@ task_main(int argc, char *argv[])
 	/* worker thread is shutting down but still processing requests */
 	g_fd = g_task_fd;
 
-	warnx("Initialized, data manager file '%s' size is %d bytes", k_data_manager_device_path, max_offset);
+	printf(", data manager file '%s' size is %d bytes\n", k_data_manager_device_path, max_offset);
 
 	/* Tell startup that the worker thread has completed its initialization */
 	sem_post(&g_init_sema);
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 c16e72ea0..e0b4cb2a0 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
@@ -937,7 +937,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
 		// Convert GPS measurements to Pos NE, hgt and Vel NED
 
 		// set fusion flags
-		_ekf->fuseVelData = true;
+		_ekf->fuseVelData = _gps.vel_ned_valid;
 		_ekf->fusePosData = true;
 
 		// recall states stored at time of measurement after adjusting for delays
@@ -1422,30 +1422,41 @@ void AttitudePositionEstimatorEKF::pollData()
 
 		int last_mag_main = _mag_main;
 
-		// XXX we compensate the offsets upfront - should be close to zero.
+		Vector3f mag0(_sensor_combined.magnetometer_ga[0], _sensor_combined.magnetometer_ga[1],
+			_sensor_combined.magnetometer_ga[2]);
+
+		Vector3f mag1(_sensor_combined.magnetometer1_ga[0], _sensor_combined.magnetometer1_ga[1],
+			_sensor_combined.magnetometer1_ga[2]);
+
+		const unsigned mag_timeout_us = 200000;
 
 		/* fail over to the 2nd mag if we know the first is down */
-		if (_sensor_combined.magnetometer_errcount <= _sensor_combined.magnetometer1_errcount) {
-			_ekf->magData.x = _sensor_combined.magnetometer_ga[0];
+		if (hrt_elapsed_time(&_sensor_combined.magnetometer_timestamp) < mag_timeout_us &&
+			_sensor_combined.magnetometer_errcount <= _sensor_combined.magnetometer1_errcount &&
+			mag0.length() > 0.1f) {
+			_ekf->magData.x = mag0.x;
 			_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
 
-			_ekf->magData.y = _sensor_combined.magnetometer_ga[1];
+			_ekf->magData.y = mag0.y;
 			_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
 
-			_ekf->magData.z = _sensor_combined.magnetometer_ga[2];
+			_ekf->magData.z = mag0.z;
 			_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
 			_mag_main = 0;
 
-		} else {
-			_ekf->magData.x = _sensor_combined.magnetometer1_ga[0];
+		} else if (hrt_elapsed_time(&_sensor_combined.magnetometer1_timestamp) < mag_timeout_us &&
+			mag1.length() > 0.1f) {
+			_ekf->magData.x = mag1.x;
 			_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
 
-			_ekf->magData.y = _sensor_combined.magnetometer1_ga[1];
+			_ekf->magData.y = mag1.y;
 			_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
 
-			_ekf->magData.z = _sensor_combined.magnetometer1_ga[2];
+			_ekf->magData.z = mag1.z;
 			_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
 			_mag_main = 1;
+		} else {
+			_mag_valid = false;
 		}
 
 		if (last_mag_main != _mag_main) {
@@ -1516,7 +1527,7 @@ int AttitudePositionEstimatorEKF::trip_nan()
 
 int ekf_att_pos_estimator_main(int argc, char *argv[])
 {
-	if (argc < 1) {
+	if (argc < 2) {
 		errx(1, "usage: ekf_att_pos_estimator {start|stop|status|logging}");
 	}
 
diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
index 71b387b1e..bd1486324 100644
--- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
+++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
@@ -97,8 +97,9 @@ usage(const char *reason)
 int fixedwing_backside_main(int argc, char *argv[])
 {
 
-	if (argc < 1)
+	if (argc < 2) {
 		usage("missing command");
+	}
 
 	if (!strcmp(argv[1], "start")) {
 
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 8b41144f6..0a8d07fed 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -1134,8 +1134,9 @@ FixedwingAttitudeControl::start()
 
 int fw_att_control_main(int argc, char *argv[])
 {
-	if (argc < 1)
+	if (argc < 2) {
 		errx(1, "usage: fw_att_control {start|stop|status}");
+	}
 
 	if (!strcmp(argv[1], "start")) {
 
diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c
index cfc5ae965..6b248cbe2 100644
--- a/src/modules/fw_att_control/fw_att_control_params.c
+++ b/src/modules/fw_att_control/fw_att_control_params.c
@@ -136,8 +136,7 @@ PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f);
  * @max 2.0
  * @group FW Attitude Control
  */
-PARAM_DEFINE_FLOAT(FW_P_ROLLFF,
-		   0.0f); //xxx: set to 0 as default, see comment in ECL_PitchController::control_attitude (float turn_offset = ...)
+PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f);
 
 /**
  * Roll rate proportional Gain
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 427df9739..34265d6a4 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
@@ -1638,8 +1638,9 @@ FixedwingPositionControl::start()
 
 int fw_pos_control_l1_main(int argc, char *argv[])
 {
-	if (argc < 1)
+	if (argc < 2) {
 		errx(1, "usage: fw_pos_control_l1 {start|stop|status}");
+	}
 
 	if (!strcmp(argv[1], "start")) {
 
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
index 58a1e9f6b..fff506865 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
@@ -157,8 +157,8 @@ PARAM_DEFINE_FLOAT(MT_THR_MAX, 1.0f);
 /**
  * Minimal Pitch Setpoint in Degrees
  *
- * @min -90.0f
- * @max 90.0f
+ * @min -90.0
+ * @max 90.0
  * @unit deg
  * @group mTECS
  */
@@ -167,8 +167,8 @@ PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f);
 /**
  * Maximal Pitch Setpoint in Degrees
  *
- * @min -90.0f
- * @max 90.0f
+ * @min -90.0
+ * @max 90.0
  * @unit deg
  * @group mTECS
  */
@@ -192,8 +192,8 @@ PARAM_DEFINE_FLOAT(MT_FPA_LP, 1.0f);
  * P gain for the altitude control
  * Maps the altitude error to the flight path angle setpoint
  *
- * @min 0.0f
- * @max 10.0f
+ * @min 0.0
+ * @max 10.0
  * @group mTECS
  */
 PARAM_DEFINE_FLOAT(MT_FPA_P, 0.3f);
@@ -202,8 +202,8 @@ PARAM_DEFINE_FLOAT(MT_FPA_P, 0.3f);
  * D gain for the altitude control
  * Maps the change of altitude error to the flight path angle setpoint
  *
- * @min 0.0f
- * @max 10.0f
+ * @min 0.0
+ * @max 10.0
  * @group mTECS
  */
 PARAM_DEFINE_FLOAT(MT_FPA_D, 0.0f);
@@ -219,8 +219,8 @@ PARAM_DEFINE_FLOAT(MT_FPA_D_LP, 1.0f);
 /**
  * Minimal flight path angle setpoint
  *
- * @min -90.0f
- * @max 90.0f
+ * @min -90.0
+ * @max 90.0
  * @unit deg
  * @group mTECS
  */
@@ -229,8 +229,8 @@ PARAM_DEFINE_FLOAT(MT_FPA_MIN, -20.0f);
 /**
  * Maximal flight path angle setpoint
  *
- * @min -90.0f
- * @max 90.0f
+ * @min -90.0
+ * @max 90.0
  * @unit deg
  * @group mTECS
  */
@@ -254,8 +254,8 @@ PARAM_DEFINE_FLOAT(MT_AD_LP, 0.5f);
  * P gain for the airspeed control
  * Maps the airspeed error to the acceleration setpoint
  *
- * @min 0.0f
- * @max 10.0f
+ * @min 0.0
+ * @max 10.0
  * @group mTECS
  */
 PARAM_DEFINE_FLOAT(MT_ACC_P, 0.3f);
@@ -264,8 +264,8 @@ PARAM_DEFINE_FLOAT(MT_ACC_P, 0.3f);
  * D gain for the airspeed control
  * Maps the change of airspeed error to the acceleration setpoint
  *
- * @min 0.0f
- * @max 10.0f
+ * @min 0.0
+ * @max 10.0
  * @group mTECS
  */
 PARAM_DEFINE_FLOAT(MT_ACC_D, 0.0f);
@@ -296,8 +296,8 @@ PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f);
 /**
  * Minimal throttle during takeoff
  *
- * @min 0.0f
- * @max 1.0f
+ * @min 0.0
+ * @max 1.0
  * @group mTECS
  */
 PARAM_DEFINE_FLOAT(MT_TKF_THR_MIN, 1.0f);
@@ -305,8 +305,8 @@ PARAM_DEFINE_FLOAT(MT_TKF_THR_MIN, 1.0f);
 /**
  * Maximal throttle during takeoff
  *
- * @min 0.0f
- * @max 1.0f
+ * @min 0.0
+ * @max 1.0
  * @group mTECS
  */
 PARAM_DEFINE_FLOAT(MT_TKF_THR_MAX, 1.0f);
@@ -314,8 +314,8 @@ PARAM_DEFINE_FLOAT(MT_TKF_THR_MAX, 1.0f);
 /**
  * Minimal pitch during takeoff
  *
- * @min -90.0f
- * @max 90.0f
+ * @min -90.0
+ * @max 90.0
  * @unit deg
  * @group mTECS
  */
@@ -324,8 +324,8 @@ PARAM_DEFINE_FLOAT(MT_TKF_PIT_MIN, 0.0f);
 /**
  * Maximal pitch during takeoff
  *
- * @min -90.0f
- * @max 90.0f
+ * @min -90.0
+ * @max 90.0
  * @unit deg
  * @group mTECS
  */
@@ -334,8 +334,8 @@ PARAM_DEFINE_FLOAT(MT_TKF_PIT_MAX, 45.0f);
 /**
  * Minimal throttle in underspeed mode
  *
- * @min 0.0f
- * @max 1.0f
+ * @min 0.0
+ * @max 1.0
  * @group mTECS
  */
 PARAM_DEFINE_FLOAT(MT_USP_THR_MIN, 1.0f);
@@ -343,8 +343,8 @@ PARAM_DEFINE_FLOAT(MT_USP_THR_MIN, 1.0f);
 /**
  * Maximal throttle in underspeed mode
  *
- * @min 0.0f
- * @max 1.0f
+ * @min 0.0
+ * @max 1.0
  * @group mTECS
  */
 PARAM_DEFINE_FLOAT(MT_USP_THR_MAX, 1.0f);
@@ -352,8 +352,8 @@ PARAM_DEFINE_FLOAT(MT_USP_THR_MAX, 1.0f);
 /**
  * Minimal pitch in underspeed mode
  *
- * @min -90.0f
- * @max 90.0f
+ * @min -90.0
+ * @max 90.0
  * @unit deg
  * @group mTECS
  */
@@ -362,8 +362,8 @@ PARAM_DEFINE_FLOAT(MT_USP_PIT_MIN, -45.0f);
 /**
  * Maximal pitch in underspeed mode
  *
- * @min -90.0f
- * @max 90.0f
+ * @min -90.0
+ * @max 90.0
  * @unit deg
  * @group mTECS
  */
@@ -372,8 +372,8 @@ PARAM_DEFINE_FLOAT(MT_USP_PIT_MAX, 0.0f);
 /**
  * Minimal throttle in landing mode (only last phase of landing)
  *
- * @min 0.0f
- * @max 1.0f
+ * @min 0.0
+ * @max 1.0
  * @group mTECS
  */
 PARAM_DEFINE_FLOAT(MT_LND_THR_MIN, 0.0f);
@@ -381,8 +381,8 @@ PARAM_DEFINE_FLOAT(MT_LND_THR_MIN, 0.0f);
 /**
  * Maximal throttle in landing mode (only last phase of landing)
  *
- * @min 0.0f
- * @max 1.0f
+ * @min 0.0
+ * @max 1.0
  * @group mTECS
  */
 PARAM_DEFINE_FLOAT(MT_LND_THR_MAX, 0.0f);
@@ -390,8 +390,8 @@ PARAM_DEFINE_FLOAT(MT_LND_THR_MAX, 0.0f);
 /**
  * Minimal pitch in landing mode
  *
- * @min -90.0f
- * @max 90.0f
+ * @min -90.0
+ * @max 90.0
  * @unit deg
  * @group mTECS
  */
@@ -400,8 +400,8 @@ PARAM_DEFINE_FLOAT(MT_LND_PIT_MIN, -5.0f);
 /**
  * Maximal pitch in landing mode
  *
- * @min -90.0f
- * @max 90.0f
+ * @min -90.0
+ * @max 90.0
  * @unit deg
  * @group mTECS
  */
@@ -410,8 +410,8 @@ PARAM_DEFINE_FLOAT(MT_LND_PIT_MAX, 15.0f);
 /**
  * Integrator Limit for Total Energy Rate Control
  *
- * @min 0.0f
- * @max 10.0f
+ * @min 0.0
+ * @max 10.0
  * @group mTECS
  */
 PARAM_DEFINE_FLOAT(MT_THR_I_MAX, 10.0f);
@@ -419,8 +419,8 @@ PARAM_DEFINE_FLOAT(MT_THR_I_MAX, 10.0f);
 /**
  * Integrator Limit for Energy Distribution Rate Control
  *
- * @min 0.0f
- * @max 10.0f
+ * @min 0.0
+ * @max 10.0
  * @group mTECS
  */
 PARAM_DEFINE_FLOAT(MT_PIT_I_MAX, 10.0f);
diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp
index 011567e57..b4b7c33a2 100644
--- a/src/modules/land_detector/land_detector_main.cpp
+++ b/src/modules/land_detector/land_detector_main.cpp
@@ -178,7 +178,7 @@ static int land_detector_start(const char *mode)
 int land_detector_main(int argc, char *argv[])
 {
 
-	if (argc < 1) {
+	if (argc < 2) {
 		goto exiterr;
 	}
 
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 9afe74af1..796d5cbf2 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -64,14 +64,18 @@ PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
 PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
 /**
  * Use/Accept HIL GPS message (even if not in HIL mode)
+ *
  * If set to 1 incomming HIL GPS messages are parsed.
+ *
  * @group MAVLink
  */
 PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
 /**
  * Forward external setpoint messages
+ *
  * If set to 1 incomming external setpoint messages will be directly forwarded to the controllers if in offboard
  * control mode
+ *
  * @group MAVLink
  */
 PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1);
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index ba049bac4..22ff3edf6 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -90,7 +90,7 @@
 static const int ERROR = -1;
 
 #define DEFAULT_DEVICE_NAME			"/dev/ttyS1"
-#define MAX_DATA_RATE				20000	///< max data rate in bytes/s
+#define MAX_DATA_RATE				60000	///< max data rate in bytes/s
 #define MAIN_LOOP_DELAY 			10000	///< 100 Hz @ 1000 bytes/s data rate
 #define FLOW_CONTROL_DISABLE_THRESHOLD		40	///< picked so that some messages still would fit it.
 
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 7d6b60e22..7ddc52fd1 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -539,7 +539,8 @@ protected:
 			msg.errors_count2 = status.errors_count2;
 			msg.errors_count3 = status.errors_count3;
 			msg.errors_count4 = status.errors_count4;
-			msg.battery_remaining = status.battery_remaining * 100.0f;
+			msg.battery_remaining = (msg.voltage_battery > 0) ?
+							status.battery_remaining * 100.0f : -1;
 
 			_mavlink->send_message(MAVLINK_MSG_ID_SYS_STATUS, &msg);
 		}
@@ -1867,29 +1868,8 @@ protected:
 		struct rc_input_values rc;
 
 		if (_rc_sub->update(&_rc_time, &rc)) {
-			const unsigned port_width = 8;
-
-			/* deprecated message (but still needed for compatibility!) */
-			for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) {
-				/* channels are sent in MAVLink main loop at a fixed interval */
-				mavlink_rc_channels_raw_t msg;
-
-				msg.time_boot_ms = rc.timestamp_publication / 1000;
-				msg.port = i;
-				msg.chan1_raw = (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX;
-				msg.chan2_raw = (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX;
-				msg.chan3_raw = (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX;
-				msg.chan4_raw = (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX;
-				msg.chan5_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX;
-				msg.chan6_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX;
-				msg.chan7_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX;
-				msg.chan8_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX;
-				msg.rssi = rc.rssi;
-
-				_mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_RAW, &msg);
-			}
 
-			/* new message */
+			/* send RC channel data and RSSI */
 			mavlink_rc_channels_t msg;
 
 			msg.time_boot_ms = rc.timestamp_publication / 1000;
@@ -1913,55 +1893,7 @@ protected:
 			msg.chan17_raw = (rc.channel_count > 16) ? rc.values[16] : UINT16_MAX;
 			msg.chan18_raw = (rc.channel_count > 17) ? rc.values[17] : UINT16_MAX;
 
-			/* RSSI has a max value of 100, and when Spektrum or S.BUS are
-			 * available, the RSSI field is invalid, as they do not provide
-			 * an RSSI measurement. Use an out of band magic value to signal
-			 * these digital ports. XXX revise MAVLink spec to address this.
-			 * One option would be to use the top bit to toggle between RSSI
-			 * and input source mode.
-			 *
-			 * Full RSSI field: 0b 1 111 1111
-			 *
-			 *                     ^ If bit is set, RSSI encodes type + RSSI
-			 *
-			 *                       ^ These three bits encode a total of 8
-			 *                         digital RC input types.
-			 *                         0: PPM, 1: SBUS, 2: Spektrum, 2: ST24
-			 *                           ^ These four bits encode a total of
-			 *                             16 RSSI levels. 15 = full, 0 = no signal
-			 *
-			 */
-
-			/* Initialize RSSI with the special mode level flag */
-			msg.rssi = (1 << 7);
-
-			/* Set RSSI */
-			msg.rssi |= (rc.rssi <= 100) ? ((rc.rssi / 7) + 1) : 15;
-
-			switch (rc.input_source) {
-				case RC_INPUT_SOURCE_PX4FMU_PPM:
-				/* fallthrough */
-				case RC_INPUT_SOURCE_PX4IO_PPM:
-					msg.rssi |= (0 << 4);
-					break;
-				case RC_INPUT_SOURCE_PX4IO_SPEKTRUM:
-					msg.rssi |= (1 << 4);
-					break;
-				case RC_INPUT_SOURCE_PX4IO_SBUS:
-					msg.rssi |= (2 << 4);
-					break;
-				case RC_INPUT_SOURCE_PX4IO_ST24:
-					msg.rssi |= (3 << 4);
-					break;
-				case RC_INPUT_SOURCE_UNKNOWN:
-					// do nothing
-					break;
-			}
-
-			if (rc.rc_lost) {
-				/* RSSI is by definition zero */
-				msg.rssi = 0;
-			}
+			msg.rssi = rc.rssi;
 
 			_mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg);
 		}
@@ -2299,7 +2231,7 @@ protected:
 };
 
 
-StreamListItem *streams_list[] = {
+const StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static),
 	new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static),
 	new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static),
diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h
index 7e4416609..5b6b9d633 100644
--- a/src/modules/mavlink/mavlink_messages.h
+++ b/src/modules/mavlink/mavlink_messages.h
@@ -56,6 +56,6 @@ public:
 	~StreamListItem() {};
 };
 
-extern StreamListItem *streams_list[];
+extern const StreamListItem *streams_list[];
 
 #endif /* MAVLINK_MESSAGES_H_ */
diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp
index e7e0c11df..9c8794017 100644
--- a/src/modules/mavlink/mavlink_parameters.cpp
+++ b/src/modules/mavlink/mavlink_parameters.cpp
@@ -74,7 +74,6 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 			    (req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) {
 
 				_send_all_index = 0;
-				_mavlink->send_statustext_info("[pm] sending list");
 			}
 			break;
 		}
@@ -131,7 +130,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 
 				} else {
 					/* when index is >= 0, send this parameter again */
-					send_param(param_for_index(req_read.param_index));
+					send_param(param_for_used_index(req_read.param_index));
 				}
 			}
 			break;
@@ -193,6 +192,7 @@ MavlinkParametersManager::send(const hrt_abstime t)
 		/* look for the first parameter which is used */
 		param_t p;
 		do {
+			/* walk through all parameters, including unused ones */
 			p = param_for_index(_send_all_index);
 			_send_all_index++;
 		} while (p != PARAM_INVALID && !param_used(p));
@@ -201,7 +201,7 @@ MavlinkParametersManager::send(const hrt_abstime t)
 			send_param(p);
 		}
 
-		if (_send_all_index >= (int) param_count()) {
+		if ((p == PARAM_INVALID) || (_send_all_index >= (int) param_count())) {
 			_send_all_index = -1;
 		}
 	}
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 3f9f7e139..c4e332bf1 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -121,6 +121,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_rc_pub(-1),
 	_manual_pub(-1),
 	_land_detector_pub(-1),
+	_time_offset_pub(-1),
 	_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
 	_hil_frames(0),
 	_old_timestamp(0),
@@ -729,8 +730,8 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 	// Use the component ID to identify the vision sensor
 	vision_position.id = msg->compid;
 
-	vision_position.timestamp_boot = to_hrt(pos.usec); // Synced time
-	vision_position.timestamp_computer = pos.usec;
+	vision_position.timestamp_boot = hrt_absolute_time(); // Monotonic time
+	vision_position.timestamp_computer = sync_stamp(pos.usec); // Synced time
 	vision_position.x = pos.x;
 	vision_position.y = pos.y;
 	vision_position.z = pos.z;
@@ -821,7 +822,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 
 				/* Publish attitude setpoint if attitude and thrust ignore bits are not set */
 				if (!(_offboard_control_mode.ignore_attitude)) {
-					static struct vehicle_attitude_setpoint_s att_sp = {};
+					struct vehicle_attitude_setpoint_s att_sp = {};
 					att_sp.timestamp = hrt_absolute_time();
 					mavlink_quaternion_to_euler(set_attitude_target.q,
 							&att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body);
@@ -1013,6 +1014,9 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg)
 	mavlink_timesync_t tsync;
 	mavlink_msg_timesync_decode(msg, &tsync);
 
+	struct time_offset_s tsync_offset;
+	memset(&tsync_offset, 0, sizeof(tsync_offset));
+
 	uint64_t now_ns = hrt_absolute_time() * 1000LL ;
 
 	if (tsync.tc1 == 0) {
@@ -1039,6 +1043,15 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg)
 		}
 	}
 
+	tsync_offset.offset_ns = _time_offset ;
+
+	if (_time_offset_pub < 0) {
+		_time_offset_pub = orb_advertise(ORB_ID(time_offset), &tsync_offset);
+
+	} else {
+		orb_publish(ORB_ID(time_offset), _time_offset_pub, &tsync_offset);
+	}
+
 }
 
 void
@@ -1522,9 +1535,12 @@ void MavlinkReceiver::print_status()
 
 }
 
-uint64_t MavlinkReceiver::to_hrt(uint64_t usec)
+uint64_t MavlinkReceiver::sync_stamp(uint64_t usec)
 {
-	return usec - (_time_offset / 1000) ;
+	if(_time_offset > 0)
+		return usec - (_time_offset / 1000) ;
+	else
+		return hrt_absolute_time();
 }
 
 
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index ffacb59a6..2b6174f8f 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -73,6 +73,7 @@
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/vehicle_force_setpoint.h>
+#include <uORB/topics/time_offset.h>
 
 #include "mavlink_ftp.h"
 
@@ -138,9 +139,10 @@ private:
 	void *receive_thread(void *arg);
 
 	/**
-	* Convert remote nsec timestamp to local hrt time (usec)
+	* Convert remote timestamp to local hrt time (usec)
+	* Use timesync if available, monotonic boot time otherwise
 	*/
-	uint64_t to_hrt(uint64_t nsec);
+	uint64_t sync_stamp(uint64_t usec);
 	/**
 	* Exponential moving average filter to smooth time offset
 	*/
@@ -177,6 +179,7 @@ private:
 	orb_advert_t _rc_pub;
 	orb_advert_t _manual_pub;
 	orb_advert_t _land_detector_pub;
+	orb_advert_t _time_offset_pub;
 	int _control_mode_sub;
 	int _hil_frames;
 	uint64_t _old_timestamp;
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index d47731622..ec512ab56 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -77,6 +77,8 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/multirotor_motor_limits.h>
+#include <uORB/topics/mc_att_ctrl_status.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/perf_counter.h>
@@ -129,10 +131,12 @@ private:
 	int		_manual_control_sp_sub;	/**< manual control setpoint subscription */
 	int		_armed_sub;				/**< arming status subscription */
 	int		_vehicle_status_sub;	/**< vehicle status subscription */
+	int 	_motor_limits_sub;		/**< motor limits subscription */
 
 	orb_advert_t	_att_sp_pub;			/**< attitude setpoint publication */
 	orb_advert_t	_v_rates_sp_pub;		/**< rate setpoint publication */
 	orb_advert_t	_actuators_0_pub;		/**< attitude actuator controls publication */
+	orb_advert_t	_controller_status_pub;	/**< controller status publication */
 
 	orb_id_t _rates_sp_id;	/**< pointer to correct rates setpoint uORB metadata structure */
 	orb_id_t _actuators_id;	/**< pointer to correct actuator controls0 uORB metadata structure */
@@ -147,6 +151,8 @@ private:
 	struct actuator_controls_s			_actuators;			/**< actuator controls */
 	struct actuator_armed_s				_armed;				/**< actuator arming status */
 	struct vehicle_status_s				_vehicle_status;	/**< vehicle status */
+	struct multirotor_motor_limits_s	_motor_limits;		/**< motor limits */
+	struct mc_att_ctrl_status_s 		_controller_status; /**< controller status */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 	perf_counter_t	_controller_latency_perf;
@@ -262,6 +268,11 @@ private:
 	void		vehicle_status_poll();
 
 	/**
+	 * Check for vehicle motor limits status.
+	 */
+	void		vehicle_motor_limits_poll();
+
+	/**
 	 * Shim for calling task_main from task_create.
 	 */
 	static void	task_main_trampoline(int argc, char *argv[]);
@@ -302,6 +313,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_att_sp_pub(-1),
 	_v_rates_sp_pub(-1),
 	_actuators_0_pub(-1),
+	_controller_status_pub(-1),
 	_rates_sp_id(0),
 	_actuators_id(0),
 
@@ -320,6 +332,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	memset(&_actuators, 0, sizeof(_actuators));
 	memset(&_armed, 0, sizeof(_armed));
 	memset(&_vehicle_status, 0, sizeof(_vehicle_status));
+	memset(&_motor_limits, 0, sizeof(_motor_limits));
+	memset(&_controller_status,0,sizeof(_controller_status));
 	_vehicle_status.is_rotary_wing = true;
 
 	_params.att_p.zero();
@@ -570,6 +584,18 @@ MulticopterAttitudeControl::vehicle_status_poll()
 	}
 }
 
+void
+MulticopterAttitudeControl::vehicle_motor_limits_poll()
+{
+	/* check if there is a new message */
+	bool updated;
+	orb_check(_motor_limits_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(multirotor_motor_limits), _motor_limits_sub, &_motor_limits);
+	}
+}
+
 /*
  * Attitude controller.
  * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode)
@@ -689,8 +715,8 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
 	_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + _params.rate_ff.emult(_rates_sp);
 	_rates_prev = rates;
 
-	/* update integral only if not saturated on low limit */
-	if (_thrust_sp > MIN_TAKEOFF_THRUST) {
+	/* update integral only if not saturated on low limit and if motor commands are not saturated */
+	if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit ) {
 		for (int i = 0; i < 3; i++) {
 			if (fabsf(_att_control(i)) < _thrust_sp) {
 				float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
@@ -725,6 +751,7 @@ MulticopterAttitudeControl::task_main()
 	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	_motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits));
 
 	/* initialize parameters cache */
 	parameters_update();
@@ -777,6 +804,7 @@ MulticopterAttitudeControl::task_main()
 			arming_status_poll();
 			vehicle_manual_poll();
 			vehicle_status_poll();
+			vehicle_motor_limits_poll();
 
 			if (_v_control_mode.flag_control_attitude_enabled) {
 				control_attitude(dt);
@@ -837,6 +865,11 @@ MulticopterAttitudeControl::task_main()
 				_actuators.timestamp = hrt_absolute_time();
 				_actuators.timestamp_sample = _v_att.timestamp;
 
+				_controller_status.roll_rate_integ = _rates_int(0);
+				_controller_status.pitch_rate_integ = _rates_int(1);
+				_controller_status.yaw_rate_integ = _rates_int(2);
+				_controller_status.timestamp = hrt_absolute_time();
+
 				if (!_actuators_0_circuit_breaker_enabled) {
 					if (_actuators_0_pub > 0) {
 						orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
@@ -847,6 +880,13 @@ MulticopterAttitudeControl::task_main()
 					}
 
 				}
+
+				/* publish controller status */
+				if(_controller_status_pub > 0) {
+					orb_publish(ORB_ID(mc_att_ctrl_status),_controller_status_pub, &_controller_status);
+				} else {
+					_controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status);
+				}
 			}
 		}
 
@@ -868,7 +908,7 @@ MulticopterAttitudeControl::start()
 	_control_task = task_spawn_cmd("mc_att_control",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       1600,
+				       1500,
 				       (main_t)&MulticopterAttitudeControl::task_main_trampoline,
 				       nullptr);
 
@@ -882,8 +922,9 @@ MulticopterAttitudeControl::start()
 
 int mc_att_control_main(int argc, char *argv[])
 {
-	if (argc < 1)
+	if (argc < 2) {
 		errx(1, "usage: mc_att_control {start|stop|status}");
+	}
 
 	if (!strcmp(argv[1], "start")) {
 
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp
index 40eb498b4..dec1e1745 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp
@@ -54,7 +54,7 @@ extern int main(int argc, char **argv);
 extern "C" __EXPORT int mc_att_control_m_main(int argc, char *argv[]);
 int mc_att_control_m_main(int argc, char *argv[])
 {
-	if (argc < 1) {
+	if (argc < 2) {
 		errx(1, "usage: mc_att_control_m {start|stop|status}");
 	}
 
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 4910454bd..6ffb37d97 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -1426,7 +1426,7 @@ MulticopterPositionControl::start()
 	_control_task = task_spawn_cmd("mc_pos_control",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       1600,
+				       1500,
 				       (main_t)&MulticopterPositionControl::task_main_trampoline,
 				       nullptr);
 
@@ -1440,7 +1440,7 @@ MulticopterPositionControl::start()
 
 int mc_pos_control_main(int argc, char *argv[])
 {
-	if (argc < 1) {
+	if (argc < 2) {
 		errx(1, "usage: mc_pos_control {start|stop|status}");
 	}
 
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp
index 1082061f6..0db67d83f 100644
--- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp
@@ -54,7 +54,7 @@ extern int main(int argc, char **argv);
 extern "C" __EXPORT int mc_pos_control_m_main(int argc, char *argv[]);
 int mc_pos_control_m_main(int argc, char *argv[])
 {
-	if (argc < 1) {
+	if (argc < 2) {
 		errx(1, "usage: mc_pos_control_m {start|stop|status}");
 	}
 
diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp
index 87a6e023a..7925809f1 100644
--- a/src/modules/navigator/datalinkloss.cpp
+++ b/src/modules/navigator/datalinkloss.cpp
@@ -186,7 +186,7 @@ DataLinkLoss::advance_dll()
 		if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) {
 			warnx("%d data link losses, limit is %d, fly to airfield home",
 					_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");
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "too many DL losses, fly to airfield home");
 			_navigator->get_mission_result()->stay_in_failsafe = true;
 			_navigator->set_mission_result_updated();
 			reset_mission_item_reached();
@@ -194,19 +194,19 @@ DataLinkLoss::advance_dll()
 		} else {
 			if (!_param_skipcommshold.get()) {
 				warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter);
-				mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold");
+				mavlink_log_critical(_navigator->get_mavlink_fd(), "fly to comms hold");
 				_dll_state = DLL_STATE_FLYTOCOMMSHOLDWP;
 			} else {
 				/* comms hold wp not active, fly to airfield home directly */
 				warnx("Skipping comms hold wp. Flying directly to airfield home");
-				mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped");
+				mavlink_log_critical(_navigator->get_mavlink_fd(), "fly to airfield home, comms hold skipped");
 				_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
 			}
 		}
 		break;
 	case DLL_STATE_FLYTOCOMMSHOLDWP:
 		warnx("fly to airfield home");
-			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home");
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "fly to airfield home");
 		_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
 		_navigator->get_mission_result()->stay_in_failsafe = true;
 		_navigator->set_mission_result_updated();
@@ -215,7 +215,7 @@ DataLinkLoss::advance_dll()
 	case DLL_STATE_FLYTOAIRFIELDHOMEWP:
 		_dll_state = DLL_STATE_TERMINATE;
 		warnx("time is up, state should have been changed manually by now");
-		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating");
+		mavlink_log_critical(_navigator->get_mavlink_fd(), "no manual control, terminating");
 		_navigator->get_mission_result()->stay_in_failsafe = true;
 		_navigator->set_mission_result_updated();
 		reset_mission_item_reached();
diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c
index 6780c0c88..9abc012cf 100644
--- a/src/modules/navigator/datalinkloss_params.c
+++ b/src/modules/navigator/datalinkloss_params.c
@@ -54,7 +54,7 @@
  *
  * @unit seconds
  * @min 0.0
- * @group DLL
+ * @group Data Link Loss
  */
 PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f);
 
@@ -64,8 +64,8 @@ PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f);
  * Latitude of comms hold waypoint
  *
  * @unit degrees * 1e7
- * @min 0.0
- * @group DLL
+ * @min 0
+ * @group Data Link Loss
  */
 PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120);
 
@@ -75,8 +75,8 @@ PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120);
  * Longitude of comms hold waypoint
  *
  * @unit degrees * 1e7
- * @min 0.0
- * @group DLL
+ * @min 0
+ * @group Data Link Loss
  */
 PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890);
 
@@ -87,7 +87,7 @@ PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890);
  *
  * @unit m
  * @min 0.0
- * @group DLL
+ * @group Data Link Loss
  */
 PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f);
 
@@ -98,7 +98,7 @@ PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f);
  *
  * @unit seconds
  * @min 0.0
- * @group DLL
+ * @group Data Link Loss
  */
 PARAM_DEFINE_FLOAT(NAV_DLL_AH_T, 120.0f);
 
@@ -107,7 +107,7 @@ PARAM_DEFINE_FLOAT(NAV_DLL_AH_T, 120.0f);
  *
  * After more than this number of data link timeouts the aircraft returns home directly
  *
- * @group DLL
+ * @group Data Link Loss
  * @min 0
  * @max 1000
  */
@@ -119,7 +119,7 @@ PARAM_DEFINE_INT32(NAV_DLL_N, 2);
  * If set to 1 the system will skip the comms hold wp on data link loss and will directly fly to
  * airfield home
  *
- * @group DLL
+ * @group Data Link Loss
  * @min 0
  * @max 1
  */
diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp
index e007b208b..b0130a1f5 100644
--- a/src/modules/navigator/enginefailure.cpp
+++ b/src/modules/navigator/enginefailure.cpp
@@ -140,7 +140,7 @@ EngineFailure::advance_ef()
 {
 	switch (_ef_state) {
 	case EF_STATE_NONE:
-		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: Engine failure. Loitering down");
+		mavlink_log_emergency(_navigator->get_mavlink_fd(), "Engine failure. Loitering down");
 		_ef_state = EF_STATE_LOITERDOWN;
 		break;
 	default:
diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp
index 9bc9be245..1b6610734 100644
--- a/src/modules/navigator/geofence.cpp
+++ b/src/modules/navigator/geofence.cpp
@@ -39,6 +39,7 @@
  */
 #include "geofence.h"
 
+#include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <string.h>
 #include <dataman/dataman.h>
@@ -49,6 +50,15 @@
 #include <nuttx/config.h>
 #include <unistd.h>
 #include <mavlink/mavlink_log.h>
+#include <geo/geo.h>
+#include <drivers/drv_hrt.h>
+
+#define GEOFENCE_OFF 0
+#define GEOFENCE_FILE_ONLY 1
+#define GEOFENCE_MAX_DISTANCES_ONLY 2
+#define GEOFENCE_FILE_AND_MAX_DISTANCES 3
+
+#define GEOFENCE_RANGE_WARNING_LIMIT 3000000
 
 
 /* Oddly, ERROR is not defined for C++ */
@@ -60,13 +70,19 @@ static const int ERROR = -1;
 Geofence::Geofence() :
 	SuperBlock(NULL, "GF"),
 	_fence_pub(-1),
+	_home_pos{},
+	_home_pos_set(false),
+	_last_horizontal_range_warning(0),
+	_last_vertical_range_warning(0),
 	_altitude_min(0),
 	_altitude_max(0),
 	_verticesCount(0),
-	_param_geofence_on(this, "ON"),
+	_param_geofence_mode(this, "MODE"),
 	_param_altitude_mode(this, "ALTMODE"),
 	_param_source(this, "SOURCE"),
 	_param_counter_threshold(this, "COUNT"),
+	_param_max_hor_distance(this, "MAX_HOR_DIST"),
+	_param_max_ver_distance(this, "MAX_VER_DIST"),
 	_outside_counter(0),
 	_mavlinkFd(-1)
 {
@@ -92,10 +108,14 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, f
 
 
 bool Geofence::inside(const struct vehicle_global_position_s &global_position,
-		      const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl)
+		      const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl,
+		      const struct home_position_s home_pos, bool home_position_set)
 {
 	updateParams();
 
+	_home_pos = home_pos;
+	_home_pos_set = home_position_set;
+
 	if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) {
 		if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
 			return inside(global_position);
@@ -118,13 +138,48 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position,
 
 bool Geofence::inside(double lat, double lon, float altitude)
 {
+	if (_param_geofence_mode.get() >= GEOFENCE_MAX_DISTANCES_ONLY) {
+		int32_t max_horizontal_distance = _param_max_hor_distance.get();
+		int32_t max_vertical_distance = _param_max_ver_distance.get();
+
+		if (max_horizontal_distance > 0 || max_vertical_distance > 0) {
+			if (_home_pos_set) {
+				float dist_xy = -1.0f;
+				float dist_z = -1.0f;
+				get_distance_to_point_global_wgs84(lat, lon, altitude,
+								   _home_pos.lat, _home_pos.lon, _home_pos.alt,
+								   &dist_xy, &dist_z);
+
+				if (max_vertical_distance > 0 && (dist_z > max_vertical_distance)) {
+					if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
+						mavlink_log_critical(_mavlinkFd, "Geofence exceeded max vertical distance by %.1f m",
+								     (double)(dist_z - max_vertical_distance));
+						_last_vertical_range_warning = hrt_absolute_time();
+					}
+
+					return false;
+				}
+
+				if (max_horizontal_distance > 0 && (dist_xy > max_horizontal_distance)) {
+					if (hrt_elapsed_time(&_last_horizontal_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
+						mavlink_log_critical(_mavlinkFd, "Geofence exceeded max horizontal distance by %.1f m",
+								     (double)(dist_xy - max_horizontal_distance));
+						_last_horizontal_range_warning = hrt_absolute_time();
+					}
+
+					return false;
+				}
+			}
+		}
+	}
+
 	bool inside_fence = inside_polygon(lat, lon, altitude);
 
 	if (inside_fence) {
 		_outside_counter = 0;
 		return inside_fence;
-	} {
 
+	} else {
 		_outside_counter++;
 
 		if (_outside_counter > _param_counter_threshold.get()) {
@@ -139,8 +194,9 @@ bool Geofence::inside(double lat, double lon, float altitude)
 
 bool Geofence::inside_polygon(double lat, double lon, float altitude)
 {
-	/* Return true if geofence is disabled */
-	if (_param_geofence_on.get() != 1) {
+	/* Return true if geofence is disabled or only checking max distances */
+	if ((_param_geofence_mode.get() == GEOFENCE_OFF)
+	    || (_param_geofence_mode.get() == GEOFENCE_MAX_DISTANCES_ONLY)) {
 		return true;
 	}
 
@@ -366,7 +422,7 @@ Geofence::loadFromFile(const char *filename)
 
 	} else {
 		warnx("Geofence: import error");
-		mavlink_log_critical(_mavlinkFd, "#audio: Geofence import error");
+		mavlink_log_critical(_mavlinkFd, "Geofence import error");
 	}
 
 error:
diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h
index 37a41b68a..96764cc97 100644
--- a/src/modules/navigator/geofence.h
+++ b/src/modules/navigator/geofence.h
@@ -45,8 +45,10 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/home_position.h>
 #include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
+#include <drivers/drv_hrt.h>
 
 #define GEOFENCE_FILENAME "/fs/microsd/etc/geofence.txt"
 
@@ -76,7 +78,9 @@ public:
 	 * @return true: system is inside fence, false: system is outside fence
 	 */
 	bool inside(const struct vehicle_global_position_s &global_position,
-		    const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl);
+		    const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl,
+		    const struct home_position_s home_pos, bool home_position_set);
+
 	bool inside_polygon(double lat, double lon, float altitude);
 
 	int clearDm();
@@ -103,16 +107,24 @@ public:
 private:
 	orb_advert_t	_fence_pub;			/**< publish fence topic */
 
+	home_position_s _home_pos;
+	bool _home_pos_set;
+
+	hrt_abstime _last_horizontal_range_warning;
+	hrt_abstime _last_vertical_range_warning;
+
 	float			_altitude_min;
 	float			_altitude_max;
 
 	unsigned 			_verticesCount;
 
 	/* Params */
-	control::BlockParamInt _param_geofence_on;
+	control::BlockParamInt _param_geofence_mode;
 	control::BlockParamInt _param_altitude_mode;
 	control::BlockParamInt _param_source;
 	control::BlockParamInt _param_counter_threshold;
+	control::BlockParamInt _param_max_hor_distance;
+	control::BlockParamInt _param_max_ver_distance;
 
 	uint8_t			_outside_counter;
 
diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c
index fca3918e1..f3e7d4c84 100644
--- a/src/modules/navigator/geofence_params.c
+++ b/src/modules/navigator/geofence_params.c
@@ -48,16 +48,15 @@
  */
 
 /**
- * Enable geofence.
+ * Geofence mode.
  *
- * Set to 1 to enable geofence.
- * Defaults to 1 because geofence is only enabled when the geofence.txt file is present.
+ * 0 = disabled, 1 = geofence file only, 2 = max horizontal (GF_MAX_HOR_DIST) and vertical (GF_MAX_VER_DIST) distances, 3 = both
  *
  * @min 0
- * @max 1
+ * @max 3
  * @group Geofence
  */
-PARAM_DEFINE_INT32(GF_ON, 1);
+PARAM_DEFINE_INT32(GF_MODE, 0);
 
 /**
  * Geofence altitude mode
@@ -94,3 +93,21 @@ PARAM_DEFINE_INT32(GF_SOURCE, 0);
  * @group Geofence
  */
 PARAM_DEFINE_INT32(GF_COUNT, -1);
+
+/**
+ * Max horizontal distance in meters.
+ *
+ * Set to > 0 to activate RTL if horizontal distance to home exceeds this value.
+ *
+ * @group Geofence
+ */
+PARAM_DEFINE_INT32(GF_MAX_HOR_DIST, -1);
+
+/**
+ * Max vertical distance in meters.
+ *
+ * Set to > 0 to activate RTL if vertical distance to home exceeds this value.
+ *
+ * @group Geofence
+ */
+PARAM_DEFINE_INT32(GF_MAX_VER_DIST, -1);
diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp
index e370796c0..0ca69f0f7 100644
--- a/src/modules/navigator/gpsfailure.cpp
+++ b/src/modules/navigator/gpsfailure.cpp
@@ -161,12 +161,12 @@ GpsFailure::advance_gpsf()
 	case GPSF_STATE_NONE:
 		_gpsf_state = GPSF_STATE_LOITER;
 		warnx("gpsf loiter");
-		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: open loop loiter");
+		mavlink_log_critical(_navigator->get_mavlink_fd(), "GPS failed: open loop loiter");
 		break;
 	case GPSF_STATE_LOITER:
 		_gpsf_state = GPSF_STATE_TERMINATE;
 		warnx("gpsf terminate");
-		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no gps recovery, termination");
+		mavlink_log_emergency(_navigator->get_mavlink_fd(), "no gps recovery, termination");
 		warnx("mavlink sent");
 		break;
 	case GPSF_STATE_TERMINATE:
diff --git a/src/modules/navigator/gpsfailure_params.c b/src/modules/navigator/gpsfailure_params.c
index 39d179eed..98104af29 100644
--- a/src/modules/navigator/gpsfailure_params.c
+++ b/src/modules/navigator/gpsfailure_params.c
@@ -55,7 +55,7 @@
  *
  * @unit seconds
  * @min 0.0
- * @group GPSF
+ * @group GPS Failure Navigation
  */
 PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 30.0f);
 
@@ -67,7 +67,7 @@ PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 30.0f);
  * @unit deg
  * @min 0.0
  * @max 30.0
- * @group GPSF
+ * @group GPS Failure Navigation
  */
 PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f);
 
@@ -79,7 +79,7 @@ PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f);
  * @unit deg
  * @min -30.0
  * @max 30.0
- * @group GPSF
+ * @group GPS Failure Navigation
  */
 PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f);
 
@@ -90,7 +90,7 @@ PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f);
  *
  * @min 0.0
  * @max 1.0
- * @group GPSF
+ * @group GPS Failure Navigation
  */
 PARAM_DEFINE_FLOAT(NAV_GPSF_TR, 0.7f);
 
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index a94082d6f..c1763ba93 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -449,7 +449,7 @@ Mission::set_mission_items()
 		}
 
 		/* check if we already above takeoff altitude */
-		if (_navigator->get_global_position()->alt < takeoff_alt - _navigator->get_acceptance_radius()) {
+		if (_navigator->get_global_position()->alt < takeoff_alt) {
 			mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
 
 			_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 52ccebac9..dce7d4517 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -159,7 +159,7 @@ MissionBlock::is_mission_item_reached()
 			_time_first_inside_orbit = now;
 
 			// if (_mission_item.time_inside > 0.01f) {
-			// 	mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs",
+			// 	mavlink_log_critical(_mavlink_fd, "waypoint reached, wait for %.1fs",
 			// 		(double)_mission_item.time_inside);
 			// }
 		}
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index 389cdd0d2..949231b15 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -121,7 +121,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
 			}
 
 			if (!geofence.inside_polygon(missionitem.lat, missionitem.lon, missionitem.altitude)) {
-				mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i);
+				mavlink_log_critical(_mavlink_fd, "Geofence violation waypoint %d", i);
 				return false;
 			}
 		}
@@ -134,7 +134,7 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
 {
 	/* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */
 	for (size_t i = 0; i < nMissionItems; i++) {
-		static struct mission_item_s missionitem;
+		struct mission_item_s missionitem;
 		const ssize_t len = sizeof(struct mission_item_s);
 
 		if (dm_read(dm_current, i, &missionitem, len) != len) {
@@ -203,25 +203,25 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
 							return true;
 						} else {
 							/* Landing waypoint is above altitude of slope at the given waypoint distance */
-							mavlink_log_info(_mavlink_fd, "#audio: Landing: last waypoint too high/too close");
-							mavlink_log_info(_mavlink_fd, "Move down to %.1fm or move further away by %.1fm",
+							mavlink_log_critical(_mavlink_fd, "Landing: last waypoint too high/too close");
+							mavlink_log_critical(_mavlink_fd, "Move down to %.1fm or move further away by %.1fm",
 									(double)(slope_alt_req),
 									(double)(wp_distance_req - wp_distance));
 							return false;
 						}
 					} else {
 						/* Landing waypoint is above last waypoint */
-						mavlink_log_info(_mavlink_fd, "#audio: Landing waypoint above last nav waypoint");
+						mavlink_log_critical(_mavlink_fd, "Landing waypoint above last nav waypoint");
 						return false;
 					}
 				} else {
 					/* Last wp is in flare region */
 					//xxx give recommendations
-					mavlink_log_info(_mavlink_fd, "#audio: Warning: Landing: last waypoint in flare region");
+					mavlink_log_critical(_mavlink_fd, "Warning: Landing: last waypoint in flare region");
 					return false;
 				}
 			} else {
-				mavlink_log_info(_mavlink_fd, "#audio: Warning: starting with land waypoint");
+				mavlink_log_critical(_mavlink_fd, "Warning: starting with land waypoint");
 				return false;
 			}
 		}
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index d9d911d9c..d2acce789 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -186,6 +186,8 @@ private:
 	geofence_result_s				_geofence_result;
 	vehicle_attitude_setpoint_s			_att_sp;
 
+	bool 		_home_position_set;
+
 	bool 		_mission_item_valid;		/**< flags if the current mission item is valid */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index ad2349c94..042f926d3 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -123,6 +123,7 @@ Navigator::Navigator() :
 	_pos_sp_triplet{},
 	_mission_result{},
 	_att_sp{},
+	_home_position_set(false),
 	_mission_item_valid(false),
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
 	_geofence{},
@@ -203,7 +204,13 @@ Navigator::sensor_combined_update()
 void
 Navigator::home_position_update()
 {
-	orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
+	bool updated = false;
+	orb_check(_home_pos_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
+		_home_position_set = true;
+	}
 }
 
 void
@@ -392,7 +399,7 @@ Navigator::task_main()
 		/* Check geofence violation */
 		static hrt_abstime last_geofence_check = 0;
 		if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) {
-			bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter);
+			bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, _home_position_set);
 			last_geofence_check = hrt_absolute_time();
 			have_geofence_position_data = false;
 			if (!inside) {
@@ -402,7 +409,7 @@ Navigator::task_main()
 
 				/* Issue a warning about the geofence violation once */
 				if (!_geofence_violation_warning_sent) {
-					mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation");
+					mavlink_log_critical(_mavlink_fd, "Geofence violation");
 					_geofence_violation_warning_sent = true;
 				}
 			} else {
@@ -511,7 +518,7 @@ Navigator::start()
 	_navigator_task = task_spawn_cmd("navigator",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_DEFAULT + 20,
-					 1800,
+					 1700,
 					 (main_t)&Navigator::task_main_trampoline,
 					 nullptr);
 
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index 5f8f8d02f..ef4a8dc0c 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -94,8 +94,8 @@ PARAM_DEFINE_INT32(NAV_RCL_OBC, 0);
  * Latitude of airfield home waypoint
  *
  * @unit degrees * 1e7
- * @min 0.0
- * @group DLL
+ * @min 0
+ * @group Data Link Loss
  */
 PARAM_DEFINE_INT32(NAV_AH_LAT, -265847810);
 
@@ -105,8 +105,8 @@ PARAM_DEFINE_INT32(NAV_AH_LAT, -265847810);
  * Longitude of airfield home waypoint
  *
  * @unit degrees * 1e7
- * @min 0.0
- * @group DLL
+ * @min 0
+ * @group Data Link Loss
  */
 PARAM_DEFINE_INT32(NAV_AH_LON, 1518423250);
 
@@ -117,6 +117,6 @@ PARAM_DEFINE_INT32(NAV_AH_LON, 1518423250);
  *
  * @unit m
  * @min 0.0
- * @group DLL
+ * @group Data Link Loss
  */
 PARAM_DEFINE_FLOAT(NAV_AH_ALT, 600.0f);
diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp
index a7cde6325..40bf7f022 100644
--- a/src/modules/navigator/rcloss.cpp
+++ b/src/modules/navigator/rcloss.cpp
@@ -155,11 +155,11 @@ RCLoss::advance_rcl()
 	case RCL_STATE_NONE:
 		if (_param_loitertime.get() > 0.0f) {
 			warnx("RC loss, OBC mode, loiter");
-			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, loitering");
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "rc loss, loitering");
 			_rcl_state = RCL_STATE_LOITER;
 		} else {
 			warnx("RC loss, OBC mode, slip loiter, terminate");
-			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, terminating");
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "rc loss, terminating");
 			_rcl_state = RCL_STATE_TERMINATE;
 			_navigator->get_mission_result()->stay_in_failsafe = true;
 			_navigator->set_mission_result_updated();
@@ -169,7 +169,7 @@ RCLoss::advance_rcl()
 	case RCL_STATE_LOITER:
 		_rcl_state = RCL_STATE_TERMINATE;
 		warnx("time is up, no RC regain, terminating");
-		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating");
+		mavlink_log_critical(_navigator->get_mavlink_fd(), "RC not regained, terminating");
 		_navigator->get_mission_result()->stay_in_failsafe = true;
 		_navigator->set_mission_result_updated();
 		reset_mission_item_reached();
diff --git a/src/modules/navigator/rcloss_params.c b/src/modules/navigator/rcloss_params.c
index f1a01c73b..f48aabc80 100644
--- a/src/modules/navigator/rcloss_params.c
+++ b/src/modules/navigator/rcloss_params.c
@@ -55,6 +55,6 @@
  *
  * @unit seconds
  * @min -1.0
- * @group RCL
+ * @group Radio Signal Loss
  */
 PARAM_DEFINE_FLOAT(NAV_RCL_LT, 120.0f);
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index b6c4b8fdf..c1ed8cb7c 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -89,7 +89,7 @@ RTL::on_activation()
 		/* for safety reasons don't go into RTL if landed */
 		if (_navigator->get_vstatus()->condition_landed) {
 			_rtl_state = RTL_STATE_LANDED;
-			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "no RTL when landed");
 
 		/* if lower than return altitude, climb up first */
 		} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
@@ -146,7 +146,7 @@ RTL::set_rtl_item()
 		_mission_item.autocontinue = true;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home",
+		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: climb to %d meters above home",
 			(int)(climb_alt - _navigator->get_home_position()->alt));
 		break;
 	}
@@ -177,7 +177,7 @@ RTL::set_rtl_item()
 		_mission_item.autocontinue = true;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home",
+		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: return at %d meters above home",
 			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
 		break;
 	}
@@ -197,7 +197,7 @@ RTL::set_rtl_item()
 		_mission_item.autocontinue = false;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home",
+		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: descend to %d meters above home",
 			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
 		break;
 	}
@@ -222,10 +222,10 @@ RTL::set_rtl_item()
 		_navigator->set_can_loiter_at_sp(true);
 
 		if (autoland) {
-			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", (double)_mission_item.time_inside);
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: loiter %.1fs", (double)_mission_item.time_inside);
 
 		} else {
-			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter");
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: completed, loiter");
 		}
 		break;
 	}
@@ -245,7 +245,7 @@ RTL::set_rtl_item()
 		_mission_item.autocontinue = true;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home");
+		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: land at home");
 		break;
 	}
 
@@ -264,7 +264,7 @@ RTL::set_rtl_item()
 		_mission_item.autocontinue = true;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, landed");
+		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: completed, landed");
 		break;
 	}
 
diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c
index 10394fed1..3d8c78cf1 100644
--- a/src/modules/navigator/rtl_params.c
+++ b/src/modules/navigator/rtl_params.c
@@ -48,18 +48,6 @@
  */
 
 /**
- * Loiter radius after RTL (FW only)
- *
- * Default value of loiter radius after RTL (fixedwing only).
- *
- * @unit meters
- * @min 20
- * @max 200
- * @group RTL
- */
-PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f);
-
-/**
  * RTL altitude
  *
  * Altitude to fly back in RTL in meters
@@ -67,7 +55,7 @@ PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f);
  * @unit meters
  * @min 0
  * @max 150
- * @group RTL
+ * @group Return To Land
  */
 PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 60);
 
@@ -81,7 +69,7 @@ PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 60);
  * @unit meters
  * @min 2
  * @max 100
- * @group RTL
+ * @group Return To Land
  */
 PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 30);
 
@@ -94,6 +82,6 @@ PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 30);
  * @unit seconds
  * @min -1
  * @max 300
- * @group RTL
+ * @group Return To Land
  */
 PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index ec297e7f0..437395b1f 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -134,7 +134,7 @@ static void usage(const char *reason)
  */
 int position_estimator_inav_main(int argc, char *argv[])
 {
-	if (argc < 1) {
+	if (argc < 2) {
 		usage("missing command");
 	}
 
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c
index ed98e222e..aedb478fa 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -282,8 +282,8 @@ PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f);
  *
  * Set to the appropriate key (328754) to disable vision input.
  *
- * @min 0.0
- * @max 1.0
+ * @min 0
+ * @max 1
  * @group Position Estimator INAV
  */
 PARAM_DEFINE_INT32(CBRK_NO_VISION, 0);
@@ -295,9 +295,8 @@ PARAM_DEFINE_INT32(CBRK_NO_VISION, 0);
  * the system uses the combined attitude / position
  * filter framework.
  *
- * @min 0.0
- * @max 1.0
- * @unit s
+ * @min 0
+ * @max 1
  * @group Position Estimator INAV
  */
 PARAM_DEFINE_INT32(INAV_ENABLED, 1);
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index e04ffc940..ac004f212 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -99,6 +99,9 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
 
 	if (*st24_updated) {
 
+		/* ensure ADC RSSI is disabled */
+		r_setup_features &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI);
+
 		*rssi = st24_rssi;
 		r_raw_rc_count = st24_channel_count;
 
@@ -116,14 +119,14 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
 
 	for (unsigned i = 0; i < n_bytes; i++) {
 		/* set updated flag if one complete packet was parsed */
-		st24_rssi = RC_INPUT_RSSI_MAX;
+		sumd_rssi = RC_INPUT_RSSI_MAX;
 		*sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count,
 					&sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
 	}
 
 	if (*sumd_updated) {
 
-		*rssi = sumd_rssi;
+		/* not setting RSSI since SUMD does not provide one */
 		r_raw_rc_count = sumd_channel_count;
 
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD;
@@ -187,12 +190,20 @@ controls_tick() {
 			/* use 1:1 scaling on 3.3V ADC input */
 			unsigned mV = counts * 3300 / 4096;
 
-			/* scale to 0..253 */
-			rssi = mV / 13;
+			/* scale to 0..253 and lowpass */
+			rssi = (rssi * 0.99f) + ((mV / (3300 / RC_INPUT_RSSI_MAX)) * 0.01f);
+			if (rssi > RC_INPUT_RSSI_MAX) {
+				rssi = RC_INPUT_RSSI_MAX;
+			}
 		}
 	}
 #endif
 
+	/* zero RSSI if signal is lost */
+	if (!(r_raw_rc_flags & (PX4IO_P_RAW_RC_FLAGS_RC_OK))) {
+		rssi = 0;
+	}
+
 	perf_begin(c_gather_dsm);
 	bool dsm_updated, st24_updated, sumd_updated;
 	(void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &sumd_updated);
@@ -215,22 +226,26 @@ controls_tick() {
 	if (sbus_updated) {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
 
-		rssi = 255;
+		unsigned sbus_rssi = RC_INPUT_RSSI_MAX;
 
 		if (sbus_frame_drop) {
 			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
-			rssi = 100;
+			sbus_rssi = RC_INPUT_RSSI_MAX / 2;
 		} else {
 			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
 		}
 
 		if (sbus_failsafe) {
 			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
-			rssi = 0;
 		} else {
 			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
 		}
 
+		/* set RSSI to an emulated value if ADC RSSI is off */ 
+		if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
+			rssi = sbus_rssi;
+		}
+
 	}
 
 	perf_end(c_gather_sbus);
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 90e0fb10e..e7976446c 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -544,8 +544,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			break;
 
 		case PX4IO_P_SETUP_PWM_DEFAULTRATE:
-			if (value < 50) {
-				value = 50;
+			if (value < 25) {
+				value = 25;
 			}
 			if (value > 400) {
 				value = 400;
@@ -554,8 +554,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			break;
 
 		case PX4IO_P_SETUP_PWM_ALTRATE:
-			if (value < 50) {
-				value = 50;
+			if (value < 25) {
+				value = 25;
 			}
 			if (value > 400) {
 				value = 400;
diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk
index 8fded0bdb..6964acf33 100644
--- a/src/modules/sdlog2/module.mk
+++ b/src/modules/sdlog2/module.mk
@@ -48,3 +48,4 @@ MAXOPTIMIZATION	 = -Os
 
 EXTRACFLAGS = -Wframe-larger-than=1400
 
+EXTRADEFINES = -DGIT_VERSION='"$(GIT_DESC)"'
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 0bfd356a1..ae4913559 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -51,7 +51,6 @@
 #include <errno.h>
 #include <unistd.h>
 #include <stdio.h>
-#include <poll.h>
 #include <stdlib.h>
 #include <string.h>
 #include <ctype.h>
@@ -99,6 +98,8 @@
 #include <uORB/topics/wind_estimate.h>
 #include <uORB/topics/encoders.h>
 #include <uORB/topics/vtol_vehicle_status.h>
+#include <uORB/topics/time_offset.h>
+#include <uORB/topics/mc_att_ctrl_status.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -149,11 +150,6 @@ PARAM_DEFINE_INT32(SDLOG_EXT, -1);
 		log_msgs_skipped++; \
 	}
 
-#define LOG_ORB_SUBSCRIBE(_var, _topic) subs.##_var##_sub = orb_subscribe(ORB_ID(##_topic##)); \
-	fds[fdsc_count].fd = subs.##_var##_sub; \
-	fds[fdsc_count].events = POLLIN; \
-	fdsc_count++;
-
 #define MIN(X,Y) ((X) < (Y) ? (X) : (Y))
 
 static bool main_thread_should_exit = false;		/**< Deamon exit flag */
@@ -216,7 +212,7 @@ static void *logwriter_thread(void *arg);
  */
 __EXPORT int sdlog2_main(int argc, char *argv[]);
 
-static bool copy_if_updated(orb_id_t topic, int handle, void *buffer);
+static bool copy_if_updated(orb_id_t topic, int *handle, void *buffer);
 
 /**
  * Mainloop of sd log deamon.
@@ -763,7 +759,7 @@ int write_version(int fd)
 	};
 
 	/* fill version message and write it */
-	strncpy(log_msg_VER.body.fw_git, FW_GIT, sizeof(log_msg_VER.body.fw_git));
+	strncpy(log_msg_VER.body.fw_git, GIT_VERSION, sizeof(log_msg_VER.body.fw_git));
 	strncpy(log_msg_VER.body.arch, HW_ARCH, sizeof(log_msg_VER.body.arch));
 	return write(fd, &log_msg_VER, sizeof(log_msg_VER));
 }
@@ -809,14 +805,25 @@ int write_parameters(int fd)
 	return written;
 }
 
-bool copy_if_updated(orb_id_t topic, int handle, void *buffer)
+bool copy_if_updated(orb_id_t topic, int *handle, void *buffer)
 {
-	bool updated;
-
-	orb_check(handle, &updated);
+	bool updated = false;
+
+	if (*handle < 0) {
+		if (OK == orb_exists(topic, 0)) {
+			*handle = orb_subscribe(topic);
+			/* copy first data */
+			if (*handle >= 0) {
+				orb_copy(topic, *handle, buffer);
+				updated = true;
+			}
+		}
+	} else {
+		orb_check(*handle, &updated);
 
-	if (updated) {
-		orb_copy(topic, handle, buffer);
+		if (updated) {
+			orb_copy(topic, *handle, buffer);
+		}
 	}
 
 	return updated;
@@ -1027,6 +1034,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct wind_estimate_s wind_estimate;
 		struct encoders_s encoders;
 		struct vtol_vehicle_status_s vtol_status;
+		struct time_offset_s time_offset;
+		struct mc_att_ctrl_status_s mc_att_ctrl_status;
 	} buf;
 
 	memset(&buf, 0, sizeof(buf));
@@ -1071,6 +1080,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_TECS_s log_TECS;
 			struct log_WIND_s log_WIND;
 			struct log_ENCD_s log_ENCD;
+			struct log_TSYN_s log_TSYN;
+			struct log_MACS_s log_MACS;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -1111,54 +1122,51 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int servorail_status_sub;
 		int wind_sub;
 		int encoders_sub;
+		int tsync_sub;
+		int mc_att_ctrl_status_sub;
 	} subs;
 
-	subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
-	subs.status_sub = orb_subscribe(ORB_ID(vehicle_status));
-	subs.vtol_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status));
-	subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
-	subs.act_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs));
-	subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
-	subs.act_controls_1_sub = orb_subscribe(ORB_ID(actuator_controls_1));
-	subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
-	subs.local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
-	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));
-	subs.esc_sub = orb_subscribe(ORB_ID(esc_status));
-	subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
-	subs.battery_sub = orb_subscribe(ORB_ID(battery_status));
-	subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
-	subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
-	subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status));
-	subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
-	subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));
-	subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate));
-
-	/* we need to rate-limit wind, as we do not need the full update rate */
-	orb_set_interval(subs.wind_sub, 90);
-	subs.encoders_sub = orb_subscribe(ORB_ID(encoders));
+	subs.cmd_sub = -1;
+	subs.status_sub = -1;
+	subs.vtol_status_sub = -1;
+	subs.gps_pos_sub = -1;
+	subs.sensor_sub = -1;
+	subs.att_sub = -1;
+	subs.att_sp_sub = -1;
+	subs.rates_sp_sub = -1;
+	subs.act_outputs_sub = -1;
+	subs.act_controls_sub = -1;
+	subs.act_controls_1_sub = -1;
+	subs.local_pos_sub = -1;
+	subs.local_pos_sp_sub = -1;
+	subs.global_pos_sub = -1;
+	subs.triplet_sub = -1;
+	subs.vicon_pos_sub = -1;
+	subs.vision_pos_sub = -1;
+	subs.flow_sub = -1;
+	subs.rc_sub = -1;
+	subs.airspeed_sub = -1;
+	subs.esc_sub = -1;
+	subs.global_vel_sp_sub = -1;
+	subs.battery_sub = -1;
+	subs.range_finder_sub = -1;
+	subs.estimator_status_sub = -1;
+	subs.tecs_status_sub = -1;
+	subs.system_power_sub = -1;
+	subs.servorail_status_sub = -1;
+	subs.wind_sub = -1;
+	subs.tsync_sub = -1;
+	subs.mc_att_ctrl_status_sub = -1;
+	subs.encoders_sub = -1;
 
 	/* add new topics HERE */
 
 
-	for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
-		subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
-	}
-
-	if (_extended_logging) {
-		subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info));
-	} else {
-		subs.sat_info_sub = 0;
+	for (unsigned i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
+		subs.telemetry_subs[i] = -1;
 	}
+	
+	subs.sat_info_sub = -1;
 
 	/* close non-needed fd's */
 
@@ -1206,12 +1214,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 		usleep(sleep_delay);
 
 		/* --- VEHICLE COMMAND - LOG MANAGEMENT --- */
-		if (copy_if_updated(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd)) {
+		if (copy_if_updated(ORB_ID(vehicle_command), &subs.cmd_sub, &buf.cmd)) {
 			handle_command(&buf.cmd);
 		}
 
 		/* --- VEHICLE STATUS - LOG MANAGEMENT --- */
-		bool status_updated = copy_if_updated(ORB_ID(vehicle_status), subs.status_sub, &buf_status);
+		bool status_updated = copy_if_updated(ORB_ID(vehicle_status), &subs.status_sub, &buf_status);
 
 		if (status_updated) {
 			if (log_when_armed) {
@@ -1220,7 +1228,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- GPS POSITION - LOG MANAGEMENT --- */
-		bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos);
+		bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), &subs.gps_pos_sub, &buf_gps_pos);
 
 		if (gps_pos_updated && log_name_timestamp) {
 			gps_time = buf_gps_pos.time_utc_usec;
@@ -1251,7 +1259,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- VTOL VEHICLE STATUS --- */
-		if(copy_if_updated(ORB_ID(vtol_vehicle_status), subs.vtol_status_sub, &buf.vtol_status)) {
+		if(copy_if_updated(ORB_ID(vtol_vehicle_status), &subs.vtol_status_sub, &buf.vtol_status)) {
 			log_msg.msg_type = LOG_VTOL_MSG;
 			log_msg.body.log_VTOL.airspeed_tot = buf.vtol_status.airspeed_tot;
 			LOGBUFFER_WRITE_AND_COUNT(VTOL);
@@ -1282,7 +1290,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		/* --- SATELLITE INFO - UNIT #1 --- */
 		if (_extended_logging) {
 
-			if (copy_if_updated(ORB_ID(satellite_info), subs.sat_info_sub, &buf.sat_info)) {
+			if (copy_if_updated(ORB_ID(satellite_info), &subs.sat_info_sub, &buf.sat_info)) {
 
 				/* log the SNR of each satellite for a detailed view of signal quality */
 				unsigned sat_info_count = MIN(buf.sat_info.count, sizeof(buf.sat_info.snr) / sizeof(buf.sat_info.snr[0]));
@@ -1328,7 +1336,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- SENSOR COMBINED --- */
-		if (copy_if_updated(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor)) {
+		if (copy_if_updated(ORB_ID(sensor_combined), &subs.sensor_sub, &buf.sensor)) {
 			bool write_IMU = false;
 			bool write_IMU1 = false;
 			bool write_IMU2 = false;
@@ -1470,7 +1478,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- ATTITUDE --- */
-		if (copy_if_updated(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att)) {
+		if (copy_if_updated(ORB_ID(vehicle_attitude), &subs.att_sub, &buf.att)) {
 			log_msg.msg_type = LOG_ATT_MSG;
 			log_msg.body.log_ATT.q_w = buf.att.q[0];
 			log_msg.body.log_ATT.q_x = buf.att.q[1];
@@ -1489,7 +1497,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- ATTITUDE SETPOINT --- */
-		if (copy_if_updated(ORB_ID(vehicle_attitude_setpoint), subs.att_sp_sub, &buf.att_sp)) {
+		if (copy_if_updated(ORB_ID(vehicle_attitude_setpoint), &subs.att_sp_sub, &buf.att_sp)) {
 			log_msg.msg_type = LOG_ATSP_MSG;
 			log_msg.body.log_ATSP.roll_sp = buf.att_sp.roll_body;
 			log_msg.body.log_ATSP.pitch_sp = buf.att_sp.pitch_body;
@@ -1503,7 +1511,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- RATES SETPOINT --- */
-		if (copy_if_updated(ORB_ID(vehicle_rates_setpoint), subs.rates_sp_sub, &buf.rates_sp)) {
+		if (copy_if_updated(ORB_ID(vehicle_rates_setpoint), &subs.rates_sp_sub, &buf.rates_sp)) {
 			log_msg.msg_type = LOG_ARSP_MSG;
 			log_msg.body.log_ARSP.roll_rate_sp = buf.rates_sp.roll;
 			log_msg.body.log_ARSP.pitch_rate_sp = buf.rates_sp.pitch;
@@ -1512,14 +1520,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- ACTUATOR OUTPUTS --- */
-		if (copy_if_updated(ORB_ID(actuator_outputs), subs.act_outputs_sub, &buf.act_outputs)) {
+		if (copy_if_updated(ORB_ID(actuator_outputs), &subs.act_outputs_sub, &buf.act_outputs)) {
 			log_msg.msg_type = LOG_OUT0_MSG;
 			memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output));
 			LOGBUFFER_WRITE_AND_COUNT(OUT0);
 		}
 
 		/* --- ACTUATOR CONTROL --- */
-		if (copy_if_updated(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.act_controls_sub, &buf.act_controls)) {
+		if (copy_if_updated(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &subs.act_controls_sub, &buf.act_controls)) {
 			log_msg.msg_type = LOG_ATTC_MSG;
 			log_msg.body.log_ATTC.roll = buf.act_controls.control[0];
 			log_msg.body.log_ATTC.pitch = buf.act_controls.control[1];
@@ -1529,7 +1537,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- ACTUATOR CONTROL FW VTOL --- */
-		if(copy_if_updated(ORB_ID(actuator_controls_1),subs.act_controls_1_sub,&buf.act_controls)) {
+		if(copy_if_updated(ORB_ID(actuator_controls_1), &subs.act_controls_1_sub,&buf.act_controls)) {
 			log_msg.msg_type = LOG_ATC1_MSG;
 			log_msg.body.log_ATTC.roll = buf.act_controls.control[0];
 			log_msg.body.log_ATTC.pitch = buf.act_controls.control[1];
@@ -1539,7 +1547,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- LOCAL POSITION --- */
-		if (copy_if_updated(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos)) {
+		if (copy_if_updated(ORB_ID(vehicle_local_position), &subs.local_pos_sub, &buf.local_pos)) {
 			log_msg.msg_type = LOG_LPOS_MSG;
 			log_msg.body.log_LPOS.x = buf.local_pos.x;
 			log_msg.body.log_LPOS.y = buf.local_pos.y;
@@ -1565,7 +1573,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- LOCAL POSITION SETPOINT --- */
-		if (copy_if_updated(ORB_ID(vehicle_local_position_setpoint), subs.local_pos_sp_sub, &buf.local_pos_sp)) {
+		if (copy_if_updated(ORB_ID(vehicle_local_position_setpoint), &subs.local_pos_sp_sub, &buf.local_pos_sp)) {
 			log_msg.msg_type = LOG_LPSP_MSG;
 			log_msg.body.log_LPSP.x = buf.local_pos_sp.x;
 			log_msg.body.log_LPSP.y = buf.local_pos_sp.y;
@@ -1581,7 +1589,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- GLOBAL POSITION --- */
-		if (copy_if_updated(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos)) {
+		if (copy_if_updated(ORB_ID(vehicle_global_position), &subs.global_pos_sub, &buf.global_pos)) {
 			log_msg.msg_type = LOG_GPOS_MSG;
 			log_msg.body.log_GPOS.lat = buf.global_pos.lat * 1e7;
 			log_msg.body.log_GPOS.lon = buf.global_pos.lon * 1e7;
@@ -1600,7 +1608,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- GLOBAL POSITION SETPOINT --- */
-		if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) {
+		if (copy_if_updated(ORB_ID(position_setpoint_triplet), &subs.triplet_sub, &buf.triplet)) {
 
 			if (buf.triplet.current.valid) {
 				log_msg.msg_type = LOG_GPSP_MSG;
@@ -1618,7 +1626,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- VICON POSITION --- */
-		if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) {
+		if (copy_if_updated(ORB_ID(vehicle_vicon_position), &subs.vicon_pos_sub, &buf.vicon_pos)) {
 			log_msg.msg_type = LOG_VICN_MSG;
 			log_msg.body.log_VICN.x = buf.vicon_pos.x;
 			log_msg.body.log_VICN.y = buf.vicon_pos.y;
@@ -1630,7 +1638,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- VISION POSITION --- */
-		if (copy_if_updated(ORB_ID(vision_position_estimate), subs.vision_pos_sub, &buf.vision_pos)) {
+		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;
@@ -1638,15 +1646,15 @@ int sdlog2_thread_main(int argc, char *argv[])
 			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];
+			log_msg.body.log_VISN.qw = buf.vision_pos.q[0]; // vision_position_estimate uses [w,x,y,z] convention
+			log_msg.body.log_VISN.qx = buf.vision_pos.q[1];
+			log_msg.body.log_VISN.qy = buf.vision_pos.q[2];
+			log_msg.body.log_VISN.qz = buf.vision_pos.q[3];
 			LOGBUFFER_WRITE_AND_COUNT(VISN);
 		}
 
 		/* --- FLOW --- */
-		if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) {
+		if (copy_if_updated(ORB_ID(optical_flow), &subs.flow_sub, &buf.flow)) {
 			log_msg.msg_type = LOG_FLOW_MSG;
 			log_msg.body.log_FLOW.ground_distance_m = buf.flow.ground_distance_m;
 			log_msg.body.log_FLOW.gyro_temperature = buf.flow.gyro_temperature;
@@ -1662,7 +1670,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- RC CHANNELS --- */
-		if (copy_if_updated(ORB_ID(rc_channels), subs.rc_sub, &buf.rc)) {
+		if (copy_if_updated(ORB_ID(rc_channels), &subs.rc_sub, &buf.rc)) {
 			log_msg.msg_type = LOG_RC_MSG;
 			/* Copy only the first 8 channels of 14 */
 			memcpy(log_msg.body.log_RC.channel, buf.rc.channels, sizeof(log_msg.body.log_RC.channel));
@@ -1672,7 +1680,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- AIRSPEED --- */
-		if (copy_if_updated(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed)) {
+		if (copy_if_updated(ORB_ID(airspeed), &subs.airspeed_sub, &buf.airspeed)) {
 			log_msg.msg_type = LOG_AIRS_MSG;
 			log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s;
 			log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s;
@@ -1681,7 +1689,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- ESCs --- */
-		if (copy_if_updated(ORB_ID(esc_status), subs.esc_sub, &buf.esc)) {
+		if (copy_if_updated(ORB_ID(esc_status), &subs.esc_sub, &buf.esc)) {
 			for (uint8_t i = 0; i < buf.esc.esc_count; i++) {
 				log_msg.msg_type = LOG_ESC_MSG;
 				log_msg.body.log_ESC.counter = buf.esc.counter;
@@ -1701,7 +1709,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- GLOBAL VELOCITY SETPOINT --- */
-		if (copy_if_updated(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp)) {
+		if (copy_if_updated(ORB_ID(vehicle_global_velocity_setpoint), &subs.global_vel_sp_sub, &buf.global_vel_sp)) {
 			log_msg.msg_type = LOG_GVSP_MSG;
 			log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx;
 			log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy;
@@ -1710,7 +1718,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- BATTERY --- */
-		if (copy_if_updated(ORB_ID(battery_status), subs.battery_sub, &buf.battery)) {
+		if (copy_if_updated(ORB_ID(battery_status), &subs.battery_sub, &buf.battery)) {
 			log_msg.msg_type = LOG_BATT_MSG;
 			log_msg.body.log_BATT.voltage = buf.battery.voltage_v;
 			log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v;
@@ -1720,7 +1728,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- SYSTEM POWER RAILS --- */
-		if (copy_if_updated(ORB_ID(system_power), subs.system_power_sub, &buf.system_power)) {
+		if (copy_if_updated(ORB_ID(system_power), &subs.system_power_sub, &buf.system_power)) {
 			log_msg.msg_type = LOG_PWR_MSG;
 			log_msg.body.log_PWR.peripherals_5v = buf.system_power.voltage5V_v;
 			log_msg.body.log_PWR.usb_ok = buf.system_power.usb_connected;
@@ -1738,8 +1746,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- TELEMETRY --- */
-		for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
-			if (copy_if_updated(telemetry_status_orb_id[i], subs.telemetry_subs[i], &buf.telemetry)) {
+		for (unsigned i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
+			if (copy_if_updated(telemetry_status_orb_id[i], &subs.telemetry_subs[i], &buf.telemetry)) {
 				log_msg.msg_type = LOG_TEL0_MSG + i;
 				log_msg.body.log_TEL.rssi = buf.telemetry.rssi;
 				log_msg.body.log_TEL.remote_rssi = buf.telemetry.remote_rssi;
@@ -1754,7 +1762,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- BOTTOM DISTANCE --- */
-		if (copy_if_updated(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder)) {
+		if (copy_if_updated(ORB_ID(sensor_range_finder), &subs.range_finder_sub, &buf.range_finder)) {
 			log_msg.msg_type = LOG_DIST_MSG;
 			log_msg.body.log_DIST.bottom = buf.range_finder.distance;
 			log_msg.body.log_DIST.bottom_rate = 0.0f;
@@ -1763,7 +1771,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- ESTIMATOR STATUS --- */
-		if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) {
+		if (copy_if_updated(ORB_ID(estimator_status), &subs.estimator_status_sub, &buf.estimator_status)) {
 			log_msg.msg_type = LOG_EST0_MSG;
 			unsigned maxcopy0 = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_EST0.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_EST0.s);
 			memset(&(log_msg.body.log_EST0.s), 0, sizeof(log_msg.body.log_EST0.s));
@@ -1782,7 +1790,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- TECS STATUS --- */
-		if (copy_if_updated(ORB_ID(tecs_status), subs.tecs_status_sub, &buf.tecs_status)) {
+		if (copy_if_updated(ORB_ID(tecs_status), &subs.tecs_status_sub, &buf.tecs_status)) {
 			log_msg.msg_type = LOG_TECS_MSG;
 			log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp;
 			log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered;
@@ -1802,7 +1810,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- WIND ESTIMATE --- */
-		if (copy_if_updated(ORB_ID(wind_estimate), subs.wind_sub, &buf.wind_estimate)) {
+		if (copy_if_updated(ORB_ID(wind_estimate), &subs.wind_sub, &buf.wind_estimate)) {
 			log_msg.msg_type = LOG_WIND_MSG;
 			log_msg.body.log_WIND.x = buf.wind_estimate.windspeed_north;
 			log_msg.body.log_WIND.y = buf.wind_estimate.windspeed_east;
@@ -1812,7 +1820,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- ENCODERS --- */
-		if (copy_if_updated(ORB_ID(encoders), subs.encoders_sub, &buf.encoders)) {
+		if (copy_if_updated(ORB_ID(encoders), &subs.encoders_sub, &buf.encoders)) {
 			log_msg.msg_type = LOG_ENCD_MSG;
 			log_msg.body.log_ENCD.cnt0 = buf.encoders.counts[0];
 			log_msg.body.log_ENCD.vel0 = buf.encoders.velocity[0];
@@ -1821,6 +1829,22 @@ int sdlog2_thread_main(int argc, char *argv[])
 			LOGBUFFER_WRITE_AND_COUNT(ENCD);
 		}
 
+		/* --- TIMESYNC OFFSET --- */
+		if (copy_if_updated(ORB_ID(time_offset), &subs.tsync_sub, &buf.time_offset)) {
+			log_msg.msg_type = LOG_TSYN_MSG;
+			log_msg.body.log_TSYN.time_offset = buf.time_offset.offset_ns;
+			LOGBUFFER_WRITE_AND_COUNT(TSYN);
+		}
+
+		/* --- MULTIROTOR ATTITUDE CONTROLLER STATUS --- */
+		if (copy_if_updated(ORB_ID(mc_att_ctrl_status), &subs.mc_att_ctrl_status_sub, &buf.mc_att_ctrl_status)) {
+			log_msg.msg_type = LOG_MACS_MSG;
+			log_msg.body.log_MACS.roll_rate_integ = buf.mc_att_ctrl_status.roll_rate_integ;
+			log_msg.body.log_MACS.pitch_rate_integ = buf.mc_att_ctrl_status.pitch_rate_integ;
+			log_msg.body.log_MACS.yaw_rate_integ = buf.mc_att_ctrl_status.yaw_rate_integ;
+			LOGBUFFER_WRITE_AND_COUNT(MACS);
+		}
+
 		/* signal the other thread new data, but not yet unlock */
 		if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
 			/* only request write if several packets can be written at once */
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index a1fe2c95d..abdf518c5 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -441,7 +441,7 @@ struct log_ENCD_s {
 };
 
 /* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */
-#define LOG_AIR1_MSG 40
+#define LOG_AIR1_MSG 41
 
 /* --- VTOL - VTOL VEHICLE STATUS */
 #define LOG_VTOL_MSG 42
@@ -449,6 +449,20 @@ struct log_VTOL_s {
 	float airspeed_tot;
 };
 
+/* --- TIMESYNC - TIME SYNCHRONISATION OFFSET */
+#define LOG_TSYN_MSG 43
+struct log_TSYN_s {
+	uint64_t time_offset;
+};
+
+/* --- MACS - MULTIROTOR ATTITUDE CONTROLLER STATUS */
+#define LOG_MACS_MSG 44
+struct log_MACS_s {
+	float roll_rate_integ;
+	float pitch_rate_integ;
+	float yaw_rate_integ;
+};
+
 /********** SYSTEM MESSAGES, ID > 0x80 **********/
 
 /* --- TIME - TIME STAMP --- */
@@ -517,6 +531,8 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(TECS, "fffffffffffffB",	"ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
 	LOG_FORMAT(WIND, "ffff",	"X,Y,CovX,CovY"),
 	LOG_FORMAT(ENCD, "qfqf",	"cnt0,vel0,cnt1,vel1"),
+	LOG_FORMAT(TSYN, "Q", 		"TimeOffset"),
+	LOG_FORMAT(MACS, "fff", "RRint,PRint,YRint"),
 
 	/* system-level messages, ID >= 0x80 */
 	/* FMT: don't write format of format message, it's useless */
diff --git a/src/modules/segway/segway_main.cpp b/src/modules/segway/segway_main.cpp
index ee492f85a..b36d56d6d 100644
--- a/src/modules/segway/segway_main.cpp
+++ b/src/modules/segway/segway_main.cpp
@@ -92,8 +92,9 @@ usage(const char *reason)
 int segway_main(int argc, char *argv[])
 {
 
-	if (argc < 1)
+	if (argc < 2) {
 		usage("missing command");
+	}
 
 	if (!strcmp(argv[1], "start")) {
 
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 1acc14fc6..1bebea206 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -960,6 +960,7 @@ PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000);
 /**
  * Scaling factor for battery voltage sensor on FMU v2.
  *
+ * @board CONFIG_ARCH_BOARD_PX4FMU_V2
  * @group Battery Calibration
  */
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
@@ -969,6 +970,7 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
  *
  * For R70 = 133K, R71 = 10K --> scale = 1.8 * 143 / (4096*10) = 0.0063
  *
+ * @board CONFIG_ARCH_BOARD_AEROCORE
  * @group Battery Calibration
  */
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0063f);
@@ -1099,7 +1101,7 @@ PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
  *
  * @min 0
  * @max 18
- * @group Radio Calibration
+ * @group Radio Switches
  */
 PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0);
 
@@ -1108,7 +1110,7 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0);
  *
  * @min 0
  * @max 18
- * @group Radio Calibration
+ * @group Radio Switches
  */
 PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
 
@@ -1117,7 +1119,7 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
  *
  * @min 0
  * @max 18
- * @group Radio Calibration
+ * @group Radio Switches
  */
 PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0);
 
@@ -1126,7 +1128,7 @@ PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0);
  *
  * @min 0
  * @max 18
- * @group Radio Calibration
+ * @group Radio Switches
  */
 PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0);
 
@@ -1135,7 +1137,7 @@ PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0);
  *
  * @min 0
  * @max 18
- * @group Radio Calibration
+ * @group Radio Switches
  */
 PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0);
 
@@ -1144,7 +1146,7 @@ PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0);
  *
  * @min 0
  * @max 18
- * @group Radio Calibration
+ * @group Radio Switches
  */
 PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
 
@@ -1153,7 +1155,7 @@ PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
  *
  * @min 0
  * @max 18
- * @group Radio Calibration
+ * @group Radio Switches
  */
 PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0);
 
@@ -1238,9 +1240,6 @@ PARAM_DEFINE_INT32(RC_FAILS_THR, 0);
 /**
  * Threshold for selecting assist mode
  *
- * min:-1
- * max:+1
- *
  * 0-1 indicate where in the full channel range the threshold sits
  * 		0 : min
  * 		1 : max
@@ -1248,15 +1247,17 @@ PARAM_DEFINE_INT32(RC_FAILS_THR, 0);
  * 		positive : true when channel>th
  * 		negative : true when channel<th
  *
+ * @min -1
+ * @max 1
+ * @group Radio Switches
+ *
+ *
  */
 PARAM_DEFINE_FLOAT(RC_ASSIST_TH, 0.25f);
 
 /**
  * Threshold for selecting auto mode
  *
- * min:-1
- * max:+1
- *
  * 0-1 indicate where in the full channel range the threshold sits
  * 		0 : min
  * 		1 : max
@@ -1264,15 +1265,17 @@ PARAM_DEFINE_FLOAT(RC_ASSIST_TH, 0.25f);
  * 		positive : true when channel>th
  * 		negative : true when channel<th
  *
+ * @min -1
+ * @max 1
+ * @group Radio Switches
+ *
+ *
  */
 PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f);
 
 /**
  * Threshold for selecting posctl mode
  *
- * min:-1
- * max:+1
- *
  * 0-1 indicate where in the full channel range the threshold sits
  * 		0 : min
  * 		1 : max
@@ -1280,15 +1283,16 @@ PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f);
  * 		positive : true when channel>th
  * 		negative : true when channel<th
  *
+ * @min -1
+ * @max 1
+ *
+ *
  */
 PARAM_DEFINE_FLOAT(RC_POSCTL_TH, 0.5f);
 
 /**
  * Threshold for selecting return to launch mode
  *
- * min:-1
- * max:+1
- *
  * 0-1 indicate where in the full channel range the threshold sits
  * 		0 : min
  * 		1 : max
@@ -1296,15 +1300,17 @@ PARAM_DEFINE_FLOAT(RC_POSCTL_TH, 0.5f);
  * 		positive : true when channel>th
  * 		negative : true when channel<th
  *
+ * @min -1
+ * @max 1
+ * @group Radio Switches
+ *
+ *
  */
 PARAM_DEFINE_FLOAT(RC_RETURN_TH, 0.5f);
 
 /**
  * Threshold for selecting loiter mode
  *
- * min:-1
- * max:+1
- *
  * 0-1 indicate where in the full channel range the threshold sits
  * 		0 : min
  * 		1 : max
@@ -1312,15 +1318,17 @@ PARAM_DEFINE_FLOAT(RC_RETURN_TH, 0.5f);
  * 		positive : true when channel>th
  * 		negative : true when channel<th
  *
+ * @min -1
+ * @max 1
+ * @group Radio Switches
+ *
+ *
  */
 PARAM_DEFINE_FLOAT(RC_LOITER_TH, 0.5f);
 
 /**
  * Threshold for selecting acro mode
  *
- * min:-1
- * max:+1
- *
  * 0-1 indicate where in the full channel range the threshold sits
  * 		0 : min
  * 		1 : max
@@ -1328,6 +1336,11 @@ PARAM_DEFINE_FLOAT(RC_LOITER_TH, 0.5f);
  * 		positive : true when channel>th
  * 		negative : true when channel<th
  *
+ * @min -1
+ * @max 1
+ * @group Radio Switches
+ *
+ *
  */
 PARAM_DEFINE_FLOAT(RC_ACRO_TH, 0.5f);
 
@@ -1335,9 +1348,6 @@ PARAM_DEFINE_FLOAT(RC_ACRO_TH, 0.5f);
 /**
  * Threshold for selecting offboard mode
  *
- * min:-1
- * max:+1
- *
  * 0-1 indicate where in the full channel range the threshold sits
  * 		0 : min
  * 		1 : max
@@ -1345,5 +1355,49 @@ PARAM_DEFINE_FLOAT(RC_ACRO_TH, 0.5f);
  * 		positive : true when channel>th
  * 		negative : true when channel<th
  *
+ * @min -1
+ * @max 1
+ * @group Radio Switches
+ *
+ *
  */
 PARAM_DEFINE_FLOAT(RC_OFFB_TH, 0.5f);
+
+/**
+ * PWM input channel that provides RSSI.
+ *
+ * 0: do not read RSSI from input channel
+ * 1-18: read RSSI from specified input channel
+ *
+ * Specify the range for RSSI input with RC_RSSI_PWM_MIN and RC_RSSI_PWM_MAX parameters.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ *
+ */
+PARAM_DEFINE_INT32(RC_RSSI_PWM_CHAN, 0);
+
+/**
+ * Max input value for RSSI reading.
+ *
+ * Only used if RC_RSSI_PWM_CHAN > 0
+ *
+ * @min 0
+ * @max 2000
+ * @group Radio Calibration
+ *
+ */
+PARAM_DEFINE_INT32(RC_RSSI_PWM_MAX, 1000);
+
+/**
+ * Min input value for RSSI reading.
+ *
+ * Only used if RC_RSSI_PWM_CHAN > 0
+ *
+ * @min 0
+ * @max 2000
+ * @group Radio Calibration
+ *
+ */
+PARAM_DEFINE_INT32(RC_RSSI_PWM_MIN, 2000);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 259361be6..4fbc0416e 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -145,7 +145,7 @@
 #endif
 static const int ERROR = -1;
 
-#define CAL_FAILED_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u"
+#define CAL_ERROR_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u"
 
 /**
  * Sensor app start / stop handling function
@@ -246,6 +246,7 @@ private:
 	struct differential_pressure_s _diff_pres;
 	struct airspeed_s _airspeed;
 	struct rc_parameter_map_s _rc_parameter_map;
+	float _param_rc_values[RC_PARAM_MAP_NCHAN];	/**< parameter values for RC control */
 
 	math::Matrix<3, 3>	_board_rotation;	/**< rotation matrix for the orientation that the board is mounted */
 	math::Matrix<3, 3>	_mag_rotation[3];		/**< rotation matrix for the orientation that the external mag0 is mounted */
@@ -523,6 +524,7 @@ Sensors::Sensors() :
 	/* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
 
+	_param_rc_values{},
 	_board_rotation{},
 	_mag_rotation{},
 
@@ -620,6 +622,18 @@ Sensors::Sensors() :
 	/* Barometer QNH */
 	_parameter_handles.baro_qnh = param_find("SENS_BARO_QNH");
 
+	// These are parameters for which QGroundControl always expects to be returned in a list request.
+	// We do a param_find here to force them into the list.
+	(void)param_find("RC_CHAN_CNT");
+	(void)param_find("RC_TH_USER");
+	(void)param_find("CAL_MAG0_ID");
+	(void)param_find("CAL_MAG1_ID");
+	(void)param_find("CAL_MAG2_ID");
+	(void)param_find("CAL_MAG0_ROT");
+	(void)param_find("CAL_MAG1_ROT");
+	(void)param_find("CAL_MAG2_ROT");
+	(void)param_find("SYS_PARAM_VER");
+	
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -1382,14 +1396,13 @@ Sensors::parameter_update_poll(bool forced)
 					failed = failed || (OK != param_get(param_find(str), &gscale.z_scale));
 
 					if (failed) {
-						warnx(CAL_FAILED_APPLY_CAL_MSG, "gyro", i);
+						warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i);
 					} else {
 						/* apply new scaling and offsets */
 						res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale);
 						if (res) {
-							warnx(CAL_FAILED_APPLY_CAL_MSG, "gyro", i);
+							warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i);
 						} else {
-							gyro_count++;
 							config_ok = true;
 						}
 					}
@@ -1397,11 +1410,11 @@ Sensors::parameter_update_poll(bool forced)
 				}
 			}
 
-			close(fd);
-
-			if (!config_ok) {
-				warnx("NO CONFIG FOR GYRO #%u", s);
+			if (config_ok) {
+				gyro_count++;
 			}
+
+			close(fd);
 		}
 
 		/* run through all accel sensors */
@@ -1449,14 +1462,13 @@ Sensors::parameter_update_poll(bool forced)
 					failed = failed || (OK != param_get(param_find(str), &gscale.z_scale));
 
 					if (failed) {
-						warnx(CAL_FAILED_APPLY_CAL_MSG, "accel", i);
+						warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i);
 					} else {
 						/* apply new scaling and offsets */
 						res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&gscale);
 						if (res) {
-							warnx(CAL_FAILED_APPLY_CAL_MSG, "accel", i);
+							warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i);
 						} else {
-							accel_count++;
 							config_ok = true;
 						}
 					}
@@ -1464,11 +1476,11 @@ Sensors::parameter_update_poll(bool forced)
 				}
 			}
 
-			close(fd);
-
-			if (!config_ok) {
-				warnx("NO CONFIG FOR ACCEL #%u", s);
+			if (config_ok) {
+				accel_count++;
 			}
+
+			close(fd);
 		}
 
 		/* run through all mag sensors */
@@ -1480,9 +1492,16 @@ Sensors::parameter_update_poll(bool forced)
 			int fd = open(str, 0);
 
 			if (fd < 0) {
+				/* the driver is not running, abort */
 				continue;
 			}
 
+			/* set a valid default rotation (same as board).
+			 * if the mag is configured, this might be replaced
+			 * in the section below.
+			 */
+			_mag_rotation[s] = _board_rotation;
+
 			bool config_ok = false;
 
 			/* run through all stored calibrations */ 
@@ -1566,14 +1585,13 @@ Sensors::parameter_update_poll(bool forced)
 					}
 
 					if (failed) {
-						warnx(CAL_FAILED_APPLY_CAL_MSG, "mag", i);
+						warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i);
 					} else {
 						/* apply new scaling and offsets */
 						res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&gscale);
 						if (res) {
-							warnx(CAL_FAILED_APPLY_CAL_MSG, "mag", i);
+							warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i);
 						} else {
-							mag_count++;
 							config_ok = true;
 						}
 					}
@@ -1581,11 +1599,11 @@ Sensors::parameter_update_poll(bool forced)
 				}
 			}
 
-			close(fd);
-
-			if (!config_ok) {
-				warnx("NO CONFIG FOR MAG #%u", s);
+			if (config_ok) {
+				mag_count++;
 			}
+
+			close(fd);
 		}
 
 		int fd = open(AIRSPEED0_DEVICE_PATH, 0);
@@ -1605,7 +1623,8 @@ Sensors::parameter_update_poll(bool forced)
 			close(fd);
 		}
 
-		warnx("config: %u gyros, %u mags, %u accels", gyro_count, mag_count, accel_count);
+		/* do not output this for now, as its covered in preflight checks */
+		// warnx("valid configs: %u gyros, %u mags, %u accels", gyro_count, mag_count, accel_count);
 	}
 }
 
@@ -1629,7 +1648,7 @@ Sensors::rc_parameter_map_poll(bool forced)
 
 			/* Set the handle by index if the index is set, otherwise use the id */
 			if (_rc_parameter_map.param_index[i] >= 0) {
-				_parameter_handles.rc_param[i] = param_for_index((unsigned)_rc_parameter_map.param_index[i]);
+				_parameter_handles.rc_param[i] = param_for_used_index((unsigned)_rc_parameter_map.param_index[i]);
 
 			} else {
 				_parameter_handles.rc_param[i] = param_find(_rc_parameter_map.param_id[i]);
@@ -1834,8 +1853,6 @@ Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv)
 void
 Sensors::set_params_from_rc()
 {
-	static float param_rc_values[RC_PARAM_MAP_NCHAN] = {};
-
 	for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
 		if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
 			/* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
@@ -1847,8 +1864,8 @@ Sensors::set_params_from_rc()
 		float rc_val = get_rc_value((rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0);
 		/* Check if the value has changed,
 		 * maybe we need to introduce a more aggressive limit here */
-		if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) {
-			param_rc_values[i] = rc_val;
+		if (rc_val > _param_rc_values[i] + FLT_EPSILON || rc_val < _param_rc_values[i] - FLT_EPSILON) {
+			_param_rc_values[i] = rc_val;
 			float param_val = math::constrain(
 						  _rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val,
 						  _rc_parameter_map.value_min[i], _rc_parameter_map.value_max[i]);
@@ -2252,7 +2269,7 @@ Sensors::start()
 
 int sensors_main(int argc, char *argv[])
 {
-	if (argc < 1) {
+	if (argc < 2) {
 		errx(1, "usage: sensors {start|stop|status}");
 	}
 
diff --git a/src/modules/systemlib/circuit_breaker_params.c b/src/modules/systemlib/circuit_breaker_params.c
index e499ae27a..e5cc034bc 100644
--- a/src/modules/systemlib/circuit_breaker_params.c
+++ b/src/modules/systemlib/circuit_breaker_params.c
@@ -43,7 +43,6 @@
  */
 
 #include <px4.h>
-#include <systemlib/circuit_breaker_params.h>
 
 /**
  * Circuit breaker for power supply check
@@ -56,7 +55,7 @@
  * @max 894281
  * @group Circuit Breaker
  */
-PX4_PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK);
+PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0);
 
 /**
  * Circuit breaker for rate controller output
@@ -69,7 +68,7 @@ PX4_PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK);
  * @max 140253
  * @group Circuit Breaker
  */
-PX4_PARAM_DEFINE_INT32(CBRK_RATE_CTRL);
+PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0);
 
 /**
  * Circuit breaker for IO safety
@@ -81,7 +80,7 @@ PX4_PARAM_DEFINE_INT32(CBRK_RATE_CTRL);
  * @max 22027
  * @group Circuit Breaker
  */
-PX4_PARAM_DEFINE_INT32(CBRK_IO_SAFETY);
+PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0);
 
 /**
  * Circuit breaker for airspeed sensor
@@ -93,7 +92,7 @@ PX4_PARAM_DEFINE_INT32(CBRK_IO_SAFETY);
  * @max 162128
  * @group Circuit Breaker
  */
-PX4_PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK);
+PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
 
 /**
  * Circuit breaker for flight termination
@@ -106,7 +105,7 @@ PX4_PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK);
  * @max 121212
  * @group Circuit Breaker
  */
-PX4_PARAM_DEFINE_INT32(CBRK_FLIGHTTERM);
+PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212);
 
 /**
  * Circuit breaker for engine failure detection
@@ -120,4 +119,4 @@ PX4_PARAM_DEFINE_INT32(CBRK_FLIGHTTERM);
  * @max 284953
  * @group Circuit Breaker
  */
-PX4_PARAM_DEFINE_INT32(CBRK_ENGINEFAIL);
+PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953);
diff --git a/src/modules/systemlib/circuit_breaker_params.h b/src/modules/systemlib/circuit_breaker_params.h
deleted file mode 100644
index 768bf7f53..000000000
--- a/src/modules/systemlib/circuit_breaker_params.h
+++ /dev/null
@@ -1,7 +0,0 @@
-#define PARAM_CBRK_SUPPLY_CHK_DEFAULT 0
-#define PARAM_CBRK_RATE_CTRL_DEFAULT 0
-#define PARAM_CBRK_IO_SAFETY_DEFAULT 0
-#define PARAM_CBRK_AIRSPD_CHK_DEFAULT 0
-#define PARAM_CBRK_FLIGHTTERM_DEFAULT 121212
-#define PARAM_CBRK_ENGINEFAIL_DEFAULT 284953
-#define PARAM_CBRK_GPSFAIL_DEFAULT 240024
diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index 8dea6e6cc..4ec885ab3 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -92,7 +92,7 @@ struct param_wbuf_s {
 };
 
 // XXX this should be param_info_count, but need to work out linking
-uint8_t param_changed_storage[(600 / sizeof(uint8_t)) + 1] = {};
+uint8_t param_changed_storage[(700 / sizeof(uint8_t)) + 1] = {};
 
 /** flexible array holding modified parameter values */
 UT_array	*param_values;
@@ -265,8 +265,37 @@ param_count_used(void)
 param_t
 param_for_index(unsigned index)
 {
-	if (index < param_info_count)
+	if (index < param_info_count) {
 		return (param_t)index;
+	}
+
+	return PARAM_INVALID;
+}
+
+param_t
+param_for_used_index(unsigned index)
+{
+	if (index < param_info_count) {
+
+		/* walk all params and count */
+		int count = 0;
+
+		for (unsigned i = 0; i < (unsigned)param_info_count + 1; i++) {
+			for (unsigned j = 0; j < 8; j++) {
+				if (param_changed_storage[i] & (1 << j)) {
+
+					/* we found the right used count,
+					 * return the param value
+					 */
+					if (index == count) {
+						return (param_t)i;
+					}
+
+					count++;
+				}
+			}
+		}
+	}
 
 	return PARAM_INVALID;
 }
@@ -274,8 +303,9 @@ param_for_index(unsigned index)
 int
 param_get_index(param_t param)
 {
-	if (handle_in_range(param))
+	if (handle_in_range(param)) {
 		return (unsigned)param;
+	}
 
 	return -1;
 }
@@ -283,7 +313,9 @@ param_get_index(param_t param)
 int
 param_get_used_index(param_t param)
 {
-	if (!handle_in_range(param)) {
+	int param_storage_index = param_get_index(param);
+
+	if (param_storage_index < 0) {
 		return -1;
 	}
 
@@ -293,12 +325,17 @@ param_get_used_index(param_t param)
 	for (unsigned i = 0; i < (unsigned)param + 1; i++) {
 		for (unsigned j = 0; j < 8; j++) {
 			if (param_changed_storage[i] & (1 << j)) {
+
+				if (param_storage_index == i) {
+					return count;
+				}
+
 				count++;
 			}
 		}
 	}
 
-	return count;
+	return -1;
 }
 
 const char *
diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h
index b29a7e51d..9cbe3570b 100644
--- a/src/modules/systemlib/param/param.h
+++ b/src/modules/systemlib/param/param.h
@@ -130,6 +130,14 @@ __EXPORT bool		param_used(param_t param);
 __EXPORT param_t	param_for_index(unsigned index);
 
 /**
+ * Look up an used parameter by index.
+ *
+ * @param param		The parameter to obtain the index for.
+ * @return		The index of the parameter in use, or -1 if the parameter does not exist.
+ */
+__EXPORT param_t	param_for_used_index(unsigned index);
+
+/**
  * Look up the index of a parameter.
  *
  * @param param		The parameter to obtain the index for.
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index dbed29774..1a38b981e 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -250,3 +250,9 @@ ORB_DEFINE(wind_estimate, struct wind_estimate_s);
 
 #include "topics/rc_parameter_map.h"
 ORB_DEFINE(rc_parameter_map, struct rc_parameter_map_s);
+
+#include "topics/time_offset.h"
+ORB_DEFINE(time_offset, struct time_offset_s);
+
+#include "topics/mc_att_ctrl_status.h"
+ORB_DEFINE(mc_att_ctrl_status, struct mc_att_ctrl_status_s);
diff --git a/src/modules/uORB/topics/time_offset.h b/src/modules/uORB/topics/time_offset.h
new file mode 100644
index 000000000..99e526c76
--- /dev/null
+++ b/src/modules/uORB/topics/time_offset.h
@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 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 time_offset.h
+ * Time synchronisation offset
+ */
+
+#ifndef TOPIC_TIME_OFFSET_H_
+#define TOPIC_TIME_OFFSET_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Timesync offset for synchronisation with companion computer, GCS, etc.
+ */
+struct time_offset_s {
+
+	uint64_t offset_ns;		/**< time offset between companion system and PX4, in nanoseconds */
+
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(time_offset);
+
+#endif /* TOPIC_TIME_OFFSET_H_ */
diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp
index b4f81d429..d531c6b0b 100644
--- a/src/modules/uORB/uORB.cpp
+++ b/src/modules/uORB/uORB.cpp
@@ -41,6 +41,7 @@
 #include <drivers/device/device.h>
 
 #include <sys/types.h>
+#include <sys/stat.h>
 #include <stdint.h>
 #include <stdbool.h>
 #include <string.h>
@@ -1196,6 +1197,25 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve
 
 } // namespace
 
+int
+orb_exists(const struct orb_metadata *meta, int instance)
+{
+	/*
+	 * Generate the path to the node and try to open it.
+	 */
+	char path[orb_maxpath];
+	int inst = instance;
+	int ret = node_mkpath(path, PUBSUB, meta, &inst);
+
+	if (ret != OK) {
+		errno = -ret;
+		return ERROR;
+	}
+
+	struct stat buffer;
+	return stat(path, &buffer);
+}
+
 orb_advert_t
 orb_advertise(const struct orb_metadata *meta, const void *data)
 {
diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h
index 44dc6614f..a8b19d91f 100644
--- a/src/modules/uORB/uORB.h
+++ b/src/modules/uORB/uORB.h
@@ -319,6 +319,15 @@ extern int	orb_check(int handle, bool *updated) __EXPORT;
 extern int	orb_stat(int handle, uint64_t *time) __EXPORT;
 
 /**
+ * Check if a topic has already been created.
+ *
+ * @param meta		ORB topic metadata.
+ * @param instance	ORB instance
+ * @return		OK if the topic exists, ERROR otherwise with errno set accordingly.
+ */
+extern int	orb_exists(const struct orb_metadata *meta, int instance) __EXPORT;
+
+/**
  * Return the priority of the topic
  *
  * @param handle	A handle returned from orb_subscribe.
diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk
index 4f63629a0..437feb301 100644
--- a/src/modules/uavcan/module.mk
+++ b/src/modules/uavcan/module.mk
@@ -58,17 +58,20 @@ SRCS += sensors/sensor_bridge.cpp   \
 # libuavcan
 #
 include $(PX4_LIB_DIR)uavcan/libuavcan/include.mk
-SRCS += $(LIBUAVCAN_SRC)
+# Use the relitive path to keep the genrated files in the BUILD_DIR
+SRCS += $(subst  $(PX4_MODULE_SRC),../../,$(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
 # because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode.
-override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS
+override EXTRADEFINES := $(EXTRADEFINES) -DGIT_VERSION='"$(GIT_DESC)"' -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS
+
 
 #
 # libuavcan drivers for STM32
 #
 include $(PX4_LIB_DIR)uavcan/libuavcan_drivers/stm32/driver/include.mk
-SRCS += $(LIBUAVCAN_STM32_SRC)
+# Use the relitive path to keep the genrated files in the BUILD_DIR
+SRCS += $(subst  $(PX4_MODULE_SRC),../../,$(LIBUAVCAN_STM32_SRC))
 INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC)
 override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2
 
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 02cde9656..f04ab9f17 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -210,9 +210,9 @@ void UavcanNode::fill_node_info()
 	/* software version */
 	uavcan::protocol::SoftwareVersion swver;
 
-	// Extracting the first 8 hex digits of FW_GIT and converting them to int
+	// Extracting the first 8 hex digits of GIT_VERSION and converting them to int
 	char fw_git_short[9] = {};
-	std::memmove(fw_git_short, FW_GIT, 8);
+	std::memmove(fw_git_short, GIT_VERSION, 8);
 	assert(fw_git_short[8] == '\0');
 	char *end = nullptr;
 	swver.vcs_commit = std::strtol(fw_git_short, &end, 16);
@@ -698,7 +698,9 @@ int uavcan_main(int argc, char *argv[])
 
 	if (!std::strcmp(argv[1], "start")) {
 		if (UavcanNode::instance()) {
-			errx(1, "already started");
+			// Already running, no error
+			warnx("already started");
+			::exit(0);
 		}
 
 		// Node ID
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index 74e1efd6c..2ae10bd27 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -176,7 +176,7 @@ private:
 	bool flag_idle_mc;		//false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode"
 	unsigned _motor_count;	// number of motors
 	float _airspeed_tot;
-
+	float _tilt_control;
 //*****************Member functions***********************************************************************
 
 	void 		task_main();	//main task
@@ -241,6 +241,7 @@ VtolAttitudeControl::VtolAttitudeControl() :
 
 	flag_idle_mc = true;
 	_airspeed_tot = 0.0f;
+	_tilt_control = 0.0f;
 
 	memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status));
 	_vtol_vehicle_status.vtol_in_rw_mode = true;	/* start vtol in rotary wing mode*/
@@ -521,6 +522,7 @@ void VtolAttitudeControl::fill_mc_att_control_output()
 	//set neutral position for elevons
 	_actuators_out_1.control[0] = _actuators_mc_in.control[2];	//roll elevon
 	_actuators_out_1.control[1] = _actuators_mc_in.control[1];;	//pitch elevon
+	_actuators_out_1.control[4] = _tilt_control;	// for tilt-rotor control
 }
 
 /**
@@ -763,7 +765,7 @@ void VtolAttitudeControl::task_main()
 		vehicle_battery_poll();
 
 
-		if (_manual_control_sp.aux1 <= 0.0f) {		/* vehicle is in mc mode */
+		if (_manual_control_sp.aux1 < 0.0f) {		/* vehicle is in mc mode */
 			_vtol_vehicle_status.vtol_in_rw_mode = true;
 
 			if (!flag_idle_mc) {	/* we want to adjust idle speed for mc mode */
@@ -877,7 +879,7 @@ VtolAttitudeControl::start()
 
 int vtol_att_control_main(int argc, char *argv[])
 {
-	if (argc < 1) {
+	if (argc < 2) {
 		errx(1, "usage: vtol_att_control {start|stop|status}");
 	}
 
diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c
index 33752b2c4..6da28b130 100644
--- a/src/modules/vtol_att_control/vtol_att_control_params.c
+++ b/src/modules/vtol_att_control/vtol_att_control_params.c
@@ -43,7 +43,7 @@
 /**
  * VTOL number of engines
  *
- * @min 1.0
+ * @min 1
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_INT32(VT_MOT_COUNT,0);
@@ -92,8 +92,8 @@ PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM,10.0f);
  * If set to one this parameter will cause permanent attitude stabilization in fw mode.
  * This parameter has been introduced for pure convenience sake.
  *
- * @min 0.0
- * @max 1.0
+ * @min 0
+ * @max 1
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_INT32(VT_FW_PERM_STAB,0);
diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c
index 6b855cf58..98443e716 100644
--- a/src/systemcmds/param/param.c
+++ b/src/systemcmds/param/param.c
@@ -284,9 +284,9 @@ do_show_print(void *arg, param_t param)
 		}
 	}
 
-	printf("%c %c %s: ", (param_used(param) ? 'x' : ' '),
+	printf("%c %c %s [%d,%d] : ", (param_used(param) ? 'x' : ' '),
 	       param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
-	       param_name(param));
+	       param_name(param), param_get_used_index(param), param_get_index(param));
 
 	/*
 	 * This case can be expanded to handle printing common structure types.
diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c
deleted file mode 100644
index 361ca6754..000000000
--- a/src/systemcmds/preflight_check/preflight_check.c
+++ /dev/null
@@ -1,286 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file preflight_check.c
- *
- * Preflight check for main system components
- *
- * @author Lorenz Meier <lorenz@px4.io>
- */
-
-#include <nuttx/config.h>
-#include <unistd.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <string.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <math.h>
-
-#include <systemlib/err.h>
-#include <systemlib/param/param.h>
-
-#include <drivers/drv_led.h>
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_tone_alarm.h>
-#include <drivers/drv_mag.h>
-#include <drivers/drv_gyro.h>
-#include <drivers/drv_accel.h>
-#include <drivers/drv_baro.h>
-
-#include <mavlink/mavlink_log.h>
-#include <systemlib/rc_check.h>
-
-__EXPORT int preflight_check_main(int argc, char *argv[]);
-static int led_toggle(int leds, int led);
-static int led_on(int leds, int led);
-static int led_off(int leds, int led);
-
-int preflight_check_main(int argc, char *argv[])
-{
-	bool fail_on_error = false;
-
-	if (argc > 1 && !strcmp(argv[1], "--help")) {
-		warnx("usage: preflight_check [--fail-on-error]\n\tif fail on error is enabled, will return 1 on error");
-		exit(1);
-	}
-
-	if (argc > 1 && !strcmp(argv[1], "--fail-on-error")) {
-		fail_on_error = true;
-	}
-
-	bool system_ok = true;
-
-	int fd;
-	/* open text message output path */
-	int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-	int ret;
-	int32_t devid, calibration_devid;
-
-	/* give the system some time to sample the sensors in the background */
-	usleep(150000);
-
-	/* ---- MAG ---- */
-	fd = open(MAG0_DEVICE_PATH, 0);
-	if (fd < 0) {
-		warn("failed to open magnetometer - start with 'hmc5883 start' or 'lsm303d start'");
-		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: NO MAG");
-		system_ok = false;
-		goto system_eval;
-	}
-
-	devid = ioctl(fd, DEVIOCGDEVICEID,0);
-	param_get(param_find("CAL_MAG0_ID"), &(calibration_devid));
-	if (devid != calibration_devid){
-		warnx("magnetometer calibration is for a different device - calibrate magnetometer first");
-		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CAL ID");
-		system_ok = false;
-		goto system_eval;
-	}
-
-	ret = ioctl(fd, MAGIOCSELFTEST, 0);
-
-	if (ret != OK) {
-		warnx("magnetometer calibration missing or bad - calibrate magnetometer first");
-		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CHECK/CAL");
-		system_ok = false;
-		goto system_eval;
-	}
-
-	/* ---- ACCEL ---- */
-
-	close(fd);
-	fd = open(ACCEL0_DEVICE_PATH, O_RDONLY);
-
-	devid = ioctl(fd, DEVIOCGDEVICEID,0);
-	param_get(param_find("CAL_ACC0_ID"), &(calibration_devid));
-	if (devid != calibration_devid){
-		warnx("accelerometer calibration is for a different device - calibrate accelerometer first");
-		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACC CAL ID");
-		system_ok = false;
-		goto system_eval;
-	}
-
-	ret = ioctl(fd, ACCELIOCSELFTEST, 0);
-
-	if (ret != OK) {
-		warnx("accel self test failed");
-		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK/CAL");
-		system_ok = false;
-		goto system_eval;
-	}
-
-	/* check measurement result range */
-	struct accel_report acc;
-	ret = read(fd, &acc, sizeof(acc));
-
-	if (ret == sizeof(acc)) {
-		/* evaluate values */
-		if (sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z) > 30.0f /* m/s^2 */) {
-			warnx("accel with spurious values");
-			mavlink_log_critical(mavlink_fd, "SENSOR FAIL: |ACCEL| > 30 m/s^2");
-			/* this is frickin' fatal */
-			fail_on_error = true;
-			system_ok = false;
-			goto system_eval;
-		}
-	} else {
-		warnx("accel read failed");
-		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL READ");
-		/* this is frickin' fatal */
-		fail_on_error = true;
-		system_ok = false;
-		goto system_eval;
-	}
-
-	/* ---- GYRO ---- */
-
-	close(fd);
-	fd = open(GYRO0_DEVICE_PATH, 0);
-
-	devid = ioctl(fd, DEVIOCGDEVICEID,0);
-	param_get(param_find("CAL_GYRO0_ID"), &(calibration_devid));
-	if (devid != calibration_devid){
-		warnx("gyro calibration is for a different device - calibrate gyro first");
-		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CAL ID");
-		system_ok = false;
-		goto system_eval;
-	}
-
-	ret = ioctl(fd, GYROIOCSELFTEST, 0);
-
-	if (ret != OK) {
-		warnx("gyro self test failed");
-		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK/CAL");
-		system_ok = false;
-		goto system_eval;
-	}
-
-	/* ---- BARO ---- */
-
-	close(fd);
-	fd = open(BARO0_DEVICE_PATH, 0);
-	close(fd);
-
-	/* ---- RC CALIBRATION ---- */
-
-	bool rc_ok = (OK == rc_calibration_check(mavlink_fd));
-
-	/* warn */
-	if (!rc_ok)
-		warnx("rc calibration test failed");
-
-	/* require RC ok to keep system_ok */
-	system_ok &= rc_ok;
-
-
-
-
-system_eval:
-
-	if (system_ok) {
-		/* all good, exit silently */
-		exit(0);
-	} else {
-		fflush(stdout);
-
-		warnx("PREFLIGHT CHECK ERROR! TRIGGERING ALARM");
-		fflush(stderr);
-
-		int buzzer = open(TONEALARM0_DEVICE_PATH, O_WRONLY);
-		int leds = open(LED0_DEVICE_PATH, 0);
-
-		if (leds < 0) {
-			close(buzzer);
-			errx(1, "failed to open leds, aborting");
-		}
-
-		/* flip blue led into alternating amber */
-		led_off(leds, LED_BLUE);
-		led_off(leds, LED_AMBER);
-		led_toggle(leds, LED_BLUE);
-
-		/* display and sound error */
-		for (int i = 0; i < 14; i++)
-		{
-			led_toggle(leds, LED_BLUE);
-			led_toggle(leds, LED_AMBER);
-
-			if (i % 10 == 0) {
-				ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE);
-			} else if (i % 5 == 0) {
-				ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE);
-			}
-			usleep(100000);
-		}
-
-		/* stop alarm */
-		ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE);
-
-		/* switch off leds */
-		led_on(leds, LED_BLUE);
-		led_on(leds, LED_AMBER);
-		close(leds);
-
-		if (fail_on_error) {
-			/* exit with error message */
-			exit(1);
-		} else {
-			/* do not emit an error code to make sure system still boots */
-			exit(0);
-		}
-	}
-}
-
-static int led_toggle(int leds, int led)
-{
-	static int last_blue = LED_ON;
-	static int last_amber = LED_ON;
-
-	if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON;
-
-	if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON;
-
-	return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led);
-}
-
-static int led_off(int leds, int led)
-{
-	return ioctl(leds, LED_OFF, led);
-}
-
-static int led_on(int leds, int led)
-{
-	return ioctl(leds, LED_ON, led);
-}
diff --git a/src/systemcmds/tests/test_gpio.c b/src/systemcmds/tests/test_gpio.c
index 62a873270..82c122550 100644
--- a/src/systemcmds/tests/test_gpio.c
+++ b/src/systemcmds/tests/test_gpio.c
@@ -89,10 +89,11 @@
 
 int test_gpio(int argc, char *argv[])
 {
-	int		fd;
 	int		ret = 0;
 
-	fd = open(PX4IO_DEVICE_PATH, 0);
+#ifdef PX4IO_DEVICE_PATH
+
+	int fd = open(PX4IO_DEVICE_PATH, 0);
 
 	if (fd < 0) {
 		printf("GPIO: open fail\n");
@@ -109,7 +110,11 @@ int test_gpio(int argc, char *argv[])
 	/* Go back to default */
 	ioctl(fd, GPIO_RESET, ~0);
 
+	close(fd);
 	printf("\t GPIO test successful.\n");
 
+#endif
+
+
 	return ret;
 }
diff --git a/src/systemcmds/ver/module.mk b/src/systemcmds/ver/module.mk
index 2eeb80b61..4597b5f11 100644
--- a/src/systemcmds/ver/module.mk
+++ b/src/systemcmds/ver/module.mk
@@ -42,3 +42,5 @@ SRCS		 		= ver.c
 MODULE_STACKSIZE 	= 1024
 
 MAXOPTIMIZATION	 	= -Os
+
+EXTRADEFINES = -DGIT_VERSION='"$(GIT_DESC)"'
diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c
index 087eb52e3..b794e8b2f 100644
--- a/src/systemcmds/ver/ver.c
+++ b/src/systemcmds/ver/ver.c
@@ -101,7 +101,7 @@ int ver_main(int argc, char *argv[])
 			}
 
 			if (show_all || !strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) {
-				printf("FW git-hash: %s\n", FW_GIT);
+				printf("FW git-hash: %s\n", GIT_VERSION);
 				ret = 0;
 
 			}