Merge remote-tracking branch 'PX4/master' into logHandler

* PX4/master: (45 commits) don't use default source address for onboard udp link, wait on remote Configure Easystar HIL setup to do Runway takeoff ROMFS: Set 3DR quad tuning to more realistic default values Fix incomplete boot on new EKF config Fix px4fmu-v2_ekf2 config Updated MAVLink protocol version MAVLink: Start slightly differently on USB Start shell only if SD card not present Update ECL NuttX configs: added px4fmu-v2_ekf2 target for EKF2 development on Pixhawk Get QuRT drivers out of the way, as we are using our own Fix POSIX eagle config Remove unmaintained NuttX config VDev: fix code style Add new posix_eagle_default and qurt_eagle_default targets Fix QuRT build error Fix FMUv4 USB PID Speed up Vagrant VTOL: Fix motor index use in VT_FW_MOT_OFF. Create new param to re-default all deployed vehicles to not shut down motors. VTOL: Fix MOT_OFF bug ...
2026-05-22 01:12:31 +00:00 · 2016-01-05 01:03:11 -05:00
parent c0773c0157 11ed5169cc
commit bd6e4b9d9d
42 changed files with 687 additions and 253 deletions
--- a/.travis.yml
+++ b/.travis.yml
@@ -110,6 +110,7 @@ script:
  - ../Tools/uavcan_copy.sh
  - cd ..
  - echo 'Building NuttX px4fmu-v4 Firmware..' && make px4fmu-v4_default
+  - echo 'Building NuttX px4-stm32f4discovery Firmware..' && make px4-stm32f4discovery_default
  - echo 'Running Tests..' && make px4fmu-v2_default test

 after_success:
@@ -117,8 +118,9 @@ after_success:
         cp build_px4fmu-v1_default/src/firmware/nuttx/nuttx-px4fmu-v1-default.px4 px4fmu-v1_default.px4
      && cp build_px4fmu-v2_default/src/firmware/nuttx/nuttx-px4fmu-v2-default.px4 px4fmu-v2_default.px4
      && cp build_px4fmu-v4_default/src/firmware/nuttx/nuttx-px4fmu-v4-default.px4 px4fmu-v4_default.px4
-      && zip Firmware.zip px4fmu-v1_default.px4 px4fmu-v2_default.px4 px4fmu-v4_default.px4
-      && ./CI-Tools/s3cmd-put px4fmu-v1_default.px4 px4fmu-v2_default.px4 px4fmu-v4_default.px4 build_px4fmu-v2_default/parameters.xml build_px4fmu-v2_default/airframes.xml CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/
+      && cp build_px4-stm32f4discovery_default/src/firmware/nuttx/nuttx-px4-stm32f4discovery-default.px4 px4-stm32f4discovery-default.px4
+      && zip Firmware.zip px4fmu-v1_default.px4 px4fmu-v2_default.px4 px4fmu-v4_default.px4 px4-stm32f4discovery-default.px4
+      && ./CI-Tools/s3cmd-put px4fmu-v1_default.px4 px4fmu-v2_default.px4 px4fmu-v4_default.px4 px4-stm32f4discovery-default.px4 build_px4fmu-v2_default/parameters.xml build_px4fmu-v2_default/airframes.xml CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/
      && ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/
      && ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/
      && ./CI-Tools/s3cmd-put CI-Tools/index.html index.html
--- a/16
+++ b/16
@@ -133,15 +133,15 @@ px4fmu-v2_default:
 px4fmu-v4_default:
 	$(call cmake-build,nuttx_px4fmu-v4_default)

-px4fmu-v2_simple:
-	$(call cmake-build,nuttx_px4fmu-v2_simple)
+px4-stm32f4discovery_default:
+	$(call cmake-build,nuttx_px4-stm32f4discovery_default)
+
+px4fmu-v2_ekf2:
+	$(call cmake-build,nuttx_px4fmu-v2_ekf2)

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

-nuttx_sim_simple:
-	$(call cmake-build,$@)
-
 posix_sitl_default:
 	$(call cmake-build,$@)

@@ -163,6 +163,12 @@ qurt_eagle_release:
 posix_eagle_release:
 	$(call cmake-build,$@)

+qurt_eagle_default:
+	$(call cmake-build,$@)
+
+posix_eagle_default:
+	$(call cmake-build,$@)
+
 posix: posix_sitl_default

 sitl_deprecation:
--- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
+++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
@@ -39,5 +39,7 @@ then
 	param set FW_RR_P 0.3
 fi

+param set RWTO_TKOFF 1
+
 set HIL yes
 set MIXER AERT
--- a/ROMFS/px4fmu_common/init.d/10020_3dr_quad
+++ b/ROMFS/px4fmu_common/init.d/10020_3dr_quad
@@ -12,11 +12,11 @@ sh /etc/init.d/rc.mc_defaults
 if [ $AUTOCNF == yes ]
 then
 	param set MC_ROLL_P 6.5
-	param set MC_ROLLRATE_P 0.08
+	param set MC_ROLLRATE_P 0.14
 	param set MC_ROLLRATE_I 0.1
 	param set MC_ROLLRATE_D 0.004
 	param set MC_PITCH_P 6.0
-	param set MC_PITCHRATE_P 0.16
+	param set MC_PITCHRATE_P 0.14
 	param set MC_PITCHRATE_I 0.09
 	param set MC_PITCHRATE_D 0.004
 	param set MC_YAW_P 4
--- a/ROMFS/px4fmu_common/init.d/13002_firefly6
+++ b/ROMFS/px4fmu_common/init.d/13002_firefly6
@@ -46,6 +46,6 @@ set PWM_AUX_MAX 2000
 set MAV_TYPE 21

 param set VT_MOT_COUNT 6
-param set VT_FW_MOT_OFF 23
+param set VT_FW_MOT_OFFID 34
 param set VT_IDLE_PWM_MC 1080
 param set VT_TYPE 1
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -60,9 +60,6 @@ unset FRC

 if [ $MODE == autostart ]
 then
-	# Try to get an USB console
-	# REBOOTWORK this needs to start after the flight control loop
-	nshterm /dev/ttyACM0 &

 	#
 	# Start the ORB (first app to start)
@@ -844,6 +841,15 @@ then
 	px4flow start &
 fi

+# Start USB shell if no microSD present, MAVLink else
+if [ $LOG_FILE == /dev/null ]
+then
+	# Try to get an USB console
+	nshterm /dev/ttyACM0 &
+else
+	mavlink start -r 800000 -d /dev/ttyACM0 -m config -x
+fi
+
 if [ $EXIT_ON_END == yes ]
 then
 	echo "Exit from nsh"
--- a/Tools/jMAVSim
+++ b/Tools/jMAVSim
--- a/29
+++ b/29
@@ -37,17 +37,42 @@ Vagrant.configure(2) do |config|
  # the path on the host to the actual folder. The second argument is
  # the path on the guest to mount the folder. And the optional third
  # argument is a set of non-required options.
+  # NFS should be faster: https://stefanwrobel.com/how-to-make-vagrant-performance-not-suck
  config.vm.synced_folder ".", "/Firmware", type: "nfs"

  # Provider-specific configuration so you can fine-tune various
  # backing providers for Vagrant. These expose provider-specific options.
  # Example for VirtualBox:
  #
+
+  # This is to configure the machine to be as fast as possible
+  # Alternative: https://github.com/rdsubhas/vagrant-faster
  config.vm.provider "virtualbox" do |vb|
    # Display the VirtualBox GUI when booting the machine
    vb.gui = false
    vb.customize ["modifyvm", :id, "--ioapic", "on"]
