land_detector: fix timestamp type (#4710 )

The overflow of the uint32_t lead to the land_detector start getting aborted.
Fixed stack overflow in UAVCAN process (#4643 )
2026-06-20 10:30:35 +08:00 · 2016-06-01 17:16:29 +02:00 · 2016-05-26 20:00:46 +02:00 · 2016-05-21 15:55:32 +02:00 · 2016-05-21 14:13:30 +02:00 · 2016-05-21 14:13:22 +02:00
128 changed files with 3736 additions and 1150 deletions
@@ -1,6 +1,6 @@
 {
    "board_id": 9, 
-    "magic": "PX4FWv1", 
+    "magic": "PX4FWv2",
    "description": "Firmware for the MindPx-V2  board", 
    "image": "", 
    "build_time": 0, 
@@ -146,9 +146,6 @@ px4-stm32f4discovery_default:
 px4fmu-v2_ekf2:
 	$(call cmake-build,nuttx_px4fmu-v2_ekf2)

-px4fmu-v2_lpe:
-	$(call cmake-build,nuttx_px4fmu-v2_lpe)
-
 mindpx-v2_default:
 	$(call cmake-build,nuttx_mindpx-v2_default)

@@ -173,18 +170,6 @@ ros_sitl_default:
 qurt_eagle_travis:
 	$(call cmake-build,$@)

-qurt_eagle_release:
-	$(call cmake-build,$@)
-	
-qurt_eagle_legacy_driver_release:
-	$(call cmake-build,$@)
-
-posix_eagle_release:
-	$(call cmake-build,$@)
-	
-posix_eagle_legacy_driver_release:
-	$(call cmake-build,$@)
-
 qurt_eagle_default:
 	$(call cmake-build,$@)

@@ -199,6 +184,14 @@ qurt_eagle_legacy_driver_default:
 posix_eagle_legacy_driver_default:
 	$(call cmake-build,$@) 

+qurt_excelsior_default:
+	$(call cmake-build,$@)
+
+posix_excelsior_default:
+	$(call cmake-build,$@)
+
+excelsior_default: posix_excelsior_default qurt_excelsior_default
+
 posix_rpi2_default:
 	$(call cmake-build,$@)

@@ -262,8 +255,8 @@ clean:
 	@(cd NuttX/nuttx && make clean)

 submodulesclean:
-	@git submodule deinit -f .
 	@git submodule sync
+	@git submodule deinit -f .
 	@git submodule update --init --recursive --force

 distclean: submodulesclean
@@ -15,20 +15,22 @@ sh /etc/init.d/4001_quad_x

 if [ $AUTOCNF == yes ]
 then
-	param set MC_ROLL_P 7.0
-	param set MC_ROLLRATE_P 0.05
-	param set MC_ROLLRATE_I 0.05
-	param set MC_ROLLRATE_D 0.002
+	param set MC_ROLL_P 6.0
+	param set MC_ROLLRATE_P 0.14
+	param set MC_ROLLRATE_I 0.23
+	param set MC_ROLLRATE_D 0.0025
 	param set MC_PITCH_P 7.0
-	param set MC_PITCHRATE_P 0.08
-	param set MC_PITCHRATE_I 0.1
-	param set MC_PITCHRATE_D 0.003
-	param set MC_YAW_P 2.8
-	param set MC_YAWRATE_P 0.2
-	param set MC_YAWRATE_I 0.1
+	param set MC_PITCHRATE_P 0.235
+	param set MC_PITCHRATE_I 0.17
+	param set MC_PITCHRATE_D 0.004
+	param set MC_YAW_P 4
+	param set MC_YAWRATE_P 0.3
+	param set MC_YAWRATE_I 0.2
 	param set MC_YAWRATE_D 0.0
+	param set MC_YAW_FF 0.5
 	param set PWM_MIN 1075
 	param set MPC_THR_MIN 0.06
 	param set MPC_MANTHR_MIN 0.06
 	param set CBRK_IO_SAFETY 22027
+	param set ATT_BIAS_MAX 0.0
 fi
@@ -15,24 +15,31 @@ sh /etc/init.d/4001_quad_x

 if [ $AUTOCNF == yes ]
 then
-	param set MC_ROLL_P 7.0
-	param set MC_ROLLRATE_P 0.1
+	param set MC_ROLL_P 8.0
+	param set MC_ROLLRATE_P 0.19
 	param set MC_ROLLRATE_I 0.1
-	param set MC_ROLLRATE_D 0.005
-	param set MC_PITCH_P 7.0
-	param set MC_PITCHRATE_P 0.1
-	param set MC_PITCHRATE_I 0.2
-	param set MC_PITCHRATE_D 0.005
-	param set MC_YAW_P 2.8
+	param set MC_ROLLRATE_D 0.0055
+	param set MC_PITCH_P 8.0
+	param set MC_PITCHRATE_P 0.24
+	param set MC_PITCHRATE_I 0.1
+	param set MC_PITCHRATE_D 0.0065
+	param set MC_YAW_P 4.0
 	param set MC_YAWRATE_P 0.2
 	param set MC_YAWRATE_I 0.1
 	param set MC_YAWRATE_D 0.0
+	param set MC_YAW_FF 0.5
+	param set MC_ROLLRATE_MAX 720.0
+	param set MC_PITCHRATE_MAX 720.0
+	param set MC_YAWRATE_MAX 400.0
+	param set MC_ACRO_R_MAX 360.0
+	param set MC_ACRO_P_MAX 360.0
+
 	param set PWM_MIN 1075
+
 	param set MPC_THR_MIN 0.06
 	param set MPC_MANTHR_MIN 0.06
-	param set MC_ROLLRATE_MAX 1000.0
-	param set MC_PITCHRATE_MAX 1000.0
-	param set MC_YAWRATE_MAX 400.0
+
 	param set ATT_BIAS_MAX 0.0
+
 	param set CBRK_IO_SAFETY 22027
 fi
@@ -18,6 +18,4 @@ fi

 if px4io limit $PX4IO_LIMIT
 then
-else
-	echo "[i] Set PX4IO update rate to $PX4IO_LIMIT Hz failed!"
 fi
@@ -1,26 +0,0 @@
-#!nsh
-#
-# Initialize logging services.
-#
-
-if [ -d /fs/microsd ]
-then
-	if ver hwcmp PX4FMU_V1
-	then
-		if sdlog2 start -r 40 -a -b 3 -t
-		then
-		fi
-	else
-		# check if we should increase logging rate for ekf2 replay message logging
-		if param greater EKF2_REC_RPL 0
-		then
-			if sdlog2 start -r 500 -e -b 20 -t
-			then
-			fi
-		else
-			if sdlog2 start -r 100 -a -b 12 -t
-			then
-			fi
-		fi
-	fi
-fi
@@ -24,7 +24,6 @@ set LOG_FILE /fs/microsd/bootlog.txt
 # REBOOTWORK this needs to start after the flight control loop
 if mount -t vfat /dev/mmcsd0 /fs/microsd
 then
-	echo "[i] microSD mounted: /fs/microsd"
 	# Start playing the startup tune
 	tone_alarm start
 else
@@ -77,9 +76,7 @@ then
 	param select $PARAM_FILE
 	if param load
 	then
-		echo "[param] Loaded: $PARAM_FILE"
 	else
-		echo "[param] FAILED loading $PARAM_FILE"
 		if param reset
 		then
 		fi
@@ -195,7 +192,7 @@ then
 	set FCONFIG /fs/microsd/etc/config.txt
 	if [ -f $FCONFIG ]
 	then
-		echo "[i] Custom config: $FCONFIG"
+		echo "[i] Custom: $FCONFIG"
 		sh $FCONFIG
 	fi
 	unset FCONFIG
@@ -230,8 +227,6 @@ then

 			set IO_PRESENT yes
 		else
-			echo "PX4IO Trying to update" >> $LOG_FILE
-
 			tone_alarm MLL32CP8MB

 			if px4io start
@@ -256,11 +251,11 @@ then

 					set IO_PRESENT yes
 				else
-					echo "ERROR: PX4IO update failed" >> $LOG_FILE
+					echo "PX4IO update failed" >> $LOG_FILE
 					tone_alarm $TUNE_ERR
 				fi
 			else
-				echo "ERROR: PX4IO update failed" >> $LOG_FILE
+				echo "PX4IO update failed" >> $LOG_FILE
 				tone_alarm $TUNE_ERR
 			fi
 		fi
@@ -268,7 +263,6 @@ then

 		if [ $IO_PRESENT == no ]
 		then
-			echo "[i] ERROR: PX4IO not found"
 			echo "ERROR: PX4IO not found" >> $LOG_FILE
 			tone_alarm $TUNE_ERR
 		fi
@@ -291,7 +285,6 @@ then
 	then
 		# Need IO for output but it not present, disable output
 		set OUTPUT_MODE none
-		echo "[i] ERROR: PX4IO not found, disabling output"

 		# Avoid using ttyS0 for MAVLink on FMUv1
 		if ver hwcmp PX4FMU_V1
@@ -368,7 +361,7 @@ then
 			then
 				sh /etc/init.d/rc.io
 			else
-				echo "ERROR: PX4IO start failed" >> $LOG_FILE
+				echo "PX4IO start failed" >> $LOG_FILE
 				tone_alarm $TUNE_ERR
 			fi
 		fi
@@ -377,10 +370,8 @@ then
 		then
 			if fmu mode_$FMU_MODE
 			then
-				echo "[i] FMU mode_$FMU_MODE started"
 			else
-				echo "[i] ERROR: FMU mode_$FMU_MODE start failed"
-				echo "ERROR: FMU start failed" >> $LOG_FILE
+				echo "FMU start failed" >> $LOG_FILE
 				tone_alarm $TUNE_ERR
 			fi

@@ -411,9 +402,7 @@ then

 			if mkblctrl $MKBLCTRL_ARG
 			then
-				echo "[i] MK started"
 			else
-				echo "[i] ERROR: MK start failed"
 				echo "ERROR: MK start failed" >> $LOG_FILE
 				tone_alarm $TUNE_ERR
 			fi
@@ -425,10 +414,8 @@ then
 		then
 			if pwm_out_sim mode_port2_pwm8
 			then
-				echo "[i] PWM SIM output started"
 			else
-				echo "[i] ERROR: PWM SIM output start failed"
-				echo "ERROR: PWM SIM output start failed" >> $LOG_FILE
+				echo "PWM SIM start failed" >> $LOG_FILE
 				tone_alarm $TUNE_ERR
 			fi
 		fi
@@ -442,11 +429,9 @@ then
 			then
 				if px4io start
 				then
-					echo "[i] PX4IO started"
 					sh /etc/init.d/rc.io
 				else
-					echo "[i] ERROR: PX4IO start failed"
-					echo "ERROR: PX4IO start failed" >> $LOG_FILE
+					echo "PX4IO start failed" >> $LOG_FILE
 					tone_alarm $TUNE_ERR
 				fi
 			fi
@@ -455,10 +440,8 @@ then
 			then
 				if fmu mode_$FMU_MODE
 				then
-					echo "[i] FMU mode_$FMU_MODE started"
 				else
-					echo "[i] ERROR: FMU mode_$FMU_MODE start failed"
-					echo "ERROR: FMU mode_$FMU_MODE start failed" >> $LOG_FILE
+					echo "FMU mode_$FMU_MODE start failed" >> $LOG_FILE
 					tone_alarm $TUNE_ERR
 				fi

@@ -633,7 +616,27 @@ then
 	#
 	# Logging
 	#
-	sh /etc/init.d/rc.logging
+	if [ -d /fs/microsd ]
+	then
+		if ver hwcmp PX4FMU_V1
+		then
+			if sdlog2 start -r 30 -a -b 2 -t
+			then
+			fi
+		else
+			# check if we should increase logging rate for ekf2 replay message logging
+			if param greater EKF2_REC_RPL 0
+			then
+				if sdlog2 start -r 500 -e -b 20 -t
+				then
+				fi
+			else
+				if sdlog2 start -r 100 -a -b 12 -t
+				then
+				fi
+			fi
+		fi
+	fi

 	#
 	# Start up ARDrone Motor interface
@@ -648,7 +651,7 @@ then
 	#
 	if [ $VEHICLE_TYPE == fw ]
 	then
-		echo "[i] FIXED WING"
+		echo "FIXED WING"

 		if [ $MIXER == none ]
 		then
@@ -676,7 +679,7 @@ then
 	#
 	if [ $VEHICLE_TYPE == mc ]
 	then
-		echo "[i] MULTICOPTER"
+		echo "MULTICOPTER"

 		if [ $MIXER == none ]
 		then
@@ -741,11 +744,11 @@ then
 	#
 	if [ $VEHICLE_TYPE == vtol ]
 	then
-		echo "[init] Vehicle type: VTOL"
+		echo "VTOL"

 		if [ $MIXER == none ]
 		then
-			echo "Default mixer for vtol not defined"
+			echo "VTOL mixer undefined"
 		fi

 		if [ $MAV_TYPE == none ]
@@ -853,7 +856,6 @@ then
 	if [ $VEHICLE_TYPE == none ]
 	then
 		echo "[i] No autostart ID found"
-
 	fi

 	# Start any custom addons
@@ -868,7 +870,6 @@ then
 	# Run no SD alarm
 	if [ $LOG_FILE == /dev/null ]
 	then
-		echo "[i] No microSD card found"
 		# Play SOS
 		tone_alarm error
 	fi
@@ -885,7 +886,7 @@ mavlink boot_complete

 if [ $EXIT_ON_END == yes ]
 then
-	echo "Exit from nsh"
+	echo "NSH EXIT"
 	exit
 fi
 unset EXIT_ON_END
@@ -54,7 +54,7 @@ def to_degree(value, loc):
    sec = round((t1 - min)* 60, 5)
    return (deg, min, sec, loc_value)

-def SetGpsLocation(file_name, lat, lng):
+def SetGpsLocation(file_name, lat, lng, alt):
    """
        Adding GPS tag

@@ -72,6 +72,8 @@ def SetGpsLocation(file_name, lat, lng):
    exiv_image["Exif.GPSInfo.GPSLatitudeRef"] = lat_deg[3]
    exiv_image["Exif.GPSInfo.GPSLongitude"] = exiv_lng
    exiv_image["Exif.GPSInfo.GPSLongitudeRef"] = lng_deg[3]
+    exiv_image["Exif.GPSInfo.GPSAltitude"] = pyexiv2.Rational(alt, 1)
+    exiv_image["Exif.GPSInfo.GPSAltitudeRef"] = '0'
    exiv_image["Exif.Image.GPSTag"] = 654
    exiv_image["Exif.GPSInfo.GPSMapDatum"] = "WGS-84"
    exiv_image["Exif.GPSInfo.GPSVersionID"] = '2 0 0 0'
@@ -119,6 +121,8 @@ def LoadImageList(input_folder):
        print("Unequal number of jpg and raw images")
    if len(image_list.jpg) == 0 and len(image_list.raw) == 0:
        print("No images found")
+    image_list.jpg = sorted(image_list.jpg)
+    image_list.raw = sorted(image_list.raw)
    return image_list

 def FilterTrigger(trigger_list, image_list):
@@ -135,17 +139,14 @@ def TagImages(trigger_list, image_list, output_folder):
    """
       load px4 log file and extract trigger locations
    """
-    print len(image_list.jpg)
-    print len(trigger_list.GPOS_Lat)
-    print len(trigger_list.GPOS_Lon)
    for image in range(len(image_list.jpg)):
        base_path, filename = os.path.split(image_list.jpg[image])
        copyfile(image_list.jpg[image], output_folder + "/" + filename)
-        SetGpsLocation(output_folder + "/" + filename, float(trigger_list.GPOS_Lat[image]), float(trigger_list.GPOS_Lon[image]))
+        SetGpsLocation(output_folder + "/" + filename, float(trigger_list.GPOS_Lat[image]), float(trigger_list.GPOS_Lon[image]), float(trigger_list.GPOS_Alt[image]))
    for image in range(len(image_list.raw)):
        base_path, filename = os.path.split(image_list.raw[image])
        copyfile(image_list.raw[image], output_folder + "/" + filename)
-        SetGpsLocation(output_folder + "/" + filename, float(trigger_list.GPOS_Lat[image]), float(trigger_list.GPOS_Lon[image]))
+        SetGpsLocation(output_folder + "/" + filename, float(trigger_list.GPOS_Lat[image]), float(trigger_list.GPOS_Lon[image]), float(trigger_list.GPOS_Alt[image]))

 def main():
    """
@@ -156,7 +157,7 @@ def main():
                    help = "PX4 log file containing recorded positions", 
                    metavar = "string")
    parser.add_option("-i", "--input", dest = "InputFolder",
-                    help = "Input folder containing untagged images",
+                    help = "Input folder containing untagged images in alphabetical order",
                    type = "string")
    parser.add_option("-o", "--output", dest = "OutputFolder",
                    help = "Output folder to contain tagged images",
@@ -541,8 +541,9 @@ function(px4_add_common_flags)
 		-Wall
 		-Werror
 		-Wextra
+		-Wpacked
 		-Wno-sign-compare
-		#-Wshadow # very verbose due to eigen
+		-Wshadow
 		-Wfloat-equal
 		-Wpointer-arith
 		-Wmissing-declarations
@@ -649,6 +650,15 @@ function(px4_add_common_flags)
 		-D__CUSTOM_FILE_IO__
 		)

+	if (NOT (${CMAKE_C_COMPILER_ID} MATCHES ".*Clang.*"))
+		# -fcheck-new is a no-op for Clang in general
+		# and has no effect, but can generate a compile
+		# error for some OS
+		list(APPEND cxx_compile_flags
+			-fcheck-new
+		)
+	endif()
+
 	set(visibility_flags
 		-fvisibility=hidden
 		-include visibility.h
@@ -706,7 +716,7 @@ function(px4_add_common_flags)
 		set(added_exe_linker_flags
 			-Wl,--warn-common
 			-Wl,--gc-sections
-			#,--print-gc-sections 
+			#,--print-gc-sections
 			)
 	endif()

@@ -765,6 +775,13 @@ function(px4_create_git_hash_header)
 		ONE_VALUE HEADER
 		REQUIRED HEADER
 		ARGN ${ARGN})
+	execute_process(
+		COMMAND git describe --tags
+		OUTPUT_VARIABLE git_tag
+		OUTPUT_STRIP_TRAILING_WHITESPACE
+		WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+		)
+	#message(STATUS "GIT_TAG = ${git_tag}")
 	execute_process(
 		COMMAND git rev-parse HEAD
 		OUTPUT_VARIABLE git_desc
@@ -844,9 +861,12 @@ function(px4_generate_parameters_source)
 		${CMAKE_CURRENT_BINARY_DIR}/px4_parameters.c)
 	set_source_files_properties(${generated_files}
 		PROPERTIES GENERATED TRUE)
+	if ("${config_generate_parameters_scope}" STREQUAL "ALL")
+		set(SCOPE "")
+	endif()
 	add_custom_command(OUTPUT ${generated_files}
 		COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_SOURCE_DIR}/Tools/px_generate_params.py ${XML} ${SCOPE}
-		DEPENDS ${XML} ${DEPS} ${CMAKE_SOURCE_DIR}/cmake/configs/${OS}_${BOARD}_${LABEL}.cmake
+		DEPENDS ${XML} ${DEPS} ${SCOPE}
 		)
 	set(${OUT} ${generated_files} PARENT_SCOPE)
 endfunction()
@@ -48,6 +48,7 @@ set(config_module_list
 	drivers/bst
 	drivers/snapdragon_rc_pwm
 	drivers/lis3mdl
+	drivers/bmi160

 	#
 	# System commands
@@ -1,9 +0,0 @@
-include(cmake/configs/nuttx_px4fmu-v2_default.cmake)
-
-list(REMOVE_ITEM config_module_list
-	modules/ekf_att_pos_estimator
-	)
-
-list(APPEND config_module_list
-	modules/local_position_estimator
-	)
@@ -1,6 +1,10 @@
-include(posix/px4_impl_posix)
+# The Eagle board is the first generation Snapdragon Flight board by Qualcomm.
+#
+# This cmake config builds for POSIX, so the part of the flight stack running
+# on the Linux side of the Snapdragon.
+include(configs/posix_sdflight_default)

-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-arm-linux-gnueabihf.cmake)

 set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")

@@ -10,54 +14,4 @@ set(CONFIG_SHMEM "1")
 # or if it is for the Snapdragon.
 add_definitions(
 	-D__PX4_POSIX_EAGLE
-	)
-
-set(config_module_list
-	drivers/device
-	drivers/blinkm
-	drivers/pwm_out_sim
-	drivers/rgbled
-	drivers/led
-	drivers/boards/sitl
-	drivers/qshell/posix
-
-	systemcmds/param
-	systemcmds/mixer
-	systemcmds/ver
-	systemcmds/topic_listener
-
-	modules/mavlink
-
-	modules/attitude_estimator_ekf
-	modules/ekf_att_pos_estimator
-
-	modules/mc_pos_control
-	modules/mc_att_control
-
-	modules/param
-	modules/systemlib
-	modules/systemlib/mixer
-	modules/uORB
-	modules/muorb/krait
-	modules/sensors
-	modules/dataman
-	modules/sdlog2
-	modules/simulator
-	modules/commander
-
-	lib/controllib
-	lib/mathlib
-	lib/mathlib/math/filter
-	lib/conversion
-	lib/ecl
-	lib/geo
-	lib/geo_lookup
-	lib/terrain_estimation
-	lib/runway_takeoff
-	lib/tailsitter_recovery
-	lib/DriverFramework/framework
-
-	platforms/common
-	platforms/posix/px4_layer
-	platforms/posix/work_queue
-	)
+)
@@ -9,6 +9,8 @@ endif()

 set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake)

+set(config_generate_parameters_scope ALL)
+
 set(config_module_list
 	drivers/device
 	drivers/boards/sitl
@@ -4,6 +4,8 @@ set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linu

 set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")

+set(config_generate_parameters_scope ALL)
+
 set(CONFIG_SHMEM "1")

 # This definition allows to differentiate if this just the usual POSIX build
@@ -4,6 +4,8 @@ set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linu

 set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")

+set(config_generate_parameters_scope ALL)
+
 set(config_module_list
 	drivers/device

@@ -1,40 +0,0 @@
-include(posix/px4_impl_posix)
-
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake)
-
-set(CONFIG_SHMEM "1")
-
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
-
-set(config_module_list
-	drivers/device
-	drivers/boards/sitl
-	drivers/led
-
-	systemcmds/param
-	systemcmds/ver
-
-	modules/mavlink
-
-	modules/param
-	modules/systemlib
-	modules/uORB
-	modules/dataman
-	modules/sdlog2
-	modules/simulator
-	modules/commander
-
-	lib/mathlib
-	lib/mathlib/math/filter
-	lib/geo
-	lib/geo_lookup
-	lib/conversion
-	lib/DriverFramework/framework
-
-	platforms/common
-	platforms/posix/px4_layer
-	platforms/posix/work_queue
-
-	modules/muorb/krait
-	)
-
@@ -0,0 +1,10 @@
+# Excelsior is the code name of a board currently in development.
+#
+# This cmake config builds for POSIX, so the part of the flight stack running
+# on the Linux side of the Snapdragon.
+include(configs/posix_sdflight_default)
+
+# This definition allows to differentiate the specific board.
+add_definitions(
+	-D__PX4_POSIX_EXCELSIOR
+)
@@ -2,28 +2,40 @@ include(posix/px4_impl_posix)

 set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake)

-set(CONFIG_SHMEM "1")
-
 set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")

-# A temporary build option to support the use of the legacy (non DriverFramework) drivers.
-add_definitions(
-   -D__USING_SNAPDRAGON_LEGACY_DRIVER
-   )
+set(config_generate_parameters_scope ALL)
+
+set(CONFIG_SHMEM "1")

 set(config_module_list
 	drivers/device
-	drivers/boards/sitl
+	drivers/blinkm
+	drivers/pwm_out_sim
+	drivers/rgbled
 	drivers/led
+	drivers/boards/sitl
+	drivers/qshell/posix

 	systemcmds/param
+	systemcmds/mixer
 	systemcmds/ver
+	systemcmds/topic_listener

 	modules/mavlink

+	modules/attitude_estimator_ekf
+	modules/ekf_att_pos_estimator
+
+	modules/mc_pos_control
+	modules/mc_att_control
+
 	modules/param
 	modules/systemlib
+	modules/systemlib/mixer
 	modules/uORB
+	modules/muorb/krait
+	modules/sensors
 	modules/dataman
 	modules/sdlog2
 	modules/simulator
@@ -32,15 +44,16 @@ set(config_module_list
 	lib/controllib
 	lib/mathlib
 	lib/mathlib/math/filter
+	lib/conversion
+	lib/ecl
 	lib/geo
 	lib/geo_lookup
-	lib/conversion
+	lib/terrain_estimation
+	lib/runway_takeoff
+	lib/tailsitter_recovery
 	lib/DriverFramework/framework

 	platforms/common
 	platforms/posix/px4_layer
 	platforms/posix/work_queue
-
-	modules/muorb/krait
 	)
-
@@ -1,98 +1,12 @@
-include(qurt/px4_impl_qurt)
+# The Eagle board is the first generation Snapdragon Flight board by Qualcomm.
+#
+# This cmake config builds for QURT which is the operating system running on
+# the DSP side.

-if ("$ENV{HEXAGON_SDK_ROOT}" STREQUAL "")
-	message(FATAL_ERROR "Enviroment variable HEXAGON_SDK_ROOT must be set")
-else()
-	set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT})
-endif()
+# The config between different QURT builds is shared.
+include(configs/qurt_sdflight_default)

-set(CONFIG_SHMEM "1")
-
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
-
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
-
-set(config_module_list
-	#
-	# Board support modules
-	#
-	drivers/device
-	modules/sensors
-	platforms/posix/drivers/df_mpu9250_wrapper
-	platforms/posix/drivers/df_bmp280_wrapper
-	platforms/posix/drivers/df_hmc5883_wrapper
-	platforms/posix/drivers/df_trone_wrapper
-	platforms/posix/drivers/df_isl29501_wrapper
-
-	#
-	# System commands
-	#
-	systemcmds/param
-
-	#
-	# Estimation modules (EKF/ SO3 / other filters)
-	#
-	#modules/attitude_estimator_ekf
-	modules/ekf_att_pos_estimator
-	modules/attitude_estimator_q
-	modules/position_estimator_inav
-	modules/ekf2
-
-	#
-	# Vehicle Control
-	#
-	modules/mc_att_control
-	modules/mc_pos_control
-
-	#
-	# Library modules
-	#
-	modules/param
-	modules/systemlib
-	modules/systemlib/mixer
-	modules/uORB
-	modules/commander
-	modules/land_detector
-
-	#
-	# PX4 drivers
-	#
-	drivers/gps
-	drivers/uart_esc
-	drivers/qshell/qurt
-
-	#
-	# Libraries
-	#
-	lib/controllib
-	lib/mathlib
-	lib/mathlib/math/filter
-	lib/geo
-	lib/ecl
-	lib/geo_lookup
-	lib/conversion
-	lib/terrain_estimation
-	lib/runway_takeoff
-	lib/tailsitter_recovery
-	lib/DriverFramework/framework
-
-	#
-	# QuRT port
-	#
-	platforms/common
-	platforms/qurt/px4_layer
-	platforms/posix/work_queue
-
-	#
-	# sources for muorb over fastrpc
-	#
-	modules/muorb/adsp
-	)
-
-set(config_df_driver_list
-	mpu9250
-	bmp280
-	hmc5883
-	trone
-	isl29501
-	)
+# This definition allows to differentiate the specific board.
+add_definitions(
+	-D__PX4_QURT_EAGLE
+)
@@ -10,6 +10,8 @@ set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolc

 set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")

