diff --git a/.gitmodules b/.gitmodules
index cb9a6ccf05..2820e68539 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -4,8 +4,8 @@
 [submodule "NuttX"]
 	path = NuttX
 	url = git://github.com/PX4/NuttX.git
-[submodule "src/lib/uavcan"]
-	path = src/lib/uavcan
+[submodule "src/modules/uavcan/libuavcan"]
+	path = src/modules/uavcan/libuavcan
 	url = git://github.com/UAVCAN/libuavcan.git
 [submodule "Tools/genmsg"]
 	path = Tools/genmsg
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 948959ca32..6aec7f2e0c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -224,7 +224,7 @@ px4_add_git_submodule(TARGET git_genmsg PATH "Tools/genmsg")
 px4_add_git_submodule(TARGET git_gencpp PATH "Tools/gencpp")
 px4_add_git_submodule(TARGET git_mavlink PATH "mavlink/include/mavlink/v1.0")
 px4_add_git_submodule(TARGET git_gtest PATH "unittets/gtest")
-px4_add_git_submodule(TARGET git_uavcan PATH "src/lib/uavcan")
+px4_add_git_submodule(TARGET git_uavcan PATH "src/modules/uavcan/libuavcan")
 px4_add_git_submodule(TARGET git_eigen PATH "src/lib/eigen")
 px4_add_git_submodule(TARGET git_nuttx PATH "NuttX")
 px4_add_git_submodule(TARGET git_dspal PATH "src/lib/dspal")
diff --git a/Makefile b/Makefile
index 2473eeff43..e2f1814f0c 100644
--- a/Makefile
+++ b/Makefile
@@ -74,7 +74,13 @@ ifdef NINJA_BUILD
     PX4_MAKE = ninja
     PX4_MAKE_ARGS = 
 else
-    PX4_CMAKE_GENERATOR ?= "Unix Makefiles"
+
+ifdef SYSTEMROOT
+	# Windows
+	PX4_CMAKE_GENERATOR ?= "MSYS Makefiles"
+else
+	PX4_CMAKE_GENERATOR ?= "Unix Makefiles"
+endif
     PX4_MAKE = make
     PX4_MAKE_ARGS = -j$(j) --no-print-directory
 endif
@@ -83,7 +89,7 @@ endif
 # --------------------------------------------------------------------
 # describe how to build a cmake config
 define cmake-build
-+@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1); fi
++@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then git submodule update --init --recursive --force && mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1); fi
 +$(PX4_MAKE) -C $(PWD)/build_$@ $(PX4_MAKE_ARGS) $(ARGS)
 endef
 
@@ -166,14 +172,8 @@ check_format:
 
 clean:
 	@rm -rf build_*/
-
-distclean: clean
-	@cd NuttX
-	@git clean -d -f -x
-	@cd ..
-	@cd src/lib/uavcan
-	@git clean -d -f -x
-	@cd ../../..
+	@(cd NuttX && git clean -d -f -x)
+	@(cd src/modules/uavcan/libuavcan && git clean -d -f -x)
 
 # targets handled by cmake
 cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak libuavcan
diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh
index db140fca45..bc59128317 100755
--- a/Tools/check_code_style.sh
+++ b/Tools/check_code_style.sh
@@ -25,7 +25,7 @@ for fn in $(find src/examples \
                    -path './NuttX' -prune -o \
                    -path './src/lib/eigen' -prune -o \
                    -path './src/lib/mathlib/CMSIS' -prune -o \
-                   -path './src/lib/uavcan' -prune -o \
+                   -path './src/modules/uavcan/libuavcan' -prune -o \
                    -path './src/modules/attitude_estimator_ekf/codegen' -prune -o \
                    -path './src/modules/ekf_att_pos_estimator' -prune -o \
                    -path './src/modules/sdlog2' -prune -o \
diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh
deleted file mode 100755
index 5fa0831438..0000000000
--- a/Tools/check_submodules.sh
+++ /dev/null
@@ -1,144 +0,0 @@
-#!/bin/sh
-
-[ -n "$GIT_SUBMODULES_ARE_EVIL" ] && {
-    # GIT_SUBMODULES_ARE_EVIL is set, meaning user doesn't want submodules
-    echo "Skipping submodules. NUTTX_SRC is set to $NUTTX_SRC"
-    exit 0
-}
-
-if [ -d NuttX/nuttx ];
-	then
-	STATUSRETVAL=$(git submodule summary | grep -A20 -i "NuttX" | grep "<")
-	if [ -z "$STATUSRETVAL" ]; then
-		echo "Checked NuttX submodule, correct version found"
-	else
-		echo ""
-		echo ""
-		echo "New commits required:"
-		echo "$(git submodule summary)"
-		echo ""
-		echo ""
-		echo "   NuttX sub repo not at correct version. Try 'git submodule update'"
-		echo "   or follow instructions on http://pixhawk.org/dev/git/submodules"
-		echo ""
-		echo "   DO NOT FORGET TO RUN 'make distclean && make archives' AFTER EACH NUTTX UPDATE!"
-		exit 1
-	fi
-else
-	git submodule update --init --recursive
-fi
-
-
-if [ -d mavlink/include/mavlink/v1.0 ];
-	then
-	STATUSRETVAL=$(git submodule summary | grep -A20 -i "mavlink/include/mavlink/v1.0" | grep "<")
-	if [ -z "$STATUSRETVAL" ]; then
-		echo "Checked mavlink submodule, correct version found"
-	else
-		echo ""
-		echo ""
-		echo "New commits required:"
-		echo "$(git submodule summary)"
-		echo ""
-		echo ""
-		echo "mavlink sub repo not at correct version. Try 'git submodule update'"
-		echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
-		exit 1
-	fi
-else
-	git submodule update --init --recursive
-fi
-
-
-if [ -d uavcan ]
-then
-	STATUSRETVAL=$(git submodule summary | grep -A20 -i uavcan | grep "<")
-	if [ -z "$STATUSRETVAL" ]
-	then
-		echo "Checked uavcan submodule, correct version found"
-	else
-		echo ""
-		echo ""
-		echo "New commits required:"
-		echo "$(git submodule summary)"
-		echo ""
-		echo ""
-		echo "uavcan sub repo not at correct version. Try 'git submodule update'"
-		echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
-		exit 1
-	fi
-else
-	git submodule update --init --recursive
-fi
-
-if [ -d src/lib/eigen ]
-then
-	echo "ARG = $1"
-	if [ $1 = "qurt" ]
-	then
-		# QuRT needs to use Eigen 3.2 because the toolchain doews not support C++11
-		STATUSRETVAL=$(true)
-	else
-		STATUSRETVAL=$(git submodule summary | grep -A20 -i eigen | grep "<")
-		if [ -z "$STATUSRETVAL" ]
-		then
-			echo "Checked Eigen submodule, correct version found"
-		else
-			echo ""
-			echo ""
-			echo "New commits required:"
-			echo "$(git submodule summary)"
-			echo ""
-			echo ""
-			echo "eigen sub repo not at correct version. Try 'git submodule update'"
-			echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
-			exit 1
-		fi
-	fi
-else
-	git submodule update --init --recursive
-fi
-
-if [ -d Tools/gencpp ]
-then
-	STATUSRETVAL=$(git submodule summary | grep -A20 -i gencpp | grep "<")
-	if [ -z "$STATUSRETVAL" ]
-	then
-		echo "Checked gencpp submodule, correct version found"
-	else
-		echo ""
-		echo ""
-		echo "New commits required:"
-		echo "$(git submodule summary)"
-		echo ""
-		echo ""
-		echo "gencpp sub repo not at correct version. Try 'git submodule update'"
-		echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
-		exit 1
-	fi
-else
-	git submodule update --init --recursive
-fi
-
-if [ -d Tools/genmsg ]
-then
-	STATUSRETVAL=$(git submodule summary | grep -A20 -i genmsg | grep "<")
-	if [ -z "$STATUSRETVAL" ]
-	then
-		echo "Checked genmsg submodule, correct version found"
-	else
-		echo ""
-		echo ""
-		echo "New commits required:"
-		echo "$(git submodule summary)"
-		echo ""
-		echo ""
-		echo "genmsg sub repo not at correct version. Try 'git submodule update'"
-		echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
-		exit 1
-	fi
-else
-	git submodule update --init --recursive
-fi
-
-exit 0
diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py
index 124d9908eb..88f7edcdb6 100755
--- a/Tools/generate_listener.py
+++ b/Tools/generate_listener.py
@@ -1,6 +1,7 @@
 #!/usr/bin/python
 
 import glob
+import os
 import sys
 
 # This script is run from Build/<target>_default.build/$(PX4_BASE)/Firmware/src/systemcmds/topic_listener
@@ -48,7 +49,9 @@ for index,m in enumerate(raw_messages):
 				temp_list.append(("int8",line.split(' ')[1].split('\t')[0].split('\n')[0]))
 
 		f.close()
-		messages.append(m.split('/')[-1].split('.')[0])
+		(m_head, m_tail) = os.path.split(m)
+		messages.append(m_tail.split('.')[0])
+		#messages.append(m.split('/')[-1].split('.')[0])
 		message_elements.append(temp_list)
 
 num_messages = len(messages);
diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py
index 3c9f5379c6..7ed0b3af19 100755
--- a/Tools/px_generate_uorb_topic_headers.py
+++ b/Tools/px_generate_uorb_topic_headers.py
@@ -43,6 +43,11 @@ import shutil
 import filecmp
 import argparse
 
+import sys
+px4_tools_dir = os.path.dirname(os.path.abspath(__file__))
+sys.path.append(px4_tools_dir + "/genmsg/src")
+sys.path.append(px4_tools_dir + "/gencpp/src")
+
 try:
         import genmsg.template_tools
 except ImportError as e:
diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake
index 257254ba43..086f77070f 100644
--- a/cmake/common/px4_base.cmake
+++ b/cmake/common/px4_base.cmake
@@ -136,15 +136,17 @@ function(px4_add_git_submodule)
 		REQUIRED TARGET PATH
 		ARGN ${ARGN})
 	string(REPLACE "/" "_" NAME ${PATH})