-    vb.customize ["modifyvm", :id, "--cpus", "2"]
+    #vb.customize ["modifyvm", :id, "--cpus", "2"]
+
+    config.vm.provider "virtualbox" do |v|
+      host = RbConfig::CONFIG['host_os']
+
+      # Give VM 1/4 system memory & access to all cpu cores on the host
+      if host =~ /darwin/
+        cpus = `sysctl -n hw.ncpu`.to_i
+        # sysctl returns Bytes and we need to convert to MB
+        mem = `sysctl -n hw.memsize`.to_i / 1024 / 1024 / 4
+      elsif host =~ /linux/
+        cpus = `nproc`.to_i
+        # meminfo shows KB and we need to convert to MB
+        mem = `grep 'MemTotal' /proc/meminfo | sed -e 's/MemTotal://' -e 's/ kB//'`.to_i / 1024 / 4
+      else # sorry Windows folks, I can't help you
+        cpus = 2
+        mem = 1024
+      end
+
+      v.customize ["modifyvm", :id, "--memory", mem]
+      v.customize ["modifyvm", :id, "--cpus", cpus]
+    end

    # Since make and other tools freak out if they see timestamps
    # from the future and we share directories, tightly lock the host and guest clocks together (clock sync if more than 2 seconds off)
@@ -57,7 +82,7 @@ Vagrant.configure(2) do |config|
    vb.customize ["guestproperty", "set", :id, "/VirtualBox/GuestAdd/VBoxService/--timesync-set-on-restore", "1"]

    # Customize the amount of memory on the VM:
-    vb.memory = "2048"
+    #vb.memory = "2048"
  end
  #
  # View the documentation for the provider you are using for more