+set(config_generate_parameters_scope ALL)
+
 set(config_module_list
 	drivers/device

@@ -10,6 +10,8 @@ set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolc

 set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")

+set(config_generate_parameters_scope ALL)
+
 set(config_module_list
 	drivers/device
 	drivers/boards/sitl
@@ -8,6 +8,8 @@ endif()

 set(CONFIG_SHMEM "1")

+set(config_generate_parameters_scope ALL)
+
 set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)

 set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
@@ -10,6 +10,8 @@ set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolc

 set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")

+set(config_generate_parameters_scope ALL)
+
 set(config_module_list
 	drivers/device

@@ -1,99 +0,0 @@
-include(qurt/px4_impl_qurt)
-
-if ("$ENV{HEXAGON_SDK_ROOT}" STREQUAL "")
-	message(FATAL_ERROR "Enviroment variable HEXAGON_SDK_ROOT must be set")
-else()
-	set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT})
-endif()
-
-if ("$ENV{EAGLE_DRIVERS_SRC}" STREQUAL "")
-	message(FATAL_ERROR "Environment variable EAGLE_DRIVERS_SRC must be set")
-else()
-	set(EAGLE_DRIVERS_SRC $ENV{EAGLE_DRIVERS_SRC})
-endif()
-
-STRING(REGEX REPLACE "//" "/" EAGLE_DRIVERS_SRC ${EAGLE_DRIVERS_SRC})
-STRING(REGEX REPLACE "/" "__" EAGLE_DRIVERS_MODULE_PREFIX ${EAGLE_DRIVERS_SRC})
-
-#include_directories(${EAGLE_ADDON_ROOT}/flight_controller/hexagon/inc)
-include_directories(
-	${HEXAGON_SDK_ROOT}/inc
-	${HEXAGON_SDK_ROOT}/inc/stddef
-	${HEXAGON_SDK_ROOT}/lib/common/qurt/ADSPv5MP/include
-	)
-
-message("hexagon_sdk_root is ${HEXAGON_SDK_ROOT}")
-
-set(QURT_ENABLE_STUBS "0")
-
-set(CONFIG_SHMEM "1")
-
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
-
-set(config_module_list
-	#
-	# Board support modules
-	#
-	drivers/device
-	modules/sensors
-	${EAGLE_DRIVERS_SRC}/mpu_spi
-	${EAGLE_DRIVERS_SRC}/uart_esc
-	${EAGLE_DRIVERS_SRC}/rc_receiver
-	${EAGLE_DRIVERS_SRC}/csr_gps
-
-	#
-	# System commands
-	#
-	systemcmds/param
-
-	#
-	# Estimation modules (EKF/ SO3 / other filters)
-	#
-	#modules/attitude_estimator_ekf
-	modules/ekf_att_pos_estimator
-	modules/attitude_estimator_q
-	modules/position_estimator_inav
-
-	#
-	# Vehicle Control
-	#
-	modules/mc_att_control
-	modules/mc_pos_control
-
-	#
-	# Library modules
-	#
-	modules/param
-	modules/systemlib
-	modules/systemlib/mixer
-	modules/uORB
-	modules/commander
-
-	#
-	# Libraries
-	#
-	lib/controllib
-	lib/mathlib
-	lib/mathlib/math/filter
-	lib/geo
-	lib/ecl
-	lib/geo_lookup
-	lib/conversion
-	lib/terrain_estimation
-	lib/runway_takeoff
-	lib/tailsitter_recovery
-	lib/DriverFramework/framework
-
-	#
-	# QuRT port
-	#
-	platforms/common
-	platforms/qurt/px4_layer
-	platforms/posix/work_queue
-
-	#
-	# sources for muorb over fastrpc
-	#
-	modules/muorb/adsp
-	)
@@ -9,6 +9,8 @@ endif()
 set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
 set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")

+set(config_generate_parameters_scope ALL)
+
 set(config_module_list
 	drivers/device

@@ -17,6 +17,8 @@ endif()

 include_directories(${HEXAGON_SDK_ROOT}/lib/common/qurt/ADSPv5MP/include)

+set(config_generate_parameters_scope ALL)
+
 set(config_module_list
 	drivers/device
 	drivers/boards/sitl
@@ -0,0 +1,12 @@
+# Excelsior is the code name of a board currently in development.
+#
+# This cmake config builds for QURT which is the operating system running on
+# the DSP side.
+
+# The config between different QURT builds is shared.
+include(configs/qurt_sdflight_default)
+
+# This definition allows to differentiate the specific board.
+add_definitions(
+	-D__PX4_QURT_EXCELSIOR
+)
@@ -6,34 +6,13 @@ else()
 	set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT})
 endif()

-if ("$ENV{EAGLE_DRIVERS_SRC}" STREQUAL "")
-	message(FATAL_ERROR "Environment variable EAGLE_DRIVERS_SRC must be set")
-else()
-	set(EAGLE_DRIVERS_SRC $ENV{EAGLE_DRIVERS_SRC})
-endif()
-
-STRING(REGEX REPLACE "//" "/" EAGLE_DRIVERS_SRC ${EAGLE_DRIVERS_SRC})
-STRING(REGEX REPLACE "/" "__" EAGLE_DRIVERS_MODULE_PREFIX ${EAGLE_DRIVERS_SRC})
-
-#include_directories(${EAGLE_ADDON_ROOT}/flight_controller/hexagon/inc)
-include_directories(
-	${HEXAGON_SDK_ROOT}/inc
-	${HEXAGON_SDK_ROOT}/inc/stddef
-	${HEXAGON_SDK_ROOT}/lib/common/qurt/ADSPv5MP/include
-	)
-
-message("hexagon_sdk_root is ${HEXAGON_SDK_ROOT}")
-
-set(QURT_ENABLE_STUBS "0")
-
 set(CONFIG_SHMEM "1")

-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
+set(config_generate_parameters_scope ALL)

-add_definitions(
-   -D__USING_SNAPDRAGON_LEGACY_DRIVER
-   )
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
+
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")

 set(config_module_list
 	#
@@ -41,10 +20,11 @@ set(config_module_list
 	#
 	drivers/device
 	modules/sensors
+	platforms/posix/drivers/df_mpu9250_wrapper
 	platforms/posix/drivers/df_bmp280_wrapper
-	${EAGLE_DRIVERS_SRC}/mpu_spi
-	${EAGLE_DRIVERS_SRC}/uart_esc
-	${EAGLE_DRIVERS_SRC}/rc_receiver
+	platforms/posix/drivers/df_hmc5883_wrapper
+	platforms/posix/drivers/df_trone_wrapper
+	platforms/posix/drivers/df_isl29501_wrapper

 	#
 	# System commands
@@ -58,6 +38,7 @@ set(config_module_list
 	modules/ekf_att_pos_estimator
 	modules/attitude_estimator_q
 	modules/position_estimator_inav
+	modules/ekf2

 	#
 	# Vehicle Control
@@ -73,6 +54,14 @@ set(config_module_list
 	modules/systemlib/mixer
 	modules/uORB
 	modules/commander
+	modules/land_detector
+
+	#
+	# PX4 drivers
+	#
+	drivers/gps
+	drivers/uart_esc
+	drivers/qshell/qurt

 	#
 	# Libraries
@@ -103,5 +92,9 @@ set(config_module_list
 	)

 set(config_df_driver_list
+	mpu9250
 	bmp280
+	hmc5883
+	trone
+	isl29501
 	)
@@ -197,7 +197,7 @@ else()

 endif()

-if ("${BOARD}" STREQUAL "eagle")
+if ("${BOARD}" STREQUAL "eagle" OR "${BOARD}" STREQUAL "excelsior")

 	if ("$ENV{HEXAGON_ARM_SYSROOT}" STREQUAL "")
 		message(FATAL_ERROR "HEXAGON_ARM_SYSROOT not set")
@@ -2,3 +2,4 @@
 /* Do not edit! */
 #define PX4_GIT_VERSION_STR  "@git_desc@"
 #define PX4_GIT_VERSION_BINARY 0x@git_desc_short@
+#define PX4_GIT_TAG_STR  "@git_tag@"
@@ -1,93 +0,0 @@
-# defines:
-#
-# NM
-# OBJCOPY
-# LD
-# CXX_COMPILER
-# C_COMPILER
-# CMAKE_SYSTEM_NAME
-# CMAKE_SYSTEM_VERSION
-# GENROMFS
-# LINKER_FLAGS
-# CMAKE_EXE_LINKER_FLAGS
-# CMAKE_FIND_ROOT_PATH
-# CMAKE_FIND_ROOT_PATH_MODE_PROGRAM
-# CMAKE_FIND_ROOT_PATH_MODE_LIBRARY
-# CMAKE_FIND_ROOT_PATH_MODE_INCLUDE
-
-include(CMakeForceCompiler)
-
-if ("$ENV{HEXAGON_SDK_ROOT}" STREQUAL "")
-        message(FATAL_ERROR "HEXAGON_SDK_ROOT not set")
-else()
-        set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT})
-endif()
-
-if ("$ENV{HEXAGON_ARM_SYSROOT}" STREQUAL "")
-        message(FATAL_ERROR "HEXAGON_ARM_SYSROOT not set")
-else()
-        set(HEXAGON_TOOLS_ROOT $ENV{HEXAGON_ARM_SYSROOT})
-endif()
-
-# this one is important
-set(CMAKE_SYSTEM_NAME Generic)
-
-#this one not so much
-set(CMAKE_SYSTEM_VERSION 1)
-
-# specify the cross compiler
-find_program(C_COMPILER arm-linux-gnueabihf-gcc
-	PATHS ${HEXAGON_SDK_ROOT}/gcc-linaro-arm-linux-gnueabihf-4.8-2013.08_linux/bin
-	NO_DEFAULT_PATH
-	)
-
-if(NOT C_COMPILER)
-	message(FATAL_ERROR "could not find arm-linux-gnueabihf-gcc compiler")
-endif()
-cmake_force_c_compiler(${C_COMPILER} GNU)
-
-find_program(CXX_COMPILER arm-linux-gnueabihf-g++
-	PATHS ${HEXAGON_SDK_ROOT}/gcc-linaro-arm-linux-gnueabihf-4.8-2013.08_linux/bin
-	NO_DEFAULT_PATH
-	)
-
-if(NOT CXX_COMPILER)
-	message(FATAL_ERROR "could not find arm-linux-gnueabihf-g++ compiler")
-endif()
-cmake_force_cxx_compiler(${CXX_COMPILER} GNU)
-
-# compiler tools
-foreach(tool objcopy nm ld)
-	string(TOUPPER ${tool} TOOL)
-	find_program(${TOOL} arm-linux-gnueabihf-${tool}
-		PATHS ${HEXAGON_SDK_ROOT}/gcc-linaro-arm-linux-gnueabihf-4.8-2013.08_linux/bin
-		NO_DEFAULT_PATH
-		)
-	if(NOT ${TOOL})
-		message(FATAL_ERROR "could not find arm-linux-gnueabihf-${tool}")
-	endif()
-endforeach()
-
-# os tools
-foreach(tool echo patch grep rm mkdir nm genromfs cp touch make unzip)
-	string(TOUPPER ${tool} TOOL)
-	find_program(${TOOL} ${tool})
-	if(NOT ${TOOL})
-		message(FATAL_ERROR "could not find ${TOOL}")
-	endif()
-endforeach()
-
-set(C_FLAGS "--sysroot=${HEXAGON_ARM_SYSROOT}")
-set(LINKER_FLAGS "-Wl,-gc-sections")
-set(CMAKE_EXE_LINKER_FLAGS ${LINKER_FLAGS})
-set(CMAKE_C_FLAGS ${C_FLAGS})
-set(CMAKE_CXX_LINKER_FLAGS ${C_FLAGS})
-
-# where is the target environment
-set(CMAKE_FIND_ROOT_PATH  get_file_component(${C_COMPILER} PATH))
-
-# search for programs in the build host directories
-set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER)
-# for libraries and headers in the target directories
-set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY)
-set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY)
@@ -2,9 +2,11 @@ uint64 timestamp		# microseconds since system boot, needed to integrate
 float32 voltage_v		# Battery voltage in volts, 0 if unknown
 float32 voltage_filtered_v	# Battery voltage in volts, filtered, 0 if unknown
 float32 current_a		# Battery current in amperes, -1 if unknown
+float32 current_filtered_a	# Battery current in amperes, filtered, 0 if unknown
 float32 discharged_mah		# Discharged amount in mAh, -1 if unknown
 float32 remaining		# From 1 to 0, -1 if unknown
 int32 cell_count		# Number of cells
+bool connected			# Wether or not a battery is connected
 #bool is_powering_off		# Power off event imminent indication, false if unknown


@@ -6,6 +6,7 @@ uint64 magnetometer_timestamp 		# timestamp of magnetometer measurement in us
 uint64 baro_timestamp			# timestamp of barometer measurement in us
 uint64 rng_timestamp			# timestamp of range finder measurement in us
 uint64 flow_timestamp			# timestamp of optical flow measurement in us
+uint64 asp_timestamp			# timestamp of airspeed measurement in us

 float32[3] gyro_integral_rad		# integrated gyro vector in rad
 float32[3] accelerometer_integral_m_s  	# integrated accelerometer vector in m/s
@@ -37,3 +38,11 @@ float32[2] flow_pixel_integral		# integrated optical flow rate around x and y ax
 float32[2] flow_gyro_integral		# integrated gyro rate around x and y axes (rad)
 uint32 flow_time_integral		# integration timespan (usec)
 uint8 flow_quality			# Quality of accumulated optical flow data (0 - 255)
+
+# airspeed
+float32 indicated_airspeed_m_s		# indicated airspeed in meters per second, -1 if unknown
+float32 true_airspeed_m_s		# true filtered airspeed in meters per second, -1 if unknown
+float32 true_airspeed_unfiltered_m_s	# true airspeed in meters per second, -1 if unknown
+float32 air_temperature_celsius		# air temperature in degrees celsius, -1000 if unknown
+float32 confidence			# confidence value from 0 to 1 for this sensor
+
@@ -667,9 +667,9 @@ CONFIG_CDCACM_BULKIN_REQLEN=96
 CONFIG_CDCACM_RXBUFSIZE=1000
 CONFIG_CDCACM_TXBUFSIZE=8000
 CONFIG_CDCACM_VENDORID=0x26ac
-CONFIG_CDCACM_PRODUCTID=0x0011
+CONFIG_CDCACM_PRODUCTID=0x0030
 CONFIG_CDCACM_VENDORSTR="3D Robotics"
-CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2.x"
+CONFIG_CDCACM_PRODUCTSTR="MindPX FMU v2.x"
 # CONFIG_USBMSC is not set
 # CONFIG_USBHOST is not set
 # CONFIG_WIRELESS is not set