-	add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/git_${NAME}.stamp
+	add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/git_init_${NAME}.stamp
 		WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
 		COMMAND git submodule init ${PATH}
-		COMMAND git submodule update -f ${PATH}
-		COMMAND touch ${CMAKE_BINARY_DIR}/git_${NAME}.stamp
+		COMMAND touch ${CMAKE_BINARY_DIR}/git_init_${NAME}.stamp
+		DEPENDS ${CMAKE_SOURCE_DIR}/.gitmodules
 		)
 	add_custom_target(${TARGET}
-		DEPENDS git_${NAME}.stamp
-	)
+		WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+		COMMAND git submodule update --recursive ${PATH}
+		DEPENDS ${CMAKE_BINARY_DIR}/git_init_${NAME}.stamp
+		)
 endfunction()
 
 #=============================================================================
@@ -330,7 +332,6 @@ function(px4_generate_messages)
 	if(NOT VERBOSE)
 		set(QUIET "-q")
 	endif()
-	set(PYTHONPATH "${CMAKE_SOURCE_DIR}/Tools/genmsg/src:${CMAKE_SOURCE_DIR}/Tools/gencpp/src:$ENV{PYTHONPATH}")
 	set(msg_out_path ${CMAKE_BINARY_DIR}/src/modules/uORB/topics)
 	set(msg_list)
 	foreach(msg_file ${MSG_FILES})