--- a/cmake/configs/nuttx_px4-stm32f4discovery_default.cmake
+++ b/cmake/configs/nuttx_px4-stm32f4discovery_default.cmake
@@ -0,0 +1,87 @@
+include(nuttx/px4_impl_nuttx)
+
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
+
+set(config_module_list
+	#
+	# Board support modules
+	#
+	drivers/device
+	drivers/stm32
+	drivers/led
+	drivers/boards/px4-stm32f4discovery
+
+	#
+	# System commands
+	#
+	systemcmds/bl_update
+	systemcmds/mixer
+	systemcmds/param
+	systemcmds/perf
+	systemcmds/reboot
+	systemcmds/top
+	systemcmds/config
+	systemcmds/nshterm
+	systemcmds/ver
+
+	#
+	# Library modules
+	#
+	modules/param
+	modules/systemlib
+	modules/systemlib/mixer
+	modules/controllib
+	modules/uORB
+
+	#
+	# Libraries
+	#
+	#lib/mathlib/CMSIS
+	lib/mathlib
+	lib/mathlib/math/filter
+	lib/ecl
+	lib/external_lgpl
+	lib/geo
+	lib/conversion
+	platforms/nuttx
+
+	# had to add for cmake, not sure why wasn't in original config
+	platforms/common
+	platforms/nuttx/px4_layer
+
+	#
+	# Demo apps
+	#
+	#examples/math_demo
+	# Tutorial code from
+	# https://px4.io/dev/px4_simple_app
+	examples/px4_simple_app
+
+	# Tutorial code from
+	# https://px4.io/dev/daemon
+	#examples/px4_daemon_app
+
+	# Tutorial code from
+	# https://px4.io/dev/debug_values
+	#examples/px4_mavlink_debug
+
+	# Tutorial code from
+	# https://px4.io/dev/example_fixedwing_control
+	#examples/fixedwing_control
+
+	# Hardware test
+	#examples/hwtest
+)
+
+set(config_extra_builtin_cmds
+	serdis
+	sercon
+	)
+
+add_custom_target(sercon)
+set_target_properties(sercon PROPERTIES
+	MAIN "sercon" STACK "2048")
+
+add_custom_target(serdis)
+set_target_properties(serdis PROPERTIES
+	MAIN "serdis" STACK "2048")
--- a/cmake/configs/nuttx_px4fmu-v1_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v1_default.cmake
@@ -73,6 +73,7 @@ set(config_module_list
 	modules/attitude_estimator_q
 	modules/ekf_att_pos_estimator
 	modules/position_estimator_inav
+	modules/ekf2

 	#
 	# Vehicle Control
--- a/cmake/configs/nuttx_px4fmu-v2_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake
@@ -83,7 +83,6 @@ set(config_module_list
 	modules/attitude_estimator_q
 	modules/ekf_att_pos_estimator
 	modules/position_estimator_inav
-	modules/ekf2

 	#
 	# Vehicle Control
--- a/cmake/configs/nuttx_px4fmu-v2_simple.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_simple.cmake
@@ -2,6 +2,8 @@ include(nuttx/px4_impl_nuttx)

 set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)

+set(config_uavcan_num_ifaces 2)
+
 set(config_module_list
 	#
 	# Board support modules
@@ -21,27 +23,29 @@ set(config_module_list
 	drivers/l3gd20
 	drivers/hmc5883
 	drivers/ms5611
-	drivers/mb12xx
+	#drivers/mb12xx
+	drivers/srf02
 	drivers/sf0x
 	drivers/ll40ls
 	drivers/trone
 	drivers/gps
 	drivers/pwm_out_sim
-	drivers/hott
-	drivers/hott/hott_telemetry
-	drivers/hott/hott_sensors
+	#drivers/hott
+	#drivers/hott/hott_telemetry
+	#drivers/hott/hott_sensors
 	drivers/blinkm
 	drivers/airspeed
 	drivers/ets_airspeed
 	drivers/meas_airspeed
 	drivers/frsky_telemetry
 	modules/sensors
-	drivers/mkblctrl
+	#drivers/mkblctrl
 	drivers/px4flow
 	drivers/oreoled
 	drivers/gimbal
 	drivers/pwm_input
 	drivers/camera_trigger
+	drivers/bst

 	#
 	# System commands
@@ -53,6 +57,7 @@ set(config_module_list
 	systemcmds/pwm
 	systemcmds/esc_calib
 	systemcmds/reboot
+	#systemcmds/topic_listener
 	systemcmds/top
 	systemcmds/config
 	systemcmds/nshterm
@@ -60,6 +65,35 @@ set(config_module_list
 	systemcmds/dumpfile
 	systemcmds/ver

+	#
+	# General system control
+	#
+	modules/commander
+	modules/navigator
+	modules/mavlink
+	modules/gpio_led
+	modules/uavcan
+	modules/land_detector
+
+	#
+	# Estimation modules (EKF/ SO3 / other filters)
+	#
+	# Too high RAM usage due to static allocations
+	# modules/attitude_estimator_ekf
+	modules/attitude_estimator_q
+	modules/ekf2
+	modules/position_estimator_inav
+
+	#
+	# Vehicle Control
+	#
+	# modules/segway # XXX Needs GCC 4.7 fix
+	modules/fw_pos_control_l1
+	modules/fw_att_control
+	modules/mc_att_control
+	modules/mc_pos_control
+	modules/vtol_att_control
+
 	#
 	# Logging
 	#
@@ -87,34 +121,47 @@ set(config_module_list
 	lib/geo_lookup
 	lib/conversion
 	lib/launchdetection
+	lib/terrain_estimation
+	lib/runway_takeoff
+	lib/tailsitter_recovery
 	platforms/nuttx

 	# had to add for cmake, not sure why wasn't in original config
 	platforms/common 
 	platforms/nuttx/px4_layer

+	#
+	# OBC challenge
+	#
+	#modules/bottle_drop
+
+	#
+	# Rover apps
+	#
+	examples/rover_steering_control
+
 	#
 	# Demo apps
 	#
 	#examples/math_demo
 	# Tutorial code from
 	# https://px4.io/dev/px4_simple_app
-	examples/px4_simple_app
+	#examples/px4_simple_app

 	# Tutorial code from
 	# https://px4.io/dev/daemon
-	examples/px4_daemon_app
+	#examples/px4_daemon_app

 	# Tutorial code from
 	# https://px4.io/dev/debug_values
-	examples/px4_mavlink_debug
+	#examples/px4_mavlink_debug

 	# Tutorial code from
 	# https://px4.io/dev/example_fixedwing_control
-	examples/fixedwing_control
+	#examples/fixedwing_control

 	# Hardware test
-	examples/hwtest
+	#examples/hwtest
 )

 set(config_extra_builtin_cmds
@@ -128,10 +175,12 @@ set(config_io_board

 set(config_extra_libs
 	${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a
+	uavcan
+	uavcan_stm32_driver
 	)

 set(config_io_extra_libs
-	${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a
+	#${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a
 	)

 add_custom_target(sercon)
--- a/cmake/configs/nuttx_sim_simple.cmake
+++ b/cmake/configs/nuttx_sim_simple.cmake
@@ -1,21 +0,0 @@
-include(nuttx/px4_impl_nuttx)
-
-message(WARNING "this is a work in progress and doesn't build yet")
-
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake)
-
-set(config_module_list
-	#platforms/nuttx
-	#platforms/nuttx/px4_layer
-	platforms/common
-	#drivers/led
-	drivers/device
-	#modules/systemlib
-	#modules/uORB
-	#examples/px4_simple_app
-	#lib/mathlib/math/filter
-	#lib/conversion
-	)
-
-set(config_extra_builtin_cmds
-	)
--- a/cmake/configs/posix_eagle_default.cmake
+++ b/cmake/configs/posix_eagle_default.cmake
@@ -35,9 +35,10 @@ set(config_module_list

 	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
--- a/cmake/configs/posix_sitl_ekf2.cmake
+++ b/cmake/configs/posix_sitl_ekf2.cmake
@@ -32,18 +32,24 @@ set(config_module_list
 	modules/sensors
 	modules/simulator
 	modules/mavlink
-	modules/dataman
 	modules/attitude_estimator_ekf
-	#modules/attitude_estimator_q
+	modules/attitude_estimator_q
 	modules/ekf_att_pos_estimator
+	modules/ekf2
 	modules/position_estimator_inav
+	modules/navigator
+	modules/vtol_att_control
 	modules/mc_pos_control
 	modules/mc_att_control
-	modules/navigator
+	modules/mc_pos_control_multiplatform
+	modules/mc_att_control_multiplatform
+	modules/land_detector
+	modules/fw_att_control
+	modules/fw_pos_control_l1
+	modules/dataman
+	modules/sdlog2
 	modules/commander
-	modules/vtol_att_control
 	modules/controllib
-	modules/ekf2
 	lib/mathlib
 	lib/mathlib/math/filter
 	lib/conversion
@@ -54,6 +60,8 @@ set(config_module_list
 	lib/launchdetection
 	lib/terrain_estimation
 	lib/runway_takeoff
+	lib/tailsitter_recovery
+	examples/px4_simple_app
 	)

 set(config_extra_builtin_cmds
--- a/cmake/configs/qurt_eagle_default.cmake
+++ b/cmake/configs/qurt_eagle_default.cmake
@@ -0,0 +1,90 @@
+include(qurt/px4_impl_qurt)
+
+#if ("${HEXAGON_DRIVERS_ROOT}" #STREQUAL "")
+#	message(FATAL_ERROR "HEXAGON_DRIVERS_ROOT is not set")
+#endif()
+
+#if ("${EAGLE_DRIVERS_SRC}" STREQUAL "")
+#	message(FATAL_ERROR "EAGLE_DRIVERS_SRC is not set")
+#endif()
+
+#include_directories(${HEXAGON_DRIVERS_ROOT}/inc)
+
+# For Actual flight we need to link against the driver dynamic libraries
+set(target_libraries
+	-L${HEXAGON_DRIVERS_ROOT}/libs
+# The plan is to replace these with our drivers
+#	mpu9x50
+#	uart_esc
+#	csr_gps
+#	rc_receiver
+	)
+
+
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake)
+
+set(config_module_list
+	#
+	# Board support modules
+	#
+	drivers/device
+	modules/sensors
+# The plan is to replace these with our drivers
+#	$(EAGLE_DRIVERS_SRC)/mpu9x50
+#	$(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
+	modules/controllib
+
+	#
+	# Libraries
+	#
+	lib/mathlib
+	lib/mathlib/math/filter
+	lib/geo
+	lib/geo_lookup
+	lib/conversion
+	lib/terrain_estimation
+	lib/runway_takeoff
+	lib/tailsitter_recovery
+
+	#
+	# QuRT port
+	#
+	platforms/common
+	platforms/qurt/px4_layer
+	platforms/posix/work_queue
+
+	#
+	# sources for muorb over fastrpc
+	#
+	modules/muorb/adsp
+	)
--- a/cmake/nuttx/px4_impl_nuttx.cmake
+++ b/cmake/nuttx/px4_impl_nuttx.cmake
@@ -467,6 +467,14 @@ function(px4_os_add_flags)
 			-mfpu=fpv4-sp-d16
 			-mfloat-abi=hard
 			)
+	elseif (${BOARD} STREQUAL "px4-stm32f4discovery")
+		set(cpu_flags
+			-mcpu=cortex-m4
+			-mthumb
+			-march=armv7e-m
+			-mfpu=fpv4-sp-d16
+			-mfloat-abi=hard
+			)
 	elseif (${BOARD} STREQUAL "aerocore")
 		set(cpu_flags
 			-mcpu=cortex-m4
--- a/mavlink/include/mavlink/v1.0
+++ b/mavlink/include/mavlink/v1.0
--- a/msg/vehicle_global_position.msg
+++ b/msg/vehicle_global_position.msg
@@ -4,18 +4,18 @@
 # estimator, which will take more sources of information into account than just GPS,
 # e.g. control inputs of the vehicle in a Kalman-filter implementation.
 #
-uint64 timestamp		# Time of this estimate, in microseconds since system start
-uint64 time_utc_usec		# GPS UTC timestamp in microseconds
-float64 lat			# Latitude in degrees
-float64 lon			# Longitude in degrees
-float32 alt			# Altitude AMSL in meters
-float32 vel_n			# Ground north velocity, m/s
-float32 vel_e			# Ground east velocity, m/s
-float32 vel_d			# Ground downside velocity, m/s
-float32 yaw 			# Yaw in radians -PI..+PI.
-float32 eph			# Standard deviation of position estimate horizontally
-float32 epv			# Standard deviation of position vertically
-float32 terrain_alt		# Terrain altitude in m, WGS84
+uint64 timestamp		# Time of this estimate since system start, (microseconds)
+uint64 time_utc_usec		# GPS UTC timestamp, (microseconds)
+float64 lat			# Latitude, (degrees)
+float64 lon			# Longitude, (degrees)
+float32 alt			# Altitude AMSL, (meters)
+float32 vel_n			# North velocity in NED earth-fixed frame, (metres/sec)
+float32 vel_e			# East velocity in NED earth-fixed frame, (metres/sec)
+float32 vel_d			# Down velocity in NED earth-fixed frame, (metres/sec)
+float32 yaw 			# Euler yaw angle relative to NED earth-fixed frame, -PI..+PI, (radians)
+float32 eph			# Standard deviation of horizontal position error, (metres)
+float32 epv			# Standard deviation of vertical position error, (metres)
+float32 terrain_alt		# Terrain altitude WGS84, (metres)
 bool terrain_alt_valid		# Terrain altitude estimate is valid
 bool dead_reckoning		# True if this position is estimated through dead-reckoning
-float32 pressure_alt		# Pressure altitude
+float32 pressure_alt		# Pressure altitude AMSL, (metres)
--- a/msg/vehicle_gps_position.msg
+++ b/msg/vehicle_gps_position.msg
@@ -1,30 +1,30 @@
 # GPS position in WGS84 coordinates.
-uint64 timestamp_position	# Timestamp for position information 
-int32 lat			# Latitude in 1E-7 degrees 
+uint64 timestamp_position	# Time of the position estimates since system start, (microseconds)
+int32 lat			# Latitude in 1E-7 degrees
 int32 lon			# Longitude in 1E-7 degrees 
-int32 alt			# Altitude in 1E-3 meters (millimeters) above MSL  
-int32 alt_ellipsoid # Altitude in 1E-3 meters (millimeters) above Ellipsoid
+int32 alt			# Altitude in 1E-3 meters above MSL, (millimetres)
+int32 alt_ellipsoid 		# Altitude in 1E-3 meters bove Ellipsoid, (millimetres)

-uint64 timestamp_variance
-float32 s_variance_m_s		# speed accuracy estimate m/s 
-float32 c_variance_rad		# course accuracy estimate rad 
+uint64 timestamp_variance	# Time of the accuracy estimates since system start, (microseconds)
+float32 s_variance_m_s		# GPS speed accuracy estimate, (metres/sec)
+float32 c_variance_rad		# GPS course accuracy estimate, (radians) 
 uint8 fix_type # 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.   

-float32 eph			# GPS HDOP horizontal dilution of position in m 
-float32 epv			# GPS VDOP horizontal dilution of position in m 
+float32 eph			# GPS horizontal position accuracy (metres) 
+float32 epv			# GPS vertical position accuracy (metres) 

 int32 noise_per_ms		# GPS noise per millisecond
 int32 jamming_indicator		# indicates jamming is occurring

-uint64 timestamp_velocity	# Timestamp for velocity informations 
-float32 vel_m_s			# GPS ground speed (m/s) 
-float32 vel_n_m_s		# GPS ground speed in m/s 
-float32 vel_e_m_s		# GPS ground speed in m/s 
-float32 vel_d_m_s		# GPS ground speed in m/s 
-float32 cog_rad			# Course over ground (NOT heading, but direction of movement) in rad, -PI..PI 
-bool vel_ned_valid		# Flag to indicate if NED speed is valid 
+uint64 timestamp_velocity	# Time of the velocity estimates since system start, (microseconds)
+float32 vel_m_s			# GPS ground speed, (metres/sec) 
+float32 vel_n_m_s		# GPS North velocity, (metres/sec) 
+float32 vel_e_m_s		# GPS East velocity, (metres/sec)
+float32 vel_d_m_s		# GPS Down velocity, (metres/sec) 
+float32 cog_rad			# Course over ground (NOT heading, but direction of movement), -PI..PI, (radians) 
+bool vel_ned_valid		# True if NED velocity is valid 

-uint64 timestamp_time		# Timestamp for time information 
+uint64 timestamp_time		# Time of the UTC timestamp since system start, (microseconds) 
 uint64 time_utc_usec		# Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 

 uint8 satellites_used		# Number of satellites used 
--- a/msg/vehicle_local_position.msg
+++ b/msg/vehicle_local_position.msg
@@ -1,36 +1,36 @@
 # Fused local position in NED.

-uint64 timestamp		# Time of this estimate, in microseconds since system start
-bool xy_valid			# true if x and y are valid
-bool z_valid			# true if z is valid
-bool v_xy_valid		# true if vy and vy are valid
-bool v_z_valid			# true if vz is valid
+uint64 timestamp			# Time of this estimate since system start, (microseconds)
+bool xy_valid				# true if x and y are valid
+bool z_valid				# true if z is valid
+bool v_xy_valid				# true if vy and vy are valid
+bool v_z_valid				# true if vz is valid

 # Position in local NED frame
-float32 x				# X position in meters in NED earth-fixed frame
-float32 y				# X position in meters in NED earth-fixed frame
-float32 z				# Z position in meters in NED earth-fixed frame (negative altitude)
+float32 x				# North position in NED earth-fixed frame, (metres)
+float32 y				# East position in NED earth-fixed frame, (metres)
+float32 z				# Down position (negative altitude) in NED earth-fixed frame, (metres)

 # Velocity in NED frame
-float32 vx 				# Ground X Speed (Latitude), m/s in NED
-float32 vy				# Ground Y Speed (Longitude), m/s in NED
-float32 vz				# Ground Z Speed (Altitude), m/s	in NED
+float32 vx 				# North velocity in NED earth-fixed frame, (metres/sec)
+float32 vy				# East velocity in NED earth-fixed frame, (metres/sec)
+float32 vz				# Down velocity in NED earth-fixed frame, (metres/sec)

 # Heading
-float32 yaw
+float32 yaw				# Euler yaw angle transforming the tangent plane relative to NED earth-fixed frame, -PI..+PI,  (radians) 

-# Reference position in GPS / WGS84 frame
-bool xy_global			# true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
-bool z_global			# true if z is valid and has valid global reference (ref_alt)
-uint64 ref_timestamp	# Time when reference position was set
-float64 ref_lat		# Reference point latitude in degrees
-float64 ref_lon		# Reference point longitude in degrees
-float32 ref_alt			# Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level
+# Position of reference point (local NED frame origin) in global (GPS / WGS84) frame
+bool xy_global				# true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
+bool z_global				# true if z is valid and has valid global reference (ref_alt)
+uint64 ref_timestamp			# Time when reference position was set since system start, (microseconds)
+float64 ref_lat				# Reference point latitude, (degrees)
+float64 ref_lon				# Reference point longitude, (degrees)
+float32 ref_alt				# Reference altitude AMSL, MUST be set to current (not at reference point!) ground level, (metres)

 # Distance to surface
-float32 dist_bottom		# Distance to bottom surface (ground)
-float32 dist_bottom_rate		# Distance to bottom surface (ground) change rate
-uint64 surface_bottom_timestamp		# Time when new bottom surface found
-bool dist_bottom_valid	# true if distance to bottom surface is valid
-float32 eph
-float32 epv
+float32 dist_bottom			# Distance from from bottom surface to ground, (metres)
+float32 dist_bottom_rate		# Rate of change of distance from bottom surface to ground, (metres/sec)
+uint64 surface_bottom_timestamp		# Time when new bottom surface found since system start, (microseconds)
+bool dist_bottom_valid			# true if distance to bottom surface is valid
+float32 eph				# Standard deviation of horizontal position error, (metres)
+float32 epv				# Standard deviation of vertical position error, (metres)
--- a/nuttx-configs/px4fmu-v4/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v4/nsh/defconfig
@@ -669,7 +669,7 @@ CONFIG_CDCACM_BULKIN_REQLEN=96
 CONFIG_CDCACM_RXBUFSIZE=600
 CONFIG_CDCACM_TXBUFSIZE=4000
 CONFIG_CDCACM_VENDORID=0x26ac
-CONFIG_CDCACM_PRODUCTID=0x0011
+CONFIG_CDCACM_PRODUCTID=0x0012
 CONFIG_CDCACM_VENDORSTR="3D Robotics"
 CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v4.x"
 # CONFIG_USBMSC is not set
--- a/posix-configs/SITL/init/rcS_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_gazebo_iris
@@ -50,6 +50,7 @@ mc_pos_control start
 mc_att_control start
 mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix
 mavlink start -u 14556 -r 2000000
+mavlink start -u 14557 -r 2000000 -m onboard
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
 mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
--- a/posix-configs/SITL/init/rcS_jmavsim_iris
+++ b/posix-configs/SITL/init/rcS_jmavsim_iris
@@ -57,6 +57,7 @@ mc_pos_control start
 mc_att_control start
 mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix
 mavlink start -u 14556 -r 2000000
+mavlink start -u 14557 -r 2000000 -m onboard
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
 mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
--- a/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c
+++ b/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c
@@ -129,18 +129,6 @@ stm32_boardinitialize(void)

 #include <math.h>

-#ifdef __cplusplus
-__EXPORT int matherr(struct __exception *e)
-{
-	return 1;
-}
-#else
-__EXPORT int matherr(struct exception *e)
-{
-	return 1;
-}
-#endif
-
 __EXPORT int nsh_archinitialize(void)
 {

--- a/src/drivers/device/vdev_posix.cpp
+++ b/src/drivers/device/vdev_posix.cpp
@@ -245,12 +245,16 @@ extern "C" {

 		const unsigned NAMELEN = 32;
 		char thread_name[NAMELEN] = {};
+
+#ifndef __PX4_QURT
 		int nret = pthread_getname_np(pthread_self(), thread_name, NAMELEN);

 		if (nret || thread_name[0] == 0) {
 			PX4_WARN("failed getting thread name");
 		}

+#endif
+
 		PX4_DEBUG("Called px4_poll timeout = %d", timeout);
 		px4_sem_init(&sem, 0, 0);

--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -81,7 +81,7 @@ __BEGIN_DECLS
 /**
 * Default value for a shutdown motor
 */
-#define PWM_MOTOR_OFF	800
+#define PWM_MOTOR_OFF	900

 /**
 * Default minimum PWM in us
--- a/src/firmware/nuttx/CMakeLists.txt
+++ b/src/firmware/nuttx/CMakeLists.txt
@@ -51,7 +51,7 @@ if(NOT ${BOARD} STREQUAL "sim")
 		)
 	if (config_io_board)
 	    add_dependencies(romfs fw_io)
-    endif()
+	endif()
 	set(fw_file
 		${CMAKE_CURRENT_BINARY_DIR}/${OS}-${BOARD}-${LABEL}.px4)

--- a/src/lib/ecl
+++ b/src/lib/ecl
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -73,7 +73,7 @@ static constexpr unsigned int calibration_sides = 6;			///< The total number of
 static constexpr unsigned int calibration_total_points = 240;		///< The total points per magnetometer
 static constexpr unsigned int calibraton_duration_seconds = 42; 	///< The total duration the routine is allowed to take

-static constexpr float MAG_MAX_OFFSET_LEN = 0.6f;	///< 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 = 0.9f;	///< 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.

 int32_t	device_ids[max_mags];
 int device_prio_max = 0;
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -70,6 +70,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/control_state.h>
+#include <uORB/topics/vehicle_global_position.h>

 #include <ecl/EKF/ekf.h>

@@ -111,6 +112,8 @@ public:

 	void print();

+	void print_status();
+
 private:
 	static constexpr float _dt_max = 0.02;
 	bool		_task_should_exit = false;		/**< if true, task should exit */
@@ -123,6 +126,7 @@ private:
 	orb_advert_t _att_pub;
 	orb_advert_t _lpos_pub;
 	orb_advert_t _control_state_pub;
+	orb_advert_t _vehicle_global_position_pub;

 	/* Low pass filter for attitude rates */
 	math::LowPassFilter2p _lp_roll_rate;
@@ -162,6 +166,11 @@ void Ekf2::print()
 	_ekf->printStoredIMU();
 }

+void Ekf2::print_status()
+{
+	warnx("position OK %s", (_ekf->position_is_valid()) ? "[YES]" : "[NO]");
+}
+
 void Ekf2::task_main()
 {
 	// subscribe to relevant topics
@@ -244,12 +253,13 @@ void Ekf2::task_main()
 			_ekf->setAirspeedData(airspeed.timestamp, &airspeed.indicated_airspeed_m_s);
 		}

-		struct vehicle_attitude_s att;
-		struct vehicle_local_position_s lpos;
-		att.timestamp = hrt_absolute_time();
-		lpos.timestamp = hrt_absolute_time();
+		// run the EKF update
 		_ekf->update();

+		// generate vehicle attitude data
+		struct vehicle_attitude_s att;
+		att.timestamp = hrt_absolute_time();
+
 		_ekf->copy_quaternion(att.q);
 		matrix::Quaternion<float> q(att.q[0], att.q[1], att.q[2], att.q[3]);
 		matrix::Euler<float> euler(q);
@@ -257,25 +267,64 @@ void Ekf2::task_main()
 		att.pitch = euler(1);
 		att.yaw = euler(2);

+		// generate vehicle local position data
+		struct vehicle_local_position_s lpos;
 		float pos[3] = {};
 		float vel[3] = {};

+		lpos.timestamp = hrt_absolute_time();
+
+		// Position in local NED frame
 		_ekf->copy_position(pos);
 		lpos.x = pos[0];
 		lpos.y = pos[1];
 		lpos.z = pos[2];

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

+		// TODO: better status reporting
+		lpos.xy_valid = _ekf->position_is_valid();
+		lpos.z_valid = true;
+		lpos.v_xy_valid = _ekf->position_is_valid();
+		lpos.v_z_valid = true;
+
+		// Position of local NED origin in GPS / WGS84 frame
+		lpos.ref_timestamp = _ekf->_last_gps_origin_time_us; // Time when reference position was set
+ 		lpos.xy_global = _ekf->position_is_valid();// true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
+ 		lpos.z_global = true;// true if z is valid and has valid global reference (ref_alt)
+		lpos.ref_lat = _ekf->_posRef.lat_rad * (double)180.0 * M_PI; // Reference point latitude in degrees
+		lpos.ref_lon = _ekf->_posRef.lon_rad * (double)180.0 * M_PI; // Reference point longitude in degrees
+		lpos.ref_alt = _ekf->_gps_alt_ref; // Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level		
+
+		// The rotation of the tangent plane vs. geographical north
+		lpos.yaw = 0.0f;
+
+		lpos.dist_bottom = 0.0f; // Distance to bottom surface (ground) in meters
+		lpos.dist_bottom_rate = 0.0f; // Distance to bottom surface (ground) change rate
+		lpos.surface_bottom_timestamp	= 0; // Time when new bottom surface found
+		lpos.dist_bottom_valid = false; // true if distance to bottom surface is valid
+
+ 		// TODO: uORB definition does not define what thes variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres 
+		// TODO: Should use sqrt of filter position variances
+		lpos.eph = gps.eph;
+		lpos.epv = gps.epv;
+
+		// publish vehicle local position data
+		if (_lpos_pub == nullptr) {
+			_lpos_pub = orb_advertise(ORB_ID(vehicle_local_position), &lpos);
+		} else {
+			orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos);
+		}
+
+		// generate control state data
 		control_state_s ctrl_state = {};
 		ctrl_state.timestamp = hrt_absolute_time();
 		ctrl_state.roll_rate = _lp_roll_rate.apply(sensors.gyro_rad_s[0]);
-
 		ctrl_state.pitch_rate = _lp_pitch_rate.apply(sensors.gyro_rad_s[1]);
-
 		ctrl_state.yaw_rate = _lp_yaw_rate.apply(sensors.gyro_rad_s[2]);

 		ctrl_state.q[0] = q(0);
@@ -283,6 +332,14 @@ void Ekf2::task_main()
 		ctrl_state.q[2] = q(2);
 		ctrl_state.q[3] = q(3);

+		// publish control state data
+		if (_control_state_pub == nullptr) {
+			_control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);
+		} else {
+			orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state);
+		}
+
+		// generate vehicle attitude data
 		att.q[0] = q(0);
 		att.q[1] = q(1);
 		att.q[2] = q(2);
@@ -293,25 +350,51 @@ void Ekf2::task_main()
 		att.pitchspeed = sensors.gyro_rad_s[1];
 		att.yawspeed = sensors.gyro_rad_s[2];

-		if (_control_state_pub == nullptr) {
-			_control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);
-		} else {
-			orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state);
-		}
-
+		// publish vehicle attitude data
 		if (_att_pub == nullptr) {
 			_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
 		} else {
 			orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
 		}

-		if (_lpos_pub == nullptr) {
-			_lpos_pub = orb_advertise(ORB_ID(vehicle_local_position), &lpos);
-		} else {
-			orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos);
+		// generate and publish global position data
+		struct vehicle_global_position_s global_pos;
+		if (_ekf->position_is_valid()) {
+			// TODO: local origin is currenlty at GPS height origin - this is different to ekf_att_pos_estimator
+
+			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
+
+			double est_lat, est_lon;
+			map_projection_reproject(&_ekf->_posRef, lpos.x, lpos.y, &est_lat, &est_lon);
+			global_pos.lat = est_lat; // Latitude in degrees
+			global_pos.lon = est_lon; // Longitude in degrees
+
+			global_pos.alt = -pos[2]; // Altitude AMSL in meters
+
+			global_pos.vel_n = vel[0]; // Ground north velocity, m/s
+			global_pos.vel_e = vel[1]; // Ground east velocity, m/s
+			global_pos.vel_d = vel[2]; // Ground downside velocity, m/s
+
+			global_pos.yaw = euler(2); // Yaw in radians -PI..+PI.
+
+			global_pos.eph = gps.eph; // Standard deviation of position estimate horizontally
+			global_pos.epv = gps.epv; // Standard deviation of position vertically
+
+			// TODO: implement terrain estimator
+			global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84
+			global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid
+			// TODO use innovatun consistency check timouts to set this
+			global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning
+
+			global_pos.pressure_alt = sensors.baro_alt_meter[0]; // Pressure altitude AMSL (m)
+
+			if (_vehicle_global_position_pub == nullptr) {
+				_vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
+			} else {
+				orb_publish(ORB_ID(vehicle_global_position), _vehicle_global_position_pub, &global_pos);
+			}
 		}
-
-
 	}
 }

@@ -394,6 +477,7 @@ int ekf2_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "status")) {
 		if (ekf2::instance) {
 			PX4_WARN("running");
+			ekf2::instance->print_status();
 			return 0;

 		} else {
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -662,9 +662,48 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 		return -EINVAL;
 	}

+	/* back off 1800 ms to avoid running into the USB setup timing */
+	while (_mode == MAVLINK_MODE_CONFIG &&
+		hrt_absolute_time() < 1800U * 1000U) {
+		usleep(50000);
+	}
+
 	/* open uart */
 	_uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY);

+	/* if this is a config link, stay here and wait for it to open */
+	if (_uart_fd < 0 && _mode == MAVLINK_MODE_CONFIG) {
+
+		int armed_fd = orb_subscribe(ORB_ID(actuator_armed));
+		struct actuator_armed_s armed;
+
+		/* get the system arming state and abort on arming */
+		while (_uart_fd < 0) {
+
+			/* abort if an arming topic is published and system is armed */
+			bool updated = false;
+			orb_check(armed_fd, &updated);
+
+			if (updated) {
+				/* the system is now providing arming status feedback.
+				 * instead of timing out, we resort to abort bringing
+				 * up the terminal.
+				 */
+				orb_copy(ORB_ID(actuator_armed), armed_fd, &armed);
+
+				if (armed.armed) {
+					/* this is not an error, but we are done */
+					return -1;
+				}
+			}
+
+			usleep(100000);
+			_uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY);
+		};
+
+		close(armed_fd);
+	}
+
 	if (_uart_fd < 0) {
 		return _uart_fd;
 	}
@@ -892,14 +931,17 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
 	}
 #else
 	if (get_protocol() == UDP) {
-		ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr));
+		if (get_client_source_initialized()) {
+			ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr));
+		}

 		struct telemetry_status_s &tstatus = get_rx_status();

 		/* resend heartbeat via broadcast */