@@ -0,0 +1,47 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__bmi160
+	MAIN bmi160
+	STACK_MAIN 1200
+	COMPILE_FLAGS
+		-Weffc++
+		-Os
+	SRCS
+		bmi160.cpp
+		bmi160_gyro.cpp
+		bmi160_main.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
@@ -0,0 +1,501 @@
+#ifndef BMI160_HPP_
+#define BMI160_HPP_
+
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+#include <getopt.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+#include <systemlib/conversions.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/clock.h>
+
+#include <board_config.h>
+#include <drivers/drv_hrt.h>
+
+#include <drivers/device/spi.h>
+#include <drivers/device/ringbuffer.h>
+#include <drivers/device/integrator.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_mag.h>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
+#include <lib/conversion/rotation.h>
+
+#define DIR_READ                0x80
+#define DIR_WRITE               0x00
+
+#define BMI160_DEVICE_PATH_ACCEL    "/dev/bmi160_accel"
+#define BMI160_DEVICE_PATH_GYRO		"/dev/bmi160_gyro"
+#define BMI160_DEVICE_PATH_MAG		"/dev/bmi160_mag"
+#define BMI160_DEVICE_PATH_ACCEL_EXT    "/dev/bmi160_accel_ext"
+#define BMI160_DEVICE_PATH_GYRO_EXT		"/dev/bmi160_gyro_ext"
+#define BMI160_DEVICE_PATH_MAG_EXT	"/dev/bmi160_mag_ext"
+
+// BMI 160 registers
+
+#define BMIREG_CHIP_ID          0x00
+#define BMIREG_ERR_REG          0x02
+#define BMIREG_PMU_STATUS       0x03
+#define BMIREG_DATA_0           0x04
+#define BMIREG_DATA_1           0x05
+#define BMIREG_DATA_2           0x06
+#define BMIREG_DATA_3           0x07
+#define BMIREG_DATA_4           0x08
+#define BMIREG_DATA_5           0x09
+#define BMIREG_DATA_6           0x0A
+#define BMIREG_DATA_7           0x0B
+#define BMIREG_GYR_X_L          0x0C
+#define BMIREG_GYR_X_H          0x0D
+#define BMIREG_GYR_Y_L          0x0E
+#define BMIREG_GYR_Y_H          0x0F
+#define BMIREG_GYR_Z_L          0x10
+#define BMIREG_GYR_Z_H          0x11
+#define BMIREG_ACC_X_L          0x12
+#define BMIREG_ACC_X_H          0x13
+#define BMIREG_ACC_Y_L          0x14
+#define BMIREG_ACC_Y_H          0x15
+#define BMIREG_ACC_Z_L          0x16
+#define BMIREG_ACC_Z_H          0x17
+#define BMIREG_SENSORTIME0      0x18
+#define BMIREG_SENSORTIME1      0x19
+#define BMIREG_SENSORTIME2      0x1A
+#define BMIREG_STATUS           0x1B
+#define BMIREG_INT_STATUS_0     0x1C
+#define BMIREG_INT_STATUS_1     0x1D
+#define BMIREG_INT_STATUS_2     0x1E
+#define BMIREG_INT_STATUS_3     0x1F
+#define BMIREG_TEMP_0           0x20
+#define BMIREG_TEMP_1           0x21
+#define BMIREG_FIFO_LEN_0       0x22
+#define BMIREG_FIFO_LEN_1       0x23
+#define BMIREG_FIFO_DATA        0x24
+#define BMIREG_ACC_CONF         0x40
+#define BMIREG_ACC_RANGE        0x41
+#define BMIREG_GYR_CONF         0x42
+#define BMIREG_GYR_RANGE        0x43
+#define BMIREG_MAG_CONF         0x44
+#define BMIREG_FIFO_DOWNS       0x45
+#define BMIREG_FIFO_CONFIG_0    0x46
+#define BMIREG_FIFO_CONFIG_1    0x47
+#define BMIREG_MAG_IF_0         0x4B
+#define BMIREG_MAG_IF_1         0x4C
+#define BMIREG_MAG_IF_2         0x4D
+#define BMIREG_MAG_IF_3         0x4E
+#define BMIREG_MAG_IF_4         0x4F
+#define BMIREG_INT_EN_0         0x50
+#define BMIREG_INT_EN_1         0x51
+#define BMIREG_INT_EN_2         0x52
+#define BMIREG_INT_OUT_CTRL     0x53
+#define BMIREG_INT_LANTCH       0x54
+#define BMIREG_INT_MAP_0        0x55
+#define BMIREG_INT_MAP_1        0x56
+#define BMIREG_INT_MAP_2        0x57
+#define BMIREG_INT_DATA_0       0x58
+#define BMIREG_INT_DATA_1       0x59
+#define BMIREG_INT_LH_0         0x5A
+#define BMIREG_INT_LH_1         0x5B
+#define BMIREG_INT_LH_2         0x5C
+#define BMIREG_INT_LH_3         0x5D
+#define BMIREG_INT_LH_4         0x5E
+#define BMIREG_INT_MOT_0        0x5F
+#define BMIREG_INT_MOT_1        0x60
+#define BMIREG_INT_MOT_2        0x61
+#define BMIREG_INT_MOT_3        0x62
+#define BMIREG_INT_TAP_0        0x63
+#define BMIREG_INT_TAP_1        0x64
+#define BMIREG_INT_ORIE_0       0x65
+#define BMIREG_INT_ORIE_1       0x66
+#define BMIREG_INT_FLAT_0       0x67
+#define BMIREG_INT_FLAT_1       0x68
+#define BMIREG_FOC_CONF         0x69
+#define BMIREG_CONF             0x6A
+#define BMIREG_IF_CONF          0x6B
+#define BMIREG_PMU_TRIGGER      0x6C
+#define BMIREG_SELF_TEST        0x6D
+#define BMIREG_NV_CONF          0x70
+#define BMIREG_OFFSET_ACC_X     0x71
+#define BMIREG_OFFSET_ACC_Y     0x72
+#define BMIREG_OFFSET_ACC_Z     0x73
+#define BMIREG_OFFSET_GYR_X     0x74
+#define BMIREG_OFFSET_GYR_Y     0x75
+#define BMIREG_OFFSET_GYR_Z     0x76
+#define BMIREG_OFFSET_EN        0x77
+#define BMIREG_STEP_CONT_0      0x78
+#define BMIREG_STEP_CONT_1      0x79
+#define BMIREG_STEP_CONF_0      0x7A
+#define BMIREG_STEP_CONF_1      0x7B
+#define BMIREG_CMD              0x7E
+
+
+// Configuration bits BMI 160
+#define BMI160_WHO_AM_I         0xD1
+
+//BMIREG_STATUS           0x1B
+#define BMI_DRDY_ACCEL          (1<<7)
+#define BMI_DRDY_GYRO           (1<<6)
+#define BMI_DRDY_MAG            (1<<5)
+#define BMI_GYRO_SELF_TEST_OK   (1<<1)
+
+//BMIREG_INT_STATUS_1     0x1D
+#define BMI_DRDY_INT            (1<<4)
+
+//BMIREG_ACC_CONF         0x40
+#define BMI_ACCEL_RATE_25_32    (0<<3) | (0<<2) | (0<<1) | (1<<0)
+#define BMI_ACCEL_RATE_25_16    (0<<3) | (0<<2) | (1<<1) | (0<<0)
+#define BMI_ACCEL_RATE_25_8     (0<<3) | (0<<2) | (1<<1) | (1<<0)
+#define BMI_ACCEL_RATE_25_4     (0<<3) | (1<<2) | (0<<1) | (0<<0)
+#define BMI_ACCEL_RATE_25_2     (0<<3) | (1<<2) | (0<<1) | (1<<0)
+#define BMI_ACCEL_RATE_25       (0<<3) | (1<<2) | (1<<1) | (0<<0)
+#define BMI_ACCEL_RATE_50       (0<<3) | (1<<2) | (1<<1) | (1<<0)
+#define BMI_ACCEL_RATE_100      (1<<3) | (0<<2) | (0<<1) | (0<<0)
+#define BMI_ACCEL_RATE_200      (1<<3) | (0<<2) | (0<<1) | (1<<0)
+#define BMI_ACCEL_RATE_400      (1<<3) | (0<<2) | (1<<1) | (0<<0)
+#define BMI_ACCEL_RATE_800      (1<<3) | (0<<2) | (1<<1) | (1<<0)
+#define BMI_ACCEL_RATE_1600     (1<<3) | (1<<2) | (0<<1) | (0<<0)
+#define BMI_ACCEL_US            (0<<7)
+#define BMI_ACCEL_BWP_NORMAL    (0<<6) | (1<<5) | (0<<4)
+#define BMI_ACCEL_BWP_OSR2      (0<<6) | (0<<5) | (1<<4)
+#define BMI_ACCEL_BWP_OSR4      (0<<6) | (0<<5) | (0<<4)
+
+//BMIREG_ACC_RANGE        0x41
+#define BMI_ACCEL_RANGE_2_G     (0<<3) | (0<<2) | (1<<1) | (1<<0)
+#define BMI_ACCEL_RANGE_4_G     (0<<3) | (1<<2) | (0<<1) | (1<<0)
+#define BMI_ACCEL_RANGE_8_G     (1<<3) | (0<<2) | (0<<1) | (0<<0)
+#define BMI_ACCEL_RANGE_16_G    (1<<3) | (1<<2) | (0<<1) | (0<<0)
+
+//BMIREG_GYR_CONF         0x42
+#define BMI_GYRO_RATE_25        (0<<3) | (1<<2) | (1<<1) | (0<<0)
+#define BMI_GYRO_RATE_50        (0<<3) | (1<<2) | (1<<1) | (1<<0)
+#define BMI_GYRO_RATE_100       (1<<3) | (0<<2) | (0<<1) | (0<<0)
+#define BMI_GYRO_RATE_200       (1<<3) | (0<<2) | (0<<1) | (1<<0)
+#define BMI_GYRO_RATE_400       (1<<3) | (0<<2) | (1<<1) | (0<<0)
+#define BMI_GYRO_RATE_800       (1<<3) | (0<<2) | (1<<1) | (1<<0)
+#define BMI_GYRO_RATE_1600      (1<<3) | (1<<2) | (0<<1) | (0<<0)
+#define BMI_GYRO_RATE_3200      (1<<3) | (1<<2) | (0<<1) | (1<<0)
+#define BMI_GYRO_BWP_NORMAL     (1<<5) | (0<<4)
+#define BMI_GYRO_BWP_OSR2       (0<<5) | (1<<4)
+#define BMI_GYRO_BWP_OSR4       (0<<5) | (0<<4)
+
+//BMIREG_GYR_RANGE        0x43
+#define BMI_GYRO_RANGE_2000_DPS (0<<2) | (0<<1) | (0<<0)
+#define BMI_GYRO_RANGE_1000_DPS (0<<2) | (0<<1) | (1<<0)
+#define BMI_GYRO_RANGE_500_DPS  (0<<2) | (1<<1) | (0<<0)
+#define BMI_GYRO_RANGE_250_DPS  (0<<2) | (1<<1) | (1<<0)
+#define BMI_GYRO_RANGE_125_DPS  (1<<2) | (0<<1) | (0<<0)
+
+//BMIREG_INT_EN_1         0x51
+#define BMI_DRDY_INT_EN         (1<<4)
+
+//BMIREG_INT_OUT_CTRL     0x53
+#define BMI_INT1_EN             (1<<3) | (0<<2) | (1<<1)    //Data Ready on INT1 High
+
+//BMIREG_INT_MAP_1        0x56
+#define BMI_DRDY_INT1           (1<<7)
+
+//BMIREG_IF_CONF          0x6B
+#define BMI_SPI_3_WIRE          (1<<0)
+#define BMI_SPI_4_WIRE          (0<<0)
+#define BMI_AUTO_DIS_SEC        (0<<5) | (0<<4)
+#define BMI_I2C_OIS_SEC         (0<<5) | (1<<4)
+#define BMI_AUTO_MAG_SEC        (1<<5) | (0<<4)
+
+//BMIREG_NV_CONF          0x70
+#define BMI_SPI                 (1<<0)
+
+//BMIREG_CMD              0x7E
+#define BMI_ACCEL_NORMAL_MODE   0x11 //Wait at least 3.8 ms before another CMD
+#define BMI_GYRO_NORMAL_MODE    0x15 //Wait at least 80 ms before another CMD
+#define BMI160_SOFT_RESET       0xB6
+
+#define BMI160_ACCEL_DEFAULT_RANGE_G		4
+#define BMI160_GYRO_DEFAULT_RANGE_DPS		2000
+#define BMI160_ACCEL_DEFAULT_RATE           800
+#define BMI160_ACCEL_MAX_RATE               1600
+#define BMI160_GYRO_DEFAULT_RATE            800
+#define BMI160_GYRO_MAX_RATE                3200
+
+#define BMI160_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ	324
+#define BMI160_ACCEL_DEFAULT_DRIVER_FILTER_FREQ	50
+
+#define BMI160_GYRO_DEFAULT_ONCHIP_FILTER_FREQ	254.6f
+#define BMI160_GYRO_DEFAULT_DRIVER_FILTER_FREQ	50
+
+#define BMI160_ONE_G                       9.80665f
+
+#define BMI160_LOW_BUS_SPEED				1000*1000
+#define BMI160_HIGH_BUS_SPEED				11*1000*1000
+
+#define BMI160_TIMER_REDUCTION				200
+
+#ifdef PX4_SPI_BUS_EXT
+#define EXTERNAL_BUS PX4_SPI_BUS_EXT
+#else
+#define EXTERNAL_BUS 0
+#endif
+
+
+class BMI160_gyro;
+
+class BMI160 : public device::SPI
+{
+public:
+	BMI160(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation);
+	virtual ~BMI160();
+
+	virtual int		init();
+
+	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/**
+	 * Diagnostics - print some basic information about the driver.
+	 */
+	void			print_info();
+
+	void			print_registers();
+
+	// deliberately cause a sensor error
+	void 			test_error();
+
+protected:
+	virtual int		probe();
+
+	friend class BMI160_gyro;
+
+	virtual ssize_t		gyro_read(struct file *filp, char *buffer, size_t buflen);
+	virtual int		gyro_ioctl(struct file *filp, int cmd, unsigned long arg);
+
+private:
+	BMI160_gyro		*_gyro;
+	uint8_t			_whoami;	/** whoami result */
+
+	struct hrt_call		_call;
+	unsigned		_call_interval;
+
+	ringbuffer::RingBuffer	*_accel_reports;
+
+	struct accel_calibration_s	_accel_scale;
+	float			_accel_range_scale;
+	float			_accel_range_m_s2;
+	orb_advert_t		_accel_topic;
+	int			_accel_orb_class_instance;
+	int			_accel_class_instance;
+
+	ringbuffer::RingBuffer	*_gyro_reports;
+
+	struct gyro_calibration_s	_gyro_scale;
+	float			_gyro_range_scale;
+	float			_gyro_range_rad_s;
+
+	unsigned		_dlpf_freq;
+
+	float		_accel_sample_rate;
+	float		_gyro_sample_rate;
+	perf_counter_t		_accel_reads;
+	perf_counter_t		_gyro_reads;
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_bad_transfers;
+	perf_counter_t		_bad_registers;
+	perf_counter_t		_good_transfers;
+	perf_counter_t		_reset_retries;
+	perf_counter_t		_duplicates;
+	perf_counter_t		_controller_latency_perf;
+
+	uint8_t			_register_wait;
+	uint64_t		_reset_wait;
+
+	math::LowPassFilter2p	_accel_filter_x;
+	math::LowPassFilter2p	_accel_filter_y;
+	math::LowPassFilter2p	_accel_filter_z;
+	math::LowPassFilter2p	_gyro_filter_x;
+	math::LowPassFilter2p	_gyro_filter_y;
+	math::LowPassFilter2p	_gyro_filter_z;
+
+	Integrator		_accel_int;
+	Integrator		_gyro_int;
+
+	enum Rotation		_rotation;
+
+	// this is used to support runtime checking of key
+	// configuration registers to detect SPI bus errors and sensor
+	// reset
+#define BMI160_NUM_CHECKED_REGISTERS 10
+	static const uint8_t	_checked_registers[BMI160_NUM_CHECKED_REGISTERS];
+	uint8_t			_checked_values[BMI160_NUM_CHECKED_REGISTERS];
+	uint8_t			_checked_bad[BMI160_NUM_CHECKED_REGISTERS];
+	uint8_t			_checked_next;
+
+	// last temperature reading for print_info()
+	float			_last_temperature;
+
+	// keep last accel reading for duplicate detection
+	uint16_t		_last_accel[3];
+	bool			_got_duplicate;
+
+	/**
+	 * Start automatic measurement.
+	 */
+	void			start();
+
+	/**
+	 * Stop automatic measurement.
+	 */
+	void			stop();
+
+	/**
+	 * Reset chip.
+	 *
+	 * Resets the chip and measurements ranges, but not scale and offset.
+	 */
+	int			reset();
+
+	/**
+	 * Static trampoline from the hrt_call context; because we don't have a
+	 * generic hrt wrapper yet.
+	 *
+	 * Called by the HRT in interrupt context at the specified rate if
+	 * automatic polling is enabled.
+	 *
+	 * @param arg		Instance pointer for the driver that is polling.
+	 */
+	static void		measure_trampoline(void *arg);
+
+	/**
+	 * Fetch measurements from the sensor and update the report buffers.
+	 */
+	void			measure();
+
+	/**
+	 * Read a register from the BMI160
+	 *
+	 * @param		The register to read.
+	 * @return		The value that was read.
+	 */
+	uint8_t			read_reg(unsigned reg, uint32_t speed = BMI160_LOW_BUS_SPEED);
+	uint16_t		read_reg16(unsigned reg);
+
+	/**
+	 * Write a register in the BMI160
+	 *
+	 * @param reg		The register to write.
+	 * @param value		The new value to write.
+	 */
+	void			write_reg(unsigned reg, uint8_t value);
+
+	/**
+	 * Modify a register in the BMI160
+	 *
+	 * Bits are cleared before bits are set.
+	 *
+	 * @param reg		The register to modify.
+	 * @param clearbits	Bits in the register to clear.
+	 * @param setbits	Bits in the register to set.
+	 */
+	void			modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
+
+	/**
+	 * Write a register in the BMI160, updating _checked_values
+	 *
+	 * @param reg		The register to write.
+	 * @param value		The new value to write.
+	 */
+	void			write_checked_reg(unsigned reg, uint8_t value);
+
+	/**
+	 * Set the BMI160 measurement range.
+	 *
+	 * @param max_g		The maximum G value the range must support.
+	 * @param max_dps	The maximum DPS value the range must support.
+	 * @return		OK if the value can be supported, -ERANGE otherwise.
+	 */
+	int			set_accel_range(unsigned max_g);
+	int			set_gyro_range(unsigned max_dps);
+
+	/**
+	 * Swap a 16-bit value read from the BMI160 to native byte order.
+	 */
+	uint16_t		swap16(uint16_t val) { return (val >> 8) | (val << 8);	}
+
+	/**
+	 * Get the internal / external state
+	 *
+	 * @return true if the sensor is not on the main MCU board
+	 */
+	bool			is_external() { return (_bus == EXTERNAL_BUS); }
+
+	/**
+	 * Measurement self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	int 			self_test();
+
+	/**
+	 * Accel self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	int 			accel_self_test();
+
+	/**
+	 * Gyro self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	int 			gyro_self_test();
+
+	/*
+	  set low pass filter frequency
+	 */
+	void _set_dlpf_filter(uint16_t frequency_hz);
+
+	/*
+	  set sample rate (approximate) - 10 - 952 Hz
+	*/
+	int accel_set_sample_rate(float desired_sample_rate_hz);
+	int gyro_set_sample_rate(float desired_sample_rate_hz);
+	/*
+	  check that key registers still have the right value
+	 */
+	void check_registers(void);
+
+	/* do not allow to copy this class due to pointer data members */
+	BMI160(const BMI160 &);
+	BMI160 operator=(const BMI160 &);
+
+#pragma pack(push, 1)
+	/**
+	 * Report conversation within the BMI160, including command byte and
+	 * interrupt status.
+	 */
+	struct BMIReport {
+		uint8_t		cmd;
+		int16_t		gyro_x;
+		int16_t		gyro_y;
+		int16_t		gyro_z;
+		int16_t		accel_x;
+		int16_t		accel_y;
+		int16_t		accel_z;
+	};
+#pragma pack(pop)
+};
+
+
+
+#endif /* BMI160_HPP_ */
@@ -0,0 +1,63 @@
+
+#include "bmi160_gyro.hpp"
+#include "bmi160.hpp"
+
+BMI160_gyro::BMI160_gyro(BMI160 *parent, const char *path) : CDev("BMI160_gyro", path),
+	_parent(parent),
+	_gyro_topic(nullptr),
+	_gyro_orb_class_instance(-1),
+	_gyro_class_instance(-1)
+{
+}
+
+BMI160_gyro::~BMI160_gyro()
+{
+	if (_gyro_class_instance != -1) {
+		unregister_class_devname(GYRO_BASE_DEVICE_PATH, _gyro_class_instance);
+	}
+}
+
+int
+BMI160_gyro::init()
+{
+	int ret;
+
+	// do base class init
+	ret = CDev::init();
+
+	/* if probe/setup failed, bail now */
+	if (ret != OK) {
+		DEVICE_DEBUG("gyro init failed");
+		return ret;
+	}
+
+	_gyro_class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);
+
+	return ret;
+}
+
+void
+BMI160_gyro::parent_poll_notify()
+{
+	poll_notify(POLLIN);
+}
+
+ssize_t
+BMI160_gyro::read(struct file *filp, char *buffer, size_t buflen)
+{
+	return _parent->gyro_read(filp, buffer, buflen);
+}
+
+int
+BMI160_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+
+	switch (cmd) {
+	case DEVIOCGDEVICEID:
+		return (int)CDev::ioctl(filp, cmd, arg);
+		break;
+
+	default:
+		return _parent->gyro_ioctl(filp, cmd, arg);
+	}
+}
@@ -0,0 +1,41 @@
+#ifndef BMI160_GYRO_HPP_
+#define BMI160_GYRO_HPP_
+
+#include <px4_config.h>
+
+#include "bmi160.hpp"
+
+/**
+ * Helper class implementing the gyro driver node.
+ */
+class BMI160_gyro : public device::CDev
+{
+public:
+	BMI160_gyro(BMI160 *parent, const char *path);
+	~BMI160_gyro();
+
+	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	virtual int		init();
+
+protected:
+	friend class BMI160;
+
+	void			parent_poll_notify();
+
+private:
+	BMI160			*_parent;
+	orb_advert_t		_gyro_topic;
+	int			_gyro_orb_class_instance;
+	int			_gyro_class_instance;
+
+	/* do not allow to copy this class due to pointer data members */
+	BMI160_gyro(const BMI160_gyro &);
+	BMI160_gyro operator=(const BMI160_gyro &);
+};
+
+
+
+
+#endif /* BMI160_GYRO_HPP_ */
@@ -0,0 +1,358 @@
+
+#include "bmi160.hpp"
+
+#include <board_config.h>
+
+/** driver 'main' command */
+extern "C" { __EXPORT int bmi160_main(int argc, char *argv[]); }
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace bmi160
+{
+
+BMI160	*g_dev_int; // on internal bus
+BMI160	*g_dev_ext; // on external bus
+
+void	start(bool, enum Rotation);
+void	stop(bool);
+void	test(bool);
+void	reset(bool);
+void	info(bool);
+void	regdump(bool);
+void	testerror(bool);
+void	usage();
+
+
+/**
+ * Start the driver.
+ *
+ * This function only returns if the driver is up and running
+ * or failed to detect the sensor.
+ */
+void
+start(bool external_bus, enum Rotation rotation)
+{
+	int fd;
+	BMI160 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
+	const char *path_accel = external_bus ? BMI160_DEVICE_PATH_ACCEL_EXT : BMI160_DEVICE_PATH_ACCEL;
+	const char *path_gyro  = external_bus ? BMI160_DEVICE_PATH_GYRO_EXT : BMI160_DEVICE_PATH_GYRO;
+
+	if (*g_dev_ptr != nullptr)
+		/* if already started, the still command succeeded */
+	{
+		errx(0, "already started");
+	}
+
+	/* create the driver */
+	if (external_bus) {
+#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_BMI)
+		*g_dev_ptr = new BMI160(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_BMI, rotation);
+#else
+		errx(0, "External SPI not available");
+#endif
+
+	} else {
+		*g_dev_ptr = new BMI160(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_BMI, rotation);
+	}
+
+	if (*g_dev_ptr == nullptr) {
+		goto fail;
+	}
+
+	if (OK != (*g_dev_ptr)->init()) {
+		goto fail;
+	}
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(path_accel, O_RDONLY);
+
+	if (fd < 0) {
+		goto fail;
+	}
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		goto fail;
+	}
+
+	close(fd);
+
+	exit(0);
+fail:
+
+	if (*g_dev_ptr != nullptr) {
+		delete(*g_dev_ptr);
+		*g_dev_ptr = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+void
+stop(bool external_bus)
+{
+	BMI160 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
+
+	if (*g_dev_ptr != nullptr) {
+		delete *g_dev_ptr;
+		*g_dev_ptr = nullptr;
+
+	} else {
+		/* warn, but not an error */
+		warnx("already stopped.");
+	}
+
+	exit(0);
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test(bool external_bus)
+{
+	const char *path_accel = external_bus ? BMI160_DEVICE_PATH_ACCEL_EXT : BMI160_DEVICE_PATH_ACCEL;
+	const char *path_gyro  = external_bus ? BMI160_DEVICE_PATH_GYRO_EXT : BMI160_DEVICE_PATH_GYRO;
+	accel_report a_report;
+	gyro_report g_report;
+	ssize_t sz;
+
+	/* get the driver */
+	int fd = open(path_accel, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "%s open failed (try 'bmi160 start')",
+		    path_accel);
+
+	/* get the driver */
+	int fd_gyro = open(path_gyro, O_RDONLY);
+
+	if (fd_gyro < 0) {
+		err(1, "%s open failed", path_gyro);
+	}
+
+	/* reset to manual polling */
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) {
+		err(1, "reset to manual polling");
+	}
+
+	/* do a simple demand read */
+	sz = read(fd, &a_report, sizeof(a_report));
+
+	if (sz != sizeof(a_report)) {
+		warnx("ret: %d, expected: %d", sz, sizeof(a_report));
+		err(1, "immediate acc read failed");
+	}
+
+	warnx("single read");
+	warnx("time:     %lld", a_report.timestamp);
+	warnx("acc  x:  \t%8.4f\tm/s^2", (double)a_report.x);
+	warnx("acc  y:  \t%8.4f\tm/s^2", (double)a_report.y);
+	warnx("acc  z:  \t%8.4f\tm/s^2", (double)a_report.z);
+	warnx("acc  x:  \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw);
+	warnx("acc  y:  \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
+	warnx("acc  z:  \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
+	warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
+	      (double)(a_report.range_m_s2 / BMI160_ONE_G));
+
+	/* do a simple demand read */
+	sz = read(fd_gyro, &g_report, sizeof(g_report));
+
+	if (sz != sizeof(g_report)) {
+		warnx("ret: %d, expected: %d", sz, sizeof(g_report));
+		err(1, "immediate gyro read failed");
+	}
+
+	warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x);
+	warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y);
+	warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z);
+	warnx("gyro x: \t%d\traw", (int)g_report.x_raw);
+	warnx("gyro y: \t%d\traw", (int)g_report.y_raw);
+	warnx("gyro z: \t%d\traw", (int)g_report.z_raw);
+	warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
+	      (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
+
+	warnx("temp:  \t%8.4f\tdeg celsius", (double)a_report.temperature);
+	warnx("temp:  \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw);
+
+	/* reset to default polling */
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		err(1, "reset to default polling");
+	}
+
+	close(fd);
+	close(fd_gyro);
+
+	/* XXX add poll-rate tests here too */
+
+	reset(external_bus);
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset(bool external_bus)
+{
+	const char *path_accel = external_bus ? BMI160_DEVICE_PATH_ACCEL_EXT : BMI160_DEVICE_PATH_ACCEL;
+	int fd = open(path_accel, O_RDONLY);
+
+	if (fd < 0) {
+		err(1, "failed ");
+	}
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
+		err(1, "driver reset failed");
+	}
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		err(1, "driver poll restart failed");
+	}
+
+	close(fd);
+
+	exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info(bool external_bus)
+{
+	BMI160 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
+
+	if (*g_dev_ptr == nullptr) {
+		errx(1, "driver not running");
+	}
+
+	printf("state @ %p\n", *g_dev_ptr);
+	(*g_dev_ptr)->print_info();
+
+	exit(0);
+}
+
+/**
+ * Dump the register information
+ */
+void
+regdump(bool external_bus)
+{
+	BMI160 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
+
+	if (*g_dev_ptr == nullptr) {
+		errx(1, "driver not running");
+	}
+
+	printf("regdump @ %p\n", *g_dev_ptr);
+	(*g_dev_ptr)->print_registers();
+
+	exit(0);
+}
+
+
+/**
+ * deliberately produce an error to test recovery
+ */
+void
+testerror(bool external_bus)
+{
+	BMI160 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
+
+	if (*g_dev_ptr == nullptr) {
+		errx(1, "driver not running");
+	}
+
+	(*g_dev_ptr)->test_error();
+
+	exit(0);
+}
+
+void
+usage()
+{
+	warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'testerror'");
+	warnx("options:");
+	warnx("    -X    (external bus)");
+	warnx("    -R rotation");
+}
+
+} // namespace
+
+int
+bmi160_main(int argc, char *argv[])
+{
+	bool external_bus = false;
+	int ch;
+	enum Rotation rotation = ROTATION_NONE;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = getopt(argc, argv, "XR:")) != EOF) {
+		switch (ch) {
+		case 'X':
+			external_bus = true;
+			break;
+
+		case 'R':
+			rotation = (enum Rotation)atoi(optarg);
+			break;
+
+		default:
+			bmi160::usage();
+			exit(0);
+		}
+	}
+
+	const char *verb = argv[optind];
+
+	/*
+	 * Start/load the driver.
+
+	 */
+	if (!strcmp(verb, "start")) {
+		bmi160::start(external_bus, rotation);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		bmi160::stop(external_bus);
+	}
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(verb, "test")) {
+		bmi160::test(external_bus);
+	}
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(verb, "reset")) {
+		bmi160::reset(external_bus);
+	}
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(verb, "info")) {
+		bmi160::info(external_bus);
+	}
+
+	/*
+	 * Print register information.
+	 */
+	if (!strcmp(verb, "regdump")) {
+		bmi160::regdump(external_bus);
+	}
+
+	if (!strcmp(verb, "testerror")) {
+		bmi160::testerror(external_bus);
+	}
+
+	bmi160::usage();
+	exit(1);
+}
@@ -139,10 +139,10 @@ dma_alloc_init(void)
 					6); /* 64B alignment */

 	if (dma_allocator == NULL) {
-		message("[boot] DMA allocator setup FAILED");
+		message("DMA alloc FAILED");

 	} else {
-		g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations");
+		g_dma_perf = perf_alloc(PC_COUNT, "dma_alloc");
 	}
 }

@@ -140,10 +140,10 @@ dma_alloc_init(void)
 					6); /* 64B alignment */

 	if (dma_allocator == NULL) {
-		message("[boot] DMA allocator setup FAILED");
+		message("DMA alloc FAILED");

 	} else {
-		g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations");
+		g_dma_perf = perf_alloc(PC_COUNT, "dma_alloc");
 	}
 }

@@ -125,6 +125,7 @@ __BEGIN_DECLS
 #define PX4_SPIDEV_MPU		4
 #define PX4_SPIDEV_HMC		5
 #define PX4_SPIDEV_LIS		7
+#define	PX4_SPIDEV_BMI		8

 /* External bus */
 #define PX4_SPIDEV_EXT0		1
@@ -138,6 +139,8 @@ __BEGIN_DECLS
 #define PX4_SPIDEV_EXT_ACCEL_MAG	PX4_SPIDEV_EXT2
 #define PX4_SPIDEV_EXT_GYRO		PX4_SPIDEV_EXT3

+#define PX4_SPIDEV_EXT_BMI		PX4_SPIDEV_EXT_GYRO
+
 /* I2C busses */
 #define PX4_I2C_BUS_EXPANSION	1
 #define PX4_I2C_BUS_ONBOARD	2
@@ -140,10 +140,10 @@ dma_alloc_init(void)
 					6); /* 64B alignment */

 	if (dma_allocator == NULL) {
-		message("[boot] DMA allocator setup FAILED");
+		message("DMA alloc FAILED");

 	} else {
-		g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations");
+		g_dma_perf = perf_alloc(PC_COUNT, "dma_alloc");
 	}
 }

@@ -140,10 +140,10 @@ dma_alloc_init(void)
 					6); /* 64B alignment */

 	if (dma_allocator == NULL) {
-		message("[boot] DMA allocator setup FAILED");
+		message("DMA alloc FAILED");

 	} else {
-		g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations");
+		g_dma_perf = perf_alloc(PC_COUNT, "dma_alloc");
 	}
 }

@@ -116,7 +116,7 @@ private:
 	int 			_gpio_fd;

 	int 			_polarity;
-	int			_mode;
+	int				_mode;
 	float 			_activation_time;
 	float  			_interval;
 	float  			_distance;
@@ -229,9 +229,9 @@ CameraTrigger::CameraTrigger() :
 		i++;
 	}

-	struct camera_trigger_s	trigger = {};
+	struct camera_trigger_s	camera_trigger = {};

-	_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger);
+	_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &camera_trigger);
 }

 CameraTrigger::~CameraTrigger()
@@ -111,6 +111,26 @@ Integrator::put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integ
 	}
 }

+bool
+Integrator::put_with_interval(unsigned interval_us, math::Vector<3> &val, math::Vector<3> &integral,
+			      uint64_t &integral_dt)
+{
+	if (_last_integration_time == 0) {
+		/* this is the first item in the integrator */
+		uint64_t now = hrt_absolute_time();
+		_last_integration_time = now;
+		_last_reset_time = now;
+		_last_val = val;
+
+		return false;
+	}
+
+	// Create the timestamp artifically.
+	uint64_t timestamp = _last_integration_time + interval_us;
+
+	return put(timestamp, val, integral, integral_dt);
+}
+
 math::Vector<3>
 Integrator::get(bool reset, uint64_t &integral_dt)
 {
@@ -60,7 +60,22 @@ public:
 	 * @return		true if putting the item triggered an integral reset and the integral should be
 	 *			published.
 	 */
-	bool			put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t &integral_dt);
+	bool put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t &integral_dt);
+
+	/**
+	 * Put an item into the integral but provide an interval instead of a timestamp.
+	 *
+	 * @param interval_us	Interval in us since last integration.
+	 * @param val		Item to put.
+	 * @param integral	Current integral in case the integrator did reset, else the value will not be modified
+	 * @param integral_dt	Get the dt in us of the current integration (only if reset). Note that this
+	 *			values might not be accurate vs. hrt_absolute_time because it is just the sum of the
+	 *			supplied intervals.
+	 * @return		true if putting the item triggered an integral reset and the integral should be
+	 *			published.
+	 */
+	bool put_with_interval(unsigned interval_us, math::Vector<3> &val, math::Vector<3> &integral,
+			       uint64_t &integral_dt);

 	/**
 	 * Get the current integral and reset the integrator if needed.
@@ -63,10 +63,12 @@
 #define DRV_ACC_DEVTYPE_ACCELSIM 0x14
 #define DRV_ACC_DEVTYPE_GYROSIM  0x15
 #define DRV_ACC_DEVTYPE_MPU9250  0x16
+#define DRV_ACC_DEVTYPE_BMI160   0x17
 #define DRV_GYR_DEVTYPE_MPU6000  0x21
 #define DRV_GYR_DEVTYPE_L3GD20   0x22
 #define DRV_GYR_DEVTYPE_GYROSIM  0x23
 #define DRV_GYR_DEVTYPE_MPU9250  0x24
+#define DRV_GYR_DEVTYPE_BMI160   0x25
 #define DRV_RNG_DEVTYPE_MB12XX   0x31
 #define DRV_RNG_DEVTYPE_LL40LS   0x32

@@ -78,6 +80,7 @@
 #define DRV_GYR_DEVTYPE_MPU6500 0x21
 #endif

+
 /*
 * ioctl() definitions
 *
@@ -356,7 +356,7 @@ HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rota
 	_reports(nullptr),
 	_scale{},
 	_range_scale(0), /* default range scale from counts to gauss */
-	_range_ga(1.3f),
+	_range_ga(1.9f),
 	_collect_phase(false),
 	_class_instance(-1),
 	_orb_class_instance(-1),
@@ -792,8 +792,8 @@ HMC5883::stop()
 int
 HMC5883::reset()
 {
-	/* set range */
-	return set_range(_range_ga);
+	/* set range, ceil floating point number */
+	return set_range(_range_ga + 0.5f);
 }

 void
@@ -1110,7 +1110,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 	/* Set to 2.5 Gauss. We ask for 3 to get the right part of
 	 * the chained if statement above. */
 	if (OK != ioctl(filp, MAGIOCSRANGE, 3)) {
-		warnx("FAILED: MAGIOCSRANGE 3.3 Ga");
+		warnx("FAILED: MAGIOCSRANGE 2.5 Ga");
 		ret = 1;
 		goto out;
 	}
@@ -1157,8 +1157,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 		}
 	}

-	/* read the sensor up to 100x, stopping when we have 30 good values */
-	for (uint8_t i = 0; i < 100 && good_count < 30; i++) {
+	/* read the sensor up to 150x, stopping when we have 50 good values */
+	for (uint8_t i = 0; i < 150 && good_count < 50; i++) {
 		struct pollfd fds;

 		/* wait for data to be ready */
@@ -1185,9 +1185,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 				fabsf(expected_cal[2] / report.z)
 			       };

-		if (cal[0] > 0.7f && cal[0] < 1.35f &&
-		    cal[1] > 0.7f && cal[1] < 1.35f &&
-		    cal[2] > 0.7f && cal[2] < 1.35f) {
+		if (cal[0] > 0.3f && cal[0] < 1.7f &&
+		    cal[1] > 0.3f && cal[1] < 1.7f &&
+		    cal[2] > 0.3f && cal[2] < 1.7f) {
 			good_count++;
 			sum_excited[0] += cal[0];
 			sum_excited[1] += cal[1];
@@ -1220,9 +1220,9 @@ out:
 	}

 	/* set back to normal mode */
-	/* Set to 1.1 Gauss */
-	if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
-		warnx("FAILED: MAGIOCSRANGE 1.1 Ga");
+	/* Set to 1.9 Gauss */
+	if (OK != ::ioctl(fd, MAGIOCSRANGE, 2)) {
+		warnx("FAILED: MAGIOCSRANGE 1.9 Ga");
 	}

 	if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
@@ -383,8 +383,7 @@ PWMSim::task_main()
 	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));

 	/* advertise the mixed control outputs */
-	actuator_outputs_s outputs;
-	memset(&outputs, 0, sizeof(outputs));
+	actuator_outputs_s outputs = {};

 	/* advertise the mixed control outputs, insist on the first group output */
 	_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &outputs);
@@ -483,7 +482,6 @@ PWMSim::task_main()
 			}

 			/* do mixing */
-			actuator_outputs_s outputs = {};
 			num_outputs = _mixers->mix(&outputs.output[0], num_outputs, NULL);
 			outputs.noutputs = num_outputs;
 			outputs.timestamp = hrt_absolute_time();