@@ -342,7 +343,7 @@ function(px4_generate_messages)
 		list(APPEND msg_files_out ${msg_out_path}/${msg}.h)
 	endforeach()
 	add_custom_command(OUTPUT ${msg_files_out}
-		COMMAND PYTHONPATH=${PYTHONPATH} ${PYTHON_EXECUTABLE} 
+		COMMAND ${PYTHON_EXECUTABLE} 
 			Tools/px_generate_uorb_topic_headers.py
 			${QUIET}
 			-d msg
@@ -363,7 +364,7 @@ function(px4_generate_messages)
 		list(APPEND msg_multi_files_out ${msg_multi_out_path}/px4_${msg}.h)
 	endforeach()
 	add_custom_command(OUTPUT ${msg_multi_files_out}
-		COMMAND PYTHONPATH=${PYTHONPATH} ${PYTHON_EXECUTABLE} 
+		COMMAND ${PYTHON_EXECUTABLE} 
 			Tools/px_generate_uorb_topic_headers.py
 			${QUIET}
 			-d msg
@@ -425,7 +426,7 @@ function(px4_add_upload)
 	endif()
 	px4_join(OUT serial_ports LIST "${serial_ports}" GLUE ",")
 	add_custom_target(${OUT}
-		COMMAND PYTHONPATH=${PYTHONPATH} ${PYTHON_EXECUTABLE}
+		COMMAND ${PYTHON_EXECUTABLE}
 			${CMAKE_SOURCE_DIR}/Tools/px_uploader.py --port ${serial_ports} ${BUNDLE}
 		DEPENDS ${BUNDLE}
 		WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
@@ -536,6 +537,7 @@ function(px4_add_common_flags)
 		-ffunction-sections
 		-fdata-sections
 		)
+
 	if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang")
 		list(APPEND optimization_flags
 			-fno-strength-reduce
@@ -566,6 +568,7 @@ function(px4_add_common_flags)
 	set(cxx_warnings
 		-Wno-missing-field-initializers
 		)
+
 	set(cxx_compile_flags
 		-g
 		-fno-exceptions
@@ -578,7 +581,7 @@ function(px4_add_common_flags)
 
 	set(visibility_flags
 		-fvisibility=hidden
-		"-include ${CMAKE_SOURCE_DIR}/src/include/visibility.h"
+		-include visibility.h
 		)
 
 	set(added_c_flags
diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake
index 89910bbd95..a242f4f736 100644
--- a/cmake/configs/nuttx_px4fmu-v2_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake
@@ -168,8 +168,8 @@ set(config_io_board
 
 set(config_extra_libs
 	${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a
-	libuavcan.a
-	libuavcan_stm32_driver.a
+	uavcan
+	uavcan_stm32_driver
 	)
 
 set(config_io_extra_libs
diff --git a/cmake/nuttx/bin_to_obj.py b/cmake/nuttx/bin_to_obj.py
index 64ba864295..80ce80d741 100755
--- a/cmake/nuttx/bin_to_obj.py
+++ b/cmake/nuttx/bin_to_obj.py
@@ -12,6 +12,7 @@ from subprocess import PIPE
 parser = argparse.ArgumentParser(description='Convert bin to obj.')
 parser.add_argument('--c_flags', required=True)
 parser.add_argument('--c_compiler', required=True)
+parser.add_argument('--include_path', required=True)
 parser.add_argument('--nm', required=True)
 parser.add_argument('--ld', required=True)
 parser.add_argument('--objcopy', required=True)
@@ -23,6 +24,7 @@ args = parser.parse_args()
 in_bin = args.bin
 c_flags = args.c_flags
 c_compiler = args.c_compiler
+include_path = args.include_path
 nm = args.nm
 ld = args.ld
 obj = args.obj
@@ -46,7 +48,7 @@ def run_cmd(cmd, d):
     return stdout
 
 # do compile
-run_cmd("{c_compiler:s} {c_flags:s} -c {obj:s}.c -o {obj:s}.c.o",
+run_cmd("{c_compiler:s} -I{include_path:s} {c_flags:s} -c {obj:s}.c -o {obj:s}.c.o",
         locals())
 
 # link
@@ -55,9 +57,10 @@ run_cmd("{ld:s} -r -o {obj:s}.bin.o {obj:s}.c.o -b binary {in_bin:s}",
 
 # get size of image
 stdout = run_cmd("{nm:s} -p --radix=x {obj:s}.bin.o", locals())
-re_string = r"^([0-9A-F-a-f]+) .*{sym:s}_size\n".format(**locals())
+re_string = r"^([0-9A-Fa-f]+) .*{sym:s}_size".format(**locals())
 re_size = re.compile(re_string, re.MULTILINE)
 size_match = re.search(re_size, stdout.decode())
+
 try:
     size = size_match.group(1)
 except AttributeError as e:
@@ -76,7 +79,7 @@ with open('{obj:s}.c'.format(**locals()), 'w') as f:
         **locals()))
 
 # do compile
-run_cmd("{c_compiler:s} {c_flags:s} -c {obj:s}.c -o {obj:s}.c.o",
+run_cmd("{c_compiler:s} -I{include_path:s} {c_flags:s} -c {obj:s}.c -o {obj:s}.c.o",
         locals())
 
 # link
diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake
index 13d120af26..af7391f45c 100644
--- a/cmake/nuttx/px4_impl_nuttx.cmake
+++ b/cmake/nuttx/px4_impl_nuttx.cmake
@@ -213,7 +213,9 @@ function(px4_nuttx_add_export)
 	# copy
 	add_custom_command(OUTPUT nuttx_copy_${CONFIG}.stamp
 		COMMAND ${MKDIR} -p ${CMAKE_BINARY_DIR}/${CONFIG}
-		COMMAND rsync -a --exclude=.git ${CMAKE_SOURCE_DIR}/NuttX/ ${nuttx_src}/
+		COMMAND ${MKDIR} -p ${nuttx_src}
+		COMMAND ${CP} -a ${CMAKE_SOURCE_DIR}/NuttX/. ${nuttx_src}/
+		COMMAND ${RM} -rf ${nuttx_src}/.git
 		COMMAND ${TOUCH} nuttx_copy_${CONFIG}.stamp
 		DEPENDS ${DEPENDS})
 	add_custom_target(__nuttx_copy_${CONFIG}
@@ -227,7 +229,7 @@ function(px4_nuttx_add_export)
 		COMMAND ${CP} -r ${CMAKE_SOURCE_DIR}/nuttx-configs/${CONFIG} ${nuttx_src}/nuttx/configs
 		COMMAND cd ${nuttx_src}/nuttx/tools && ./configure.sh ${CONFIG}/nsh
 		COMMAND ${ECHO} Exporting NuttX for ${CONFIG}
-		COMMAND ${MAKE} --no-print-directory --quiet -C ${nuttx_src}/nuttx -j${THREADS} -r CONFIG_ARCH_BOARD=${CONFIG} export
+		COMMAND ${MAKE} --no-print-directory --quiet -C ${nuttx_src}/nuttx -j${THREADS} -r CONFIG_ARCH_BOARD=${CONFIG} export > /dev/null
 		COMMAND ${CP} -r ${nuttx_src}/nuttx/nuttx-export.zip ${CMAKE_BINARY_DIR}/${CONFIG}.export
 		DEPENDS ${config_files} ${DEPENDS} __nuttx_copy_${CONFIG})
 
@@ -347,6 +349,7 @@ function(px4_nuttx_add_romfs)
 		#COMMAND cmake -E remove_directory ${romfs_temp_dir}
 		COMMAND ${PYTHON_EXECUTABLE} ${bin_to_obj}
 			--ld ${LD} --c_flags ${CMAKE_C_FLAGS}
+			--include_path "${CMAKE_SOURCE_DIR}/src/include"
 			--c_compiler ${CMAKE_C_COMPILER}
 			--nm ${NM} --objcopy ${OBJCOPY}
 			--obj romfs.o
diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig
index db7c41dae0..3340fdd0c9 100644
--- a/nuttx-configs/px4fmu-v2/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v2/nsh/defconfig
@@ -801,11 +801,11 @@ CONFIG_SCHED_WORKQUEUE=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_WORKPRIORITY=192
 CONFIG_SCHED_WORKPERIOD=5000
-CONFIG_SCHED_WORKSTACKSIZE=1800
+CONFIG_SCHED_WORKSTACKSIZE=1600
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
 CONFIG_SCHED_LPWORKPERIOD=50000
-CONFIG_SCHED_LPWORKSTACKSIZE=1800
+CONFIG_SCHED_LPWORKSTACKSIZE=1600
 # CONFIG_LIB_KBDCODEC is not set
 # CONFIG_LIB_SLCDCODEC is not set
 
@@ -1064,3 +1064,5 @@ CONFIG_SYSTEM_SYSINFO=y
 #
 # USB Monitor
 #
+
+CONFIG_NSOCKET_DESCRIPTORS=0
diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h
index a402f327b7..0c1ab48267 100644
--- a/src/drivers/drv_sensor.h
+++ b/src/drivers/drv_sensor.h
@@ -121,5 +121,10 @@
  */
 #define SENSORIOCGROTATION	_SENSORIOC(6)
 
+/**
+ * Test the sensor calibration
+ */
+#define SENSORIOCCALTEST	_SENSORIOC(7)
+
 #endif /* _DRV_SENSOR_H */
 
diff --git a/src/lib/uavcan b/src/lib/uavcan
deleted file mode 160000
index 8effe93d6e..0000000000
--- a/src/lib/uavcan
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 8effe93d6ee055d9169c1ba460064c90a50eddf1
diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp
index bbf5f8ec6b..2a8a0695cf 100644
--- a/src/modules/commander/PreflightCheck.cpp
+++ b/src/modules/commander/PreflightCheck.cpp
@@ -71,7 +71,50 @@
 
 namespace Commander
 {
-static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
+
+static int check_calibration(int fd, const char* param_template, int &devid);
+
+int check_calibration(int fd, const char* param_template, int &devid)
+{
+	bool calibration_found;
+
+	/* new style: ask device for calibration state */
+	int ret = px4_ioctl(fd, SENSORIOCCALTEST, 0);
+
+	calibration_found = (ret == OK);
+	
+	devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
+
+	char s[20];
+	int instance = 0;
+
+	/* old style transition: check param values */
+	while (!calibration_found) {
+		sprintf(s, param_template, instance);
+		param_t parm = param_find(s);
+
+		/* if the calibration param is not present, abort */
+		if (parm == PARAM_INVALID) {
+			break;
+		}
+
+		/* if param get succeeds */
+		int calibration_devid;
+		if (!param_get(parm, &(calibration_devid))) {
+
+			/* if the devid matches, exit early */
+			if (devid == calibration_devid) {
+				calibration_found = true;
+				break;
+			}
+		}
+		instance++;
+	}
+
+	return !calibration_found;
+}
+
+static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
 {
 	bool success = true;
 
@@ -88,13 +131,9 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
 		return false;
 	}
 
-	int calibration_devid;
-	int ret;
-	int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
-	sprintf(s, "CAL_MAG%u_ID", instance);
-	param_get(param_find(s), &(calibration_devid));
+	int ret = check_calibration(fd, "CAL_MAG%u_ID", device_id);
 
-	if (devid != calibration_devid) {
+	if (ret) {
 		mavlink_and_console_log_critical(mavlink_fd,
 						 "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
 		success = false;
@@ -115,7 +154,7 @@ out:
 	return success;
 }
 
-static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic)
+static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id)
 {
 	bool success = true;
 
@@ -132,13 +171,9 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
 		return false;
 	}
 
-	int calibration_devid;
-	int ret;
-	int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
-	sprintf(s, "CAL_ACC%u_ID", instance);
-	param_get(param_find(s), &(calibration_devid));
+	int ret = check_calibration(fd, "CAL_ACC%u_ID", device_id);
 
-	if (devid != calibration_devid) {
+	if (ret) {
 		mavlink_and_console_log_critical(mavlink_fd,
 						 "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
 		success = false;
@@ -184,7 +219,7 @@ out:
 	return success;
 }
 
-static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
+static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
 {
 	bool success = true;
 
@@ -201,13 +236,9 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
 		return false;
 	}
 
-	int calibration_devid;
-	int ret;
-	int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
-	sprintf(s, "CAL_GYRO%u_ID", instance);
-	param_get(param_find(s), &(calibration_devid));
+	int ret = check_calibration(fd, "CAL_GYRO%u_ID", device_id);
 
-	if (devid != calibration_devid) {
+	if (ret) {
 		mavlink_and_console_log_critical(mavlink_fd,
 						 "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
 		success = false;
@@ -228,7 +259,7 @@ out:
 	return success;
 }
 
-static bool baroCheck(int mavlink_fd, unsigned instance, bool optional)
+static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
 {
 	bool success = true;
 
@@ -245,6 +276,20 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional)
 		return false;
 	}
 
+	device_id = -1000;
+
+	// TODO: There is no baro calibration yet, since no external baros exist
+	// int ret = check_calibration(fd, "CAL_BARO%u_ID");
+
+	// if (ret) {
+	// 	mavlink_and_console_log_critical(mavlink_fd,
+	// 					 "PREFLIGHT FAIL: BARO #%u UNCALIBRATED", instance);
+	// 	success = false;
+	// 	goto out;
+	// }
+
+//out:
+
 	px4_close(fd);
 	return success;
 }
@@ -311,49 +356,110 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 
 	/* ---- MAG ---- */
 	if (checkMag) {
+		bool prime_found = false;
+		int32_t prime_id = 0;
+		param_get(param_find("CAL_MAG_PRIME"), &prime_id);
+
 		/* 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);
+			int device_id = -1;
 
-			if (!magnometerCheck(mavlink_fd, i, !required) && required) {
+			if (!magnometerCheck(mavlink_fd, i, !required, device_id) && required) {
 				failed = true;
 			}
+
+			if (device_id == prime_id) {
+				prime_found = true;
+			}
+		}
+
+		/* check if the primary device is present */
+		if (!prime_found && prime_id != 0) {
+			mavlink_log_critical(mavlink_fd, "warning: primary compass not operational");
+			failed = true;
 		}
 	}
 
 	/* ---- ACCEL ---- */
 	if (checkAcc) {
+		bool prime_found = false;
+		int32_t prime_id = 0;
+		param_get(param_find("CAL_ACC_PRIME"), &prime_id);
+
 		/* 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);
+			int device_id = -1;
 
-			if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic) && required) {
+			if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id) && required) {
 				failed = true;
 			}
+
+			if (device_id == prime_id) {
+				prime_found = true;
+			}
+		}
+
+		/* check if the primary device is present */
+		if (!prime_found && prime_id != 0) {
+			mavlink_log_critical(mavlink_fd, "warning: primary accelerometer not operational");
+			failed = true;
 		}
 	}
 
 	/* ---- GYRO ---- */
 	if (checkGyro) {
+		bool prime_found = false;
+		int32_t prime_id = 0;
+		param_get(param_find("CAL_GYRO_PRIME"), &prime_id);
+
 		/* 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);
+			int device_id = -1;
 
-			if (!gyroCheck(mavlink_fd, i, !required) && required) {
+			if (!gyroCheck(mavlink_fd, i, !required, device_id) && required) {
 				failed = true;
 			}
+
+			if (device_id == prime_id) {
+				prime_found = true;
+			}
+		}
+
+		/* check if the primary device is present */
+		if (!prime_found && prime_id != 0) {
+			mavlink_log_critical(mavlink_fd, "warning: primary gyro not operational");
+			failed = true;
 		}
 	}
 
 	/* ---- BARO ---- */
 	if (checkBaro) {
+		bool prime_found = false;
+		int32_t prime_id = 0;
+		param_get(param_find("CAL_BARO_PRIME"), &prime_id);
+
 		/* 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);
+			int device_id = -1;
 
-			if (!baroCheck(mavlink_fd, i, !required) && required) {
+			if (!baroCheck(mavlink_fd, i, !required, device_id) && required) {
 				failed = true;
 			}
+
+			if (device_id == prime_id) {
+				prime_found = true;
+			}
+		}
+
+		// TODO there is no logic in place to calibrate the primary baro yet
+		// // check if the primary device is present 
+		if (!prime_found && prime_id != 0) {
+			mavlink_log_critical(mavlink_fd, "warning: primary barometer not operational");
+			failed = true;
 		}
 	}
 
@@ -367,12 +473,13 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 	/* ---- RC CALIBRATION ---- */
 	if (checkRC) {
 		if (rc_calibration_check(mavlink_fd) != OK) {
+			mavlink_log_critical(mavlink_fd, "RC calibration check failed");
 			failed = true;
 		}
 	}
 
 	/* ---- Global Navigation Satellite System receiver ---- */
-	if(checkGNSS) {
+	if (checkGNSS) {
 		if(!gnssCheck(mavlink_fd)) {
 			failed = true;
 		}
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 6a38d54cb6..d7a58e6254 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -156,6 +156,10 @@ static const int ERROR = -1;
 
 static const char *sensor_name = "accel";
 
+static int32_t device_id[max_accel_sens];
+static int device_prio_max = 0;
+static int32_t device_id_primary = 0;
+
 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]);
@@ -172,7 +176,6 @@ typedef struct  {
 int do_accel_calibration(int mavlink_fd)
 {
 	int fd;
-	int32_t device_id[max_accel_sens];
 
 	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
 
@@ -259,6 +262,8 @@ int do_accel_calibration(int mavlink_fd)
 		
 		bool failed = false;
 
+		failed = failed || (OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary)));
+
 		/* set parameters */
 		(void)sprintf(str, "CAL_ACC%u_XOFF", i);
 		failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset)));
@@ -370,6 +375,15 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel
 		struct accel_report arp = {};
 		(void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
 		timestamps[i] = arp.timestamp;
+
+		// Get priority
+		int32_t prio;
+		orb_priority(worker_data.subs[i], &prio);
+
+		if (prio > device_prio_max) {
+			device_prio_max = prio;
+			device_id_primary = device_id[i];
+		}
 	}
 
 	if (result == calibrate_return_ok) {
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index 817cbcdb0e..54866d8cb3 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -168,6 +168,9 @@ int do_gyro_calibration(int mavlink_fd)
 		1.0f,	// z scale
 	};
 	
+	int device_prio_max = 0;
+	int32_t device_id_primary = 0;
+
 	for (unsigned s = 0; s < max_gyros; s++) {
 		char str[30];
 		
@@ -199,6 +202,15 @@ int do_gyro_calibration(int mavlink_fd)
 	
 	for (unsigned s = 0; s < max_gyros; s++) {
 		worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
+
+		// Get priority
+		int32_t prio;
+		orb_priority(worker_data.gyro_sensor_sub[s], &prio);
+
+		if (prio > device_prio_max) {
+			device_prio_max = prio;
+			device_id_primary = worker_data.device_id[s];
+		}
 	}
 
 	int cancel_sub  = calibrate_cancel_subscribe();
@@ -258,9 +270,12 @@ int do_gyro_calibration(int mavlink_fd)
 	}
 
 	if (res == OK) {
+
 		/* set offset parameters to new values */
 		bool failed = false;
 
+		failed = failed || (OK != param_set_no_notification(param_find("CAL_GYRO_PRIME"), &(device_id_primary)));
+
 		for (unsigned s = 0; s < max_gyros; s++) {
 			if (worker_data.device_id[s] != 0) {
 				char str[30];
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 4511882c7b..eba8feec99 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -73,6 +73,10 @@ static constexpr unsigned int calibration_sides = 6;			///< The total number of
 static constexpr unsigned int calibration_total_points = 240;		///< The total points per magnetometer
 static constexpr unsigned int calibraton_duration_seconds = 42; 	///< The total duration the routine is allowed to take
 
+int32_t	device_ids[max_mags];
+int device_prio_max = 0;
+int32_t device_id_primary = 0;
+
 calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
 
 /// Data passed to calibration worker routine
@@ -108,7 +112,6 @@ int do_mag_calibration(int mavlink_fd)
 	
 	// Determine which mags are available and reset each
 
-	int32_t	device_ids[max_mags];
 	char str[30];
 
 	for (size_t i=0; i<max_mags; i++) {
@@ -434,6 +437,15 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 					result = calibrate_return_error;
 					break;
 				}
+
+				// Get priority
+				int32_t prio;
+				orb_priority(worker_data.sub_mag[cur_mag], &prio);
+
+				if (prio > device_prio_max) {
+					device_prio_max = prio;
+					device_id_primary = device_ids[cur_mag];
+				}
 			}
 		}
 	}
@@ -550,6 +562,9 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 	}
 	
 	if (result == calibrate_return_ok) {
+
+		(void)param_set_no_notification(param_find("CAL_MAG_PRIME"), &(device_id_primary));
+
 		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
 			if (device_ids[cur_mag] != 0) {
 				int fd_mag = -1;
diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp
index f739446fa7..6a0b615910 100644
--- a/src/modules/controllib/blocks.cpp
+++ b/src/modules/controllib/blocks.cpp
@@ -39,6 +39,7 @@
 
 #include <math.h>
 #include <stdio.h>
+#include <float.h>
 
 #include "blocks.hpp"
 
@@ -51,6 +52,7 @@ int basicBlocksTest()
 	blockLimitSymTest();
 	blockLowPassTest();
 	blockHighPassTest();
+	blockLowPass2Test();
 	blockIntegralTest();
 	blockIntegralTrapTest();
 	blockDerivativeTest();
@@ -198,6 +200,47 @@ int blockHighPassTest()
 	return 0;
 }
 
+float BlockLowPass2::update(float input)
+{
+	if (!isfinite(getState())) {
+		setState(input);
+	}
+
+	if (fabsf(_lp.get_cutoff_freq() - getFCutParam()) > FLT_EPSILON) {
+		_lp.set_cutoff_frequency(_fs, getFCutParam());
+	}
+	_state = _lp.apply(input);
+	return _state;
+}
+
+int blockLowPass2Test()
+{
+	printf("Test BlockLowPass2\t\t: ");
+	BlockLowPass2 lowPass(NULL, "TEST_LP", 100);
+	// test initial state
+	ASSERT(equal(10.0f, lowPass.getFCutParam()));
+	ASSERT(equal(0.0f, lowPass.getState()));
+	ASSERT(equal(0.0f, lowPass.getDt()));
+	// set dt
+	lowPass.setDt(0.1f);
+	ASSERT(equal(0.1f, lowPass.getDt()));
+	// set state
+	lowPass.setState(1.0f);
+	ASSERT(equal(1.0f, lowPass.getState()));
+	// test update
+	ASSERT(equal(1.06745527f, lowPass.update(2.0f)));
+
+	// test end condition
+	for (int i = 0; i < 100; i++) {
+		lowPass.update(2.0f);
+	}
+
+	ASSERT(equal(2.0f, lowPass.getState()));
+	ASSERT(equal(2.0f, lowPass.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+};
+
 float BlockIntegral::update(float input)
 {
 	// trapezoidal integration
diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp
index 979b9541be..786bfc06d3 100644
--- a/src/modules/controllib/blocks.hpp
+++ b/src/modules/controllib/blocks.hpp
@@ -45,6 +45,7 @@
 #include <stdlib.h>
 #include <math.h>
 #include <mathlib/math/test/test.hpp>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
 
 #include "block/Block.hpp"
 #include "block/BlockParam.hpp"
@@ -163,6 +164,36 @@ protected:
 
 int __EXPORT blockHighPassTest();
 
+/**
+ * A 2nd order low pass filter block which uses the default px4 2nd order low pass filter
+ */
+class __EXPORT BlockLowPass2 : public Block
+{
+public:
+// methods
+	BlockLowPass2(SuperBlock *parent, const char *name, float sample_freq) :
+		Block(parent, name),
+		_state(0.0 / 0.0 /* initialize to invalid val, force into is_finite() check on first call */),
+		_fCut(this, ""), // only one parameter, no need to name
+		_fs(sample_freq),
+		_lp(_fs, _fCut.get())
+	{};
+	virtual ~BlockLowPass2() {};
+	float update(float input);
+// accessors
+	float getState() { return _state; }
+	float getFCutParam() { return _fCut.get(); }
+	void setState(float state) { _state = _lp.reset(state); }
+protected:
+// attributes
+	float _state;
+	control::BlockParamFloat _fCut;
+	float _fs;
+	math::LowPassFilter2p _lp;
+};
+
+int __EXPORT blockLowPass2Test();
+
 /**
  * A rectangular integrator.
  * A limiter is built into the class to bound the
@@ -263,6 +294,7 @@ public:
 	void setU(float u) {_u = u;}
 	float getU() {return _u;}
 	float getLP() {return _lowPass.getFCut();}
+	float getO() { return _lowPass.getState(); }
 protected:
 // attributes
 	float _u; /**< previous input */
diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
index 57df913069..421e109eb3 100644
--- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
@@ -177,6 +177,7 @@ private:
     hrt_abstime _last_accel;
     hrt_abstime _last_mag;
     unsigned _prediction_steps;
+    uint64_t _prediction_last;
 
     struct sensor_combined_s            _sensor_combined;
 
diff --git a/src/modules/ekf_att_pos_estimator/CMakeLists.txt b/src/modules/ekf_att_pos_estimator/CMakeLists.txt
index 6f109def1f..fc7fefd2bf 100644
--- a/src/modules/ekf_att_pos_estimator/CMakeLists.txt
+++ b/src/modules/ekf_att_pos_estimator/CMakeLists.txt
@@ -39,6 +39,7 @@ endif()
 px4_add_module(
 	MODULE modules__ekf_att_pos_estimator
 	MAIN ekf_att_pos_estimator
+	STACK 3000
 	COMPILE_FLAGS ${MODULE_CFLAGS}
 	SRCS
 		ekf_att_pos_estimator_main.cpp
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 295a597fcd..861b56c60f 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
@@ -157,6 +157,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_last_accel(0),
 	_last_mag(0),
 	_prediction_steps(0),
+	_prediction_last(0),
 
 	_sensor_combined{},
 
@@ -997,11 +998,12 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
 	_covariancePredictionDt += _ekf->dtIMU;
 
 	// only fuse every few steps
-	if (_prediction_steps < MAX_PREDICITION_STEPS) {
+	if (_prediction_steps < MAX_PREDICITION_STEPS && ((hrt_absolute_time() - _prediction_last) < 20 * 1000)) {
 		_prediction_steps++;
 		return;
 	} else {
 		_prediction_steps = 0;
+		_prediction_last = hrt_absolute_time();
 	}
 
 	// perform a covariance prediction if the total delta angle has exceeded the limit
@@ -1120,7 +1122,7 @@ int AttitudePositionEstimatorEKF::start()
 	_estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 20,
-					 4200,
+					 3900,
 					 (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
 					 nullptr);
 
@@ -1161,6 +1163,7 @@ void AttitudePositionEstimatorEKF::print_status()
 	PX4_INFO("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y,
 	       (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y,
 	       (double)_ekf->correctedDelAng.z);
+
 	PX4_INFO("states (quat)        [0-3]: %8.4f, %8.4f, %8.4f, %8.4f", (double)_ekf->states[0], (double)_ekf->states[1],
 	       (double)_ekf->states[2], (double)_ekf->states[3]);
 	PX4_INFO("states (vel m/s)     [4-6]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[4], (double)_ekf->states[5],
@@ -1180,11 +1183,12 @@ void AttitudePositionEstimatorEKF::print_status()
 		PX4_INFO("states (terrain)      [22]: %8.4f", (double)_ekf->states[22]);
 
 	} else {
-		PX4_INFO("states (wind)      [13-14]: %8.4f, %8.4f", (double)_ekf->states[13], (double)_ekf->states[14]);
-		PX4_INFO("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[15], (double)_ekf->states[16],
-		       (double)_ekf->states[17]);
-		PX4_INFO("states (body mag)  [18-20]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[18], (double)_ekf->states[19],
-		       (double)_ekf->states[20]);
+		PX4_INFO("states (accel offs)   [13]: %8.4f", (double)_ekf->states[13]);
+		PX4_INFO("states (wind)      [14-15]: %8.4f, %8.4f", (double)_ekf->states[14], (double)_ekf->states[15]);
+		PX4_INFO("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[16], (double)_ekf->states[17],
+		       (double)_ekf->states[18]);
+		PX4_INFO("states (mag bias)  [19-21]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[19], (double)_ekf->states[20],
+		       (double)_ekf->states[21]);
 	}
 
 	PX4_INFO("states: %s %s %s %s %s %s %s %s %s %s",
@@ -1263,7 +1267,7 @@ void AttitudePositionEstimatorEKF::pollData()
 		} else {
 			_ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]);
 			_ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]);
-			_ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]);
+			_ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]) + 9.80665f;
 		}
 
 		_ekf->accel.x = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0];
@@ -1331,25 +1335,33 @@ void AttitudePositionEstimatorEKF::pollData()
 	// leave this in as long as larger improvements are still being made.
 	#if 0
 
-	float deltaTIntegral = (_sensor_combined.gyro_integral_dt) / 1e6f;
-	float deltaTIntAcc = (_sensor_combined.accelerometer_integral_dt) / 1e6f;
+	float deltaTIntegral = (_sensor_combined.gyro_integral_dt[0]) / 1e6f;
+	float deltaTIntAcc = (_sensor_combined.accelerometer_integral_dt[0]) / 1e6f;
 
 	static unsigned dtoverflow5 = 0;
 	static unsigned dtoverflow10 = 0;
 	static hrt_abstime lastprint = 0;
 
 	if (hrt_elapsed_time(&lastprint) > 1000000) {
-		warnx("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u",
+		PX4_WARN("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u",
 			(double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc,
 			dtoverflow5, dtoverflow10);
 
-		warnx("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f",
+		PX4_WARN("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
 		(double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z,
-		(double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.z);
+		(double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z);
 
-		warnx("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f",
+		PX4_WARN("INT: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
+		(double)(_sensor_combined.gyro_integral_rad[0]), (double)(_sensor_combined.gyro_integral_rad[2]),
+		(double)(_sensor_combined.accelerometer_integral_m_s[0]),
+		(double)(_sensor_combined.accelerometer_integral_m_s[1]),
+		(double)(_sensor_combined.accelerometer_integral_m_s[2]));
+
+		PX4_WARN("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
 		(double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT),
-		(double)(_sensor_combined.accelerometer_m_s2[0] * deltaT), (double)(_sensor_combined.accelerometer_m_s2[2] * deltaT));
+		(double)(_sensor_combined.accelerometer_m_s2[0] * deltaT),
+		(double)(_sensor_combined.accelerometer_m_s2[1] * deltaT),
+		(double)((_sensor_combined.accelerometer_m_s2[2] + 9.8665f) * deltaT));
 
 		lastprint = hrt_absolute_time();
 	}
@@ -1655,8 +1667,6 @@ int ekf_att_pos_estimator_main(int argc, char *argv[])
 	}
 
 	if (!strcmp(argv[1], "status")) {
-		PX4_INFO("running");
-
 		estimator::g_estimator->print_status();
 
 		return 0;
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 1144387253..fa12c5822a 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
@@ -81,6 +81,7 @@
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/tecs_status.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/pid/pid.h>
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index c07956fd6e..49e1d735e0 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -52,15 +52,17 @@ mTecs::mTecs() :
 	_mTecsEnabled(this, "ENABLED"),
 	_airspeedMin(this, "FW_AIRSPD_MIN", false),
 	/* Publications */
+#if defined(__PX4_NUTTX)
 	_status(ORB_ID(tecs_status), &getPublications()),
+#endif // defined(__PX4_NUTTX)
 	/* control blocks */
 	_controlTotalEnergy(this, "THR"),
 	_controlEnergyDistribution(this, "PIT", true),
 	_controlAltitude(this, "FPA", true),
 	_controlAirSpeed(this, "ACC"),
-	_flightPathAngleLowpass(this, "FPA_LP"),
-	_altitudeLowpass(this, "ALT_LP"),
-	_airspeedLowpass(this, "A_LP"),
+	_flightPathAngleLowpass(this, "FPA_LP", 50),
+	_altitudeLowpass(this, "ALT_LP", 50),
+	_airspeedLowpass(this, "A_LP", 50),
 	_airspeedDerivative(this, "AD"),
 	_throttleSp(0.0f),
 	_pitchSp(0.0f),
@@ -107,9 +109,11 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
 		debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, altitude filtered %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)altitudeFiltered, (double)flightPathAngleSp);
 	}
 
+#if defined(__PX4_NUTTX)
 	/* Write part of the status message */
 	_status.altitudeSp = altitudeSp;
 	_status.altitude_filtered = altitudeFiltered;
+#endif // defined(__PX4_NUTTX)
 
 
 	/* use flightpath angle setpoint for total energy control */
@@ -143,9 +147,11 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
 				(double)airspeedFiltered, (double)accelerationLongitudinalSp);
 	}
 
+#if defined(__PX4_NUTTX)
 	/* Write part of the status message */
 	_status.airspeedSp = airspeedSp;
 	_status.airspeed_filtered = airspeedFiltered;
+#endif // defined(__PX4_NUTTX)
 
 
 	/* use longitudinal acceleration setpoint for total energy control */
@@ -200,23 +206,23 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	}
 
 	/* Check airspeed: if below safe value switch to underspeed mode (if not in land or takeoff mode) */
-	if (mode != tecs_status_s::TECS_MODE_LAND && mode != tecs_status_s::TECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) {
-		mode = tecs_status_s::TECS_MODE_UNDERSPEED;
+	if (mode != MTECS_MODE_LAND && mode != MTECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) {
+		mode = MTECS_MODE_UNDERSPEED;
 	}
 
-	/* Set special output limiters if we are not in TECS_MODE_NORMAL */
+	/* Set special output limiters if we are not in MTECS_MODE_NORMAL */
 	BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter();
 	BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter();
-	if (mode == tecs_status_s::TECS_MODE_TAKEOFF) {
+	if (mode == MTECS_MODE_TAKEOFF) {
 		outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle;
 		outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch;
-	} else if (mode == tecs_status_s::TECS_MODE_LAND) {
+	} else if (mode == MTECS_MODE_LAND) {
 		// only limit pitch but do not limit throttle
 		outputLimiterPitch = &_BlockOutputLimiterLandPitch;
-	} else if (mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM) {
+	} else if (mode == MTECS_MODE_LAND_THROTTLELIM) {
 		outputLimiterThrottle = &_BlockOutputLimiterLandThrottle;
 		outputLimiterPitch = &_BlockOutputLimiterLandPitch;
-	} else if (mode == tecs_status_s::TECS_MODE_UNDERSPEED) {
+	} else if (mode == MTECS_MODE_UNDERSPEED) {
 		outputLimiterThrottle = &_BlockOutputLimiterUnderspeedThrottle;
 		outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch;
 	}
@@ -226,6 +232,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	 * is running) */
 	limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
 
+// #if defined(__PX4_NUTTX)
 	// /* Write part of the status message */
 	// _status.flightPathAngleSp = flightPathAngleSp;
 	// _status.flightPathAngle = flightPathAngle;
@@ -237,6 +244,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	// _status.energyDistributionRateSp = energyDistributionRateSp;
 	// _status.energyDistributionRate = energyDistributionRate;
 	// _status.mode = mode;
+// #endif // defined(__PX4_NUTTX)
 
 	/** update control blocks **/
 	/* update total energy rate control block */
@@ -253,8 +261,10 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 				(double)accelerationLongitudinalSp, (double)airspeedDerivative);
 	}
 
+#if defined(__PX4_NUTTX)
 	/* publish status message */
 	_status.update();
+#endif // defined(__PX4_NUTTX)
 
 	/* clean up */
 	_firstIterationAfterReset = false;
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
index e114cc3ae0..09d9eec1d5 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
@@ -49,11 +49,25 @@
 #include <controllib/block/BlockParam.hpp>
 #include <drivers/drv_hrt.h>
 #include <uORB/Publication.hpp>
+
+#if defined(__PX4_NUTTX)
 #include <uORB/topics/tecs_status.h>
+#endif // defined(__PX4_NUTTX)
 
 namespace fwPosctrl
 {
 
+/* corresponds to TECS_MODE in tecs_status.msg */
+enum MTECS_MODE {
+	MTECS_MODE_NORMAL = 0,
+	MTECS_MODE_UNDERSPEED = 1,
+	MTECS_MODE_TAKEOFF = 2,
+	MTECS_MODE_LAND = 3,
+	MTECS_MODE_LAND_THROTTLELIM = 4,
+	MTECS_MODE_BAD_DESCENT = 5,
+	MTECS_MODE_CLIMBOUT = 6
+};
+
 /* Main class of the mTecs */
 class mTecs : public control::SuperBlock
 {
@@ -94,6 +108,10 @@ public:
 	float getThrottleSetpoint() { return _throttleSp; }
 	float getPitchSetpoint() { return _pitchSp; }
 	float airspeedLowpassUpdate(float input) { return _airspeedLowpass.update(input); }
+	float getFlightPathAngleLowpassState() { return _flightPathAngleLowpass.getState(); }
+	float getAltitudeLowpassState() { return _altitudeLowpass.getState(); }
+	float getAirspeedLowpassState() { return _airspeedLowpass.getState(); }
+	float getAirspeedDerivativeLowpassState() { return _airspeedDerivative.getO(); }
 
 protected:
 	/* parameters */
@@ -101,7 +119,9 @@ protected:
 	control::BlockParamFloat _airspeedMin;		/**< minimal airspeed */
 
 	/* Publications */
+#if defined(__PX4_NUTTX)
 	uORB::Publication<tecs_status_s> _status;	/**< publish internal values for logging */
+#endif // defined(__PX4_NUTTX)
 
 	/* control blocks */
 	BlockFFPILimitedCustom _controlTotalEnergy;		/**< FFPI controller for total energy control: output
@@ -114,9 +134,9 @@ protected:
 								  setpoint */
 
 	/* Other calculation Blocks */
-	control::BlockLowPass _flightPathAngleLowpass;	/**< low pass filter for the flight path angle */
-	control::BlockLowPass _altitudeLowpass;	/**< low pass filter for altitude */
-	control::BlockLowPass _airspeedLowpass;		/**< low pass filter for airspeed */
+	control::BlockLowPass2 _flightPathAngleLowpass;	/**< low pass filter for the flight path angle */
+	control::BlockLowPass2 _altitudeLowpass;	/**< low pass filter for altitude */
+	control::BlockLowPass2 _airspeedLowpass;		/**< low pass filter for airspeed */
 	control::BlockDerivative _airspeedDerivative;	/**< airspeed derivative calulation */
 
 	/* Output setpoints */
diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index 883fcd0f34..7e9f546d26 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -779,13 +779,12 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 		mission_item->altitude_is_relative = true;
 		break;
 
-	case MAV_FRAME_LOCAL_NED:
-	case MAV_FRAME_LOCAL_ENU:
-		return MAV_MISSION_UNSUPPORTED_FRAME;
-
 	case MAV_FRAME_MISSION:
+		// This is a mission item with no coordinate
+		break;
+			
 	default:
-		return MAV_MISSION_ERROR;
+		return MAV_MISSION_UNSUPPORTED_FRAME;
 	}
 
 	switch (mavlink_mission_item->command) {
diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp
index 747b21f6a9..368712e752 100644
--- a/src/modules/mavlink/mavlink_parameters.cpp
+++ b/src/modules/mavlink/mavlink_parameters.cpp
@@ -49,6 +49,7 @@
 
 ORB_DEFINE(uavcan_parameter_request, struct uavcan_parameter_request_s);
 ORB_DEFINE(uavcan_parameter_value, struct uavcan_parameter_value_s);
+#define HASH_PARAM "_HASH_CHECK"
 
 MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink),
 	_send_all_index(-1),
@@ -163,13 +164,27 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 
 				/* when no index is given, loop through string ids and compare them */
 				if (req_read.param_index < 0) {
-					/* local name buffer to enforce null-terminated string */
-					char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
-					strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
-					/* enforce null termination */
-					name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
-					/* attempt to find parameter and send it */
-					send_param(param_find_no_notification(name));
+					if (strncmp(req_read.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN) == 0) {
+						/* return hash check for cached params */
+						uint32_t hash = param_hash_check();
+
+						/* build the one-off response message */
+						mavlink_param_value_t msg;
+						msg.param_count = param_count_used();
+						msg.param_index = -1;
+						strncpy(msg.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+						msg.param_type = MAV_PARAM_TYPE_UINT32;
+						memcpy(&msg.param_value, &hash, sizeof(hash));
+						_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg);
+					} else {
+						/* local name buffer to enforce null-terminated string */
+						char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
+						strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+						/* enforce null termination */
+						name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
+						/* attempt to find parameter and send it */
+						send_param(param_find_no_notification(name));
+					}
 
 				} else {
 					/* when index is >= 0, send this parameter again */
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index a31aa25805..880d4bd98e 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -336,7 +336,7 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
 
 					} else {
 						/* item is too far from home */
-						mavlink_log_critical(_mavlink_fd, "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)dist_first_wp);
+						mavlink_log_critical(_mavlink_fd, "First waypoint too far: %d m,refusing mission", (int)dist_to_1wp, (int)dist_first_wp);
 						warning_issued = true;
 						return false;
 					}
diff --git a/src/modules/sdlog2/logbuffer.c b/src/modules/sdlog2/logbuffer.c
index 9e25dd2f23..2768994dde 100644
--- a/src/modules/sdlog2/logbuffer.c
+++ b/src/modules/sdlog2/logbuffer.c
@@ -50,8 +50,8 @@ int logbuffer_init(struct logbuffer_s *lb, int size)
 	lb->size  = size;
 	lb->write_ptr = 0;
 	lb->read_ptr = 0;
-	lb->data = malloc(lb->size);
-	return (lb->data == 0) ? PX4_ERROR : PX4_OK;
+	lb->data = NULL;
+	return PX4_OK;
 }
 
 int logbuffer_count(struct logbuffer_s *lb)
@@ -72,6 +72,16 @@ int logbuffer_is_empty(struct logbuffer_s *lb)
 
 bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size)
 {
+	// allocate buffer if not yet present
+	if (lb->data == NULL) {
+		lb->data = malloc(lb->size);
+	}
+
+	// allocation failed, bail out
+	if (lb->data == NULL) {
+		return false;
+	}
+
 	// bytes available to write
 	int available = lb->read_ptr - lb->write_ptr - 1;
 
diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt
index fdfc2dd774..e296ebe103 100644
--- a/src/modules/sensors/CMakeLists.txt
+++ b/src/modules/sensors/CMakeLists.txt
@@ -40,7 +40,6 @@ px4_add_module(
 		-O3
 	SRCS
 		sensors.cpp
-		sensor_params.c
 	DEPENDS
 		platforms__common
 	)
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 1db6ed1b77..393cd63a06 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -585,6 +585,34 @@ PARAM_DEFINE_FLOAT(CAL_ACC2_YSCALE, 1.0f);
  */
 PARAM_DEFINE_FLOAT(CAL_ACC2_ZSCALE, 1.0f);
 
+/**
+ * Primary accel ID
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_ACC_PRIME, 0);
+
+/**
+ * Primary gyro ID
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_GYRO_PRIME, 0);
+
+/**
+ * Primary mag ID
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_MAG_PRIME, 0);
+
+/**
+ * Primary baro ID
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_BARO_PRIME, 0);
+
 /**
  * Differential pressure sensor offset
  *
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 60e1a9aabb..6009e2dd5c 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1412,6 +1412,8 @@ Sensors::parameter_update_poll(bool forced)
 				(void)sprintf(str, "CAL_MAG%u_ID", i);
 				int device_id;
 				failed = failed || (OK != param_get(param_find(str), &device_id));
+				(void)sprintf(str, "CAL_MAG%u_ROT", i);
+				(void)param_find(str);
 
 				if (failed) {
 					px4_close(fd);
@@ -2020,6 +2022,11 @@ Sensors::task_main()
 	 * do subscriptions
 	 */
 
+	int gcount_prev = _gyro_count;
+	int mcount_prev = _mag_count;
+	int acount_prev = _accel_count;
+	int bcount_prev = _baro_count;
+
 	_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0],
 		&raw.gyro_priority[0], &raw.gyro_errcount[0]);
 
@@ -2032,6 +2039,15 @@ Sensors::task_main()
 	_baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0],
 		&raw.baro_priority[0], &raw.baro_errcount[0]);
 
+	if (gcount_prev != _gyro_count ||
+	    mcount_prev != _mag_count ||
+	    acount_prev != _accel_count ||
+	    bcount_prev != _baro_count) {
+
+		/* reload calibration params */
+		parameter_update_poll(true);
+	}
+
 	_rc_sub = orb_subscribe(ORB_ID(input_rc));
 	_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
 	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index 097e61e350..f606f20702 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -65,6 +65,8 @@
 #include "uORB/topics/parameter_update.h"
 #include "px4_parameters.h"
 
+#include <crc32.h>
+
 #if 0
 # define debug(fmt, args...)		do { warnx(fmt, ##args); } while(0)
 #else
@@ -1035,3 +1037,26 @@ param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_chang
 		func(arg, param);
 	}
 }