-		if (((hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000) ||
+		if ((_mode != MAVLINK_MODE_ONBOARD)
+			&& (((hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000) ||
 			(tstatus.heartbeat_time == 0)) &&
-			msgid == MAVLINK_MSG_ID_HEARTBEAT) {
+			msgid == MAVLINK_MSG_ID_HEARTBEAT)) {

 			int bret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr));

@@ -1006,11 +1048,14 @@ Mavlink::init_udp()
 		return;
 	}

-	/* set default target address */
+	/* set default target address, but not for onboard mode (will be set on first recieved packet) */
 	memset((char *)&_src_addr, 0, sizeof(_src_addr));
-	_src_addr.sin_family = AF_INET;
-	inet_aton("127.0.0.1", &_src_addr.sin_addr);
-	_src_addr.sin_port = htons(DEFAULT_REMOTE_PORT_UDP);
+	if (_mode != MAVLINK_MODE_ONBOARD) {
+		set_client_source_initialized();
+		_src_addr.sin_family = AF_INET;
+		inet_aton("127.0.0.1", &_src_addr.sin_addr);
+		_src_addr.sin_port = htons(DEFAULT_REMOTE_PORT_UDP);
+	}

 	/* default broadcast address */
 	memset((char *)&_bcast_addr, 0, sizeof(_bcast_addr));