@@ -861,17 +861,17 @@ PX4FMU::fill_rc_in(uint16_t raw_rc_count,

 		/* set RSSI if analog RSSI input is present */
 		if (_analog_rc_rssi_stable) {
-			float rssi = ((_analog_rc_rssi_volt - 0.2f) / 3.0f) * 100.0f;
+			float rssi_analog = ((_analog_rc_rssi_volt - 0.2f) / 3.0f) * 100.0f;

-			if (rssi > 100.0f) {
-				rssi = 100.0f;
+			if (rssi_analog > 100.0f) {
+				rssi_analog = 100.0f;
 			}

-			if (rssi < 0.0f) {
-				rssi = 0.0f;
+			if (rssi_analog < 0.0f) {
+				rssi_analog = 0.0f;
 			}

-			_rc_in.rssi = rssi;
+			_rc_in.rssi = rssi_analog;

 		} else {
 			_rc_in.rssi = 255;
@@ -301,6 +301,7 @@ private:

 	bool			_primary_pwm_device;	///< true if we are the default PWM output
 	bool			_lockdown_override;	///< allow to override the safety lockdown
+	bool			_armed;			///< wether the system is armed

 	float			_battery_amp_per_volt;	///< current sensor amps/volt
 	float			_battery_amp_bias;	///< current sensor bias
@@ -537,6 +538,7 @@ PX4IO::PX4IO(device::Device *interface) :
 	_servorail_status{},
 	_primary_pwm_device(false),
 	_lockdown_override(false),
+	_armed(false),
 	_battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor
 	_battery_amp_bias(0),
 	_battery_mamphour_total(0),
@@ -685,7 +687,7 @@ PX4IO::init()
 	_max_transfer  = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2;
 	_max_rc_input  = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT);

-	if ((_max_actuators < 1) || (_max_actuators > 255) ||
+	if ((_max_actuators < 1) || (_max_actuators > 16) ||
 	    (_max_relays > 32)   ||
 	    (_max_transfer < 16) || (_max_transfer > 255)  ||
 	    (_max_rc_input < 1)  || (_max_rc_input > 255)) {
@@ -900,7 +902,7 @@ PX4IO::init()
 	_task = px4_task_spawn_cmd("px4io",
 				   SCHED_DEFAULT,
 				   SCHED_PRIORITY_ACTUATOR_OUTPUTS,
-				   1500,
+				   1400,
 				   (main_t)&PX4IO::task_main_trampoline,
 				   nullptr);

@@ -1348,6 +1350,8 @@ PX4IO::io_set_arming_state()
 			clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
 		}

+		_armed = armed.armed;
+
 		if (armed.lockdown && !_lockdown_override) {
 			set |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
 			_lockdown_override = true;
@@ -1683,7 +1687,7 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
 	float current_a = ibatt * (3.3f / 4096.0f) * _battery_amp_per_volt;
 	current_a += _battery_amp_bias;

-	_battery.updateBatteryStatus(timestamp, voltage_v, current_a, _last_throttle, &battery_status);
+	_battery.updateBatteryStatus(timestamp, voltage_v, current_a, _last_throttle, _armed, &battery_status);

 	/* the announced battery status would conflict with the simulated battery status in HIL */
 	if (!(_pub_blocked)) {
@@ -2471,16 +2475,19 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 			break;
 		}

-	case PWM_SERVO_GET_FAILSAFE_PWM:
+	case PWM_SERVO_GET_FAILSAFE_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+			pwm->channel_count = _max_actuators;

-		ret = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, 0, (uint16_t *)arg, _max_actuators);
+			ret = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, _max_actuators);

-		if (ret != OK) {
-			ret = -EIO;
+			if (ret != OK) {
+				ret = -EIO;
+			}
+
+			break;
 		}

-		break;
-
 	case PWM_SERVO_SET_DISARMED_PWM: {
 			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;

@@ -2495,16 +2502,19 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 			break;
 		}

-	case PWM_SERVO_GET_DISARMED_PWM:
+	case PWM_SERVO_GET_DISARMED_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+			pwm->channel_count = _max_actuators;

-		ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, (uint16_t *)arg, _max_actuators);
+			ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, _max_actuators);

-		if (ret != OK) {
-			ret = -EIO;
+			if (ret != OK) {
+				ret = -EIO;
+			}
+
+			break;
 		}

-		break;
-
 	case PWM_SERVO_SET_MIN_PWM: {
 			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;

@@ -2519,16 +2529,19 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 			break;
 		}

-	case PWM_SERVO_GET_MIN_PWM:
+	case PWM_SERVO_GET_MIN_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+			pwm->channel_count = _max_actuators;

-		ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, (uint16_t *)arg, _max_actuators);
+			ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, _max_actuators);

-		if (ret != OK) {
-			ret = -EIO;
+			if (ret != OK) {
+				ret = -EIO;
+			}
+
+			break;
 		}

-		break;
-
 	case PWM_SERVO_SET_MAX_PWM: {
 			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;

@@ -2543,12 +2556,15 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 			break;
 		}

-	case PWM_SERVO_GET_MAX_PWM:
+	case PWM_SERVO_GET_MAX_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+			pwm->channel_count = _max_actuators;

-		ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, (uint16_t *)arg, _max_actuators);
+			ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, _max_actuators);

-		if (ret != OK) {
-			ret = -EIO;
+			if (ret != OK) {
+				ret = -EIO;
+			}
 		}

 		break;
@@ -180,7 +180,8 @@ PX4IO_serial::PX4IO_serial() :
 	_rx_dma_status(_dma_status_inactive),
 	_bus_semaphore(SEM_INITIALIZER(0)),
 	_completion_semaphore(SEM_INITIALIZER(0)),
-	_pc_txns(perf_alloc(PC_ELAPSED, "io_txns     ")),
+	_pc_txns(perf_alloc(PC_ELAPSED, "io_txns")),
+#if 0
 	_pc_dmasetup(perf_alloc(PC_ELAPSED,	"io_dmasetup ")),
 	_pc_retries(perf_alloc(PC_COUNT,	"io_retries  ")),
 	_pc_timeouts(perf_alloc(PC_COUNT,	"io_timeouts ")),
@@ -190,6 +191,17 @@ PX4IO_serial::PX4IO_serial() :
 	_pc_uerrs(perf_alloc(PC_COUNT,		"io_uarterrs ")),
 	_pc_idle(perf_alloc(PC_COUNT,		"io_idle     ")),
 	_pc_badidle(perf_alloc(PC_COUNT,	"io_badidle  "))
+#else
+	_pc_dmasetup(nullptr),
+	_pc_retries(nullptr),
+	_pc_timeouts(nullptr),
+	_pc_crcerrs(nullptr),
+	_pc_dmaerrs(nullptr),
+	_pc_protoerrs(nullptr),
+	_pc_uerrs(nullptr),
+	_pc_idle(nullptr),
+	_pc_badidle(nullptr)
+#endif
 {
 	g_interface = this;
 }
@@ -35,6 +35,8 @@ px4_add_module(
 	MAIN snapdragon_rc_pwm
 	COMPILE_FLAGS
 		-Os
+		-Wno-attributes
+		-Wno-packed
 	SRCS
 		snapdragon_rc_pwm.cpp
 	DEPENDS
@@ -35,6 +35,7 @@ px4_add_module(
 	MAIN uart_esc
 	COMPILE_FLAGS
 		-Os
+		-Wno-packed
 	SRCS
 		uart_esc.cpp
 	DEPENDS
@@ -4,7 +4,7 @@ px4_posix_generate_builtin_commands(
 	OUT apps.h
 	MODULE_LIST ${module_libraries})

-if ("${BOARD}" STREQUAL "eagle")
+if ("${BOARD}" STREQUAL "eagle" OR ("${BOARD}" STREQUAL "excelsior"))

 	include(fastrpc)
 	include(linux_app)
@@ -249,5 +249,11 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
 			z = -0.932324f * tmpx +  0.361625f * tmpy +  0.000000f * tmpz;
 			return;
 		}
+
+	case ROTATION_PITCH_90_ROLL_270: {
+			tmp = x; x = -y;
+			y = z; z = -tmp;
+			return;
+		}
 	}
 }
@@ -79,6 +79,7 @@ enum Rotation {
 	ROTATION_PITCH_90_YAW_180    = 28,
 	ROTATION_PITCH_90_ROLL_90	 = 29,
 	ROTATION_YAW_293_PITCH_68_ROLL_90 = 30,
+	ROTATION_PITCH_90_ROLL_270	 = 31,
 	ROTATION_MAX
 };

@@ -120,6 +121,7 @@ const rot_lookup_t rot_lookup[] = {
 	{  0,  90, 180 },
 	{ 90,  90,   0 },
 	{ 90,  68, 293 },
+	{270,  90,   0 },
 };

 /**
@@ -606,9 +606,9 @@ sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num
 		chancount = 18;

 		/* channel 17 (index 16) */
-		values[16] = (frame[SBUS_FLAGS_BYTE] & (1 << 0)) * 1000 + 998;
+		values[16] = (((frame[SBUS_FLAGS_BYTE] & (1 << 0)) > 0) ? 1 : 0) * 1000 + 998;
 		/* channel 18 (index 17) */
-		values[17] = (frame[SBUS_FLAGS_BYTE] & (1 << 1)) * 1000 + 998;
+		values[17] = (((frame[SBUS_FLAGS_BYTE] & (1 << 1)) > 0) ? 1 : 0) * 1000 + 998;
 	}

 	/* note the number of channels decoded */
@@ -75,6 +75,11 @@
 #define	HW_ARCH "LINUXTEST"
 #endif

+#ifdef CONFIG_ARCH_BOARD_EXCELSIOR
+#define HW_ARCH "LINUXTEST"
+#endif
+
+
 #ifdef CONFIG_ARCH_BOARD_RPI2
 #define	HW_ARCH "LINUXTEST"
 #endif
@@ -174,6 +174,7 @@ BottleDrop::BottleDrop() :

 	_task_should_exit(false),
 	_main_task(-1),
+	_mavlink_log_pub(nullptr),
 	_command_sub(-1),
 	_wind_estimate_sub(-1),
 	_command {},
@@ -36,6 +36,8 @@ px4_add_module(
 	STACK_MAIN 4096
 	STACK_MAX 2450
 	COMPILE_FLAGS
+		-Wno-attributes
+		-Wno-packed
 		-Os
 	SRCS
 		commander.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2016 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
@@ -192,12 +192,12 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 		goto error_return;
 	}

-	calibration_log_critical(mavlink_log_pub, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
+	calibration_log_info(mavlink_log_pub, "[cal] Offset of %d Pascal", (int)diff_pres_offset);

 	/* wait 500 ms to ensure parameter propagated through the system */
 	usleep(500 * 1000);

-	calibration_log_critical(mavlink_log_pub, "[cal] Create airflow now");
+	calibration_log_critical(mavlink_log_pub, "[cal] Blow across front of pitot without touching");

 	calibration_counter = 0;

@@ -138,7 +138,7 @@ extern struct system_load_s system_load;
 static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60;		/**< Maximum percentage signal to noise ratio allowed for GPS reception */

 /* Decouple update interval and hysteris counters, all depends on intervals */
-#define COMMANDER_MONITORING_INTERVAL 50000
+#define COMMANDER_MONITORING_INTERVAL 10000
 #define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))

 #define MAVLINK_OPEN_INTERVAL 50000
@@ -210,7 +210,8 @@ static struct commander_state_s internal_state = {};

 static uint8_t main_state_before_rtl = commander_state_s::MAIN_STATE_MAX;
 static unsigned _last_mission_instance = 0;
-static manual_control_setpoint_s _last_sp_man = {};
+struct manual_control_setpoint_s sp_man = {};		///< the current manual control setpoint
+static manual_control_setpoint_s _last_sp_man = {};	///< the manual control setpoint valid at the last mode switch

 static struct vtol_vehicle_status_s vtol_status = {};

@@ -262,7 +263,7 @@ 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);
+transition_result_t set_main_state_rc(struct vehicle_status_s *status);

 void set_control_mode();

@@ -677,7 +678,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 		// the data at the exact same time.

 		// Check if a mode switch had been requested
-		if ((((uint8_t)cmd->param1) & 1) > 0) {
+		if ((((uint32_t)cmd->param2) & 1) > 0) {
 			transition_result_t main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state);

 			if ((main_ret != TRANSITION_DENIED)) {
@@ -837,12 +838,20 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 				else {

 					// Refuse to arm if preflight checks have failed
-					if ((!status.hil_state) != vehicle_status_s::HIL_STATE_ON && !status_flags.condition_system_sensors_initialized) {
+					if ((!status_local->hil_state) != vehicle_status_s::HIL_STATE_ON && !status_flags.condition_system_sensors_initialized) {
 						mavlink_log_critical(&mavlink_log_pub, "Arming DENIED. Preflight checks have failed.");
 						cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
 						break;
 					}

+					// Refuse to arm if in manual with non-zero throttle
+					if ((status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL || status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_STAB ||
+						status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO) && sp_man.z > 0.1f) {
+						mavlink_log_critical(&mavlink_log_pub, "Arming DENIED. Manual throttle non-zero.");
+						cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
+						break;
+					}
+
 				}

 				transition_result_t arming_res = arm_disarm(cmd_arms,&mavlink_log_pub,  "arm/disarm component command");
@@ -1032,9 +1041,9 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 	case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: {
 			/* ok, home set, use it to take off */
 			if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, main_state_prev, &status_flags, &internal_state)) {
-				warnx("taking off!");
+				mavlink_and_console_log_info(&mavlink_log_pub, "Taking off");
 			} else {
-				warnx("takeoff denied");
+				mavlink_and_console_log_critical(&mavlink_log_pub, "Takeoff denied, disarm and re-try");
 			}

 		}
@@ -1043,9 +1052,9 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 	case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
 			/* ok, home set, use it to take off */
 			if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_LAND, main_state_prev, &status_flags, &internal_state)) {
-				warnx("landing!");
+				mavlink_and_console_log_info(&mavlink_log_pub, "Landing at current position");
 			} else {
-				warnx("landing denied");
+				mavlink_and_console_log_critical(&mavlink_log_pub, "Landing denied, land manually.");
 			}

 		}
@@ -1384,7 +1393,6 @@ int commander_thread_main(int argc, char *argv[])

 	/* Subscribe to manual control data */
 	int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	struct manual_control_setpoint_s sp_man;
 	memset(&sp_man, 0, sizeof(sp_man));

 	/* Subscribe to offboard control data */