+
+uint32_t param_hash_check(void)
+{
+	uint32_t param_hash = 0;
+
+	param_lock();
+
+	/* compute the CRC32 over all string param names and 4 byte values */
+	for (param_t param = 0; handle_in_range(param); param++) {
+		if (!param_used(param)) {
+			continue;
+		}
+
+		const char *name = param_name(param);
+		const void *val = param_get_value_ptr(param);
+		param_hash = crc32part((const uint8_t *)name, strlen(name), param_hash);
+		param_hash = crc32part(val, sizeof(union param_value_u), param_hash);
+	}
+
+	param_unlock();
+
+	return param_hash;
+}
diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h
index d7cf666a71..f8334361f1 100644
--- a/src/modules/systemlib/param/param.h
+++ b/src/modules/systemlib/param/param.h
@@ -333,6 +333,13 @@ __EXPORT int 		param_save_default(void);
  */
 __EXPORT int 		param_load_default(void);
 
+/**
+ * Generate the hash of all parameters and their values
+ *
+ * @return		CRC32 hash of all param_ids and values
+ */
+__EXPORT uint32_t	param_hash_check(void);
+
 /*
  * Macros creating static parameter definitions.
  *
diff --git a/src/modules/uavcan/CMakeLists.txt b/src/modules/uavcan/CMakeLists.txt
index 38bf0bc111..b8c899840a 100644
--- a/src/modules/uavcan/CMakeLists.txt
+++ b/src/modules/uavcan/CMakeLists.txt
@@ -31,65 +31,30 @@
 #
 ############################################################################
 
-# uavcan project
-set(uavcan_c_flags ${c_flags})
-list(REMOVE_ITEM uavcan_c_flags -std=gnu++0x -D__CUSTOM_FILE_IO__)
-set(uavcan_cxx_flags ${cxx_flags})
-list(REMOVE_ITEM uavcan_cxx_flags -std=gnu++0x -std=c++11 -Wundef -Werror -D__CUSTOM_FILE_IO__)
-set(uavcan_deps git_uavcan)
-set(uavcan_platform generic)
-
-set(nuttx_export_dir ${CMAKE_BINARY_DIR}/${BOARD}/NuttX/nuttx-export)
+set(UAVCAN_USE_CPP03 ON CACHE BOOL "uavcan cpp03")
+set(UAVCAN_PLATFORM stm32 CACHE STRING "uavcan platform")
 
 string(TOUPPER "${OS}" OS_UPPER)
-
-set(uavcan_definitions
-	-DUAVCAN_NO_ASSERTIONS
-	-DUAVCAN_STM32_NUM_IFACES=2
-	-DUAVCAN_USE_EXTERNAL_SNPRINT
-	-DUAVCAN_MEM_POOL_BLOCK_SIZE=48
-	-DUAVCAN_MAX_NETWORK_SIZE_HINT=16
-	-DUAVCAN_STM32_TIMER_NUMBER=5
-	-DUAVCAN_STM32_${OS_UPPER}=1
-	-DUAVCAN_CPP_VERSION=UAVCAN_CPP03
-	)
-set(uavcan_extra_flags ${uavcan_definitions})
-
-if (${OS} STREQUAL "nuttx")
-	set(uavcan_platform stm32)
-	list(APPEND uavcan_extra_flags
-		-I${nuttx_export_dir}/include
-		-I${nuttx_export_dir}/include/cxx
-		-I${nuttx_export_dir}/arch/chip
-		-I${nuttx_export_dir}/arch/common
-		)
-	list(APPEND uavcan_deps nuttx_export_${BOARD})
-endif()
-
-list(APPEND uavcan_c_flags ${uavcan_extra_flags})
-list(APPEND uavcan_cxx_flags ${uavcan_extra_flags})
-
-px4_join(OUT uavcan_c_flags LIST "${uavcan_c_flags}" GLUE " ")
-px4_join(OUT uavcan_cxx_flags LIST "${uavcan_cxx_flags}" GLUE " ")
-
-externalproject_add(libuavcan
-	DEPENDS ${uavcan_deps}
-	DOWNLOAD_COMMAND ""
-	UPDATE_COMMAND git submodule update --init
-	LOG_INSTALL 1
-	SOURCE_DIR ${CMAKE_SOURCE_DIR}/src/lib/uavcan
-	CMAKE_ARGS -DCMAKE_TOOLCHAIN_FILE=${CMAKE_TOOLCHAIN_FILE}
+add_definitions(
+		-DUAVCAN_CPP_VERSION=UAVCAN_CPP03
+		-DUAVCAN_MAX_NETWORK_SIZE_HINT=16
+		-DUAVCAN_MEM_POOL_BLOCK_SIZE=48
+		-DUAVCAN_NO_ASSERTIONS
+		-DUAVCAN_PLATFORM=stm32
+		-DUAVCAN_STM32_${OS_UPPER}=1
+		-DUAVCAN_STM32_NUM_IFACES=2
+		-DUAVCAN_STM32_TIMER_NUMBER=5
 		-DUAVCAN_USE_CPP03=ON
-		-DUAVCAN_PLATFORM=${uavcan_platform}
-		-DUAVCAN_OS=${OS}
-		-DCMAKE_CXX_FLAGS=${uavcan_cxx_flags}
-		-DCMAKE_C_FLAGS=${uavcan_c_flags}
-		-DCMAKE_INSTALL_PREFIX=${ep_base}/Install
-)
+		-DUAVCAN_USE_EXTERNAL_SNPRINT
+		)
 
-string(TOUPPER ${OS} OS_UPPER)
+add_subdirectory(libuavcan EXCLUDE_FROM_ALL)
+add_dependencies(uavcan platforms__nuttx)
 
-add_definitions(${uavcan_definitions})
+include_directories(libuavcan/libuavcan/include)
+include_directories(libuavcan/libuavcan/include/dsdlc_generated)
+include_directories(libuavcan/libuavcan_drivers/posix/include)
+include_directories(libuavcan/libuavcan_drivers/stm32/driver/include)
 
 px4_add_module(
 	MODULE modules__uavcan
@@ -98,9 +63,8 @@ px4_add_module(
 	COMPILE_FLAGS
 		-Wframe-larger-than=1500
 		-Wno-deprecated-declarations
-		-O3
+		-Os
 	SRCS
-
 		# Main
 		uavcan_main.cpp
 		uavcan_servers.cpp
@@ -117,7 +81,7 @@ px4_add_module(
 
 	DEPENDS
 		platforms__common
-		libuavcan	
+		uavcan
 	)
 
 ## vim: set noet ft=cmake fenc=utf-8 ff=unix : 
diff --git a/src/modules/uavcan/libuavcan b/src/modules/uavcan/libuavcan
new file mode 160000
index 0000000000..0643879922
--- /dev/null
+++ b/src/modules/uavcan/libuavcan
@@ -0,0 +1 @@
+Subproject commit 0643879922239930cf7482777356f06891c26616
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
index 2b5e01e246..6a1d2f391d 100644
--- a/src/modules/uavcan/uavcan_main.hpp
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -94,7 +94,7 @@ class UavcanNode : public device::CDev
 	 */
 
 	static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At