@@ -1573,14 +1618,20 @@ Mavlink::task_main(int argc, char *argv[])
 		_datarate = MAX_DATA_RATE;
 	}

-	if (Mavlink::instance_exists(_device_name, this)) {
-		warnx("%s already running", _device_name);
-		return ERROR;
-	}
-
 	if (get_protocol() == SERIAL) {
-	warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate);
+		if (Mavlink::instance_exists(_device_name, this)) {
+			warnx("%s already running", _device_name);
+			return ERROR;
+		}
+
+		warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate);
+
 	} else if (get_protocol() == UDP) {
+		if (Mavlink::get_instance_for_network_port(_network_port) != nullptr) {
+			warnx("port %d already occupied", _network_port);
+			return ERROR;
+		}
+
 		warnx("mode: %u, data rate: %d B/s on udp port %hu", _mode, _datarate, _network_port);
 	}
 	/* flush stdout in case MAVLink is about to take it over */
@@ -1592,9 +1643,12 @@ Mavlink::task_main(int argc, char *argv[])
 	/* default values for arguments */
 	_uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original);

-	if (_uart_fd < 0) {
+	if (_uart_fd < 0 && _mode != MAVLINK_MODE_CONFIG) {
 		warn("could not open %s", _device_name);
 		return ERROR;
+	} else if (_uart_fd < 0 && _mode == MAVLINK_MODE_CONFIG) {
+		/* the config link is optional */
+		return OK;
 	}

 #endif
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -340,9 +340,15 @@ public:
 	int 			get_socket_fd () { return _socket_fd; };
 #ifdef __PX4_POSIX
 	struct sockaddr_in * get_client_source_address() {return &_src_addr;};
+
+	void			set_client_source_initialized() { _src_addr_initialized = true; };
+
+	bool			get_client_source_initialized() { return _src_addr_initialized; };
 #endif
 	static bool		boot_complete() { return _boot_complete; }

+	bool			is_usb_uart() { return _is_usb_uart; }
+
 protected:
 	Mavlink			*next;

@@ -423,6 +429,7 @@ private:
 	struct sockaddr_in _myaddr;
 	struct sockaddr_in _src_addr;
 	struct sockaddr_in _bcast_addr;
+	bool _src_addr_initialized;

 #endif
 	int _socket_fd;
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1814,9 +1814,12 @@ MavlinkReceiver::receive_thread(void *arg)

 			struct sockaddr_in * srcaddr_last = _mavlink->get_client_source_address();
 			int localhost = (127 << 24) + 1;
-			if (srcaddr_last->sin_addr.s_addr == htonl(localhost) && srcaddr.sin_addr.s_addr != htonl(localhost)) {
+			if ((srcaddr_last->sin_addr.s_addr == htonl(localhost) && srcaddr.sin_addr.s_addr != htonl(localhost))
+					|| (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_ONBOARD && !_mavlink->get_client_source_initialized())) {
 				// if we were sending to localhost before but have a new host then accept him
 				memcpy(srcaddr_last, &srcaddr, sizeof(srcaddr));
+				PX4_WARN("UDP source addr changed: %s", inet_ntoa(srcaddr.sin_addr));
+				_mavlink->set_client_source_initialized();
 			}
 #endif
 			/* if read failed, this loop won't execute */
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -189,8 +189,9 @@ Mission::on_active()
 	}

 	/* see if we need to update the current yaw heading for rotary wing types */
-	if (_navigator->get_vstatus()->is_rotary_wing 
+	if (_navigator->get_vstatus()->is_rotary_wing
 			&& _param_yawmode.get() != MISSION_YAWMODE_NONE
+			&& _param_yawmode.get() < MISSION_YAWMODE_MAX
 			&& _mission_type != MISSION_TYPE_NONE) {
 		heading_sp_update();
 	}
@@ -525,36 +526,52 @@ Mission::heading_sp_update()
 		return;
 	}

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

 	/* set yaw angle for the waypoint iff a loiter time has been specified */
 	if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) {
 		_mission_item.yaw = _on_arrival_yaw;
-	/* always keep the front of the rotary wing pointing to the next waypoint */
-	} else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT) {
-		_mission_item.yaw = get_bearing_to_next_waypoint(
-		        _navigator->get_global_position()->lat,
-		        _navigator->get_global_position()->lon,
-		        _mission_item.lat,
-		        _mission_item.lon);
-	/* always keep the back of the rotary wing pointing towards home */
-	} else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME) {
-		_mission_item.yaw = get_bearing_to_next_waypoint(
-		        _navigator->get_global_position()->lat,
-		        _navigator->get_global_position()->lon,
-		        _navigator->get_home_position()->lat,
-		        _navigator->get_home_position()->lon);
-	/* always keep the back of the rotary wing pointing towards home */
-	} else if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) {
-		_mission_item.yaw = _wrap_pi(get_bearing_to_next_waypoint(
-		        _navigator->get_global_position()->lat,
-		        _navigator->get_global_position()->lon,
-		        _navigator->get_home_position()->lat,
-		        _navigator->get_home_position()->lon) + M_PI_F);
+	} else {
+
+		/* calculate direction the vehicle should point to:
+		 * normal waypoint: current position to {waypoint or home or home + 180deg}
+		 * landing waypoint: last waypoint to {waypoint or home or home + 180deg}
+		 * For landing the last waypoint (= constant) is used to avoid excessive yawing near the ground
+		 */
+		double point_from_latlon[2];
+		if (_mission_item.nav_cmd == NAV_CMD_LAND) {
+			point_from_latlon[0] = pos_sp_triplet->previous.lat;
+			point_from_latlon[1] = pos_sp_triplet->previous.lon;
+		} else {
+			point_from_latlon[0] = _navigator->get_global_position()->lat;
+			point_from_latlon[1] = _navigator->get_global_position()->lon;
+		}
+
+		/* always keep the front of the rotary wing pointing to the next waypoint */
+		if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT) {
+			_mission_item.yaw = get_bearing_to_next_waypoint(
+				point_from_latlon[0],
+				point_from_latlon[1],
+				_mission_item.lat,
+				_mission_item.lon);
+		/* always keep the back of the rotary wing pointing towards home */
+		} else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME) {
+			_mission_item.yaw = get_bearing_to_next_waypoint(
+				point_from_latlon[0],
+				point_from_latlon[1],
+				_navigator->get_home_position()->lat,
+				_navigator->get_home_position()->lon);
+		/* always keep the back of the rotary wing pointing towards home */
+		} else if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) {
+			_mission_item.yaw = _wrap_pi(get_bearing_to_next_waypoint(
+				point_from_latlon[0],
+				point_from_latlon[1],
+				_navigator->get_home_position()->lat,
+				_navigator->get_home_position()->lon) + M_PI_F);
+		}
 	}

 	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -88,7 +88,8 @@ public:
 		MISSION_YAWMODE_NONE = 0,
 		MISSION_YAWMODE_FRONT_TO_WAYPOINT = 1,
 		MISSION_YAWMODE_FRONT_TO_HOME = 2,