@@ -1700,7 +1708,7 @@ int commander_thread_main(int argc, char *argv[])
 				    !armed.armed) {

 					bool chAirspeed = false;
-					bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
+					hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;

 					/* Perform airspeed check only if circuit breaker is not
 					 * engaged and it's not a rotary wing
@@ -2267,7 +2275,7 @@ int commander_thread_main(int argc, char *argv[])
 			main_state_before_rtl == commander_state_s::MAIN_STATE_STAB)) {

 			// transition to previous state if sticks are increased
-			const float min_stick_change = 0.2;
+			const float min_stick_change = 0.2f;
 			if ((_last_sp_man.timestamp != sp_man.timestamp) &&
 				((fabsf(sp_man.x) - fabsf(_last_sp_man.x) > min_stick_change) ||
 				 (fabsf(sp_man.y) - fabsf(_last_sp_man.y) > min_stick_change) ||
@@ -2446,7 +2454,7 @@ int commander_thread_main(int argc, char *argv[])

 			/* evaluate the main state machine according to mode switches */
 			bool first_rc_eval = (_last_sp_man.timestamp == 0) && (sp_man.timestamp > 0);
-			transition_result_t main_res = set_main_state_rc(&status, &sp_man);
+			transition_result_t main_res = set_main_state_rc(&status);

 			/* play tune on mode change only if armed, blink LED always */
 			if (main_res == TRANSITION_CHANGED || first_rc_eval) {
@@ -2582,15 +2590,12 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}

-		/* reset main state after takeoff or land has been completed */
-		/* only switch back to at least altitude controlled modes */
-		if (main_state_prev == commander_state_s::MAIN_STATE_POSCTL ||
-			main_state_prev == commander_state_s::MAIN_STATE_ALTCTL) {
+		/* reset main state after takeoff has completed */
+		/* only switch back to posctl */
+		if (main_state_prev == commander_state_s::MAIN_STATE_POSCTL) {

-			if ((internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF
-					&& mission_result.finished) ||
-				(internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND
-					&& land_detector.landed)) {
+			if (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF
+					&& mission_result.finished) {

 				main_state_transition(&status, main_state_prev, main_state_prev, &status_flags, &internal_state);
 			}
@@ -2874,7 +2879,7 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val
 }

 void
-control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed, battery_status_s *battery)
+control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed, battery_status_s *battery_status)
 {
 	/* driving rgbled */
 	if (changed) {
@@ -2907,9 +2912,9 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
 			/* set color */
 			if (status.failsafe) {
 				rgbled_set_color(RGBLED_COLOR_PURPLE);
-			} else if (battery->warning == battery_status_s::BATTERY_WARNING_LOW) {
+			} else if (battery_status->warning == battery_status_s::BATTERY_WARNING_LOW) {
 				rgbled_set_color(RGBLED_COLOR_AMBER);
-			} else if (battery->warning == battery_status_s::BATTERY_WARNING_CRITICAL) {
+			} else if (battery_status->warning == battery_status_s::BATTERY_WARNING_CRITICAL) {
 				rgbled_set_color(RGBLED_COLOR_RED);
 			} else {
 				if (status_flags.condition_home_position_valid && status_flags.condition_global_position_valid) {
@@ -2958,7 +2963,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
 }

 transition_result_t
-set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_setpoint_s *sp_man)
+set_main_state_rc(struct vehicle_status_s *status_local)
 {
 	/* set main state according to RC switches */
 	transition_result_t res = TRANSITION_DENIED;
@@ -2971,31 +2976,34 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 	}

 	/* manual setpoint has not updated, do not re-evaluate it */
-	if (((_last_sp_man.timestamp != 0) && (_last_sp_man.timestamp == sp_man->timestamp)) ||
-		((_last_sp_man.offboard_switch == sp_man->offboard_switch) &&
-		 (_last_sp_man.return_switch == sp_man->return_switch) &&
-		 (_last_sp_man.mode_switch == sp_man->mode_switch) &&
-		 (_last_sp_man.acro_switch == sp_man->acro_switch) &&
-		 (_last_sp_man.rattitude_switch == sp_man->rattitude_switch) &&
-		 (_last_sp_man.posctl_switch == sp_man->posctl_switch) &&
-		 (_last_sp_man.loiter_switch == sp_man->loiter_switch) &&
-		 (_last_sp_man.mode_slot == sp_man->mode_slot))) {
+	if (((_last_sp_man.timestamp != 0) && (_last_sp_man.timestamp == sp_man.timestamp)) ||
+		((_last_sp_man.offboard_switch == sp_man.offboard_switch) &&
+		 (_last_sp_man.return_switch == sp_man.return_switch) &&
+		 (_last_sp_man.mode_switch == sp_man.mode_switch) &&
+		 (_last_sp_man.acro_switch == sp_man.acro_switch) &&
+		 (_last_sp_man.rattitude_switch == sp_man.rattitude_switch) &&
+		 (_last_sp_man.posctl_switch == sp_man.posctl_switch) &&
+		 (_last_sp_man.loiter_switch == sp_man.loiter_switch) &&
+		 (_last_sp_man.mode_slot == sp_man.mode_slot))) {

 		// update these fields for the geofence system
-		_last_sp_man.timestamp = sp_man->timestamp;
-		_last_sp_man.x = sp_man->x;
-		_last_sp_man.y = sp_man->y;
-		_last_sp_man.z = sp_man->z;
-		_last_sp_man.r = sp_man->r;
+
+		if (!rtl_on) {
+			_last_sp_man.timestamp = sp_man.timestamp;
+			_last_sp_man.x = sp_man.x;
+			_last_sp_man.y = sp_man.y;
+			_last_sp_man.z = sp_man.z;
+			_last_sp_man.r = sp_man.r;
+		}

 		/* no timestamp change or no switch change -> nothing changed */
 		return TRANSITION_NOT_CHANGED;
 	}

-	_last_sp_man = *sp_man;
+	_last_sp_man = sp_man;

 	/* offboard switch overrides main switch */
-	if (sp_man->offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
+	if (sp_man.offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
 		res = main_state_transition(status_local, commander_state_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags, &internal_state);

 		if (res == TRANSITION_DENIED) {
@@ -3009,7 +3017,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 	}

 	/* RTL switch overrides main switch */
-	if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
+	if (sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
 		warnx("RTL switch changed and ON!");
 		res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags, &internal_state);

@@ -3029,14 +3037,14 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 	}

 	/* we know something has changed - check if we are in mode slot operation */
-	if (sp_man->mode_slot != manual_control_setpoint_s::MODE_SLOT_NONE) {
+	if (sp_man.mode_slot != manual_control_setpoint_s::MODE_SLOT_NONE) {

-		if (sp_man->mode_slot >= sizeof(_flight_mode_slots) / sizeof(_flight_mode_slots[0])) {
-			warnx("overflow");
+		if (sp_man.mode_slot >= sizeof(_flight_mode_slots) / sizeof(_flight_mode_slots[0])) {
+			warnx("m slot overflow");
 			return TRANSITION_DENIED;
 		}

-		int new_mode = _flight_mode_slots[sp_man->mode_slot];
+		int new_mode = _flight_mode_slots[sp_man.mode_slot];

 		if (new_mode < 0) {
 			/* slot is unused */
@@ -3168,13 +3176,13 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 	}

 	/* offboard and RTL switches off or denied, check main mode switch */
-	switch (sp_man->mode_switch) {
+	switch (sp_man.mode_switch) {
 	case manual_control_setpoint_s::SWITCH_POS_NONE:
 		res = TRANSITION_NOT_CHANGED;
 		break;

 	case manual_control_setpoint_s::SWITCH_POS_OFF:		// MANUAL
-		if (sp_man->acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
+		if (sp_man.acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {

 			/* manual mode is stabilized already for multirotors, so switch to acro
 			 * for any non-manual mode
@@ -3189,7 +3197,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 			}

 		}
-		else if(sp_man->rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON){
+		else if(sp_man.rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON){
 			/* Similar to acro transitions for multirotors.  FW aircraft don't need a
 			 * rattitude mode.*/
 			if (status.is_rotary_wing) {
@@ -3205,7 +3213,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 		break;

 	case manual_control_setpoint_s::SWITCH_POS_MIDDLE:		// ASSIST
-		if (sp_man->posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
+		if (sp_man.posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
 			res = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state);

 			if (res != TRANSITION_DENIED) {
@@ -3222,7 +3230,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 			break;	// changed successfully or already in this mode
 		}

-		if (sp_man->posctl_switch != manual_control_setpoint_s::SWITCH_POS_ON) {
+		if (sp_man.posctl_switch != manual_control_setpoint_s::SWITCH_POS_ON) {
 			print_reject_mode(status_local, "ALTITUDE CONTROL");
 		}

@@ -3232,7 +3240,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->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, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state);

 			if (res != TRANSITION_DENIED) {
@@ -88,13 +88,17 @@ bool is_multirotor(const struct vehicle_status_s *current_status)
 bool is_rotary_wing(const struct vehicle_status_s *current_status)
 {
 	return is_multirotor(current_status) || (current_status->system_type == MAV_TYPE_HELICOPTER)
-	       || (current_status->system_type == MAV_TYPE_COAXIAL);
+		   || (current_status->system_type == MAV_TYPE_COAXIAL);
 }

 bool is_vtol(const struct vehicle_status_s * current_status) {
 	return (current_status->system_type == MAV_TYPE_VTOL_DUOROTOR ||
 		current_status->system_type == MAV_TYPE_VTOL_QUADROTOR ||
-		current_status->system_type == MAV_TYPE_VTOL_TILTROTOR);
+		current_status->system_type == MAV_TYPE_VTOL_TILTROTOR ||
+		current_status->system_type == MAV_TYPE_VTOL_RESERVED2 ||
+		current_status->system_type == MAV_TYPE_VTOL_RESERVED3 ||
+		current_status->system_type == MAV_TYPE_VTOL_RESERVED4 ||
+		current_status->system_type == MAV_TYPE_VTOL_RESERVED5);
 }

 static hrt_abstime blink_msg_end = 0;	// end time for currently blinking LED message, 0 if no blink message
@@ -91,7 +91,7 @@ int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* a
 	bool	batt_connected = false;

 	hrt_abstime battery_connect_wait_timeout = 30000000;
-	hrt_abstime pwm_high_timeout = 10000000;
+	hrt_abstime pwm_high_timeout = 4000000;
 	hrt_abstime timeout_start;

 	calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc");
@@ -69,11 +69,11 @@ static const int ERROR = -1;
 static const char *sensor_name = "mag";
 static constexpr unsigned max_mags = 3;
 static constexpr float mag_sphere_radius = 0.2f;
-static constexpr unsigned int calibration_sides = 3;			///< The total number of sides
+static unsigned int calibration_sides = 6;			///< The total number of sides
 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

-static constexpr float MAG_MAX_OFFSET_LEN = 0.75f;	///< The maximum measurement range is ~1.4 Ga, the earth field is ~0.6 Ga, so an offset larger than ~0.8-0.6 Ga means the mag will saturate in some directions.
+static constexpr float MAG_MAX_OFFSET_LEN = 1.3f;	///< The maximum measurement range is ~1.9 Ga, the earth field is ~0.6 Ga, so an offset larger than ~1.3 Ga means the mag will saturate in some directions.

 int32_t	device_ids[max_mags];
 bool internal[max_mags];
@@ -81,7 +81,7 @@ int device_prio_max = 0;
 int32_t device_id_primary = 0;
 static unsigned _last_mag_progress = 0;

-calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t (&device_ids)[max_mags]);
+calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub);

 /// Data passed to calibration worker routine
 typedef struct  {
@@ -117,7 +117,7 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)

 	char str[30];

-	for (size_t i=0; i<max_mags; i++) {
+	for (size_t i=0; i < max_mags; i++) {
 		device_ids[i] = 0; // signals no mag
 	}

@@ -202,7 +202,7 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)

 	// Calibrate all mags at the same time
 	if (result == OK) {
-		switch (mag_calibrate_all(mavlink_log_pub, device_ids)) {
+		switch (mag_calibrate_all(mavlink_log_pub)) {
 			case calibrate_return_cancelled:
 				// Cancel message already displayed, we're done here
 				result = ERROR;
@@ -435,7 +435,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 	return result;
 }

-calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t (&device_ids)[max_mags])
+calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
 {
 	calibrate_return result = calibrate_return_ok;

@@ -447,40 +447,32 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t (&devi
 	worker_data.calibration_interval_perside_seconds = calibraton_duration_seconds / calibration_sides;
 	worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;

-	// 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;
+	// Collect: As defined by configuration
+	// start with a full mask, all six bits set
+	uint32_t cal_mask = (1 << 6) - 1;
+	param_get(param_find("CAL_MAG_SIDES"), &cal_mask);

-	calibration_log_info(mavlink_log_pub,
-		"[cal] %s side done, rotate to a different side",
-		detect_orientation_str(DETECT_ORIENTATION_TAIL_DOWN));
-	usleep(100000);
-	calibration_log_info(mavlink_log_pub,
-		"[cal] %s side done, rotate to a different side",
-		detect_orientation_str(DETECT_ORIENTATION_TAIL_DOWN));
-	usleep(100000);
-	calibration_log_info(mavlink_log_pub,
-		"[cal] %s side done, rotate to a different side",
-		detect_orientation_str(DETECT_ORIENTATION_UPSIDE_DOWN));
-	usleep(100000);
-	calibration_log_info(mavlink_log_pub,
-		"[cal] %s side done, rotate to a different side",
-		detect_orientation_str(DETECT_ORIENTATION_UPSIDE_DOWN));
-	usleep(100000);
-	calibration_log_info(mavlink_log_pub,
-		"[cal] %s side done, rotate to a different side",
-		detect_orientation_str(DETECT_ORIENTATION_RIGHT));
-	usleep(100000);
-	calibration_log_info(mavlink_log_pub,
-		"[cal] %s side done, rotate to a different side",
-		detect_orientation_str(DETECT_ORIENTATION_RIGHT));
-	usleep(100000);
+	calibration_sides = 0;

-	for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
+	for (unsigned i = 0; i < (sizeof(worker_data.side_data_collected) /
+		sizeof(worker_data.side_data_collected[0])); i++) {
+
+		if ((cal_mask & (1 << i)) > 0) {
+			// mark as missing
+			worker_data.side_data_collected[i] = false;
+			calibration_sides++;
+		} else {
+			// mark as completed from the beginning
+			worker_data.side_data_collected[i] = true;
+
+			calibration_log_info(mavlink_log_pub,
+				"[cal] %s side done, rotate to a different side",
+				detect_orientation_str(static_cast<enum detect_orientation_return>(i)));
+			usleep(100000);
+		}
+	}
+
+	for (size_t cur_mag = 0; cur_mag<max_mags; cur_mag++) {
 		// Initialize to no subscription
 		worker_data.sub_mag[cur_mag] = -1;

@@ -606,8 +598,8 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t (&devi
 				if (fabsf(sphere_x[cur_mag]) > MAG_MAX_OFFSET_LEN ||
 					fabsf(sphere_y[cur_mag]) > MAG_MAX_OFFSET_LEN ||
 					fabsf(sphere_z[cur_mag]) > MAG_MAX_OFFSET_LEN) {
-					calibration_log_emergency(mavlink_log_pub, "ERROR: Replace %s mag fault", (internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
-					calibration_log_info(mavlink_log_pub, "Excessive offsets: %8.4f, %8.4f, %8.4f, #%u", (double)sphere_x[cur_mag],
+					calibration_log_critical(mavlink_log_pub, "Warning: %s mag with large offsets", (internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
+					calibration_log_info(mavlink_log_pub, "Offsets: x: %8.4f, y: %8.4f, z: %8.4f, #%u", (double)sphere_x[cur_mag],
 						(double)sphere_y[cur_mag], (double)sphere_z[cur_mag], cur_mag);
 					result = calibrate_return_ok;
 				}
@@ -58,7 +58,6 @@
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
-#include <systemlib/mavlink_log.h>
 #include <mathlib/mathlib.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
 #include <platforms/px4_defines.h>
@@ -159,90 +158,90 @@ private:
 	math::LowPassFilter2p _lp_pitch_rate;
 	math::LowPassFilter2p _lp_yaw_rate;

-	EstimatorInterface *_ekf;
+	Ekf _ekf;

 	parameters *_params;	// pointer to ekf parameter struct (located in _ekf class instance)

-	control::BlockParamFloat *_mag_delay_ms;
-	control::BlockParamFloat *_baro_delay_ms;
-	control::BlockParamFloat *_gps_delay_ms;
-	control::BlockParamFloat *_flow_delay_ms;
-	control::BlockParamFloat *_rng_delay_ms;
-	control::BlockParamFloat *_airspeed_delay_ms;
+	control::BlockParamFloat _mag_delay_ms;
+	control::BlockParamFloat _baro_delay_ms;
+	control::BlockParamFloat _gps_delay_ms;
+	control::BlockParamFloat _flow_delay_ms;
+	control::BlockParamFloat _rng_delay_ms;
+	control::BlockParamFloat _airspeed_delay_ms;

-	control::BlockParamFloat *_gyro_noise;
-	control::BlockParamFloat *_accel_noise;
+	control::BlockParamFloat _gyro_noise;
+	control::BlockParamFloat _accel_noise;

 	// process noise
-	control::BlockParamFloat *_gyro_bias_p_noise;
-	control::BlockParamFloat *_accel_bias_p_noise;
-	control::BlockParamFloat *_gyro_scale_p_noise;
-	control::BlockParamFloat *_mage_p_noise;
-	control::BlockParamFloat *_magb_p_noise;
-	control::BlockParamFloat *_wind_vel_p_noise;
-	control::BlockParamFloat *_terrain_p_noise;	// terrain offset state random walk (m/s)
-	control::BlockParamFloat *_terrain_gradient;	// magnitude of terrain gradient (m/m)
+	control::BlockParamFloat _gyro_bias_p_noise;
+	control::BlockParamFloat _accel_bias_p_noise;
+	control::BlockParamFloat _gyro_scale_p_noise;
+	control::BlockParamFloat _mage_p_noise;
+	control::BlockParamFloat _magb_p_noise;
+	control::BlockParamFloat _wind_vel_p_noise;
+	control::BlockParamFloat _terrain_p_noise;	// terrain offset state random walk (m/s)
+	control::BlockParamFloat _terrain_gradient;	// magnitude of terrain gradient (m/m)

-	control::BlockParamFloat *_gps_vel_noise;
-	control::BlockParamFloat *_gps_pos_noise;
-	control::BlockParamFloat *_pos_noaid_noise;
-	control::BlockParamFloat *_baro_noise;
-	control::BlockParamFloat *_baro_innov_gate;     // innovation gate for barometric height innovation test (std dev)
-	control::BlockParamFloat *_posNE_innov_gate;    // innovation gate for GPS horizontal position innovation test (std dev)
-	control::BlockParamFloat *_vel_innov_gate;      // innovation gate for GPS velocity innovation test (std dev)
+	control::BlockParamFloat _gps_vel_noise;
+	control::BlockParamFloat _gps_pos_noise;
+	control::BlockParamFloat _pos_noaid_noise;
+	control::BlockParamFloat _baro_noise;
+	control::BlockParamFloat _baro_innov_gate;     // innovation gate for barometric height innovation test (std dev)
+	control::BlockParamFloat _posNE_innov_gate;    // innovation gate for GPS horizontal position innovation test (std dev)
+	control::BlockParamFloat _vel_innov_gate;      // innovation gate for GPS velocity innovation test (std dev)

-	control::BlockParamFloat *_mag_heading_noise;	// measurement noise used for simple heading fusion
-	control::BlockParamFloat *_mag_noise;           // measurement noise used for 3-axis magnetoemter fusion (Gauss)
-	control::BlockParamFloat *_eas_noise;			// measurement noise used for airspeed fusion (std m/s)
-	control::BlockParamFloat *_mag_declination_deg;	// magnetic declination in degrees
-	control::BlockParamFloat *_heading_innov_gate;	// innovation gate for heading innovation test
-	control::BlockParamFloat *_mag_innov_gate;	// innovation gate for magnetometer innovation test
+	control::BlockParamFloat _mag_heading_noise;	// measurement noise used for simple heading fusion
+	control::BlockParamFloat _mag_noise;           // measurement noise used for 3-axis magnetoemter fusion (Gauss)
+	control::BlockParamFloat _eas_noise;			// measurement noise used for airspeed fusion (std m/s)
+	control::BlockParamFloat _mag_declination_deg;	// magnetic declination in degrees
+	control::BlockParamFloat _heading_innov_gate;	// innovation gate for heading innovation test
+	control::BlockParamFloat _mag_innov_gate;	// innovation gate for magnetometer innovation test
 	control::BlockParamInt
-	*_mag_decl_source;       // bitmasked integer used to control the handling of magnetic declination
-	control::BlockParamInt *_mag_fuse_type;         // integer ued to control the type of magnetometer fusion used
+	_mag_decl_source;       // bitmasked integer used to control the handling of magnetic declination
+	control::BlockParamInt _mag_fuse_type;         // integer ued to control the type of magnetometer fusion used

-	control::BlockParamInt *_gps_check_mask;        // bitmasked integer used to activate the different GPS quality checks
-	control::BlockParamFloat *_requiredEph;         // maximum acceptable horiz position error (m)
-	control::BlockParamFloat *_requiredEpv;         // maximum acceptable vert position error (m)
-	control::BlockParamFloat *_requiredSacc;        // maximum acceptable speed error (m/s)
-	control::BlockParamInt *_requiredNsats;         // minimum acceptable satellite count
-	control::BlockParamFloat *_requiredGDoP;        // maximum acceptable geometric dilution of precision
-	control::BlockParamFloat *_requiredHdrift;      // maximum acceptable horizontal drift speed (m/s)
-	control::BlockParamFloat *_requiredVdrift;      // maximum acceptable vertical drift speed (m/s)
-	control::BlockParamInt *_param_record_replay_msg; // indicates if we want to record ekf2 replay messages
+	control::BlockParamInt _gps_check_mask;        // bitmasked integer used to activate the different GPS quality checks
+	control::BlockParamFloat _requiredEph;         // maximum acceptable horiz position error (m)
+	control::BlockParamFloat _requiredEpv;         // maximum acceptable vert position error (m)
+	control::BlockParamFloat _requiredSacc;        // maximum acceptable speed error (m/s)
+	control::BlockParamInt _requiredNsats;         // minimum acceptable satellite count
+	control::BlockParamFloat _requiredGDoP;        // maximum acceptable geometric dilution of precision
+	control::BlockParamFloat _requiredHdrift;      // maximum acceptable horizontal drift speed (m/s)
+	control::BlockParamFloat _requiredVdrift;      // maximum acceptable vertical drift speed (m/s)
+	control::BlockParamInt _param_record_replay_msg; // indicates if we want to record ekf2 replay messages

 	// measurement source control
 	control::BlockParamInt
-	*_fusion_mode;		// bitmasked integer that selects which of the GPS and optical flow aiding sources will be used
-	control::BlockParamInt *_vdist_sensor_type;	// selects the primary source for height data
+	_fusion_mode;		// bitmasked integer that selects which of the GPS and optical flow aiding sources will be used
+	control::BlockParamInt _vdist_sensor_type;	// selects the primary source for height data

 	// range finder fusion
-	control::BlockParamFloat *_range_noise;		// observation noise for range finder measurements (m)
-	control::BlockParamFloat *_range_innov_gate;	// range finder fusion innovation consistency gate size (STD)
-	control::BlockParamFloat *_rng_gnd_clearance;	// minimum valid value for range when on ground (m)
+	control::BlockParamFloat _range_noise;		// observation noise for range finder measurements (m)
+	control::BlockParamFloat _range_innov_gate;	// range finder fusion innovation consistency gate size (STD)
+	control::BlockParamFloat _rng_gnd_clearance;	// minimum valid value for range when on ground (m)

 	// optical flow fusion
 	control::BlockParamFloat
-	*_flow_noise;		// best quality observation noise for optical flow LOS rate measurements (rad/sec)
+	_flow_noise;		// best quality observation noise for optical flow LOS rate measurements (rad/sec)
 	control::BlockParamFloat
-	*_flow_noise_qual_min;	// worst quality observation noise for optical flow LOS rate measurements (rad/sec)
-	control::BlockParamInt *_flow_qual_min;		// minimum acceptable quality integer from  the flow sensor
-	control::BlockParamFloat *_flow_innov_gate;	// optical flow fusion innovation consistency gate size (STD)
-	control::BlockParamFloat *_flow_rate_max;	// maximum valid optical flow rate (rad/sec)
+	_flow_noise_qual_min;	// worst quality observation noise for optical flow LOS rate measurements (rad/sec)
+	control::BlockParamInt _flow_qual_min;		// minimum acceptable quality integer from  the flow sensor
+	control::BlockParamFloat _flow_innov_gate;	// optical flow fusion innovation consistency gate size (STD)
+	control::BlockParamFloat _flow_rate_max;	// maximum valid optical flow rate (rad/sec)

 	// sensor positions in body frame
-	control::BlockParamFloat *_imu_pos_x;	// X position of IMU in body frame (m)
-	control::BlockParamFloat *_imu_pos_y;	// Y position of IMU in body frame (m)
-	control::BlockParamFloat *_imu_pos_z;	// Z position of IMU in body frame (m)
-	control::BlockParamFloat *_gps_pos_x;	// X position of GPS antenna in body frame (m)
-	control::BlockParamFloat *_gps_pos_y;	// Y position of GPS antenna in body frame (m)
-	control::BlockParamFloat *_gps_pos_z;	// Z position of GPS antenna in body frame (m)
-	control::BlockParamFloat *_rng_pos_x;	// X position of range finder in body frame (m)
-	control::BlockParamFloat *_rng_pos_y;	// Y position of range finder in body frame (m)
-	control::BlockParamFloat *_rng_pos_z;	// Z position of range finder in body frame (m)
-	control::BlockParamFloat *_flow_pos_x;	// X position of optical flow sensor focal point in body frame (m)
-	control::BlockParamFloat *_flow_pos_y;	// Y position of optical flow sensor focal point in body frame (m)
-	control::BlockParamFloat *_flow_pos_z;	// Z position of optical flow sensor focal point in body frame (m)
+	control::BlockParamFloat _imu_pos_x;	// X position of IMU in body frame (m)
+	control::BlockParamFloat _imu_pos_y;	// Y position of IMU in body frame (m)
+	control::BlockParamFloat _imu_pos_z;	// Z position of IMU in body frame (m)
+	control::BlockParamFloat _gps_pos_x;	// X position of GPS antenna in body frame (m)
+	control::BlockParamFloat _gps_pos_y;	// Y position of GPS antenna in body frame (m)
+	control::BlockParamFloat _gps_pos_z;	// Z position of GPS antenna in body frame (m)
+	control::BlockParamFloat _rng_pos_x;	// X position of range finder in body frame (m)
+	control::BlockParamFloat _rng_pos_y;	// Y position of range finder in body frame (m)
+	control::BlockParamFloat _rng_pos_z;	// Z position of range finder in body frame (m)
+	control::BlockParamFloat _flow_pos_x;	// X position of optical flow sensor focal point in body frame (m)
+	control::BlockParamFloat _flow_pos_y;	// Y position of optical flow sensor focal point in body frame (m)
+	control::BlockParamFloat _flow_pos_z;	// Z position of optical flow sensor focal point in body frame (m)

 	int update_subscriptions();

@@ -263,70 +262,70 @@ Ekf2::Ekf2():
 	_lp_roll_rate(250.0f, 30.0f),
 	_lp_pitch_rate(250.0f, 30.0f),
 	_lp_yaw_rate(250.0f, 20.0f),
-	_ekf(new Ekf()),
-	_params(_ekf->getParamHandle()),
-	_mag_delay_ms(new control::BlockParamFloat(this, "EKF2_MAG_DELAY", false, &_params->mag_delay_ms)),
-	_baro_delay_ms(new control::BlockParamFloat(this, "EKF2_BARO_DELAY", false, &_params->baro_delay_ms)),
-	_gps_delay_ms(new control::BlockParamFloat(this, "EKF2_GPS_DELAY", false, &_params->gps_delay_ms)),
-	_flow_delay_ms(new control::BlockParamFloat(this, "EKF2_OF_DELAY", false, &_params->flow_delay_ms)),
-	_rng_delay_ms(new control::BlockParamFloat(this, "EKF2_RNG_DELAY", false, &_params->range_delay_ms)),
-	_airspeed_delay_ms(new control::BlockParamFloat(this, "EKF2_ASP_DELAY", false, &_params->airspeed_delay_ms)),
-	_gyro_noise(new control::BlockParamFloat(this, "EKF2_GYR_NOISE", false, &_params->gyro_noise)),
-	_accel_noise(new control::BlockParamFloat(this, "EKF2_ACC_NOISE", false, &_params->accel_noise)),
-	_gyro_bias_p_noise(new control::BlockParamFloat(this, "EKF2_GYR_B_NOISE", false, &_params->gyro_bias_p_noise)),
-	_accel_bias_p_noise(new control::BlockParamFloat(this, "EKF2_ACC_B_NOISE", false, &_params->accel_bias_p_noise)),
-	_gyro_scale_p_noise(new control::BlockParamFloat(this, "EKF2_GYR_S_NOISE", false, &_params->gyro_scale_p_noise)),
-	_mage_p_noise(new control::BlockParamFloat(this, "EKF2_MAG_E_NOISE", false, &_params->mage_p_noise)),
-	_magb_p_noise(new control::BlockParamFloat(this, "EKF2_MAG_B_NOISE", false, &_params->magb_p_noise)),
-	_wind_vel_p_noise(new control::BlockParamFloat(this, "EKF2_WIND_NOISE", false, &_params->wind_vel_p_noise)),
-	_terrain_p_noise(new control::BlockParamFloat(this, "EKF2_TERR_NOISE", false, &_params->terrain_p_noise)),
-	_terrain_gradient(new control::BlockParamFloat(this, "EKF2_TERR_GRAD", false, &_params->terrain_gradient)),
-	_gps_vel_noise(new control::BlockParamFloat(this, "EKF2_GPS_V_NOISE", false, &_params->gps_vel_noise)),
-	_gps_pos_noise(new control::BlockParamFloat(this, "EKF2_GPS_P_NOISE", false, &_params->gps_pos_noise)),
-	_pos_noaid_noise(new control::BlockParamFloat(this, "EKF2_NOAID_NOISE", false, &_params->pos_noaid_noise)),
-	_baro_noise(new control::BlockParamFloat(this, "EKF2_BARO_NOISE", false, &_params->baro_noise)),
-	_baro_innov_gate(new control::BlockParamFloat(this, "EKF2_BARO_GATE", false, &_params->baro_innov_gate)),
-	_posNE_innov_gate(new control::BlockParamFloat(this, "EKF2_GPS_P_GATE", false, &_params->posNE_innov_gate)),
-	_vel_innov_gate(new control::BlockParamFloat(this, "EKF2_GPS_V_GATE", false, &_params->vel_innov_gate)),
-	_mag_heading_noise(new control::BlockParamFloat(this, "EKF2_HEAD_NOISE", false, &_params->mag_heading_noise)),
-	_mag_noise(new control::BlockParamFloat(this, "EKF2_MAG_NOISE", false, &_params->mag_noise)),
-	_eas_noise(new control::BlockParamFloat(this, "EKF2_EAS_NOISE", false, &_params->eas_noise)),
-	_mag_declination_deg(new control::BlockParamFloat(this, "EKF2_MAG_DECL", false, &_params->mag_declination_deg)),
-	_heading_innov_gate(new control::BlockParamFloat(this, "EKF2_HDG_GATE", false, &_params->heading_innov_gate)),
-	_mag_innov_gate(new control::BlockParamFloat(this, "EKF2_MAG_GATE", false, &_params->mag_innov_gate)),
-	_mag_decl_source(new control::BlockParamInt(this, "EKF2_DECL_TYPE", false, &_params->mag_declination_source)),
-	_mag_fuse_type(new control::BlockParamInt(this, "EKF2_MAG_TYPE", false, &_params->mag_fusion_type)),
-	_gps_check_mask(new control::BlockParamInt(this, "EKF2_GPS_CHECK", false, &_params->gps_check_mask)),
-	_requiredEph(new control::BlockParamFloat(this, "EKF2_REQ_EPH", false, &_params->req_hacc)),
-	_requiredEpv(new control::BlockParamFloat(this, "EKF2_REQ_EPV", false, &_params->req_vacc)),
-	_requiredSacc(new control::BlockParamFloat(this, "EKF2_REQ_SACC", false, &_params->req_sacc)),
-	_requiredNsats(new control::BlockParamInt(this, "EKF2_REQ_NSATS", false, &_params->req_nsats)),
-	_requiredGDoP(new control::BlockParamFloat(this, "EKF2_REQ_GDOP", false, &_params->req_gdop)),
-	_requiredHdrift(new control::BlockParamFloat(this, "EKF2_REQ_HDRIFT", false, &_params->req_hdrift)),
-	_requiredVdrift(new control::BlockParamFloat(this, "EKF2_REQ_VDRIFT", false, &_params->req_vdrift)),
-	_param_record_replay_msg(new control::BlockParamInt(this, "EKF2_REC_RPL", false, &_publish_replay_mode)),
-	_fusion_mode(new control::BlockParamInt(this, "EKF2_AID_MASK", false, &_params->fusion_mode)),
-	_vdist_sensor_type(new control::BlockParamInt(this, "EKF2_HGT_MODE", false, &_params->vdist_sensor_type)),
-	_range_noise(new control::BlockParamFloat(this, "EKF2_RNG_NOISE", false, &_params->range_noise)),
-	_range_innov_gate(new control::BlockParamFloat(this, "EKF2_RNG_GATE", false, &_params->range_innov_gate)),
-	_rng_gnd_clearance(new control::BlockParamFloat(this, "EKF2_MIN_RNG", false, &_params->rng_gnd_clearance)),
-	_flow_noise(new control::BlockParamFloat(this, "EKF2_OF_N_MIN", false, &_params->flow_noise)),
-	_flow_noise_qual_min(new control::BlockParamFloat(this, "EKF2_OF_N_MAX", false, &_params->flow_noise_qual_min)),
-	_flow_qual_min(new control::BlockParamInt(this, "EKF2_OF_QMIN", false, &_params->flow_qual_min)),
-	_flow_innov_gate(new control::BlockParamFloat(this, "EKF2_OF_GATE", false, &_params->flow_innov_gate)),
-	_flow_rate_max(new control::BlockParamFloat(this, "EKF2_OF_RMAX", false, &_params->flow_rate_max)),
-	_imu_pos_x(new control::BlockParamFloat(this, "EKF2_IMU_POS_X", false, &_params->imu_pos_body(0))),
-	_imu_pos_y(new control::BlockParamFloat(this, "EKF2_IMU_POS_Y", false, &_params->imu_pos_body(1))),
-	_imu_pos_z(new control::BlockParamFloat(this, "EKF2_IMU_POS_Z", false, &_params->imu_pos_body(2))),
-	_gps_pos_x(new control::BlockParamFloat(this, "EKF2_GPS_POS_X", false, &_params->gps_pos_body(0))),
-	_gps_pos_y(new control::BlockParamFloat(this, "EKF2_GPS_POS_Y", false, &_params->gps_pos_body(1))),
-	_gps_pos_z(new control::BlockParamFloat(this, "EKF2_GPS_POS_Z", false, &_params->gps_pos_body(2))),
-	_rng_pos_x(new control::BlockParamFloat(this, "EKF2_RNG_POS_X", false, &_params->rng_pos_body(0))),
-	_rng_pos_y(new control::BlockParamFloat(this, "EKF2_RNG_POS_Y", false, &_params->rng_pos_body(1))),
-	_rng_pos_z(new control::BlockParamFloat(this, "EKF2_RNG_POS_Z", false, &_params->rng_pos_body(2))),
-	_flow_pos_x(new control::BlockParamFloat(this, "EKF2_OF_POS_X", false, &_params->flow_pos_body(0))),
-	_flow_pos_y(new control::BlockParamFloat(this, "EKF2_OF_POS_Y", false, &_params->flow_pos_body(1))),
-	_flow_pos_z(new control::BlockParamFloat(this, "EKF2_OF_POS_Z", false, &_params->flow_pos_body(2)))
+	_ekf(),
+	_params(_ekf.getParamHandle()),
+	_mag_delay_ms(this, "EKF2_MAG_DELAY", false, &_params->mag_delay_ms),
+	_baro_delay_ms(this, "EKF2_BARO_DELAY", false, &_params->baro_delay_ms),
+	_gps_delay_ms(this, "EKF2_GPS_DELAY", false, &_params->gps_delay_ms),
+	_flow_delay_ms(this, "EKF2_OF_DELAY", false, &_params->flow_delay_ms),
+	_rng_delay_ms(this, "EKF2_RNG_DELAY", false, &_params->range_delay_ms),
+	_airspeed_delay_ms(this, "EKF2_ASP_DELAY", false, &_params->airspeed_delay_ms),
+	_gyro_noise(this, "EKF2_GYR_NOISE", false, &_params->gyro_noise),
+	_accel_noise(this, "EKF2_ACC_NOISE", false, &_params->accel_noise),
+	_gyro_bias_p_noise(this, "EKF2_GYR_B_NOISE", false, &_params->gyro_bias_p_noise),
+	_accel_bias_p_noise(this, "EKF2_ACC_B_NOISE", false, &_params->accel_bias_p_noise),
+	_gyro_scale_p_noise(this, "EKF2_GYR_S_NOISE", false, &_params->gyro_scale_p_noise),
+	_mage_p_noise(this, "EKF2_MAG_E_NOISE", false, &_params->mage_p_noise),
+	_magb_p_noise(this, "EKF2_MAG_B_NOISE", false, &_params->magb_p_noise),
+	_wind_vel_p_noise(this, "EKF2_WIND_NOISE", false, &_params->wind_vel_p_noise),
+	_terrain_p_noise(this, "EKF2_TERR_NOISE", false, &_params->terrain_p_noise),
+	_terrain_gradient(this, "EKF2_TERR_GRAD", false, &_params->terrain_gradient),
+	_gps_vel_noise(this, "EKF2_GPS_V_NOISE", false, &_params->gps_vel_noise),
+	_gps_pos_noise(this, "EKF2_GPS_P_NOISE", false, &_params->gps_pos_noise),
+	_pos_noaid_noise(this, "EKF2_NOAID_NOISE", false, &_params->pos_noaid_noise),
+	_baro_noise(this, "EKF2_BARO_NOISE", false, &_params->baro_noise),
+	_baro_innov_gate(this, "EKF2_BARO_GATE", false, &_params->baro_innov_gate),
+	_posNE_innov_gate(this, "EKF2_GPS_P_GATE", false, &_params->posNE_innov_gate),
+	_vel_innov_gate(this, "EKF2_GPS_V_GATE", false, &_params->vel_innov_gate),
+	_mag_heading_noise(this, "EKF2_HEAD_NOISE", false, &_params->mag_heading_noise),
+	_mag_noise(this, "EKF2_MAG_NOISE", false, &_params->mag_noise),
+	_eas_noise(this, "EKF2_EAS_NOISE", false, &_params->eas_noise),
+	_mag_declination_deg(this, "EKF2_MAG_DECL", false, &_params->mag_declination_deg),
+	_heading_innov_gate(this, "EKF2_HDG_GATE", false, &_params->heading_innov_gate),
+	_mag_innov_gate(this, "EKF2_MAG_GATE", false, &_params->mag_innov_gate),
+	_mag_decl_source(this, "EKF2_DECL_TYPE", false, &_params->mag_declination_source),
+	_mag_fuse_type(this, "EKF2_MAG_TYPE", false, &_params->mag_fusion_type),
+	_gps_check_mask(this, "EKF2_GPS_CHECK", false, &_params->gps_check_mask),
+	_requiredEph(this, "EKF2_REQ_EPH", false, &_params->req_hacc),
+	_requiredEpv(this, "EKF2_REQ_EPV", false, &_params->req_vacc),
+	_requiredSacc(this, "EKF2_REQ_SACC", false, &_params->req_sacc),
+	_requiredNsats(this, "EKF2_REQ_NSATS", false, &_params->req_nsats),
+	_requiredGDoP(this, "EKF2_REQ_GDOP", false, &_params->req_gdop),
+	_requiredHdrift(this, "EKF2_REQ_HDRIFT", false, &_params->req_hdrift),
+	_requiredVdrift(this, "EKF2_REQ_VDRIFT", false, &_params->req_vdrift),
+	_param_record_replay_msg(this, "EKF2_REC_RPL", false, &_publish_replay_mode),
+	_fusion_mode(this, "EKF2_AID_MASK", false, &_params->fusion_mode),
+	_vdist_sensor_type(this, "EKF2_HGT_MODE", false, &_params->vdist_sensor_type),
+	_range_noise(this, "EKF2_RNG_NOISE", false, &_params->range_noise),
+	_range_innov_gate(this, "EKF2_RNG_GATE", false, &_params->range_innov_gate),
+	_rng_gnd_clearance(this, "EKF2_MIN_RNG", false, &_params->rng_gnd_clearance),
+	_flow_noise(this, "EKF2_OF_N_MIN", false, &_params->flow_noise),
+	_flow_noise_qual_min(this, "EKF2_OF_N_MAX", false, &_params->flow_noise_qual_min),
+	_flow_qual_min(this, "EKF2_OF_QMIN", false, &_params->flow_qual_min),
+	_flow_innov_gate(this, "EKF2_OF_GATE", false, &_params->flow_innov_gate),
+	_flow_rate_max(this, "EKF2_OF_RMAX", false, &_params->flow_rate_max),
+	_imu_pos_x(this, "EKF2_IMU_POS_X", false, &_params->imu_pos_body(0)),
+	_imu_pos_y(this, "EKF2_IMU_POS_Y", false, &_params->imu_pos_body(1)),
+	_imu_pos_z(this, "EKF2_IMU_POS_Z", false, &_params->imu_pos_body(2)),
+	_gps_pos_x(this, "EKF2_GPS_POS_X", false, &_params->gps_pos_body(0)),
+	_gps_pos_y(this, "EKF2_GPS_POS_Y", false, &_params->gps_pos_body(1)),
+	_gps_pos_z(this, "EKF2_GPS_POS_Z", false, &_params->gps_pos_body(2)),
+	_rng_pos_x(this, "EKF2_RNG_POS_X", false, &_params->rng_pos_body(0)),
+	_rng_pos_y(this, "EKF2_RNG_POS_Y", false, &_params->rng_pos_body(1)),
+	_rng_pos_z(this, "EKF2_RNG_POS_Z", false, &_params->rng_pos_body(2)),
+	_flow_pos_x(this, "EKF2_OF_POS_X", false, &_params->flow_pos_body(0)),
+	_flow_pos_y(this, "EKF2_OF_POS_Y", false, &_params->flow_pos_body(1)),
+	_flow_pos_z(this, "EKF2_OF_POS_Z", false, &_params->flow_pos_body(2))
 {

 }
@@ -338,8 +337,8 @@ Ekf2::~Ekf2()

 void Ekf2::print_status()
 {
-	warnx("local position OK %s", (_ekf->local_position_is_valid()) ? "[YES]" : "[NO]");
-	warnx("global position OK %s", (_ekf->global_position_is_valid()) ? "[YES]" : "[NO]");
+	warnx("local position OK %s", (_ekf.local_position_is_valid()) ? "[YES]" : "[NO]");
+	warnx("global position OK %s", (_ekf.global_position_is_valid()) ? "[YES]" : "[NO]");
 }

 void Ekf2::task_main()
@@ -444,14 +443,14 @@ void Ekf2::task_main()
 		}

 		// push imu data into estimator
-		_ekf->setIMUData(now, sensors.gyro_integral_dt[0], sensors.accelerometer_integral_dt[0],
-				 &sensors.gyro_integral_rad[0], &sensors.accelerometer_integral_m_s[0]);
+		_ekf.setIMUData(now, sensors.gyro_integral_dt[0], sensors.accelerometer_integral_dt[0],
+				&sensors.gyro_integral_rad[0], &sensors.accelerometer_integral_m_s[0]);

 		// read mag data
-		_ekf->setMagData(sensors.magnetometer_timestamp[0], &sensors.magnetometer_ga[0]);
+		_ekf.setMagData(sensors.magnetometer_timestamp[0], &sensors.magnetometer_ga[0]);

 		// read baro data
-		_ekf->setBaroData(sensors.baro_timestamp[0], &sensors.baro_alt_meter[0]);
+		_ekf.setBaroData(sensors.baro_timestamp[0], &sensors.baro_alt_meter[0]);

 		// read gps data if available
 		if (gps_updated) {
@@ -474,13 +473,14 @@ void Ekf2::task_main()
 			//TODO add gdop to gps topic
 			gps_msg.gdop = 0.0f;

-			_ekf->setGpsData(gps.timestamp_position, &gps_msg);
+			_ekf.setGpsData(gps.timestamp_position, &gps_msg);
 		}

 		// read airspeed data if available
 		float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s;
+
 		if (airspeed_updated && airspeed.true_airspeed_m_s > 7.0f) {
-			_ekf->setAirspeedData(airspeed.timestamp, &airspeed.true_airspeed_m_s, &eas2tas);
+			_ekf.setAirspeedData(airspeed.timestamp, &airspeed.true_airspeed_m_s, &eas2tas);
 		}

 		if (optical_flow_updated) {
@@ -495,19 +495,19 @@ void Ekf2::task_main()

 			if (PX4_ISFINITE(optical_flow.pixel_flow_y_integral) &&
 			    PX4_ISFINITE(optical_flow.pixel_flow_x_integral)) {
-				_ekf->setOpticalFlowData(optical_flow.timestamp, &flow);
+				_ekf.setOpticalFlowData(optical_flow.timestamp, &flow);
 			}
 		}

 		if (range_finder_updated) {
-			_ekf->setRangeData(range_finder.timestamp, &range_finder.current_distance);
+			_ekf.setRangeData(range_finder.timestamp, &range_finder.current_distance);
 		}

 		orb_check(_actuator_armed_sub, &actuator_armed_updated);

 		if (actuator_armed_updated) {
 			orb_copy(ORB_ID(actuator_armed), _actuator_armed_sub, &actuator_armed);
-			_ekf->set_arm_status(actuator_armed.armed);
+			_ekf.set_arm_status(actuator_armed.armed);
 		}

 		orb_check(_vehicle_land_detected_sub, &vehicle_land_detected_updated);
@@ -515,14 +515,14 @@ void Ekf2::task_main()
 		if (vehicle_land_detected_updated) {
 			struct vehicle_land_detected_s vehicle_land_detected = {};
 			orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &vehicle_land_detected);
-			_ekf->set_in_air_status(!vehicle_land_detected.landed);
+			_ekf.set_in_air_status(!vehicle_land_detected.landed);
 		}

 		// run the EKF update and output
-		if (_ekf->update()) {
+		if (_ekf.update()) {
 			// generate vehicle attitude quaternion data
 			struct vehicle_attitude_s att = {};
-			_ekf->copy_quaternion(att.q);
+			_ekf.copy_quaternion(att.q);
 			matrix::Quaternion<float> q(att.q[0], att.q[1], att.q[2], att.q[3]);

 			// generate control state data
@@ -534,7 +534,7 @@ void Ekf2::task_main()

 			// Velocity in body frame
 			float velocity[3];
-			_ekf->get_velocity(velocity);
+			_ekf.get_velocity(velocity);
 			Vector3f v_n(velocity);
 			matrix::Dcm<float> R_to_body(q.inversed());
 			Vector3f v_b = R_to_body * v_n;
@@ -545,7 +545,7 @@ void Ekf2::task_main()

 			// Local Position NED
 			float position[3];
-			_ekf->get_position(position);
+			_ekf.get_position(position);
 			ctrl_state.x_pos = position[0];
 			ctrl_state.y_pos = position[1];
 			ctrl_state.z_pos = position[2];
@@ -560,14 +560,15 @@ void Ekf2::task_main()
 			matrix::Vector<float, 3> acceleration = {&sensors.accelerometer_m_s2[0]};

 			float accel_bias = 0.0f;
-			_ekf->get_accel_bias(&accel_bias);
+			_ekf.get_accel_bias(&accel_bias);
 			ctrl_state.x_acc = acceleration(0);
 			ctrl_state.y_acc = acceleration(1);
 			ctrl_state.z_acc = acceleration(2) - accel_bias;

 			// compute lowpass filtered horizontal acceleration
 			acceleration = R_to_body.transpose() * acceleration;
-			_acc_hor_filt = 0.95f * _acc_hor_filt + 0.05f * sqrtf(acceleration(0) * acceleration(0) + acceleration(1) * acceleration(1));
+			_acc_hor_filt = 0.95f * _acc_hor_filt + 0.05f * sqrtf(acceleration(0) * acceleration(0) + acceleration(
+						1) * acceleration(1));
 			ctrl_state.horz_acc_mag = _acc_hor_filt;

 			// Airspeed - take airspeed measurement directly here as no wind is estimated
@@ -622,28 +623,28 @@ void Ekf2::task_main()
 			lpos.timestamp = hrt_absolute_time();

 			// Position of body origin in local NED frame
-			_ekf->get_position(pos);
+			_ekf.get_position(pos);
 			lpos.x = pos[0];
 			lpos.y = pos[1];
 			lpos.z = pos[2];

 			// Velocity of body origin in local NED frame (m/s)
-			_ekf->get_velocity(vel);
+			_ekf.get_velocity(vel);
 			lpos.vx = vel[0];
 			lpos.vy = vel[1];
 			lpos.vz = vel[2];

 			// TODO: better status reporting
-			lpos.xy_valid = _ekf->local_position_is_valid();
+			lpos.xy_valid = _ekf.local_position_is_valid();
 			lpos.z_valid = true;
-			lpos.v_xy_valid = _ekf->local_position_is_valid();
+			lpos.v_xy_valid = _ekf.local_position_is_valid();
 			lpos.v_z_valid = true;

 			// Position of local NED origin in GPS / WGS84 frame
 			struct map_projection_reference_s ekf_origin = {};
 			// true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
-			_ekf->get_ekf_origin(&lpos.ref_timestamp, &ekf_origin, &lpos.ref_alt);
-			lpos.xy_global = _ekf->global_position_is_valid();
+			_ekf.get_ekf_origin(&lpos.ref_timestamp, &ekf_origin, &lpos.ref_alt);
+			lpos.xy_global = _ekf.global_position_is_valid();
 			lpos.z_global = true;                                // true if z is valid and has valid global reference (ref_alt)
 			lpos.ref_lat = ekf_origin.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees
 			lpos.ref_lon = ekf_origin.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees
@@ -652,15 +653,15 @@ void Ekf2::task_main()
 			lpos.yaw = att.yaw;

 			float terrain_vpos;
-			lpos.dist_bottom_valid = _ekf->get_terrain_vert_pos(&terrain_vpos);
+			lpos.dist_bottom_valid = _ekf.get_terrain_vert_pos(&terrain_vpos);
 			lpos.dist_bottom = terrain_vpos - pos[2]; // Distance to bottom surface (ground) in meters
 			lpos.dist_bottom_rate = -vel[2]; // Distance to bottom surface (ground) change rate
 			lpos.surface_bottom_timestamp	= hrt_absolute_time(); // Time when new bottom surface found

 			// TODO: uORB definition does not define what these variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres
 			Vector3f pos_var, vel_var;
-			_ekf->get_pos_var(pos_var);
-			_ekf->get_vel_var(vel_var);
+			_ekf.get_pos_var(pos_var);
+			_ekf.get_vel_var(vel_var);
 			lpos.eph = sqrt(pos_var(0) + pos_var(1));
 			lpos.epv = sqrt(pos_var(2));

@@ -675,7 +676,7 @@ void Ekf2::task_main()
 			// generate and publish global position data
 			struct vehicle_global_position_s global_pos = {};

-			if (_ekf->global_position_is_valid()) {
+			if (_ekf.global_position_is_valid()) {
 				global_pos.timestamp = hrt_absolute_time(); // Time of this estimate, in microseconds since system start
 				global_pos.time_utc_usec = gps.time_utc_usec; // GPS UTC timestamp in microseconds

@@ -728,10 +729,10 @@ void Ekf2::task_main()
 		// publish estimator status
 		struct estimator_status_s status = {};
 		status.timestamp = hrt_absolute_time();
-		_ekf->get_state_delayed(status.states);
-		_ekf->get_covariances(status.covariances);
-		_ekf->get_gps_check_status(&status.gps_check_fail_flags);
-		_ekf->get_control_mode(&status.control_mode_flags);
+		_ekf.get_state_delayed(status.states);
+		_ekf.get_covariances(status.covariances);
+		_ekf.get_gps_check_status(&status.gps_check_fail_flags);
+		_ekf.get_control_mode(&status.control_mode_flags);

 		if (_estimator_status_pub == nullptr) {
 			_estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &status);
@@ -758,19 +759,19 @@ void Ekf2::task_main()
 		// publish estimator innovation data
 		struct ekf2_innovations_s innovations = {};
 		innovations.timestamp = hrt_absolute_time();
-		_ekf->get_vel_pos_innov(&innovations.vel_pos_innov[0]);
-		_ekf->get_mag_innov(&innovations.mag_innov[0]);
-		_ekf->get_heading_innov(&innovations.heading_innov);
-		_ekf->get_airspeed_innov(&innovations.airspeed_innov);
-		_ekf->get_flow_innov(&innovations.flow_innov[0]);
-		_ekf->get_hagl_innov(&innovations.hagl_innov);
+		_ekf.get_vel_pos_innov(&innovations.vel_pos_innov[0]);
+		_ekf.get_mag_innov(&innovations.mag_innov[0]);
+		_ekf.get_heading_innov(&innovations.heading_innov);
+		_ekf.get_airspeed_innov(&innovations.airspeed_innov);
+		_ekf.get_flow_innov(&innovations.flow_innov[0]);
+		_ekf.get_hagl_innov(&innovations.hagl_innov);

-		_ekf->get_vel_pos_innov_var(&innovations.vel_pos_innov_var[0]);
-		_ekf->get_mag_innov_var(&innovations.mag_innov_var[0]);
-		_ekf->get_heading_innov_var(&innovations.heading_innov_var);
-		_ekf->get_airspeed_innov_var(&innovations.airspeed_innov_var);
-		_ekf->get_flow_innov_var(&innovations.flow_innov_var[0]);
-		_ekf->get_hagl_innov_var(&innovations.hagl_innov_var);
+		_ekf.get_vel_pos_innov_var(&innovations.vel_pos_innov_var[0]);
+		_ekf.get_mag_innov_var(&innovations.mag_innov_var[0]);
+		_ekf.get_heading_innov_var(&innovations.heading_innov_var);
+		_ekf.get_airspeed_innov_var(&innovations.airspeed_innov_var);
+		_ekf.get_flow_innov_var(&innovations.flow_innov_var[0]);
+		_ekf.get_hagl_innov_var(&innovations.hagl_innov_var);

 		if (_estimator_innovations_pub == nullptr) {
 			_estimator_innovations_pub = orb_advertise(ORB_ID(ekf2_innovations), &innovations);
@@ -782,12 +783,12 @@ void Ekf2::task_main()
 		// save the declination to the EKF2_MAG_DECL parameter when a dis-arm event is detected
 		if ((_params->mag_declination_source & (1 << 1)) && _prev_motors_armed && !actuator_armed.armed) {
 			float decl_deg;
-			_ekf->copy_mag_decl_deg(&decl_deg);
-			_mag_declination_deg->set(decl_deg);
+			_ekf.copy_mag_decl_deg(&decl_deg);
+			_mag_declination_deg.set(decl_deg);
 		}

 		// publish replay message if in replay mode
-		bool publish_replay_message = (bool)_param_record_replay_msg->get();
+		bool publish_replay_message = (bool)_param_record_replay_msg.get();

 		if (publish_replay_message) {
 			struct ekf2_replay_s replay = {};
@@ -846,6 +847,19 @@ void Ekf2::task_main()
 				replay.rng_timestamp = 0;
 			}

+			if (airspeed_updated) {
+				replay.asp_timestamp = airspeed.timestamp;
+				replay.indicated_airspeed_m_s = airspeed.indicated_airspeed_m_s;
+				replay.true_airspeed_m_s = airspeed.true_airspeed_m_s;
+				replay.true_airspeed_unfiltered_m_s = airspeed.true_airspeed_unfiltered_m_s;
+				replay.air_temperature_celsius = airspeed.air_temperature_celsius;
+				replay.confidence = airspeed.confidence;
+
+			} else {
+				replay.asp_timestamp = 0;
+			}
+
+
 			if (_replay_pub == nullptr) {
 				_replay_pub = orb_advertise(ORB_ID(ekf2_replay), &replay);

@@ -900,17 +914,17 @@ int ekf2_main(int argc, char *argv[])

 		ekf2::instance = new Ekf2();

+		if (ekf2::instance == nullptr) {
+			PX4_WARN("alloc failed");
+			return 1;
+		}
+
 		if (argc >= 3) {
 			if (!strcmp(argv[2], "--replay")) {
 				ekf2::instance->set_replay_mode(true);
 			}
 		}

-		if (ekf2::instance == nullptr) {
-			PX4_WARN("alloc failed");
-			return 1;
-		}
-
 		if (OK != ekf2::instance->start()) {
 			delete ekf2::instance;
 			ekf2::instance = nullptr;
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2016 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
@@ -176,14 +176,23 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_baro_gps_offset(0.0f),

 	/* performance counters */
-	_loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")),
+	_loop_perf(perf_alloc(PC_ELAPSED, "ekf_dt")),
+#if 0
 	_loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")),
 	_perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")),
 	_perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")),
 	_perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")),
 	_perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")),
 	_perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")),
-	_perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")),
+#else
+	_loop_intvl(nullptr),
+	_perf_gyro(nullptr),
+	_perf_mag(nullptr),
+	_perf_gps(nullptr),
+	_perf_baro(nullptr),
+	_perf_airspeed(nullptr),
+#endif
+	_perf_reset(perf_alloc(PC_COUNT, "ekf_rst")),

 	/* states */
 	_gps_alt_filt(0.0f),
@@ -212,6 +221,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_newAdsData(false),
 	_newDataMag(false),
 	_newRangeData(false),
+	_mavlink_log_pub(nullptr),

 	_mag_offset_x(this, "MAGB_X"),
 	_mag_offset_y(this, "MAGB_Y"),
@@ -412,8 +422,7 @@ int AttitudePositionEstimatorEKF::check_filter_state()

 		// Do not warn about accel offset if we have no position updates
 		if (!(warn_index == 5 && _ekf->staticMode)) {
-			PX4_WARN("reset: %s", feedback[warn_index]);
-			mavlink_log_critical(&_mavlink_log_pub, "[ekf check] %s", feedback[warn_index]);
+			mavlink_and_console_log_critical(&_mavlink_log_pub, "[ekf check] %s", feedback[warn_index]);
 		}
 	}

@@ -690,8 +699,6 @@ void AttitudePositionEstimatorEKF::task_main()

 					_filter_ref_offset = -_baro.altitude;

-					PX4_INFO("filter ref off: baro_alt: %8.4f", (double)_filter_ref_offset);
-
 				} else {

 					if (!_gps_initialized && _gpsIsGood) {
@@ -776,7 +783,6 @@ void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp,
 		_local_pos.ref_timestamp = timestamp;

 		map_projection_init(&_pos_ref, lat, lon);
-		mavlink_and_console_log_info(&_mavlink_log_pub, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
 	}
 }

@@ -2029,63 +2029,62 @@ void AttPosEKF::FuseOptFlow()
            varInnovOptFlow[1] = 1.0f/SK_LOS[1];
            innovOptFlow[1] = losPred[1] - flowRadXYcomp[1];

-        }
-
-        // loop through the X and Y observations and fuse them sequentially
-        for (uint8_t obsIndex = 0; obsIndex < 2; obsIndex++) {
-            // Check the innovation for consistency and don't fuse if > 5Sigma
-            if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 25.0f) {
-                // correct the state vector
-                for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++)
-                {
-                    states[j] = states[j] - K_LOS[obsIndex][j] * innovOptFlow[obsIndex];
-                }
-                // normalise the quaternion states
-                float quatMag = sqrtf(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
-                if (quatMag > 1e-12f)
-                {
-                    for (uint8_t j= 0; j<=3; j++)
-                    {
-                        float quatMagInv = 1.0f/quatMag;
-                        states[j] = states[j] * quatMagInv;
-                    }
-                }
-                // correct the covariance P = (I - K*H)*P
-                // take advantage of the empty columns in KH to reduce the
-                // number of operations
-                for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++)
-                {
-                    for (uint8_t j = 0; j <= 6; j++)
-                    {
-                        KH[i][j] = K_LOS[obsIndex][i] * H_LOS[obsIndex][j];
-                    }
-                    for (uint8_t j = 7; j <= 8; j++)
-                    {
-                        KH[i][j] = 0.0f;
-                    }
-                    KH[i][9] = K_LOS[obsIndex][i] * H_LOS[obsIndex][9];
-                    for (uint8_t j = 10; j < EKF_STATE_ESTIMATES; j++)
-                    {
-                        KH[i][j] = 0.0f;
-                    }
-                }
-                for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++)
-                {
+            // loop through the X and Y observations and fuse them sequentially
+            for (uint8_t obsIndex = 0; obsIndex < 2; obsIndex++) {
+                // Check the innovation for consistency and don't fuse if > 5Sigma
+                if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 25.0f) {
+                    // correct the state vector
                    for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++)
                    {
-                        KHP[i][j] = 0.0f;
-                        for (uint8_t k = 0; k <= 6; k++)
-                        {
-                            KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
-                        }
-                        KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j];
+                        states[j] = states[j] - K_LOS[obsIndex][j] * innovOptFlow[obsIndex];
                    }
-                }
-                for (uint8_t i = 0; i <  EKF_STATE_ESTIMATES; i++)
-                {
-                    for (uint8_t j = 0; j <  EKF_STATE_ESTIMATES; j++)
+                    // normalise the quaternion states
+                    float quatMag = sqrtf(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
+                    if (quatMag > 1e-12f)
                    {
-                        P[i][j] = P[i][j] - KHP[i][j];
+                        for (uint8_t j= 0; j<=3; j++)
+                        {
+                            float quatMagInv = 1.0f/quatMag;
+                            states[j] = states[j] * quatMagInv;
+                        }
+                    }
+                    // correct the covariance P = (I - K*H)*P
+                    // take advantage of the empty columns in KH to reduce the
+                    // number of operations
+                    for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++)
+                    {
+                        for (uint8_t j = 0; j <= 6; j++)
+                        {
+                            KH[i][j] = K_LOS[obsIndex][i] * H_LOS[obsIndex][j];
+                        }
+                        for (uint8_t j = 7; j <= 8; j++)
+                        {
+                            KH[i][j] = 0.0f;
+                        }
+                        KH[i][9] = K_LOS[obsIndex][i] * H_LOS[obsIndex][9];
+                        for (uint8_t j = 10; j < EKF_STATE_ESTIMATES; j++)
+                        {
+                            KH[i][j] = 0.0f;
+                        }
+                    }
+                    for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++)
+                    {
+                        for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++)
+                        {
+                            KHP[i][j] = 0.0f;
+                            for (uint8_t k = 0; k <= 6; k++)
+                            {
+                                KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
+                            }
+                            KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j];
+                        }
+                    }
+                    for (uint8_t i = 0; i <  EKF_STATE_ESTIMATES; i++)
+                    {
+                        for (uint8_t j = 0; j <  EKF_STATE_ESTIMATES; j++)
+                        {
+                            P[i][j] = P[i][j] - KHP[i][j];
+                        }
                    }
                }
            }
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2016 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
@@ -376,9 +376,14 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_attitude_setpoint_id(0),

 	/* performance counters */
-	_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
-	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")),
-	_nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")),
+	_loop_perf(perf_alloc(PC_ELAPSED, "fwa_dt")),
+#if 0
+	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fwa_nani")),
+	_nonfinite_output_perf(perf_alloc(PC_COUNT, "fwa_nano")),
+#else
+	_nonfinite_input_perf(nullptr),
+	_nonfinite_output_perf(nullptr),
+#endif
 	/* states */
 	_setpoint_valid(false),
 	_debug(false),
@@ -2361,7 +2361,7 @@ FixedwingPositionControl::start()
 	_control_task = px4_task_spawn_cmd("fw_pos_control_l1",
 					   SCHED_DEFAULT,
 					   SCHED_PRIORITY_MAX - 5,
-					   1700,
+					   1400,
 					   (px4_main_t)&FixedwingPositionControl::task_main_trampoline,
 					   nullptr);

@@ -140,7 +140,7 @@ static int land_detector_start(const char *mode)
 	}

 	/* avoid memory fragmentation by not exiting start handler until the task has fully started */
-	const uint32_t timeout = hrt_absolute_time() + 5000000; //5 second timeout
+	const uint64_t timeout = hrt_absolute_time() + 5000000; //5 second timeout

 	/* avoid printing dots just yet and do one sleep before the first check */
 	usleep(10000);
@@ -4,7 +4,7 @@
 #include <systemlib/err.h>
 #include <matrix/math.hpp>

-orb_advert_t mavlink_log_pub;
+orb_advert_t mavlink_log_pub = nullptr;

 // timeouts for sensors in microseconds
 static const uint32_t 		EST_SRC_TIMEOUT = 10000; // 0.01 s
@@ -1190,14 +1190,13 @@ void Mavlink::send_autopilot_capabilites()
 		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT;
 		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_COMMAND_INT;
 		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_FTP;
-		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_FTP;
 		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET;
 		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED;
 		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ACTUATOR_TARGET;
-		msg.flight_sw_version = 0;
-		msg.middleware_sw_version = 0;
-		msg.os_sw_version = 0;
-		msg.board_version = 0;
+		msg.flight_sw_version = version_tag_to_number(px4_git_version);
+		msg.middleware_sw_version = version_tag_to_number(px4_git_version);
+		msg.os_sw_version = version_tag_to_number(os_git_tag);
+		msg.board_version = px4_board_version;
 		memcpy(&msg.flight_custom_version, &px4_git_version_binary, sizeof(msg.flight_custom_version));
 		memcpy(&msg.middleware_custom_version, &px4_git_version_binary, sizeof(msg.middleware_custom_version));
 		memset(&msg.os_custom_version, 0, sizeof(msg.os_custom_version));
@@ -2239,7 +2238,7 @@ Mavlink::start(int argc, char *argv[])
 	px4_task_spawn_cmd(buf,
 			   SCHED_DEFAULT,
 			   SCHED_PRIORITY_DEFAULT,
-			   2900,
+			   2800,
 			   (px4_main_t)&Mavlink::start_helper,
 			   (char *const *)argv);

@@ -357,7 +357,7 @@ public:

 	bool			is_usb_uart() { return _is_usb_uart; }

-	bool			accepting_commands() { return ((!_config_link_on) || (_mode == MAVLINK_MODE_CONFIG)); }
+	bool			accepting_commands() { return true; /* non-trivial side effects ((!_config_link_on) || (_mode == MAVLINK_MODE_CONFIG));*/ }

 	/**
 	 * Wether or not the system should be logging
@@ -588,8 +588,8 @@ protected:

 	void send(const hrt_abstime t)
 	{
-		struct vehicle_status_s status;
-		struct battery_status_s battery_status;
+		struct vehicle_status_s status = {};
+		struct battery_status_s battery_status = {};

 		const bool updated_status = _status_sub->update(&status);
 		const bool updated_battery = _battery_status_sub->update(&battery_status);
@@ -605,14 +605,13 @@ protected:
 		if (updated_status || updated_battery) {
 			mavlink_sys_status_t msg;

-
 			msg.onboard_control_sensors_present = status.onboard_control_sensors_present;
 			msg.onboard_control_sensors_enabled = status.onboard_control_sensors_enabled;
 			msg.onboard_control_sensors_health = status.onboard_control_sensors_health;
 			msg.load = status.load * 1000.0f;
-			msg.voltage_battery = battery_status.voltage_v * 1000.0f;
-			msg.current_battery = battery_status.current_a * 100.0f;
-			msg.battery_remaining = battery_status.remaining >= 0 ? battery_status.remaining * 100.0f : -1;
+			msg.voltage_battery = (battery_status.connected) ? battery_status.voltage_filtered_v * 1000.0f : UINT16_MAX;
+			msg.current_battery = (battery_status.connected) ? battery_status.current_filtered_a * 100.0f : -1;
+			msg.battery_remaining = (battery_status.connected) ? battery_status.remaining * 100.0f : -1;
 			// TODO: fill in something useful in the fields below
 			msg.drop_rate_comm = 0;
 			msg.errors_comm = 0;
@@ -628,13 +627,13 @@ protected:
 			bat_msg.id = 0;
 			bat_msg.battery_function = MAV_BATTERY_FUNCTION_ALL;
 			bat_msg.type = MAV_BATTERY_TYPE_LIPO;
-			bat_msg.current_consumed = battery_status.discharged_mah;
+			bat_msg.current_consumed = (battery_status.connected) ? battery_status.discharged_mah : -1;
 			bat_msg.energy_consumed = -1;
-			bat_msg.current_battery = battery_status.current_a * 100;
-			bat_msg.battery_remaining = battery_status.remaining >= 0 ? battery_status.remaining * 100.0f : -1;
+			bat_msg.current_battery = (battery_status.connected) ? battery_status.current_filtered_a * 100 : -1;
+			bat_msg.battery_remaining = (battery_status.connected) ? battery_status.remaining * 100.0f : -1;
 			bat_msg.temperature = INT16_MAX;
 			for (unsigned int i = 0; i < (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0])); i++) {
-				if ((int)i < battery_status.cell_count) {
+				if ((int)i < battery_status.cell_count && battery_status.connected) {
 					bat_msg.voltages[i] = (battery_status.voltage_v / battery_status.cell_count) * 1000.0f;
 				} else {
 					bat_msg.voltages[i] = UINT16_MAX;
@@ -2614,7 +2613,7 @@ protected:

 	void send(const hrt_abstime t)
 	{
-		struct vehicle_status_s status;
+		struct vehicle_status_s status = {};
 		(void)_status_sub->update(&status);

 		mavlink_command_long_t msg;
@@ -2890,17 +2889,17 @@ protected:

 			msg.time_usec = hrt_absolute_time();

-			msg.altitude_monotonic = (_sensor_time > 0) ? sensor.baro_alt_meter[0] : 0.0f / 0.0f;
-			msg.altitude_amsl = (_global_pos_time > 0) ? global_pos.alt : 0.0f / 0.0f;
-			msg.altitude_local = (_local_pos_time > 0) ? -local_pos.z : 0.0f / 0.0f;
-			msg.altitude_relative = (_home_time > 0) ? home.alt : 0.0f / 0.0f;
+			msg.altitude_monotonic = (_sensor_time > 0) ? sensor.baro_alt_meter[0] : NAN;
+			msg.altitude_amsl = (_global_pos_time > 0) ? global_pos.alt : NAN;
+			msg.altitude_local = (_local_pos_time > 0) ? -local_pos.z : NAN;
+			msg.altitude_relative = (_home_time > 0) ? (global_pos.alt - home.alt) : NAN;

 			if (global_pos.terrain_alt_valid) {
 				msg.altitude_terrain = global_pos.terrain_alt;
 				msg.bottom_clearance = global_pos.alt - global_pos.terrain_alt;
 			} else {
-				msg.altitude_terrain = 0.0f / 0.0f;
-				msg.bottom_clearance = 0.0f / 0.0f;
+				msg.altitude_terrain = NAN;
+				msg.bottom_clearance = NAN;
 			}

 			_mavlink->send_message(MAVLINK_MSG_ID_ALTITUDE, &msg);
@@ -86,7 +86,7 @@ MavlinkOrbSubscription::update(uint64_t *time, void* data)
 	}

 	if (orb_copy(_topic, _fd, data)) {
-		if (data) {
+		if (data != nullptr) {
 			/* error copying topic data */
 			memset(data, 0, _topic->o_size);
 		}
@@ -182,13 +182,13 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 						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);
+						mavlink_param_value_t param_value;
+						param_value.param_count = param_count_used();
+						param_value.param_index = -1;
+						strncpy(param_value.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+						param_value.param_type = MAV_PARAM_TYPE_UINT32;
+						memcpy(&param_value.param_value, &hash, sizeof(hash));
+						_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &param_value);
 					} else {
 						/* local name buffer to enforce null-terminated string */
 						char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2016 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
@@ -67,7 +67,7 @@ PARAM_DEFINE_INT32(MAV_RADIO_ID, 0);
 * @min 1
 * @group MAVLink
 */
-PARAM_DEFINE_INT32(MAV_TYPE, 1);
+PARAM_DEFINE_INT32(MAV_TYPE, 2);

 /**
 * Use/Accept HIL GPS message even if not in HIL mode
@@ -174,7 +174,8 @@ private:
 		param_t z_vel_p;
 		param_t z_vel_i;
 		param_t z_vel_d;
-		param_t z_vel_max;
+		param_t z_vel_max_up;
+		param_t z_vel_max_down;
 		param_t z_ff;
 		param_t xy_p;
 		param_t xy_vel_p;
@@ -219,6 +220,8 @@ private:
 		float hold_max_xy;
 		float hold_max_z;
 		float acc_hor_max;
+		float vel_max_up;
+		float vel_max_down;
 		uint32_t alt_mode;

 		math::Vector<3> pos_p;
@@ -438,7 +441,16 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_params_handles.z_vel_p		= param_find("MPC_Z_VEL_P");
 	_params_handles.z_vel_i		= param_find("MPC_Z_VEL_I");
 	_params_handles.z_vel_d		= param_find("MPC_Z_VEL_D");
-	_params_handles.z_vel_max	= param_find("MPC_Z_VEL_MAX");
+	_params_handles.z_vel_max_up	= param_find("MPC_Z_VEL_MAX_UP");
+	_params_handles.z_vel_max_down	= param_find("MPC_Z_VEL_MAX");
+
+	// transitional support: Copy param values from max to down
+	// param so that max param can be renamed in 1-2 releases
+	// (currently at 1.3.0)
+	float p;
+	param_get(param_find("MPC_Z_VEL_MAX"), &p);
+	param_set(param_find("MPC_Z_VEL_MAX_DN"), &p);
+
 	_params_handles.z_ff		= param_find("MPC_Z_FF");
 	_params_handles.xy_p		= param_find("MPC_XY_P");
 	_params_handles.xy_vel_p	= param_find("MPC_XY_VEL_P");
@@ -544,13 +556,16 @@ MulticopterPositionControl::parameters_update(bool force)
 		param_get(_params_handles.xy_vel_max, &v);
 		_params.vel_max(0) = v;
 		_params.vel_max(1) = v;
-		param_get(_params_handles.z_vel_max, &v);
+		param_get(_params_handles.z_vel_max_up, &v);
+		_params.vel_max_up = v;
 		_params.vel_max(2) = v;
+		param_get(_params_handles.z_vel_max_down, &v);
+		_params.vel_max_down = v;
 		param_get(_params_handles.xy_vel_cruise, &v);
 		_params.vel_cruise(0) = v;
 		_params.vel_cruise(1) = v;
-		/* using Z max for now */
-		param_get(_params_handles.z_vel_max, &v);
+		/* using Z max up for now */
+		param_get(_params_handles.z_vel_max_up, &v);
 		_params.vel_cruise(2) = v;
 		param_get(_params_handles.xy_ff, &v);
 		v = math::constrain(v, 0.0f, 1.0f);
@@ -588,8 +603,8 @@ MulticopterPositionControl::parameters_update(bool force)
 		_params.mc_att_yaw_p = v;

 		/* takeoff and land velocities should not exceed maximum */
-		_params.tko_speed = fminf(_params.tko_speed, _params.vel_max(2));
-		_params.land_speed = fminf(_params.land_speed, _params.vel_max(2));
+		_params.tko_speed = fminf(_params.tko_speed, _params.vel_max_up);
+		_params.land_speed = fminf(_params.land_speed, _params.vel_max_down);
 	}

 	return OK;
@@ -1448,10 +1463,11 @@ MulticopterPositionControl::task_main()
 				}

 				/* make sure velocity setpoint is saturated in z*/
-				float vel_norm_z = sqrtf(_vel_sp(2) * _vel_sp(2));
-
-				if (vel_norm_z > _params.vel_max(2)) {
-					_vel_sp(2) = _vel_sp(2) * _params.vel_max(2) / vel_norm_z;
+				if (_vel_sp(2) < -1.0f * _params.vel_max_up){
+					_vel_sp(2) = -1.0f * _params.vel_max_up;
+				}
+				if (_vel_sp(2) >  _params.vel_max_down) {
+					_vel_sp(2) = _params.vel_max_down;
 				}

 				if (!_control_mode.flag_control_position_enabled) {
@@ -1617,7 +1633,7 @@ MulticopterPositionControl::task_main()
 					if (_control_mode.flag_control_acceleration_enabled && _pos_sp_triplet.current.acceleration_valid) {
 						thrust_sp = math::Vector<3>(_pos_sp_triplet.current.a_x,_pos_sp_triplet.current.a_y,_pos_sp_triplet.current.a_z);
 					} else {
-						thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int;						
+						thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int;
 					}

 					if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
@@ -1975,8 +1991,40 @@ MulticopterPositionControl::task_main()

 			math::Matrix<3, 3> R_sp;

-			/* construct attitude setpoint rotation matrix */
-			R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body);
+			// construct attitude setpoint rotation matrix. modify the setpoints for roll
+			// and pitch such that they reflect the user's intention even if a yaw error
+			// (yaw_sp - yaw) is present. In the presence of a yaw error constructing a rotation matrix
+			// from the pure euler angle setpoints will lead to unexpected attitude behaviour from
+			// the user's view as the euler angle sequence uses the  yaw setpoint and not the current
+			// heading of the vehicle.
+
+			// calculate our current yaw error
+			float yaw_error = _wrap_pi(_att_sp.yaw_body - _yaw);
+
+			// compute the vector obtained by rotating a z unit vector by the rotation
+			// given by the roll and pitch commands of the user
+			math::Vector<3> zB = {0, 0, 1};
+			math::Matrix<3,3> R_sp_roll_pitch;
+			R_sp_roll_pitch.from_euler(_att_sp.roll_body, _att_sp.pitch_body, 0);
+			math::Vector<3> z_roll_pitch_sp = R_sp_roll_pitch * zB;
+
+
+			// transform the vector into a new frame which is rotated around the z axis
+			// by the current yaw error. this vector defines the desired tilt when we look
+			// into the direction of the desired heading
+			math::Matrix<3,3> R_yaw_correction;
+			R_yaw_correction.from_euler(0.0f, 0.0f, -yaw_error);
+			z_roll_pitch_sp = R_yaw_correction * z_roll_pitch_sp;
+
+			// use the formula z_roll_pitch_sp = R_tilt * [0;0;1]
+			// to calculate the new desired roll and pitch angles
+			// R_tilt can be written as a function of the new desired roll and pitch
+			// angles. we get three equations and have to solve for 2 unknowns
+			float pitch_new = asinf(z_roll_pitch_sp(0));
+			float roll_new = -atan2f(z_roll_pitch_sp(1), z_roll_pitch_sp(2));
+
+			R_sp.from_euler(roll_new, pitch_new, _att_sp.yaw_body);
+
 			memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body));

 			/* reset the acceleration set point for all non-attitude flight modes */
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2016 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
@@ -110,6 +110,7 @@ PARAM_DEFINE_FLOAT(MPC_ALTCTL_DY, 0.0f);
 * @min 0.0
 * @max 0.95
 * @decimal 2
+ * @increment 0.01
 * @group Multicopter Position Control
 */
 PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.9f);
@@ -123,6 +124,7 @@ PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.9f);
 * @min 0.0
 * @max 1.0
 * @decimal 2
+ * @increment 0.01
 * @group Multicopter Position Control
 */
 PARAM_DEFINE_FLOAT(MPC_MANTHR_MIN, 0.08f);
@@ -139,6 +141,7 @@ PARAM_DEFINE_FLOAT(MPC_MANTHR_MIN, 0.08f);
 * @min 0.0
 * @max 1.0
 * @decimal 2
+ * @increment 0.01
 * @group Multicopter Position Control
 */
 PARAM_DEFINE_FLOAT(MPC_MANTHR_MAX, 0.9f);
@@ -147,6 +150,7 @@ PARAM_DEFINE_FLOAT(MPC_MANTHR_MAX, 0.9f);
 * Proportional gain for vertical position error
 *
 * @min 0.0
+ * @max 1.5
 * @decimal 2
 * @group Multicopter Position Control
 */
@@ -155,7 +159,8 @@ PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f);
 /**
 * Proportional gain for vertical velocity error
 *
- * @min 0.0
+ * @min 0.1
+ * @max 0.4
 * @decimal 2
 * @group Multicopter Position Control
 */
@@ -166,7 +171,8 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.2f);
 *
 * Non zero value allows hovering thrust estimation on stabilized or autonomous takeoff.
 *
- * @min 0.0
+ * @min 0.01
+ * @max 0.1
 * @decimal 3
 * @group Multicopter Position Control
 */
@@ -176,23 +182,46 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.02f);
 * Differential gain for vertical velocity error
 *
 * @min 0.0
+ * @max 0.1
 * @decimal 3
 * @group Multicopter Position Control
 */
 PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);

 /**
- * Maximum vertical velocity
+ * Maximum vertical ascent velocity
 *
 * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL, POSCTRL).
 *
 * @unit m/s
- * @min 0.0
+ * @min 0.5
 * @max 8.0
 * @decimal 1
 * @group Multicopter Position Control
 */
-PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_UP, 3.0f);
+
+/**
+ * Maximum vertical descent velocity
+ *
+ * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL, POSCTRL).
+ *
+ * @unit m/s
+ * @min 0.5
+ * @max 4.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 1.0f);
+
+/**
+ * Transitional support, do not change / use
+ *
+ * @unit m/s
+ * @min 0.5
+ * @max 4.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_DN, 1.0f);

 /**
 * Vertical velocity feed forward
@@ -210,6 +239,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f);
 * Proportional gain for horizontal position error
 *
 * @min 0.0
+ * @max 2.0
 * @decimal 2
 * @group Multicopter Position Control
 */
@@ -218,11 +248,12 @@ PARAM_DEFINE_FLOAT(MPC_XY_P, 1.25f);
 /**
 * Proportional gain for horizontal velocity error
 *
- * @min 0.0
+ * @min 0.06
+ * @max 0.15
 * @decimal 2
 * @group Multicopter Position Control
 */
-PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.065f);
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.09f);

 /**
 * Integral gain for horizontal velocity error
@@ -230,6 +261,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.065f);
 * Non-zero value allows to resist wind.
 *
 * @min 0.0
+ * @max 0.1
 * @decimal 3
 * @group Multicopter Position Control
 */
@@ -238,7 +270,8 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.02f);
 /**
 * Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
 *
- * @min 0.0
+ * @min 0.005
+ * @max 0.1
 * @decimal 3
 * @group Multicopter Position Control
 */
@@ -252,7 +285,9 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f);
 * position stabilized mode (POSCTRL).
 *
 * @unit m/s
- * @min 0.0
+ * @min 3.0
+ * @max 20.0
+ * @increment 1
 * @decimal 2
 * @group Multicopter Position Control
 */
@@ -315,7 +350,7 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.0f);
 * Landing descend rate
 *
 * @unit m/s
- * @min 0.0
+ * @min 0.2
 * @decimal 1
 * @group Multicopter Position Control
 */
@@ -325,7 +360,8 @@ PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.5f);
 * Takeoff climb rate
 *
 * @unit m/s
- * @min 0.0
+ * @min 1
+ * @max 5
 * @decimal 2
 * @group Multicopter Position Control
 */
@@ -358,6 +394,7 @@ PARAM_DEFINE_FLOAT(MPC_MAN_P_MAX, 35.0f);
 *
 * @unit deg/s
 * @min 0.0
+ * @max 400
 * @decimal 1
 * @group Multicopter Position Control
 */
@@ -378,6 +415,7 @@ PARAM_DEFINE_FLOAT(MPC_HOLD_XY_DZ, 0.1f);
 *
 * @unit m/s
 * @min 0.0
+ * @max 3.0
 * @decimal 2
 * @group Multicopter Position Control
 */
@@ -388,6 +426,7 @@ PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_XY, 0.8f);
 *
 * @unit m/s
 * @min 0.0