-	static constexpr unsigned StackSize          = 1600;
+	static constexpr unsigned StackSize          = 1800;
 
 public:
 	typedef uavcan::Node<MemPoolSize> Node;
diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h
index 5b7a65eeb6..55bf3a55bc 100644
--- a/src/platforms/px4_log.h
+++ b/src/platforms/px4_log.h
@@ -130,7 +130,7 @@ __EXPORT extern int __px4_log_level_current;
 __END_DECLS
 
 // __px4_log_level_current will be initialized to PX4_LOG_LEVEL_AT_RUN_TIME
-#define PX4_LOG_LEVEL_AT_RUN_TIME	_PX4_LOG_LEVEL_WARN
+#define PX4_LOG_LEVEL_AT_RUN_TIME	_PX4_LOG_LEVEL_ERROR
 
 /****************************************************************************
  * Implementation of log section formatting based on printf
@@ -357,8 +357,8 @@ __END_DECLS
 /****************************************************************************
  * Non-verbose settings for a Release build to minimize strings in build
  ****************************************************************************/
-#define PX4_PANIC(FMT, ...)	__px4_log_file_and_line(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__)
-#define PX4_ERR(FMT, ...)	__px4_log_file_and_line(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__)
+#define PX4_PANIC(FMT, ...)	__px4_log(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__)
+#define PX4_ERR(FMT, ...)	__px4_log(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__)
 #define PX4_WARN(FMT, ...) 	__px4_log_omit(_PX4_LOG_LEVEL_WARN,  FMT, ##__VA_ARGS__)
 #define PX4_DEBUG(FMT, ...) 	__px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__)
 
@@ -366,9 +366,9 @@ __END_DECLS
 /****************************************************************************
  * Medium verbose settings for a default build
  ****************************************************************************/
-#define PX4_PANIC(FMT, ...)	__px4_log_file_and_line(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__)
-#define PX4_ERR(FMT, ...)	__px4_log_file_and_line(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__)
-#define PX4_WARN(FMT, ...) 	__px4_log_file_and_line(_PX4_LOG_LEVEL_WARN,  FMT, ##__VA_ARGS__)
+#define PX4_PANIC(FMT, ...)	__px4_log(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__)
+#define PX4_ERR(FMT, ...)	__px4_log(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__)
+#define PX4_WARN(FMT, ...) 	__px4_log(_PX4_LOG_LEVEL_WARN,  FMT, ##__VA_ARGS__)
 #define PX4_DEBUG(FMT, ...) 	__px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__)
 
 #endif