-		MISSION_YAWMODE_BACK_TO_HOME = 3
+		MISSION_YAWMODE_BACK_TO_HOME = 3,
+		MISSION_YAWMODE_MAX = 4
 	};

 private:
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
@@ -63,7 +63,6 @@
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vision_position_estimate.h>
 #include <uORB/topics/att_pos_mocap.h>
-#include <uORB/topics/home_position.h>
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/distance_sensor.h>
 #include <mavlink/mavlink_log.h>
@@ -273,7 +272,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	const hrt_abstime ref_init_delay = 1000000;	// wait for 1s after 3D fix
 	struct map_projection_reference_s ref;
 	memset(&ref, 0, sizeof(ref));
-	hrt_abstime home_timestamp = 0;

 	uint16_t accel_updates = 0;
 	uint16_t baro_updates = 0;
@@ -346,8 +344,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	memset(&sensor, 0, sizeof(sensor));
 	struct vehicle_gps_position_s gps;
 	memset(&gps, 0, sizeof(gps));
-	struct home_position_s home;
-	memset(&home, 0, sizeof(home));
 	struct vehicle_attitude_s att;
 	memset(&att, 0, sizeof(att));
 	struct vehicle_local_position_s local_pos;
@@ -373,7 +369,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
 	int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate));
 	int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