+ * @max 3.0
 * @decimal 2
 * @group Multicopter Position Control
 */
@@ -398,6 +437,7 @@ PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_Z, 0.6f);
 *
 * @unit Hz
 * @min 0.0
+ * @max 10
 * @decimal 2
 * @group Multicopter Position Control
 */
@@ -51,12 +51,17 @@ int px4muorb_orb_initialize()
 {
 	HAP_power_request(100, 100, 1000);

-	// register the fastrpc muorb with uORBManager.
+	// The uORB Manager needs to be initialized first up, otherwise the instance is nullptr.
+	uORB::Manager::initialize();
+	// Register the fastrpc muorb with uORBManager.
 	uORB::Manager::get_instance()->set_uorb_communicator(uORB::FastRpcChannel::GetInstance());
+
+	// Now continue with the usual dspal startup.
 	const char *argv[] = {"dspal", "start"};
 	int argc = 2;
 	int rc;
 	rc = dspal_main(argc, (char **)argv);
+
 	return rc;
 }

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -45,6 +45,8 @@
 #include <math.h>
 #include <fcntl.h>

+#include <geo/geo.h>
+
 #include <systemlib/mavlink_log.h>
 #include <systemlib/err.h>

@@ -56,7 +58,8 @@

 Loiter::Loiter(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name),