-	int home_position_sub = orb_subscribe(ORB_ID(home_position));
 	int distance_sensor_sub = orb_subscribe(ORB_ID(distance_sensor));

 	/* advertise */
@@ -677,46 +672,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				flow_updates++;
 			}

-			/* home position */
-			orb_check(home_position_sub, &updated);
-
-			if (updated) {
-				orb_copy(ORB_ID(home_position), home_position_sub, &home);
-
-				if (home.timestamp != home_timestamp) {
-					home_timestamp = home.timestamp;
-
-					double est_lat, est_lon;
-					float est_alt;
-
-					if (ref_inited) {
-						/* calculate current estimated position in global frame */
-						est_alt = local_pos.ref_alt - local_pos.z;
-						map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon);
-					}
-
-					/* update reference */
-					map_projection_init(&ref, home.lat, home.lon);
-
-					/* update baro offset */
-					baro_offset += home.alt - local_pos.ref_alt;
-
-					local_pos.ref_lat = home.lat;
-					local_pos.ref_lon = home.lon;
-					local_pos.ref_alt = home.alt;
-					local_pos.ref_timestamp = home.timestamp;
-
-					if (ref_inited) {
-						/* reproject position estimate with new reference */
-						map_projection_project(&ref, est_lat, est_lon, &x_est[0], &y_est[0]);
-						z_est[0] = -(est_alt - local_pos.ref_alt);
-					}
-
-					ref_inited = true;
-				}
-			}
-
-
 			/* check no vision circuit breaker is set */
 			if (params.no_vision != CBRK_NO_VISION_KEY) {
 				/* vehicle vision position */
--- a/src/modules/simulator/simulator.h
+++ b/src/modules/simulator/simulator.h
@@ -230,6 +230,7 @@ private:
 		_perf_gps(perf_alloc_once(PC_ELAPSED, "sim_gps_delay")),
 		_perf_airspeed(perf_alloc_once(PC_ELAPSED, "sim_airspeed_delay")),
 		_perf_sim_delay(perf_alloc_once(PC_ELAPSED, "sim_network_delay")),
+		_perf_sim_interval(perf_alloc(PC_INTERVAL, "sim_network_interval")),
 		_accel_pub(nullptr),
 		_baro_pub(nullptr),
 		_gyro_pub(nullptr),
@@ -270,6 +271,7 @@ private:
 	perf_counter_t _perf_gps;
 	perf_counter_t _perf_airspeed;
 	perf_counter_t _perf_sim_delay;
+	perf_counter_t _perf_sim_interval;

 	// uORB publisher handlers
 	orb_advert_t _accel_pub;
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -248,6 +248,7 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
 			uint64_t timestamp = ts.tv_sec * 1000 * 1000 + ts.tv_nsec / 1000;

 			perf_set(_perf_sim_delay, timestamp - sim_timestamp);
+			perf_count(_perf_sim_interval);

 			if (publish) {
 				publish_sensor_topics(&imu);
@@ -426,6 +427,13 @@ void Simulator::initializeSensorData()

 void Simulator::pollForMAVLinkMessages(bool publish)
 {
+	// set the threads name
+#ifdef __PX4_DARWIN
+	pthread_setname_np("sim_rcv");
+#else
+	pthread_setname_np(pthread_self(), "sim_rcv");
+#endif
+
 	// udp socket data
 	struct sockaddr_in _myaddr;
 	const int _port = UDP_PORT;
@@ -504,14 +512,13 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 		pret = ::poll(&fds[0], fd_count, 100);
 	}

-	PX4_INFO("Found initial message, pret = %d", pret);
 	_initialized = true;
 	// reset system time
 	(void)hrt_reset();

 	if (fds[0].revents & POLLIN) {
 		len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen);
-		PX4_INFO("Sending initial controls message to jMAVSim.");
+		PX4_INFO("Sending initial controls message to simulator");
 		send_controls();
 	}

@@ -523,14 +530,6 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 	pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL);
 	pthread_attr_destroy(&sender_thread_attr);