-	_param_min_alt(this, "MIS_LTRMIN_ALT", false)
+	_param_min_alt(this, "MIS_LTRMIN_ALT", false),
+	_loiter_pos_set(false)
 {
 	// load initial params
 	updateParams();
@@ -69,6 +72,7 @@ Loiter::~Loiter()
 void
 Loiter::on_inactive()
 {
+	_loiter_pos_set = false;
 }

 void
@@ -77,19 +81,7 @@ Loiter::on_activation()
 	if (_navigator->get_reposition_triplet()->current.valid) {
 		reposition();
 	} else {
-		// set current mission item to loiter
-		set_loiter_item(&_mission_item, _param_min_alt.get());
-
-		// convert mission item to current setpoint
-		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-		pos_sp_triplet->current.velocity_valid = false;
-		pos_sp_triplet->previous.valid = false;
-		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
-		pos_sp_triplet->next.valid = false;
-
-		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
-
-		_navigator->set_position_setpoint_triplet_updated();
+		set_loiter_position();
 	}
 }

@@ -99,11 +91,46 @@ Loiter::on_active()
 	if (_navigator->get_reposition_triplet()->current.valid) {
 		reposition();
 	}
+
+	// reset the loiter position if we get disarmed
+	if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
+		_loiter_pos_set = false;
+	}
+}
+
+void
+Loiter::set_loiter_position()
+{
+	// not setting loiter position until armed
+	if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED ||
+		_loiter_pos_set) {
+		return;
+	} else {
+		_loiter_pos_set = true;
+	}
+
+	// set current mission item to loiter
+	set_loiter_item(&_mission_item, _param_min_alt.get());
+
+	// convert mission item to current setpoint
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	pos_sp_triplet->current.velocity_valid = false;
+	pos_sp_triplet->previous.valid = false;
+	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	pos_sp_triplet->next.valid = false;
+
+	_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
+
+	_navigator->set_position_setpoint_triplet_updated();
 }

 void
 Loiter::reposition()
 {
+	// we can't reposition if we are not armed yet
+	if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
+		return;
+	}

 	struct position_setpoint_triplet_s *rep = _navigator->get_reposition_triplet();

@@ -113,10 +140,27 @@ Loiter::reposition()
 		// convert mission item to current setpoint
 		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 		pos_sp_triplet->current.velocity_valid = false;
-		memcpy(&pos_sp_triplet->previous, &rep->previous, sizeof(rep->previous));
+		pos_sp_triplet->previous.yaw = _navigator->get_global_position()->yaw;
+		pos_sp_triplet->previous.lat = _navigator->get_global_position()->lat;
+		pos_sp_triplet->previous.lon = _navigator->get_global_position()->lon;
+		pos_sp_triplet->previous.alt = _navigator->get_global_position()->alt;
 		memcpy(&pos_sp_triplet->current, &rep->current, sizeof(rep->current));
 		pos_sp_triplet->next.valid = false;

+		// set yaw
+
+		float travel_dist = get_distance_to_next_waypoint(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
+								pos_sp_triplet->current.lat, pos_sp_triplet->current.lon);
+
+		if (travel_dist > 1.0f) {
+			// calculate direction the vehicle should point to.
+			pos_sp_triplet->current.yaw = get_bearing_to_next_waypoint(
+					_navigator->get_global_position()->lat,
+					_navigator->get_global_position()->lon,
+					pos_sp_triplet->current.lat,
+					pos_sp_triplet->current.lon);
+		}
+
 		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);

 		_navigator->set_position_setpoint_triplet_updated();
@@ -67,7 +67,13 @@ private:
 	 */
 	void reposition();

+	/**
+	 * Set the position to hold based on the current local position
+	 */
+	void set_loiter_position();
+
 	control::BlockParamFloat _param_min_alt;
+	bool _loiter_pos_set;
 };

 #endif
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2016 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
@@ -146,6 +146,11 @@ Mission::on_inactive()
 	/* require takeoff after non-loiter or landing */
 	if (!_navigator->get_can_loiter_at_sp() || _navigator->get_land_detected()->landed) {
 		_need_takeoff = true;
+		/* Reset work item type to default if auto take-off has been paused or aborted,
+		   and we landed in manual mode. */
+		if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) {
+			_work_item_type = WORK_ITEM_TYPE_DEFAULT;		
+		}
 	}
 }

@@ -859,11 +864,12 @@ Mission::altitude_sp_foh_update()
 		return;
 	}

-	/* Don't do FOH for landing and takeoff waypoints, the ground may be near
+	/* Don't do FOH for non-missions, landing and takeoff waypoints, the ground may be near
 	 * and the FW controller has a custom landing logic */
 	if (_mission_item.nav_cmd == NAV_CMD_LAND
 			|| _mission_item.nav_cmd == NAV_CMD_VTOL_LAND
-			|| _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
+			|| _mission_item.nav_cmd == NAV_CMD_TAKEOFF
+			|| !_navigator->is_planned_mission()) {
 		return;
 	}

@@ -539,11 +539,13 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
 	if (at_current_location) {
 		item->lat = _navigator->get_global_position()->lat;
 		item->lon = _navigator->get_global_position()->lon;
+		item->yaw = _navigator->get_global_position()->yaw;

 	/* use home position */
 	} else {
 		item->lat = _navigator->get_home_position()->lat;
 		item->lon = _navigator->get_home_position()->lon;
+		item->yaw = _navigator->get_home_position()->yaw;
 	}

 	item->altitude = 0;
@@ -143,6 +143,7 @@ public:
 	bool				    home_position_valid() { return (_home_pos.timestamp > 0); }
 	struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
 	struct position_setpoint_triplet_s* get_reposition_triplet() { return &_reposition_triplet; }
+	struct position_setpoint_triplet_s* get_takeoff_triplet() { return &_takeoff_triplet; }
 	struct mission_result_s*	    get_mission_result() { return &_mission_result; }
 	struct geofence_result_s*		    get_geofence_result() { return &_geofence_result; }
 	struct vehicle_attitude_setpoint_s* get_att_sp() { return &_att_sp; }
@@ -191,6 +192,8 @@ public:

 	void 		set_mission_failure(const char *reason);

+	bool		is_planned_mission() { return _navigation_mode == &_mission; }
+
 private:

 	bool		_task_should_exit;		/**< if true, sensor task should exit */
@@ -229,6 +232,7 @@ private:
 	navigation_capabilities_s			_nav_caps;		/**< navigation capabilities */
 	position_setpoint_triplet_s			_pos_sp_triplet;	/**< triplet of position setpoints */
 	position_setpoint_triplet_s			_reposition_triplet;	/**< triplet for non-mission direct position command */
+	position_setpoint_triplet_s			_takeoff_triplet;	/**< triplet for non-mission direct takeoff command */

 	mission_result_s				_mission_result;
 	geofence_result_s				_geofence_result;
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2016 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
@@ -129,6 +129,7 @@ Navigator::Navigator() :
 	_nav_caps{},
 	_pos_sp_triplet{},
 	_reposition_triplet{},
+	_takeoff_triplet{},
 	_mission_result{},
 	_att_sp{},
 	_mission_item_valid(false),
@@ -319,11 +320,13 @@ Navigator::task_main()
 	params_update();

 	/* wakeup source(s) */
-	px4_pollfd_struct_t fds[1] = {};
+	px4_pollfd_struct_t fds[2] = {};

 	/* Setup of loop */
 	fds[0].fd = _global_pos_sub;
 	fds[0].events = POLLIN;
+	fds[1].fd = _vehicle_command_sub;
+	fds[1].events = POLLIN;

 	bool global_pos_available_once = false;

@@ -413,7 +416,7 @@ Navigator::task_main()
 				struct position_setpoint_triplet_s *rep = get_reposition_triplet();

 				// store current position as previous position and goal as next
-				rep->previous.yaw = NAN;
+				rep->previous.yaw = get_global_position()->yaw;
 				rep->previous.lat = get_global_position()->lat;
 				rep->previous.lon = get_global_position()->lon;
 				rep->previous.alt = get_global_position()->alt;
@@ -446,9 +449,28 @@ Navigator::task_main()
 				rep->previous.valid = true;
 				rep->current.valid = true;
 				rep->next.valid = false;
-			}
+			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) {
+				struct position_setpoint_triplet_s *rep = get_takeoff_triplet();

-			if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) {
+				// store current position as previous position and goal as next
+				rep->previous.yaw = get_global_position()->yaw;
+				rep->previous.lat = get_global_position()->lat;
+				rep->previous.lon = get_global_position()->lon;
+				rep->previous.alt = get_global_position()->alt;
+
+				rep->current.loiter_radius = get_loiter_radius();
+				rep->current.loiter_direction = 1;
+				rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
+				rep->current.yaw = cmd.param4;
+				rep->current.lat = cmd.param5 / (double)1e7;
+				rep->current.lon = cmd.param6 / (double)1e7;
+				rep->current.alt = cmd.param7;
+
+				rep->previous.valid = true;
+				rep->current.valid = true;
+				rep->next.valid = false;
+
+			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) {
 				warnx("navigator: got pause/continue command");
 			}
 		}
@@ -662,6 +684,11 @@ Navigator::publish_position_setpoint_triplet()
 	/* update navigation state */
 	_pos_sp_triplet.nav_state = _vstatus.nav_state;

+	/* do not publish an empty triplet */
+	if (!_pos_sp_triplet.current.valid) {
+		return;
+	}
+
 	/* lazily publish the position setpoint triplet only once available */
 	if (_pos_sp_triplet_pub != nullptr) {
 		orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2016 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
@@ -102,7 +102,7 @@ RTL::on_activation()
 		/* for safety reasons don't go into RTL if landed */
 		if (_navigator->get_land_detected()->landed) {
 			_rtl_state = RTL_STATE_LANDED;
-			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no RTL when landed");
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already landed, not executing RTL");

 		/* if lower than return altitude, climb up first */
 		} else if (home_dist > _param_rtl_min_dist.get() && _navigator->get_global_position()->alt < _navigator->get_home_position()->alt
@@ -163,7 +163,7 @@ RTL::set_rtl_item()
 		_mission_item.autocontinue = true;
 		_mission_item.origin = ORIGIN_ONBOARD;

-		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)",
+		mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)",
 			(int)(climb_alt),
 			(int)(climb_alt - _navigator->get_home_position()->alt));
 		break;