-
-	// set the threads name
-#ifdef __PX4_DARWIN
-	pthread_setname_np("sim_rcv");
-#else
-	pthread_setname_np(pthread_self(), "sim_rcv");
-#endif
-
 	// wait for new mavlink messages to arrive
 	while (true) {

--- a/src/modules/vtol_att_control/tiltrotor.cpp
+++ b/src/modules/vtol_att_control/tiltrotor.cpp
@@ -67,7 +67,7 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
 	_params_handles_tiltrotor.airspeed_blend_start = param_find("VT_ARSP_BLEND");
 	_params_handles_tiltrotor.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK");
 	_params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR");
-	_params_handles_tiltrotor.fw_motors_off = param_find("VT_FW_MOT_OFF");
+	_params_handles_tiltrotor.fw_motors_off = param_find("VT_FW_MOT_OFFID");
 }

 Tiltrotor::~Tiltrotor()
@@ -145,7 +145,7 @@ int Tiltrotor::get_motor_off_channels(int channels)
 			break;
 		}

-		channel_bitmap |= 1 << channel;
+		channel_bitmap |= 1 << (channel - 1);
 		channels = channels / 10;
 	}

@@ -428,7 +428,7 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state)
 		break;

 	case DISABLED:
-		pwm_value = PWM_LOWEST_MAX;
+		pwm_value = PWM_MOTOR_OFF;
 		_rear_motors = DISABLED;
 		break;

@@ -446,21 +446,21 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state)
 	if (fd < 0) {PX4_WARN("can't open %s", dev);}

 	ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
-	struct pwm_output_values pwm_values;
-	memset(&pwm_values, 0, sizeof(pwm_values));
+	struct pwm_output_values pwm_max_values;
+	memset(&pwm_max_values, 0, sizeof(pwm_max_values));

 	for (int i = 0; i < _params->vtol_motor_count; i++) {
 		if (is_motor_off_channel(i)) {
-			pwm_values.values[i] = pwm_value;
+			pwm_max_values.values[i] = pwm_value;

 		} else {
-			pwm_values.values[i] = PWM_DEFAULT_MAX;
+			pwm_max_values.values[i] = PWM_DEFAULT_MAX;
 		}

-		pwm_values.channel_count = _params->vtol_motor_count;
+		pwm_max_values.channel_count = _params->vtol_motor_count;
 	}

-	ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
+	ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_max_values);

 	if (ret != OK) {PX4_WARN("failed setting max values");}

--- a/src/modules/vtol_att_control/tiltrotor_params.c
+++ b/src/modules/vtol_att_control/tiltrotor_params.c
@@ -90,7 +90,7 @@ PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f);
 *
 *
 * @min 0
- * @max 123456
+ * @max 12345678
 * @group VTOL Attitude Control
 */
-PARAM_DEFINE_INT32(VT_FW_MOT_OFF, 0);
+PARAM_DEFINE_INT32(VT_FW_MOT_OFFID, 0);
--- a/src/platforms/qurt/px4_layer/commands_default.c
+++ b/src/platforms/qurt/px4_layer/commands_default.c
@@ -0,0 +1,55 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2015 Mark Charlebois. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file commands_default.c
+ * Commands to run for the "qurt_eagle_default" config
+ *
+ * @author Mark Charlebois <charlebm@gmail.com>
+ */
+
+const char *get_commands()
+{
+
+	static const char *commands =
+		"uorb start\n"
+		"param set CAL_GYRO0_ID 2293760\n"
+		"param set CAL_ACC0_ID 1310720\n"
+		"param set CAL_ACC1_ID 1376256\n"
+		"param set CAL_MAG0_ID 196608\n"
+		"commander start\n"
+
+		;
+
+	return commands;
+
+}