@@ -205,7 +205,7 @@ RTL::set_rtl_item()
 		_mission_item.autocontinue = true;
 		_mission_item.origin = ORIGIN_ONBOARD;

-		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)",
+		mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)",
 			(int)(_mission_item.altitude),
 			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));

@@ -255,7 +255,7 @@ RTL::set_rtl_item()
 		/* disable previous setpoint to prevent drift */
 		pos_sp_triplet->previous.valid = false;

-		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)",
+		mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)",
 			(int)(_mission_item.altitude),
 			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
 		break;
@@ -280,10 +280,10 @@ RTL::set_rtl_item()
 		_navigator->set_can_loiter_at_sp(true);

 		if (autoland && (_mission_item.time_inside > FLT_EPSILON)) {
-			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs", (double)_mission_item.time_inside);
+			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs", (double)_mission_item.time_inside);

 		} else {
-			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter");
+			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter");
 		}
 		break;
 	}
@@ -292,14 +292,14 @@ RTL::set_rtl_item()
 		_mission_item.yaw = _navigator->get_home_position()->yaw;
 		set_land_item(&_mission_item, false);

-		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: land at home");
+		mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at home");
 		break;
 	}

 	case RTL_STATE_LANDED: {
 		set_idle_item(&_mission_item);

-		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: completed, landed");
+		mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, landed");
 		break;
 	}

@@ -57,7 +57,7 @@ Takeoff::Takeoff(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name),
 	_param_min_alt(this, "MIS_TAKEOFF_ALT", false)
 {
-	/* load initial params */
+	// load initial params
 	updateParams();
 }

@@ -73,14 +73,40 @@ Takeoff::on_inactive()
 void
 Takeoff::on_activation()
 {
-	/* set current mission item to Takeoff */
+	set_takeoff_position();
+}
+
+void
+Takeoff::on_active()
+{
+	struct position_setpoint_triplet_s *rep = _navigator->get_takeoff_triplet();
+	if (rep->current.valid) {
+		// reset the position
+		set_takeoff_position();
+
+	} else if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) {
+		_navigator->get_mission_result()->finished = true;
+		_navigator->set_mission_result_updated();
+
+		// set loiter item so position controllers stop doing takeoff logic
+		set_loiter_item(&_mission_item);
+		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+		_navigator->set_position_setpoint_triplet_updated();
+	}
+}
+
+void
+Takeoff::set_takeoff_position()
+{
+	// set current mission item to takeoff
 	set_takeoff_item(&_mission_item, _param_min_alt.get());
 	_navigator->get_mission_result()->reached = false;
 	_navigator->get_mission_result()->finished = false;
 	_navigator->set_mission_result_updated();
 	reset_mission_item_reached();

-	/* convert mission item to current setpoint */
+	// convert mission item to current setpoint
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 	pos_sp_triplet->previous.valid = false;
 	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
@@ -88,22 +114,28 @@ Takeoff::on_activation()
 	pos_sp_triplet->current.yaw_valid = true;
 	pos_sp_triplet->next.valid = false;

+	// check if a specific target altitude has been set
+	struct position_setpoint_triplet_s *rep = _navigator->get_takeoff_triplet();
+	if (rep->current.valid) {
+		if (PX4_ISFINITE(rep->current.alt)) {
+			pos_sp_triplet->current.alt = rep->current.alt;
+		}
+
+		// Go on and check which changes had been requested
+		if (PX4_ISFINITE(rep->current.yaw)) {
+			pos_sp_triplet->current.yaw = rep->current.yaw;
+		}
+
+		if (PX4_ISFINITE(rep->current.lat) && PX4_ISFINITE(rep->current.lon)) {
+			pos_sp_triplet->current.lat = rep->current.lat;
+			pos_sp_triplet->current.lon = rep->current.lon;
+		}
+
+		// mark this as done
+		memset(rep, 0, sizeof(*rep));
+	}
+
 	_navigator->set_can_loiter_at_sp(true);

 	_navigator->set_position_setpoint_triplet_updated();
 }
-
-void
-Takeoff::on_active()
-{
-	if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) {
-		_navigator->get_mission_result()->finished = true;
-		_navigator->set_mission_result_updated();
-
-		/* set loiter item so position controllers stop doing takeoff logic */
-		set_loiter_item(&_mission_item);
-		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
-		_navigator->set_position_setpoint_triplet_updated();
-	}
-}
@@ -62,6 +62,8 @@ public:

 private:
 	control::BlockParamFloat _param_min_alt;
+
+	void set_takeoff_position();
 };

 #endif
@@ -410,7 +410,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])

 	/* wait for initial baro value */
 	bool wait_baro = true;
-	TerrainEstimator *terrain_estimator = new TerrainEstimator();
+	TerrainEstimator terrain_estimator;

 	thread_running = true;
 	hrt_abstime baro_wait_for_sample_time = hrt_absolute_time();
@@ -1264,8 +1264,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		}

 		/* run terrain estimator */
-		terrain_estimator->predict(dt, &att, &sensor, &lidar);
-		terrain_estimator->measurement_update(hrt_absolute_time(), &gps, &lidar, &att);
+		terrain_estimator.predict(dt, &att, &sensor, &lidar);
+		terrain_estimator.measurement_update(hrt_absolute_time(), &gps, &lidar, &att);

 		if (inav_verbose_mode) {
 			/* print updates rate */
@@ -1358,8 +1358,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				global_pos.eph = eph;
 				global_pos.epv = epv;

-				if (terrain_estimator->is_valid()) {
-					global_pos.terrain_alt = global_pos.alt - terrain_estimator->get_distance_to_ground();
+				if (terrain_estimator.is_valid()) {
+					global_pos.terrain_alt = global_pos.alt - terrain_estimator.get_distance_to_ground();
 					global_pos.terrain_alt_valid = true;

 				} else {
@@ -989,6 +989,7 @@ int sdlog2_thread_main(int argc, char *argv[])

 		case 'e':
 			log_on_start = true;
+			log_when_armed = true;
 			break;

 		case 'a':
@@ -1230,6 +1231,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_EST6_s log_INO3;
 			struct log_RPL3_s log_RPL3;
 			struct log_RPL4_s log_RPL4;
+			struct log_RPL6_s log_RPL6;
 			struct log_LAND_s log_LAND;
 		} body;
 	} log_msg = {
@@ -1532,6 +1534,19 @@ int sdlog2_thread_main(int argc, char *argv[])
 				LOGBUFFER_WRITE_AND_COUNT(RPL4);
 			}

+
+			if (buf.replay.asp_timestamp > 0) {
+				log_msg.msg_type = LOG_RPL6_MSG;
+				log_msg.body.log_RPL6.timestamp = buf.replay.asp_timestamp;
+				log_msg.body.log_RPL6.indicated_airspeed_m_s = buf.replay.indicated_airspeed_m_s;
+				log_msg.body.log_RPL6.true_airspeed_m_s = buf.replay.true_airspeed_m_s;
+				log_msg.body.log_RPL6.true_airspeed_unfiltered_m_s = buf.replay.true_airspeed_unfiltered_m_s;
+				log_msg.body.log_RPL6.air_temperature_celsius = buf.replay.air_temperature_celsius;
+				log_msg.body.log_RPL6.confidence = buf.replay.confidence;
+				LOGBUFFER_WRITE_AND_COUNT(RPL6);
+			}
+
+
 		} else { /* !record_replay_log */

 			// we poll on sensor combined, so we know it has updated just now
@@ -1819,7 +1834,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.msg_type = LOG_BATT_MSG;
 				log_msg.body.log_BATT.voltage = buf.battery.voltage_v;
 				log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v;
-				log_msg.body.log_BATT.current = buf.battery.current_a;
+				log_msg.body.log_BATT.current = buf.battery.current_a;				
+				log_msg.body.log_BATT.current_filtered = buf.battery.current_filtered_a;
 				log_msg.body.log_BATT.discharged = buf.battery.discharged_mah;
 				log_msg.body.log_BATT.remaining = buf.battery.remaining;
 				log_msg.body.log_BATT.warning = buf.battery.warning;
@@ -1945,6 +1961,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.body.log_BATT.voltage = buf.battery.voltage_v;
 				log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v;
 				log_msg.body.log_BATT.current = buf.battery.current_a;
+				log_msg.body.log_BATT.current_filtered = buf.battery.current_filtered_a;
 				log_msg.body.log_BATT.discharged = buf.battery.discharged_mah;
 				LOGBUFFER_WRITE_AND_COUNT(BATT);
 			}
@@ -292,6 +292,7 @@ struct log_BATT_s {
 	float voltage;
 	float voltage_filtered;
 	float current;
+	float current_filtered;
 	float discharged;
 	float remaining;
 	uint8_t warning;
@@ -576,6 +577,18 @@ struct log_RPL4_s {
 	float range_to_ground;
 };

+/* --- EKF2 REPLAY Part 4 --- */
+#define LOG_RPL6_MSG 59
+struct log_RPL6_s {
+	uint64_t timestamp;
+	float indicated_airspeed_m_s;
+	float true_airspeed_m_s;
+	float true_airspeed_unfiltered_m_s;
+	float air_temperature_celsius;
+	float confidence;
+};
+
+

 /* --- CAMERA TRIGGER --- */
 #define LOG_CAMT_MSG 55
@@ -643,7 +656,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GPSP, "BLLffBfbf",		"NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
 	LOG_FORMAT(ESC, "HBBBHHffiffH",		"count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 	LOG_FORMAT(GVSP, "fff",			"VX,VY,VZ"),
-	LOG_FORMAT(BATT, "fffffB",		"V,VFilt,C,Discharged,Remaining,Warning"),
+	LOG_FORMAT(BATT, "ffffffB",		"V,VFilt,C,CFilt,Discharged,Remaining,Warning"),
 	LOG_FORMAT(DIST, "BBBff",			"Id,Type,Orientation,Distance,Covariance"),
 	LOG_FORMAT_S(TEL0, TEL, "BBBBHHBQ",		"RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
 	LOG_FORMAT_S(TEL1, TEL, "BBBBHHBQ",		"RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
@@ -673,6 +686,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(RPL2, "QQLLiMMfffffffM", "Tpos,Tvel,lat,lon,alt,fix,nsats,eph,epv,sacc,v,vN,vE,vD,v_val"),
 	LOG_FORMAT(RPL3, "QffffIB", "Tflow,fx,fy,gx,gy,delT,qual"),
 	LOG_FORMAT(RPL4, "Qf", "Trng,rng"),
+	LOG_FORMAT(RPL6, "Qfffff", "Tasp,inAsp,trAsp,ufAsp,tpAsp,confAsp"),
 	LOG_FORMAT(LAND, "B", "Landed"),
 	/* system-level messages, ID >= 0x80 */
 	/* FMT: don't write format of format message, it's useless */
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Julian Oes	a031f6eec8	land_detector: fix timestamp type (#4710 ) The overflow of the uint32_t lead to the land_detector start getting aborted.	2016-06-01 17:16:29 +02:00
Pavel Kirienko	0749b04c9f	Fixed stack overflow in UAVCAN process (#4643 ) * Increased uavcan stack size; the old value of 1800 was insufficient * Removed a misleading warning message from uavcan servers initialization	2016-05-26 20:00:46 +02:00
Lorenz Meier	0902401115	SBUS decoding: Fix channel 18 return value	2016-05-21 15:55:32 +02:00
Lorenz Meier	87a6d36341	EKF2: Remove unused header	2016-05-21 14:13:30 +02:00
Lorenz Meier	bdf01061d0	Bottle drop: Fix uninitialized member	2016-05-21 14:13:22 +02:00
Lorenz Meier	62c76d62cd	LPE: Fix stack smashing	2016-05-21 14:13:15 +02:00
Lorenz Meier	65c4a9190c	EKF1: Simplify output	2016-05-21 14:13:08 +02:00
Lorenz Meier	5070238504	EKF1: Fix stack smashing resulting from uninitialized publication	2016-05-21 14:13:00 +02:00
Lorenz Meier	2f52c90885	Fix compile error in MAVLink app	2016-05-19 15:34:25 +02:00
Roman Bapst	9b4b67644d	WIP: Manual attitude setpoint for large heading errors (#4564 ) * mc pos control: in manual mode calculate attitude setpoint such that it reflects the users intuition of roll and pitch for any given heading error * added some comments on the new manual attitude setpoint generation * make calculation shorter	2016-05-19 15:32:08 +02:00
Lorenz Meier	556c33cf8c	Battery lib: Set valid flag	2016-05-19 15:32:08 +02:00
Lorenz Meier	ac256eb64d	MAVLink: Use valid flag to initialize fields Conflicts: src/modules/mavlink/mavlink_messages.cpp	2016-05-19 15:32:08 +02:00
Lorenz Meier	fd3efe168a	Battery status: Add valid flag	2016-05-19 15:32:08 +02:00
Lorenz Meier	d091af4fc0	EKF: Be less verbose, avoid floating ng point printing stack smashing	2016-05-14 18:22:58 +02:00
Lorenz Meier	3ebea78d91	IO driver: Fix PWM load	2016-05-14 18:06:32 +02:00
Lorenz Meier	8361b8b3c6	PWM cmd: better reporting	2016-05-14 18:06:24 +02:00
Lorenz Meier	2807e53838	Navigator: Do not publish an empty triplet	2016-05-13 11:05:18 +02:00
Lorenz Meier	2cc613cf64	Commander: Better status feedback	2016-05-13 11:05:12 +02:00
Lorenz Meier	d017514d59	sdlog2: Start / stop logging on arming even if enabled already during boot	2016-05-11 14:46:23 +02:00
Lorenz Meier	165f75589b	MAVLink app: Use proper C99 NaN define	2016-05-11 12:59:57 +02:00
Lorenz Meier	e016f6ca38	Clang: Do not use new pointer option	2016-05-11 12:59:41 +02:00
Lorenz Meier	34baf01d7e	Fix altitude estimate	2016-05-11 10:02:03 +02:00
Julian Oes	097840ef83	eagle: fix DSP build The hexagon-clang is clang-3.5 and does not support -fcheck-new.	2016-05-11 09:32:05 +02:00
Lorenz Meier	52af5408d3	Racer: Disable attitude bias estimatition	2016-05-10 22:57:59 +02:00
Mark Whitehorn	b429fbc024	update generic 250 racing quad parameters	2016-05-10 22:33:19 +02:00
sander	29dd1ad47a	Code style	2016-05-10 21:34:11 +02:00
sander	1a04e952f8	Inline comment addition	2016-05-10 21:34:11 +02:00
sander	e0a8571254	Use filtered voltage and current values for mavlink sys message	2016-05-10 21:34:11 +02:00
sander	9a09c5af5c	Add low pass filtered current draw	2016-05-10 21:34:11 +02:00
CarlOlsson	6b5e77250f	ekf2: Added airspeed to rpl logging	2016-05-10 21:32:55 +02:00
Roman	f4c8bd9be3	updated gains of QAV 250 racer: - tested in acro mode with good orientation lock and good response - tested in stabilize mode with good attitude tracking - needs more testing and maybe small adjustments	2016-05-10 21:27:10 +02:00
Beat Küng	7d733c5ccd	cmake: add -fcheck-new to cxx_compile_flags GCC assumes that operator new never returns null, but throws an exception instead. This is defined by the C++ standard, and thus ok. But we disable exceptions with -fno-exceptions, so we break this assumption. GCC then goes ahead and removes some of our nullptr checks. This flag removes the assumption. This adds ~1.4kB to the binary size of the px4fmu-v4 target.	2016-05-10 21:23:53 +02:00
Lorenz Meier	8960ab3402	Navigator: differentiate between takeoff and reposition commands, perform calculations for repositions only when armed.	2016-05-10 13:58:42 +02:00
Lorenz Meier	befab7303f	Commander: Fix modes switching back to prev mode	2016-05-10 13:57:05 +02:00
Lorenz Meier	06938064c1	MAVLink: Always accept commands to avoid corner case with USB power	2016-05-10 12:41:48 +02:00
Lorenz Meier	86a5244bc9	RTL: Set RTL messages to info so they do not distract the user from the actual operation	2016-05-10 12:40:59 +02:00
Lorenz Meier	098db8595f	Style fix	2016-05-10 12:40:28 +02:00
Lorenz Meier	05b19c8e85	Fix yaw handling for land command	2016-05-10 12:09:47 +02:00
Lorenz Meier	65f9a86c19	Navigator: Only run FOH logic when in mission mode	2016-05-10 10:13:37 +02:00
Lorenz Meier	a686d9a166	Navigator: Add mission state feedback	2016-05-10 10:12:29 +02:00
Lorenz Meier	09d8476c1d	Navigator: update header	2016-05-10 10:12:10 +02:00
Andreas Antener	3b88937594	added ROTATION_PITCH_90_ROLL_270	2016-05-10 09:29:06 +02:00
Lorenz Meier	c31c29097d	Rename params, add transitional support	2016-05-10 09:27:00 +02:00
Adyasha Dash	468b0b25de	added different parameters for ascent and descent rates	2016-05-10 09:18:40 +02:00
Lorenz Meier	ffb0d37c8a	Commander: Fix reposition handling, run faster to allow catching of consecutive commands	2016-05-09 23:01:54 +02:00
Lorenz Meier	b9333d95f4	Navigator: Run faster	2016-05-09 23:01:54 +02:00
Kabir Mohammed	4ed112b259	Remove old toolchain	2016-05-09 16:13:01 +02:00
Lorenz Meier	c5a6442ce6	Perf print: Fix division by zero	2016-05-09 09:03:30 +02:00
Lorenz Meier	d369a26db4	Fix ESC calibration	2016-05-09 08:44:50 +02:00
Lorenz Meier	32cd154d7c	Revert "ESC cal: Increase timeouts" This reverts commit `d2575c2556`.	2016-05-09 08:37:05 +02:00
Lorenz Meier	0917a346e4	Mag cal: allow 6, 3 and 2 side calibrations (and anything in-between with bitfields)	2016-05-09 00:32:54 +02:00
Lorenz Meier	4aec95b239	HMC5883: Be less sensitive to large scaling errors and offsets	2016-05-07 14:37:55 +02:00
Lorenz Meier	3f169d9b78	Fix instructions for airspeed calibration	2016-05-07 14:37:55 +02:00
Sander Smeets	1815b47fbf	Add reserved type, fixes #4466 (#4476 ) * Add reserved type, fixes #4466 * Additional reserved vtol types added to is_vtol	2016-05-07 10:59:49 +02:00
Andreas Bircher	61d2987e6d	Geotagging enhancements (#4475 ) * adding the altitude tag * sorting list of pics before tagging * adding instruction about alphabetical image naming	2016-05-07 10:29:40 +02:00
Daniel Agar	7b0078a20d	bosch bmi160 driver (#4469 )	2016-05-06 21:07:34 +02:00
Lorenz Meier	755176b247	Sim compile fix	2016-05-06 19:22:18 +02:00
Lorenz Meier	ebaca071f6	Battery charge estimation: Factor in voltage drop for idle props	2016-05-06 18:14:34 +02:00
Lorenz Meier	48d7295be6	Default to a multicopter	2016-05-06 07:28:36 +02:00
Mark Charlebois	e2c3ea064e	Remove release configs (#4454 ) * Removed release configs for eagle These configs are only built for internal testing Signed-off-by: Mark Charlebois <charlebm@gmail.com> * Removed release build for eagle from Makefile Signed-off-by: Mark Charlebois <charlebm@gmail.com>	2016-05-06 07:18:57 +02:00
Lorenz Meier	7dbef87ca0	FW pos control: Reduce excessive stack ssize	2016-05-05 20:11:00 +02:00
Lorenz Meier	5e099d8d16	sensors: Reduce perf counters	2016-05-05 20:09:09 +02:00
Lorenz Meier	901461f301	FW att ctrl: Reduce perf counters	2016-05-05 20:08:54 +02:00
Lorenz Meier	34c7dea08c	EKF: Reduce perf counters	2016-05-05 20:08:43 +02:00
Lorenz Meier	dd774a60e3	IO driver: Reduce perf counters	2016-05-05 20:08:11 +02:00
Lorenz Meier	2e23d01b60	Systemlib: harden perf counter handling for null ptrs	2016-05-05 20:07:48 +02:00
Lorenz Meier	32e2998fe4	ROMFS: Further squeeze FMUv1 logging buffer	2016-05-05 20:07:20 +02:00
Lorenz Meier	e250d12184	Board drivers: Shorten perf names	2016-05-05 20:06:42 +02:00
Lorenz Meier	1840d2287c	Update ECL to reduce perf counter usage	2016-05-05 20:06:23 +02:00
Lorenz Meier	ae75ba26b7	MAVLink: Remove excessive stack	2016-05-05 19:36:39 +02:00
Lorenz Meier	986145ac23	ROMFS: Free flash by being less verbose	2016-05-05 19:36:28 +02:00
Lorenz Meier	29550a4cee	Strip LPE config, as it has become part of the default config	2016-05-05 18:23:33 +02:00
Lorenz Meier	639a589233	Fix RTL abort detection on stick change	2016-05-05 16:55:04 +02:00
Lorenz Meier	bbd2b763a3	Fix battery charge level filter	2016-05-05 16:13:58 +02:00
Julian Oes	6c61b67fd5	shmem: fix eagle build	2016-05-05 14:28:35 +02:00
Julian Oes	bdaa1b58f6	uart_esc: fix eagle build	2016-05-05 14:28:20 +02:00
Lorenz Meier	2801a54544	Snapdragon param build fix	2016-05-05 14:15:25 +02:00
Lorenz Meier	34ba80ea9d	Remove attributes warning for Snapdragon	2016-05-05 14:09:18 +02:00
Lorenz Meier	192510ee1c	FMUv4 compile fix	2016-05-05 14:09:03 +02:00
Lorenz Meier	084dfb4026	Fix Snapdragon no-packed	2016-05-05 14:02:31 +02:00
Lorenz Meier	1e7f19335d	Build: Less verbose	2016-05-05 13:37:02 +02:00
Daniel Agar	7aa6e85563	enable Wshadow	2016-05-05 13:32:42 +02:00
Lorenz Meier	e61f517bc6	Update ECL library to include all bugfixes	2016-05-05 13:18:03 +02:00
Lorenz Meier	bdaa2ee20e	Update DriverFramework to include recent improvements and fixes	2016-05-05 11:51:36 +02:00
Julian Oes	3e165c51d4	cmake: copy over changes that rebase missed	2016-05-05 09:14:11 +02:00
Julian Oes	efd20373ff	cmake: re-use the QURT build as well Instead of calling the eagle cmake script from excelsior, use a shared sdflight cmake file like in it is done for the POSIX build on Snapdragon.	2016-05-05 09:12:46 +02:00
Julian Oes	805ef9fff1	cmake: added some comments about eagle/excelsior	2016-05-05 09:11:59 +02:00
Julian Oes	01ad1b642b	Makefile: whitespace only	2016-05-05 09:11:29 +02:00
jwilson	5b6fae5380	Adding config changes to allow PX4 to be built for the Excelsior board.	2016-05-05 08:34:31 +02:00
jwilson	c87a8bedb6	Adding config changes to allow PX4 to be built for the Excelsior board.	2016-05-05 08:34:31 +02:00
Julian Oes	0c5c111cdd	cmake: no param sculling for all eagle configs	2016-05-05 08:09:59 +02:00
David Sidrane	8d510471a1	Turn off paramter culling on eagle	2016-05-05 08:00:26 +02:00
David Sidrane	a08cce27d7	Allow paramter culling to be tunred off	2016-05-05 08:00:26 +02:00
Daniel Agar	553818b646	make submodulesclean proper order -can't sync after deinit	2016-05-04 23:21:17 -04:00
Felix Hu	18176ea73d	change productstr	2016-05-04 23:33:48 +02:00
Felix Hu	9dd42e45d5	mod mindpx v2 prototype and defconfig	2016-05-04 23:33:48 +02:00
Lorenz Meier	45a5f2aaa4	MC vel control: Better defaults, better min and max gains	2016-05-04 22:49:36 +02:00
Julian Oes	aafcae7e6f	MPU9250: integrate using the FIFO sample interval Instead of calculating time offsets between samples, it is easier just to assume a constant sampling time. Then all samples can be integrated, and published every forth time the FIFO buffer has been emptied. The sampling in the sensor happens at 8kHz, the driver empties the buffer at 1kHz, and publishes (and resets the integration) at 250 Hz.	2016-05-04 22:28:19 +02:00
Julian Oes	f528c63030	integrator: add a put method for known intervals This adds a second put method to the integrator class. This allows to integrate with known intervals between samples, rather than based on timestamps. This makes integrating the samples coming out of the MPU9250 FIFO buffer easier.	2016-05-04 22:28:19 +02:00
Beat Küng	e24bef1f70	fix AttPosEKF::FuseOptFlow: move fuse block into 'if (fuseOptFlowData)' block if fuseOptFlowData == false, then K_LOS was not initialized, but it was accessed in the next fuse block to update states variable.	2016-05-03 17:34:42 +02:00
Beat Küng	27d821ac9f	fix position_estimator_inav_main: put terrain_estimator on the stack This fixes a memory leak	2016-05-03 17:34:42 +02:00
Beat Küng	44a0981c14	fix px4_task_spawn_cmd: unlock mutex if pthread_create fails	2016-05-03 17:34:42 +02:00
Beat Küng	99a682e7a7	fix px4_task_spawn_cmd: memory leak, if one of the pthread_* calls fails	2016-05-03 17:34:42 +02:00
Beat Küng	c942266aa6	fix airspeedsim: add missing return	2016-05-03 17:34:41 +02:00
tumbili	57f73e59b7	warn immediately if ekf2 instance allocation has failed	2016-05-03 16:09:26 +02:00
tumbili	9f5e9594f5	implement ekf instance and block parameter instance as class members in order to avoid memory management	2016-05-03 16:09:26 +02:00
Julian Oes	a6e1df06d9	Merge pull request #4422 from PX4/fix_snappy_muorbinit muorb: call `initialize()` before `get_instance()`	2016-05-03 15:51:21 +02:00
Michal Stasiak	9db80b75f4	Auto take-off corner case: Reset work item type when landed	2016-05-03 14:04:33 +02:00
Lorenz Meier	b883b30404	Simulator: Drain the battery to 15%, reffill it once disarmed to allow for more testing	2016-05-03 13:45:15 +02:00
Lorenz Meier	e1e15add01	Reposition: Do not yaw for small dispsplacements	2016-05-03 13:35:53 +02:00
Lorenz Meier	a4ad826958	Loiter: Face travel direction when performing reposition	2016-05-03 13:16:19 +02:00
Lorenz Meier	6de02c460a	Battery estimation: Widen thresholds and filter more	2016-05-03 12:35:35 +02:00
Lorenz Meier	4c61f52269	Code style fixes	2016-05-03 11:42:28 +02:00
Lorenz Meier	c958bfeaa3	MAVLink app: Report OS and Firmware version	2016-05-03 11:39:54 +02:00
Lorenz Meier	ff3e17df0d	Systemlib: Add FW and OS versioning	2016-05-03 11:39:39 +02:00