Merge branch 'master' into beta

2026-10-22 01:13:34 +08:00 · 2015-12-18 16:28:36 +00:00
parent 10430d430c 0f67d3fcd8
commit 585e1c93e8
282 changed files with 13382 additions and 3592 deletions
@@ -57,3 +57,6 @@ ROMFS/*/*/rc.autostart
 rootfs/
 *.autosave
 CMakeLists.txt.user
+GPATH
+GRTAGS
+GTAGS
@@ -15,7 +15,7 @@
 	url = https://github.com/ros/gencpp.git
 [submodule "src/lib/dspal"]
 	path = src/lib/dspal
-	url = https://github.com/mcharleb/dspal.git
+	url = https://github.com/ATLFlight/dspal.git
 [submodule "Tools/jMAVSim"]
 	path = Tools/jMAVSim
 	url = https://github.com/PX4/jMAVSim.git
@@ -28,3 +28,6 @@
 [submodule "src/lib/matrix"]
 	path = src/lib/matrix
 	url = https://github.com/PX4/Matrix.git
+[submodule "src/lib/DriverFramework"]
+	path = src/lib/DriverFramework
+	url = https://github.com/PX4/DriverFramework.git
@@ -9,7 +9,7 @@ matrix:
    - os: linux
      sudo: false
    - os: osx
-      osx_image: beta-xcode6.3
+      osx_image: xcode7
      sudo: true

 cache:
@@ -59,12 +59,10 @@ before_install:
      ;
    elif [ "${TRAVIS_OS_NAME}" = "osx" ]; then
         brew tap PX4/homebrew-px4
-      && brew update
-      && brew install astyle
-      && brew install gcc-arm-none-eabi
+      && brew update; brew update
+      && brew install cmake ninja astyle gcc-arm-none-eabi
      && brew install genromfs
      && brew install kconfig-frontends
-      && brew install ninja
      && sudo easy_install pip
      && sudo pip install pyserial empy
      ;
@@ -99,26 +97,27 @@ env:
 script:
  - make check_format
  - arm-none-eabi-gcc --version
-  - echo 'Building POSIX Firmware..' && make posix_sitl_simple
-  - echo 'Running Tests..' && make posix_sitl_simple test
+  - echo 'Building POSIX Firmware..' && make posix_sitl_default
+  - echo 'Running Tests..' && make posix_sitl_default test
+  - echo 'Running Unittests..' && cd unittests && ./run_tests.sh
+  - cd ..
  - echo 'Building UAVCAN node firmware..' && git clone https://github.com/thiemar/vectorcontrol
  - cd vectorcontrol
  - BOARD=s2740vc_1_0 make && BOARD=px4esc_1_6 make
+  - ../Tools/uavcan_copy.sh
  - cd ..
-  - mkdir -p ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/
-  - mkdir -p ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/
-  - cp vectorcontrol/firmware/com.thiemar.s2740vc-v1-1.0.*.bin ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/.
-  - cp vectorcontrol/firmware/org.pixhawk.px4esc-v1-1.0.*.bin ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/.
  - echo 'Building NuttX px4fmu-v1 Firmware..' && make px4fmu-v1_default
  - echo 'Building NuttX px4fmu-v2 Firmware..' && make px4fmu-v2_default
+  - echo 'Building NuttX px4fmu-v4 Firmware..' && make px4fmu-v4_default
  - echo 'Running Tests..' && make px4fmu-v2_default test

 after_success:
  - if [ "${TRAVIS_OS_NAME}" = "linux" ]; then
         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
-      && zip Firmware.zip px4fmu-v1_default.px4 px4fmu-v2_default.px4
-      && ./CI-Tools/s3cmd-put px4fmu-v1_default.px4 px4fmu-v2_default.px4 build_px4fmu-v2_default/parameters.xml build_px4fmu-v2_default/airframes.xml CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/
+      && 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/
      && ./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
@@ -121,6 +121,10 @@
 # and leads to wrong toolchain detection
 cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

+# Use clang
+#SET (CMAKE_C_COMPILER /usr/bin/clang-3.6)
+#SET (CMAKE_CXX_COMPILER /usr/bin/clang++-3.6)
+
 #=============================================================================
 # parameters
 #
@@ -146,6 +150,16 @@ list(GET config_args 1 BOARD)
 list(GET config_args 2 LABEL)
 set(target_name "${OS}-${BOARD}-${LABEL}")

+if("${OS}" STREQUAL "posix")
+	if (APPLE)
+		set(DF_TARGET darwin)
+	else()
+		set(DF_TARGET linux)
+	endif()
+else()
+	set(DF_TARGET ${OS})
+endif()
+
 message(STATUS "${target_name}")

 # switch to ros CMake file if building ros
@@ -230,6 +244,7 @@ px4_add_git_submodule(TARGET git_gtest PATH "unittets/gtest")
 px4_add_git_submodule(TARGET git_uavcan PATH "src/modules/uavcan/libuavcan")
 px4_add_git_submodule(TARGET git_nuttx PATH "NuttX")
 px4_add_git_submodule(TARGET git_dspal PATH "src/lib/dspal")
+px4_add_git_submodule(TARGET git_driverframework PATH "src/lib/DriverFramework")
 px4_add_git_submodule(TARGET git_jmavsim PATH "Tools/jMAVSim")
 px4_add_git_submodule(TARGET git_gazebo PATH "Tools/sitl_gazebo")
 px4_add_git_submodule(TARGET git_matrix PATH "src/lib/matrix")
@@ -321,6 +336,11 @@ endforeach()

 add_subdirectory(src/firmware/${OS})

+add_subdirectory(src/lib/DriverFramework/framework/src)
+#add_dependencies(df_driver_framework nuttx_export_${CONFIG}.stamp)
+if (NOT "${OS}" STREQUAL "nuttx")
+endif()
+
 if (config_io_board)
 	add_subdirectory(src/modules/px4iofirmware)
 endif()
@@ -0,0 +1,12 @@
+{
+    "board_id": 11,
+    "magic": "PX4FWv1",
+    "description": "Firmware for the PX4FMUv4 board",
+    "image": "",
+    "build_time": 0,
+    "summary": "PX4FMUv4",
+    "version": "0.1",
+    "image_size": 0,
+    "git_identity": "",
+    "board_revision": 0
+}
@@ -130,6 +130,9 @@ px4fmu-v1_default:
 px4fmu-v2_default:
 	$(call cmake-build,nuttx_px4fmu-v2_default)

+px4fmu-v4_default:
+	$(call cmake-build,nuttx_px4fmu-v4_default)
+
 px4fmu-v2_simple:
 	$(call cmake-build,nuttx_px4fmu-v2_simple)

@@ -139,13 +142,13 @@ px4fmu-v2_lpe:
 nuttx_sim_simple:
 	$(call cmake-build,$@)

-posix_sitl_simple:
+posix_sitl_default:
 	$(call cmake-build,$@)

 posix_sitl_lpe:
 	$(call cmake-build,$@)

-ros_sitl_simple:
+ros_sitl_default:
 	$(call cmake-build,$@)

 qurt_eagle_travis:
@@ -157,19 +160,15 @@ qurt_eagle_release:
 posix_eagle_release:
 	$(call cmake-build,$@)

-posix: posix_sitl_simple
-
-posix_sitl_default: posix_sitl_simple
-
-ros: ros_sitl_simple
+posix: posix_sitl_default

 sitl_deprecation:
 	@echo "Deprecated. Use 'make posix_sitl_default jmavsim' or"
 	@echo "'make posix_sitl_default gazebo' if Gazebo is preferred."

-sitl_quad: sitl_deprecation
-sitl_plane: sitl_deprecation
-sitl_ros: sitl_deprecation
+run_sitl_quad: sitl_deprecation
+run_sitl_plane: sitl_deprecation
+run_sitl_ros: sitl_deprecation

 # Other targets
 # --------------------------------------------------------------------
@@ -180,11 +179,12 @@ clean:
 	@rm -rf build_*/
 	@(cd NuttX && git clean -d -f -x)
 	@(cd src/modules/uavcan/libuavcan && git clean -d -f -x)
+	@(git submodule sync)

 # targets handled by cmake
 cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak \
 	run_cmake_config config gazebo gazebo_gdb gazebo_lldb jmavsim \
-	jmavsim_gdb jmavsim_lldb gazebo_gdb_iris gazebo_lldb_vtol gazebo_iris gazebo_vtol
+	jmavsim_gdb jmavsim_lldb gazebo_gdb_iris gazebo_lldb_tailsitter gazebo_iris gazebo_tailsitter
 $(foreach targ,$(cmake_targets),$(eval $(call cmake-targ,$(targ))))

 .PHONY: clean
@@ -38,10 +38,10 @@ Testing guide:
 http://px4.io/dev/unit_tests

 This repository contains code supporting these boards:
-  * [Snapdragon Flight](https://www.intrinsyc.com/qualcomm-snapdragon-flight/)
+  * [Snapdragon Flight](http://dev.px4.io/hardware-snapdragon.html)
  * FMUv1.x
-  * FMUv2.x (Pixhawk)
-  * FMUv3.x (Pixhawk 2)
+  * FMUv2.x ([Pixhawk](http://dev.px4.io/hardware-pixhawk.html), Pixhawk 2 and [Pixfalcon](http://dev.px4.io/hardware-pixfalcon.html))
+  * FMUv4.x (Pixhawk X and [Pixracer](http://dev.px4.io/hardware-pixracer.html))
  * AeroCore (v1 and v2)
  * STM32F4Discovery (basic support) [Tutorial](https://pixhawk.org/modules/stm32f4discovery)

@@ -1,6 +1,6 @@
 #!nsh
 #
-# @name Generic AAERT tailplane airframe with Quad VTOL.
+# @name Fun Cub Quad VTOL.
 #
 # @type Standard VTOL
 #
@@ -0,0 +1,51 @@
+#!nsh
+#
+# @name Generic AAVVT v-tail plane airframe with Quad VTOL.
+#
+# @type Standard VTOL
+#
+# @maintainer Sander Smeets <sander@droneslab.com>
+#
+
+sh /etc/init.d/rc.vtol_defaults
+
+if [ $AUTOCNF == yes ]
+then
+	param set VT_TYPE 2
+	param set VT_MOT_COUNT 4
+	param set VT_TRANS_THR 0.75
+
+	param set MC_ROLL_P 7.0
+	param set MC_ROLLRATE_P 0.15
+	param set MC_ROLLRATE_I 0.002
+	param set MC_ROLLRATE_D 0.003
+	param set MC_ROLLRATE_FF 0.0
+	param set MC_PITCH_P 7.0
+	param set MC_PITCHRATE_P 0.12
+	param set MC_PITCHRATE_I 0.002
+	param set MC_PITCHRATE_D 0.003
+	param set MC_PITCHRATE_FF 0.0
+	param set MC_YAW_P 2.8
+	param set MC_YAW_FF 0.5
+	param set MC_YAWRATE_P 0.22
+	param set MC_YAWRATE_I 0.02
+	param set MC_YAWRATE_D 0.0
+	param set MC_YAWRATE_FF 0.0
+	
+fi
+
+set MIXER vtol_quad_x
+set PWM_OUT 12345678
+
+set MIXER_AUX vtol_AAVVT
+set PWM_AUX_RATE 50
+set PWM_AUX_OUT 1234
+set PWM_AUX_DISARMED 1000
+set PWM_AUX_MIN 1000
+set PWM_AUX_MAX 2000
+
+set MAV_TYPE 22
+
+param set VT_MOT_COUNT 4
+param set VT_IDLE_PWM_MC 1080
+param set VT_TYPE 2
@@ -0,0 +1,65 @@
+#!nsh
+#
+# @name Coaxial Helicopter (such as Esky Lama v4 or Esky Big Lama)
+#
+# @type Coaxial Helicopter
+# 
+# @output MAIN1 Left swashplate servomotor, pitch axis
+# @output MAIN2 Right swashplate servomotor, roll axis
+# @output MAIN3 Upper rotor (CCW)
+# @output MAIN4 Lower rotor (CW)
+#
+# @maintainer Emmanuel Roussel
+#
+
+set VEHICLE_TYPE mc
+
+if [ $AUTOCNF == yes ]
+then
+	param set PE_VELNE_NOISE 0.5
+	param set PE_VELD_NOISE 0.35
+	param set PE_POSNE_NOISE 0.5
+	param set PE_POSD_NOISE 1.25
+
+	param set NAV_ACC_RAD 2.0
+	param set RTL_RETURN_ALT 30.0
+	param set RTL_DESCEND_ALT 10.0
+
+	param set PWM_DISARMED 900
+	param set PWM_MIN 1075
+	param set PWM_MAX 1950
+
+	param set MC_ROLL_P 6.5
+	param set MC_ROLLRATE_P 0.17
+	param set MC_ROLLRATE_I 0.05
+	param set MC_ROLLRATE_D 0.005
+	param set MC_ROLLRATE_FF 0
+	param set MC_PITCH_P 6.5
+	param set MC_PITCHRATE_P 0.17
+	param set MC_PITCHRATE_I 0.05
+	param set MC_PITCHRATE_D 0.005
+	param set MC_PITCHRATE_FF 0
+	param set MC_YAW_P 2
+	param set MC_YAW_FF 0.5
+	param set MC_YAWRATE_P 0.1
+	param set MC_YAWRATE_I 0.1
+	param set MC_YAWRATE_D 0.0
+	param set MC_YAWRATE_FF 0
+fi
+
+set MIXER coax
+# use PWM parameters for throttle channel
+set PWM_OUT 34
+set PWM_RATE 400
+set PWM_DISARMED p:PWM_DISARMED
+set PWM_MIN p:PWM_MIN
+set PWM_MAX p:PWM_MAX
+
+# This is the gimbal pass mixer
+set MIXER_AUX pass
+set PWM_AUX_RATE 50
+set PWM_AUX_OUT 1234
+set PWM_AUX_DISARMED 1500
+set PWM_AUX_MIN 1000
+set PWM_AUX_MAX 2000
+
@@ -6,8 +6,8 @@
 #
 # @output MAIN1 aileron
 # @output MAIN2 elevator
-# @output MAIN4 rudder
-# @output MAIN3 throttle
+# @output MAIN3 rudder
+# @output MAIN4 throttle
 # @output MAIN5 flaps
 #
 # @output AUX1 feed-through of RC AUX1 channel
@@ -21,6 +21,8 @@ sh /etc/init.d/rc.fw_defaults

 set MIXER AERT

-# The ESC requires a specific pulse to arm.
+# use PWM parameters for throttle channel
 set PWM_OUT 4
 set PWM_DISARMED p:PWM_DISARMED
+set PWM_MIN p:PWM_MIN
+set PWM_MAX p:PWM_MAX
@@ -21,6 +21,8 @@ sh /etc/init.d/rc.fw_defaults

 set MIXER AETR

-# The ESC requires a specific pulse to arm.
+# use PWM parameters for throttle channel
 set PWM_OUT 3
 set PWM_DISARMED p:PWM_DISARMED
+set PWM_MIN p:PWM_MIN
+set PWM_MAX p:PWM_MAX
@@ -0,0 +1,50 @@
+#!nsh
+#
+# @name Bormatec Maja
+#
+# @type Standard Plane
+#
+# @output MAIN1 aileron
+# @output MAIN2 aileron
+# @output MAIN3 elevator
+# @output MAIN4 rudder
+# @output MAIN5 throttle
+# @output MAIN6 wheel
+# @output MAIN7 flaps
+#
+# @output AUX1 feed-through of RC AUX1 channel
+# @output AUX2 feed-through of RC AUX2 channel
+# @output AUX3 feed-through of RC AUX3 channel
+#
+# @maintainer Andreas Antener <andreas@uaventure.com>
+#
+
+sh /etc/init.d/rc.fw_defaults
+
+if [ $AUTOCNF == yes ]
+then
+    param set FW_AIRSPD_MIN 10
+    param set FW_AIRSPD_TRIM 15
+    param set FW_AIRSPD_MAX 20
+
+    param set FW_MAN_P_MAX 55
+    param set FW_MAN_R_MAX 55
+    param set FW_R_LIM 55
+
+    param set FW_WR_FF 0.2
+    param set FW_WR_I 0.2
+    param set FW_WR_IMAX 0.8
+    param set FW_WR_P 1
+    param set FW_W_RMAX 0
+
+    # set disarmed value for the ESC
+    param set PWM_DISARMED 1000
+fi
+
+set MIXER AAERTWF
+
+# use PWM parameters for throttle channel
+set PWM_OUT 5
+set PWM_DISARMED p:PWM_DISARMED
+set PWM_MIN p:PWM_MIN
+set PWM_MAX p:PWM_MAX
@@ -0,0 +1,51 @@
+#!nsh
+#
+# @name Applied Aeronautics Albatross
+#
+# @type Standard Plane
+#
+# @output MAIN1 aileron right
+# @output MAIN2 aileron left
+# @output MAIN3 v-tail right
+# @output MAIN4 v-tail left
+# @output MAIN5 throttle
+# @output MAIN6 wheel
+# @output MAIN7 flaps right
+# @output MAIN8 flaps left
+#
+# @output AUX1 feed-through of RC AUX1 channel
+# @output AUX2 feed-through of RC AUX2 channel
+# @output AUX3 feed-through of RC AUX3 channel
+#
+# @maintainer Andreas Antener <andreas@uaventure.com>
+#
+
+sh /etc/init.d/rc.fw_defaults
+
+if [ $AUTOCNF == yes ]
+then
+    param set FW_AIRSPD_MIN 10
+    param set FW_AIRSPD_TRIM 15
+    param set FW_AIRSPD_MAX 20
+
+    param set FW_MAN_P_MAX 55
+    param set FW_MAN_R_MAX 55
+    param set FW_R_LIM 55
+
+    param set FW_WR_FF 0.2
+    param set FW_WR_I 0.2
+    param set FW_WR_IMAX 0.8
+    param set FW_WR_P 1
+    param set FW_W_RMAX 0
+
+    # set disarmed value for the ESC
+    param set PWM_DISARMED 1000
+fi
+
+set MIXER AAVVTWFF
+
+# use PWM parameters for throttle channel
+set PWM_OUT 5
+set PWM_DISARMED p:PWM_DISARMED
+set PWM_MIN p:PWM_MIN
+set PWM_MAX p:PWM_MAX
@@ -30,4 +30,7 @@ then
 	param set PWM_MIN 980
 	param set MPC_THR_MIN 0.06
 	param set MPC_MANTHR_MIN 0.06
+	param set MC_ROLLRATE_MAX 360.0
+	param set MC_PITCHRATE_MAX 360.0
+	param set MC_YAWRATE_MAX 200.0
 fi
@@ -3,8 +3,20 @@
 # Standard startup script for PX4FMU v1, v2, v3 onboard sensor drivers.
 #

-if ms5611 start
+if ver hwcmp PX4FMU_V1
 then
+	if ms5611 start
+	then
+	fi
+else
+	if ms5611 -s start
+	then
+	fi
+
+	# Blacksheep telemetry
+	if bst start
+	then
+	fi
 fi

 if adc start
@@ -67,32 +79,55 @@ then
 		fi
 	fi
 else
-	# FMUv1
-	if mpu6000 start
+	if ver hwcmp PX4FMU_V4
 	then
-	fi
+		# External I2C bus
+		if hmc5883 -C -T -X start
+		then
+		fi

-	if l3gd20 start
-	then
-	fi
+		# Internal SPI bus is rotated 90 deg yaw
+		if hmc5883 -C -T -S -R 2 start
+		then
+		fi

-	# MAG selection
-	if param compare SENS_EXT_MAG 2
-	then
-		if hmc5883 -C -I start
+		# Internal SPI bus ICM-20608-G is rotated 90 deg yaw
+		if mpu6000 -R 2 -T 20608 start
+		then
+		fi
+
+		# Internal SPI bus mpu9250 is rotated 90 deg yaw
+		if mpu9250 -R 2 start
 		then
 		fi
 	else
-		# Use only external as primary
-		if param compare SENS_EXT_MAG 1
+		# FMUv1
+		if mpu6000 start
 		then
-			if hmc5883 -C -X start
+		fi
+
+		if l3gd20 start
+		then
+		fi
+
+		# MAG selection
+		if param compare SENS_EXT_MAG 2
+		then
+			if hmc5883 -C -I start
 			then
 			fi
 		else
-		# auto-detect the primary, prefer external
-			if hmc5883 start
+			# Use only external as primary
+			if param compare SENS_EXT_MAG 1
 			then
+				if hmc5883 -C -X start
+				then
+				fi
+			else
+			# auto-detect the primary, prefer external
+				if hmc5883 start
+				then
+				fi
 			fi
 		fi
 	fi
@@ -18,4 +18,4 @@ fw_pos_control_l1 start
 # Start Land Detector
 # Multicopter for now until we have something for VTOL
 #
-land_detector start multicopter
+land_detector start vtol
@@ -217,7 +217,12 @@ then
 	#
 	if param compare SYS_USE_IO 1
 	then
-		set USE_IO yes
+		if ver hwcmp PX4FMU_V4
+		then
+			set USE_IO no
+		else
+			set USE_IO yes
+		fi
 	else
 		set USE_IO no
 	fi
@@ -233,6 +238,13 @@ then
 	fi
 	unset MODE

+	#
+	# Wipe incompatible settings for boards not having two outputs
+	if ver hwcmp PX4FMU_V4
+	then
+		set MIXER_AUX none
+	fi
+
 	#
 	# Override parameters from user configuration file
 	#
@@ -537,8 +549,14 @@ then
 			# Exit from nsh to free port for mavlink
 			set EXIT_ON_END yes
 		else
-			# Start MAVLink on default port: ttyS1
 			set MAVLINK_F "-r 1200"
+			# Avoid using ttyS1 for MAVLink on FMUv4
+			if ver hwcmp PX4FMU_V4
+			then
+				set MAVLINK_F "-r 1200 -d /dev/ttyS2"
+				# Start MAVLink on Wifi (ESP8266 port)
+				mavlink start -r 800000 -b 921600 -d /dev/ttyS0
+			fi
 		fi
 	fi

@@ -564,6 +582,10 @@ then
 		then
 			mavlink start -d /dev/ttyS2 -b 57600 -m osd -r 1000
 		fi
+		if param compare SYS_COMPANION 257600
+		then
+			mavlink start -d /dev/ttyS2 -b 57600 -m magic -r 5000 -x
+		fi
 		# Sensors on the PWM interface bank
 		# clear pins 5 and 6
 		if param compare SENS_EN_LL40LS 1
@@ -0,0 +1,96 @@
+Aileron/rudder/elevator/throttle/wheel/flaps mixer for PX4FMU
+=======================================================
+
+This file defines mixers suitable for controlling a fixed wing aircraft with
+aileron, rudder, elevator, throttle and steerable wheel controls using PX4FMU.
+The configuration assumes the aileron servo(s) are connected to PX4FMU servo
+output 0 and 1, the elevator to output 2, the rudder to output 3, the throttle
+to output 4 and the wheel to output 5.
+
+Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
+(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon).
+
+Aileron mixer (roll + flaperon)
+---------------------------------
+
+This mixer assumes that the aileron servos are set up correctly mechanically;
+depending on the actual configuration it may be necessary to reverse the scaling
+factors (to reverse the servo movement) and adjust the offset, scaling and
+endpoints to suit.
+
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 0 0  10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 0 0  10000  10000      0 -10000  10000
+S: 0 6 -10000 -10000      0 -10000  10000
+
+Elevator mixer
+------------
+Two scalers total (output, roll).
+
+This mixer assumes that the elevator servo is set up correctly mechanically;
+depending on the actual configuration it may be necessary to reverse the scaling
+factors (to reverse the servo movement) and adjust the offset, scaling and
+endpoints to suit.
+
+M: 1
+O:       10000   10000      0 -10000  10000
+S: 0 1  -10000  -10000      0 -10000  10000
+
+Rudder mixer
+------------
+Two scalers total (output, yaw).
+
+This mixer assumes that the rudder servo is set up correctly mechanically;
+depending on the actual configuration it may be necessary to reverse the scaling
+factors (to reverse the servo movement) and adjust the offset, scaling and
+endpoints to suit.
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 2  10000  10000      0 -10000  10000
+
+Motor speed mixer
+-----------------
+Two scalers total (output, thrust).
+
+This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
+range.  Inputs below zero are treated as zero.
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 3      0  20000 -10000 -10000  10000
+
+Wheel mixer
+------------
+Two scalers total (output, yaw).
+
+This mixer assumes that the wheel servo is set up correctly mechanically;
+depending on the actual configuration it may be necessary to reverse the scaling
+factors (to reverse the servo movement) and adjust the offset, scaling and
+endpoints to suit.
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 2  10000  10000      0 -10000  10000
+
+
+Flaps / gimbal / payload mixer for last three channels,
+using the payload control group
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 2 0  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 2 2  10000  10000      0 -10000  10000
@@ -0,0 +1,84 @@
+Aileron/v-tail/throttle/wheel/flaps mixer for PX4FMU
+=======================================================
+
+This file defines mixers suitable for controlling a fixed wing aircraft with
+aileron, v-tail (rudder, elevator), throttle, steerable wheel and flaps
+using PX4FMU.
+The configuration assumes the aileron servos are connected to PX4FMU servo
+output 0 and 1, the tail servos to output 2 and 3, the throttle
+to output 4, the wheel to output 5 and the flaps to output 6 and 7.
+
+Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
+(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon).
+
+Aileron mixer (roll + flaperon)
+---------------------------------
+
+This mixer assumes that the aileron servos are set up correctly mechanically;
+depending on the actual configuration it may be necessary to reverse the scaling
+factors (to reverse the servo movement) and adjust the offset, scaling and
+endpoints to suit.
+
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 0 0  10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 0 0  10000  10000      0 -10000  10000
+S: 0 6 -10000 -10000      0 -10000  10000
+
+V-tail mixers
+-------------
+Three scalers total (output, roll, pitch).
+
+On the assumption that the two tail servos are physically reversed, the pitch
+input is inverted between the two servos.
+
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 0 2  -7000  -7000      0 -10000  10000
+S: 0 1  -8000  -8000      0 -10000  10000
+
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 0 2  -7000  -7000      0 -10000  10000
+S: 0 1   8000   8000      0 -10000  10000
+
+Motor speed mixer
+-----------------
+Two scalers total (output, thrust).
+
+This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
+range.  Inputs below zero are treated as zero.
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 3      0  20000 -10000 -10000  10000
+
+Wheel mixer
+------------
+Two scalers total (output, yaw).
+
+This mixer assumes that the wheel servo is set up correctly mechanically;
+depending on the actual configuration it may be necessary to reverse the scaling
+factors (to reverse the servo movement) and adjust the offset, scaling and
+endpoints to suit.
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 2 -10000 -10000      0 -10000  10000
+
+Flaps mixer
+------------
+Flap servos are physically reversed.
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4      0   5000 -10000 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4      0  -5000  10000 -10000  10000
+
@@ -0,0 +1,33 @@
+Coaxial helicopter mixer
+- Two servomotors act on the swashplate (90° angle on the swashplate, decoupled effect on roll and pitch).
+- No collective pitch.
+- One motor per rotor.
+===========================
+
+Left swashplate servomotor, pitch axis
+-------------
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 1  -10000  -10000      0 -10000  10000
+
+Right swashplate servomotor, roll axis
+-------------
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 0  10000  10000      0 -10000  10000
+
+Upper rotor (CCW)
+Mixing between yaw and thrust
+-------------
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 0 2  10000  10000      0 -10000  10000
+S: 0 3      0  20000 -10000 -10000  10000
+
+Lower rotor (CW)
+Mixing between yaw and thrust
+-------------
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 0 2  -10000  -10000      0 -10000  10000
+S: 0 3      0  20000 -10000 -10000  10000
@@ -0,0 +1,26 @@
+Multirotor mixer for PX4FMU
+===========================
+
+This file defines a single mixer for a quadrotor in the H configuration.  All controls
+are mixed 100%.
+
+R: 4h 10000 10000 10000 0
+
+Gimbal / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
@@ -9,10 +9,10 @@ R: 4x 10000 10000 10000 0
 #mixer for the elevons
 M: 2
 O:      10000  10000      0 -10000  10000
-S: 1 0  10000  10000      0 -10000  10000
-S: 1 1  10000  10000      0 -10000  10000
+S: 1 0  5000  5000      0 -10000  10000
+S: 1 1  5000  5000      0 -10000  10000

 M: 2
 O:       10000   10000    0 -10000  10000
-S: 1 0   10000   10000    0 -10000  10000
-S: 1 1  -10000  -10000    0 -10000  10000
+S: 1 0   5000   5000    0 -10000  10000
+S: 1 1  -5000  -5000    0 -10000  10000
@@ -0,0 +1,59 @@
+Aileron/v-tail/throttle VTOL mixer for PX4FMU
+=======================================================
+
+This file defines mixers suitable for controlling a fixed wing aircraft with
+aileron, v-tail (rudder, elevator) and throttle using PX4FMU.
+The configuration assumes the aileron servos are connected to PX4FMU 
+AUX servo output 0 and 1, the tail servos to output 2 and 3, the throttle
+to output 4.
+
+Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
+(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon).
+
+Aileron mixer (roll + flaperon)
+---------------------------------
+
+This mixer assumes that the aileron servos are set up correctly mechanically;
+depending on the actual configuration it may be necessary to reverse the scaling
+factors (to reverse the servo movement) and adjust the offset, scaling and
+endpoints to suit.
+
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 1 0  10000  10000      0 -10000  10000
+S: 1 6  10000  10000      0 -10000  10000
+
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 1 0  10000  10000      0 -10000  10000
+S: 1 6 -10000 -10000      0 -10000  10000
+
+
+V-tail mixers
+-------------
+Three scalers total (output, roll, pitch).
+
+On the assumption that the two tail servos are physically reversed, the pitch
+input is inverted between the two servos.
+
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 1 2  -7000  -7000      0 -10000  10000
+S: 1 1  -8000  -8000      0 -10000  10000
+
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 1 2  -7000  -7000      0 -10000  10000
+S: 1 1   8000   8000      0 -10000  10000
+
+Motor speed mixer
+-----------------
+Two scalers total (output, thrust).
+
+This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
+range.  Inputs below zero are treated as zero.
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 1 3      0  20000 -10000 -10000  10000
+
@@ -14,6 +14,7 @@ for fn in $(find src/examples \
                 src/lib/rc \
                 src/lib/version \
                 src/modules/attitude_estimator_q \
+                 src/modules/fw_att_control \
                 src/modules/gpio_led \
                 src/modules/land_detector \
                 src/modules/muorb \
@@ -0,0 +1,123 @@
+#!/usr/bin/env python
+############################################################################
+#
+# 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.
+#
+############################################################################
+
+import os
+import subprocess
+
+# The PX4_BACKTRACE() macro can be used to generate backtraces in the
+# running code. These provide the addresses only. This utility can be
+# used to decode the backtrace into a readable form:
+#
+# Example:
+# If the following lines were pasted into the shell after running decode_backtrace.py
+#
+# INFO  Backtrace: 10
+# INFO  ./mainapp(px4_backtrace+0x27) [0x42b212]
+# INFO  ./mainapp() [0x42d608]
+# INFO  ./mainapp() [0x42d57e]
+# INFO  ./mainapp() [0x4ba48d]
+#
+# The output would be:
+#
+# 0x42b212 px4_backtrace (0x42b1eb)
+# 0x42d608 ACCELSIM::devRead(void*, unsigned long) (0x42d600)
+# 0x42d57e ACCELSIM::start() (0x42d578)
+# 0x4ba48d DriverFramework::DevObj::addHandle(DriverFramework::DevHandle&) (0x4ba42e)
+#
+# The output format is:
+# <backtrace address> <function name> (<function address>)
+
+
+def usage():
+	msg = """
+Usage: Tools/decode_backtrace.py <builddir>
+
+This will load the symbols for <builddir>/src/firmware/posix/mainapp
+The user just needs to copy and paste the backtrace into the terminal
+where decode_backtrace.py is running.
+
+"""
+	print msg
+
+funcaddr = []
+func = []
+
+# Load the symbols from the binary
+def load_symbol_map():
+	output = subprocess.check_output(["nm", "-p", "-C", os.sys.argv[1]+"/src/firmware/posix/mainapp"])
+	data = output.split("\n")
+	data.sort()
+
+	i = 0
+	for line in data:
+		if line.startswith("0"):
+			funcaddr.append(int(line[0:16],16))
+			func.append(line[19:])
+			#print(hex(funcaddr[i]), func[i])
+			i = i+1
+
+# Find the function for the specified call stack address
+def lookup(addr):
+	i=1
+	while i < len(funcaddr) and addr > funcaddr[i]:
+		i=i+1
+	if i >= len(funcaddr):
+		return -1
+
+	# return the function before this one
+	return i-1
+
+if len(os.sys.argv) != 2:
+	usage()
+	raise SystemExit
+
+load_symbol_map()
+
+print
+print "Paste the backtrace here. Use Ctrl-C to exit."
+print
+while 1:
+	# Wait for user to paste the backtrace into the shell
+	line = raw_input()
+	tmp = line.split("[")
+	if len(tmp) == 1:
+		if len(line.split("Backtrace:")) > 1:
+			print
+		continue
+	addrstr = tmp[1].split("]")[0]
+	addr = int(addrstr, 16)
+	idx = lookup(addr)
+	if idx >=  0:
+		print hex(addr), func[idx], "({0})".format(hex(funcaddr[idx]))
+
@@ -17,7 +17,7 @@ for index,m in enumerate(raw_messages):
 	temp_list_floats = []
 	temp_list_uint64 = []
 	temp_list_bool = []
-	if("actuator_control" not in m and "pwm_input" not in m and "position_setpoint" not in m):
+	if("pwm_input" not in m and "position_setpoint" not in m):
 		temp_list = []
 		f = open(m,'r')
 		for line in f.readlines():
@@ -55,9 +55,11 @@ for index,m in enumerate(raw_messages):

 		f.close()
 		(m_head, m_tail) = os.path.split(m)
-		messages.append(m_tail.split('.')[0])
+		message = m_tail.split('.')[0]
+		if message != "actuator_controls":
+			messages.append(message)
+			message_elements.append(temp_list)
 		#messages.append(m.split('/')[-1].split('.')[0])
-		message_elements.append(temp_list)

 num_messages = len(messages);

@@ -198,6 +200,7 @@ for index,m in enumerate(messages[1:]):
 print("\t} else {")
 print("\t\t printf(\" Topic did not match any known topics\\n\");")
 print("\t}")
+print("\t\torb_unsubscribe(sub);")
 print("\t return 0;")

 print("}")
@@ -130,7 +130,7 @@ def main():
    # Output to XML file
    if args.xml:
        print("Creating XML file " + args.xml)
-        out = xmlout.XMLOutput(param_groups, args.board, args.inject_xml)
+        out = xmlout.XMLOutput(param_groups, args.board, os.path.join(args.src_path, args.inject_xml))
        out.Save(args.xml)

    # Output to DokuWiki tables
@@ -148,6 +148,7 @@ class uploader(object):
        OK              = b'\x10'
        FAILED          = b'\x11'
        INVALID         = b'\x13'     # rev3+
+        BAD_SILICON_REV = b'\x14'     # rev5+

        # command bytes
        NOP             = b'\x00'     # guaranteed to be discarded by the bootloader
@@ -162,11 +163,14 @@ class uploader(object):
        GET_SN          = b'\x2b'     # rev4+  , get a word from SN area
        GET_CHIP        = b'\x2c'     # rev5+  , get chip version
        SET_BOOT_DELAY  = b'\x2d'     # rev5+  , set boot delay
+        GET_CHIP_DES    = b'\x2e'     # rev5+  , get chip description in ASCII
+        MAX_DES_LENGTH  = 20
+
        REBOOT          = b'\x30'
        
        INFO_BL_REV     = b'\x01'        # bootloader protocol revision
-        BL_REV_MIN      = 2             # minimum supported bootloader protocol 
-        BL_REV_MAX      = 4             # maximum supported bootloader protocol 
+        BL_REV_MIN      = 2              # minimum supported bootloader protocol
+        BL_REV_MAX      = 5              # maximum supported bootloader protocol
        INFO_BOARD_ID   = b'\x02'        # board type
        INFO_BOARD_REV  = b'\x03'        # board revision
        INFO_FLASH_SIZE = b'\x04'        # max firmware size in bytes
@@ -235,12 +239,16 @@ class uploader(object):
                    if (self.__recv() != self.INSYNC):
                            #print("unexpected 0x%x instead of INSYNC" % ord(c))
                            return False;
-
-                    if (self.__recv() != self.OK):
+                    c = self.__recv()
+                    if (c == self.BAD_SILICON_REV):
+                        raise NotImplementedError()
+                    if (c != self.OK):
                            #print("unexpected 0x%x instead of OK" % ord(c))
                            return False
                    return True

+                except NotImplementedError:
+                    raise RuntimeError("Programing not supported for this version of silicon!\n See https://pixhawk.org/help/errata")
                except RuntimeError:
                    #timeout, no response yet
                    return False
@@ -274,6 +282,14 @@ class uploader(object):
                value = self.__recv_int()
                self.__getSync()
                return value
+        # send the GET_CHIP command
+        def __getCHIPDes(self):
+                self.__send(uploader.GET_CHIP_DES + uploader.EOC)
+                length = self.__recv_int()
+                value = self.__recv(length)
+                self.__getSync()
+                peices = value.split(",")
+                return peices

        def __drawProgressBar(self, label, progress, maxVal):
                if maxVal < progress:
@@ -467,6 +483,12 @@ class uploader(object):
                                    print(binascii.hexlify(x).decode('Latin-1'), end='') # show user
                            print('')
                            print("chip: %08x" % self.__getCHIP())
+                            if (self.bl_rev >= 5):
+                                des = self.__getCHIPDes()
+                                if (len(des) == 2):
+                                    print("family: %s" % des[0])
+                                    print("revision: %s" % des[1])
+                                    print("flash %d" % self.fw_maxsize)
                    except Exception:
                            # ignore bad character encodings
                            pass
@@ -37,6 +37,8 @@ then
 	kill $jmavsim_pid
 fi

+set -e
+
 cp Tools/posix_lldbinit $build_path/src/firmware/posix/.lldbinit
 cp Tools/posix.gdbinit $build_path/src/firmware/posix/.gdbinit

@@ -46,14 +48,14 @@ if [ "$program" == "jmavsim" ] && [ "$no_sim" == "" ]
 then
 	cd Tools/jMAVSim
 	ant
-	java -Djava.ext.dirs= -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp 127.0.0.1:14560 &
+	nice -n -10 java -Djava.ext.dirs= -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp 127.0.0.1:14560 &
 	SIM_PID=`echo $!`
-elif [ "$3" == "gazebo" ] && [ "$no_sim" == "" ]
+elif [ "$program" == "gazebo" ] && [ "$no_sim" == "" ]
 then
 	if [ -x "$(command -v gazebo)" ]
 	then
 		# Set the plugin path so Gazebo finds our model and sim
-		export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:$curr_dir/Tools/sitl_gazebo/Build
+		export GAZEBO_PLUGIN_PATH=$curr_dir/Tools/sitl_gazebo/Build:${GAZEBO_PLUGIN_PATH}
 		# Set the model path so Gazebo finds the airframes
 		export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:$curr_dir/Tools/sitl_gazebo/models
 		# The next line would disable online model lookup, can be commented in, in case of unstable behaviour.
@@ -61,11 +63,11 @@ then
 		export SITL_GAZEBO_PATH=$curr_dir/Tools/sitl_gazebo
 		mkdir -p Tools/sitl_gazebo/Build
 		cd Tools/sitl_gazebo/Build
-		cmake ..
+		cmake -Wno-dev ..
 		make -j4
-		gzserver ../worlds/${model}.world &
+		nice -n -10 gzserver --verbose ../worlds/${model}.world &
 		SIM_PID=`echo $!`
-		gzclient &
+		gzclient --verbose &
 		GUI_PID=`echo $!`
 	else
 		echo "You need to have gazebo simulator installed!"
@@ -76,21 +78,25 @@ cd $build_path/src/firmware/posix
 mkdir -p rootfs/fs/microsd
 mkdir -p rootfs/eeprom
 touch rootfs/eeprom/parameters
+
+# Do not exit on failure now from here on because we want the complete cleanup
+set +e
+
 # Start Java simulator
 if [ "$debugger" == "lldb" ]
 then
-	lldb -- mainapp ../../../../${rc_script}_${program}
+	lldb -- mainapp ../../../../${rc_script}_${program}_${model}
 elif [ "$debugger" == "gdb" ]
 then
-	gdb --args mainapp ../../../../${rc_script}_${program}
+	gdb --args mainapp ../../../../${rc_script}_${program}_${model}
 elif [ "$debugger" == "ddd" ]
 then
-	ddd --debugger gdb --args mainapp ../../../../${rc_script}_${program}
+	ddd --debugger gdb --args mainapp ../../../../${rc_script}_${program}_${model}
 elif [ "$debugger" == "valgrind" ]
 then
-	valgrind ./mainapp ../../../../${rc_script}_${program}
+	valgrind ./mainapp ../../../../${rc_script}_${program}_${model}
 else
-	./mainapp ../../../../${rc_script}_${program}
+	nice -n -10 ./mainapp ../../../../${rc_script}_${program}_${model}
 fi

 if [ "$program" == "jmavsim" ]
@@ -0,0 +1,34 @@
+#!/bin/bash
+UAVCANFW=../ROMFS/px4fmu_common/uavcan/fw
+ROMFS_REFIX=_
+echo Removing : $UAVCANFW
+rm -fr $UAVCANFW
+for f in $(find firmware -type f -name "*.*.bin")
+do
+  UAVCAN_NAME=$(echo $f | cut -d"/" -f2 | cut -d. -f1-4 | cut -d- -f1-2)
+  UAVCAN_HW=$(echo $f cut -d/ -f2 | cut -d. -f1-4 | cut -d- -f3)
+  DST=${ROMFS_REFIX}$(echo $f | cut -d. -f3-7 | cut -d- -f1,2,3).$(echo $f | cut -d. -f6,7)
+  # deal with legacy non conforming naming
+  if [[ ${DST:(-7)} == bin.bin ]]
+  then
+    echo " WARNING: Improper name format!!!!!!!!! $f see should be <uavcan_name>-<HW_MAJOR>.<HW_MINOR)-<HW_MAJOR>.<HW_MINOR).<git hash[8]>.bin"
+    DST=${DST%????}
+  fi
+  echo Processing file: $f Length:${#DST}
+  if [ ${#DST} -le 28 ]
+  then
+    if [ -d "${UAVCANFW}/${UAVCAN_NAME}/${UAVCAN_HW}" ]
+    then
+      echo " ERROR: name colision directory ${UAVCANFW}/${UAVCAN_NAME}/${UAVCAN_HW} exits!"
+      exit 2
+    fi
+    echo " Creating Directory ${UAVCANFW}/${UAVCAN_NAME}/${UAVCAN_HW}"
+    mkdir -p ${UAVCANFW}/${UAVCAN_NAME}/${UAVCAN_HW}
+    echo " Copying $f to ${UAVCANFW}/${UAVCAN_NAME}/${UAVCAN_HW}/${DST}"
+    cp $f ${UAVCANFW}/${UAVCAN_NAME}/${UAVCAN_HW}/${DST}
+  else
+    echo " ERROR: $DST is ${#DST} charaters and needs to be less than or equal to 28"
+    exit 1
+  fi
+done
+exit 0
@@ -619,6 +619,7 @@ function(px4_add_common_flags)
 		${CMAKE_BINARY_DIR}/src/modules/px4_messages
 		${CMAKE_BINARY_DIR}/src/modules
 		${CMAKE_SOURCE_DIR}/mavlink/include/mavlink
+		${CMAKE_SOURCE_DIR}/src/lib/DriverFramework/framework/include
 		)

 	list(APPEND added_include_dirs
@@ -111,6 +111,8 @@ set(config_module_list
 	lib/geo_lookup
 	lib/conversion
 	lib/launchdetection
+	lib/terrain_estimation
+	lib/runway_takeoff
 	platforms/nuttx

 	# had to add for cmake, not sure why wasn't in original config
@@ -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
@@ -43,6 +45,7 @@ set(config_module_list
 	drivers/gimbal
 	drivers/pwm_input
 	drivers/camera_trigger
+    drivers/bst

 	#
 	# System commands
@@ -118,6 +121,8 @@ set(config_module_list
 	lib/geo_lookup
 	lib/conversion
 	lib/launchdetection
+	lib/terrain_estimation
+	lib/runway_takeoff
 	platforms/nuttx

 	# had to add for cmake, not sure why wasn't in original config
@@ -0,0 +1,185 @@
+include(nuttx/px4_impl_nuttx)
+
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
+
+set(config_uavcan_num_ifaces 1)
+
+set(config_module_list
+	#
+	# Board support modules
+	#
+	drivers/device
+	drivers/stm32
+	drivers/stm32/adc
+	drivers/stm32/tone_alarm
+	drivers/led
+	drivers/px4fmu
+	drivers/boards/px4fmu-v4
+	drivers/rgbled
+	drivers/mpu6000
+	drivers/mpu9250
+	drivers/hmc5883
+	drivers/ms5611
+	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/blinkm
+	drivers/airspeed
+	drivers/ets_airspeed
+	drivers/meas_airspeed
+	drivers/frsky_telemetry
+	modules/sensors
+	drivers/mkblctrl
+	drivers/px4flow
+	drivers/oreoled
+	drivers/gimbal
+	drivers/pwm_input
+	drivers/camera_trigger
+
+	#
+	# System commands
+	#
+	systemcmds/bl_update
+	systemcmds/mixer
+	systemcmds/param
+	systemcmds/perf
+	systemcmds/pwm
+	systemcmds/esc_calib
+	systemcmds/reboot
+	systemcmds/topic_listener
+	systemcmds/top
+	systemcmds/config
+	systemcmds/nshterm
+	systemcmds/mtd
+	systemcmds/dumpfile
+	systemcmds/ver
+	systemcmds/tests
+
+	#
+	# 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/ekf_att_pos_estimator
+	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
+	#
+	modules/sdlog2
+
+	#
+	# Library modules
+	#
+	modules/param
+	modules/systemlib
+	modules/systemlib/mixer
+	modules/controllib
+	modules/uORB
+	modules/dataman
+
+	#
+	# Libraries
+	#
+	#lib/mathlib/CMSIS
+	lib/mathlib
+	lib/mathlib/math/filter
+	lib/rc
+	lib/ecl
+	lib/external_lgpl
+	lib/geo
+	lib/geo_lookup
+	lib/conversion
+	lib/launchdetection
+	lib/terrain_estimation
+	lib/runway_takeoff
+	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
+
+	# 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
+	)
+
+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
+	)
+
+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")
@@ -38,6 +38,8 @@ set(config_module_list
 	lib/geo
 	lib/geo_lookup
 	lib/conversion
+	lib/terrain_estimation
+	lib/runway_takeoff

 	platforms/common
 	platforms/posix/px4_layer
@@ -3,11 +3,9 @@ include(posix/px4_impl_posix)
 set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake)

 set(config_module_list
-	drivers/led
 	drivers/device
 	drivers/boards/sitl
 	drivers/pwm_out_sim
-	drivers/rgbled
 	platforms/common
 	platforms/posix/px4_layer
 	platforms/posix/work_queue
@@ -18,12 +16,15 @@ set(config_module_list
 	platforms/posix/drivers/airspeedsim
 	platforms/posix/drivers/barosim
 	platforms/posix/drivers/gyrosim
+	platforms/posix/drivers/rgbledsim
+	platforms/posix/drivers/ledsim
 	systemcmds/param
 	systemcmds/mixer
 	systemcmds/ver
 	systemcmds/esc_calib
 	systemcmds/reboot
 	systemcmds/topic_listener
+	systemcmds/perf
 	modules/uORB
 	modules/param
 	modules/systemlib
@@ -56,6 +57,8 @@ set(config_module_list
 	lib/geo
 	lib/geo_lookup
 	lib/launchdetection
+	lib/terrain_estimation
+	lib/runway_takeoff
 	)

 set(config_extra_builtin_cmds
@@ -1,4 +1,4 @@
-include(cmake/configs/posix_sitl_simple.cmake)
+include(cmake/configs/posix_sitl_default.cmake)

 list(APPEND config_module_list
 	modules/local_position_estimator
@@ -47,6 +47,8 @@ set(config_module_list
 	lib/geo
 	lib/geo_lookup
 	lib/conversion
+	lib/terrain_estimation
+	lib/runway_takeoff
 	modules/controllib

 	#
@@ -70,6 +70,8 @@ set(config_module_list
 	lib/geo
 	lib/geo_lookup
 	lib/conversion
+	lib/terrain_estimation
+	lib/runway_takeoff

 	#
 	# QuRT port
@@ -52,6 +52,8 @@ set(config_module_list
 	lib/geo_lookup
 	lib/conversion
 	lib/ecl
+	lib/terrain_estimation
+	lib/runway_takeoff

 	#
 	# QuRT port
@@ -429,6 +429,7 @@ function(px4_os_add_flags)
 		)
 	set(added_definitions
 		-D__PX4_NUTTX
+		-D__DF_NUTTX
 		)
 	set(added_c_flags
 		-nodefaultlibs
@@ -458,6 +459,14 @@ function(px4_os_add_flags)
 			-mfpu=fpv4-sp-d16
 			-mfloat-abi=hard
 			)
+	elseif (${BOARD} STREQUAL "px4fmu-v4")
+		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
@@ -489,6 +498,8 @@ function(px4_os_add_flags)
 		#message(STATUS "nuttx: set(${${var}} ${${${var}}} ${added_${lower_var}} PARENT_SCOPE)")
 	endforeach()

+	set(DF_TARGET "nuttx" PARENT_SCOPE)
+
 endfunction()

 #=============================================================================
@@ -170,25 +170,32 @@ if(UNIX AND APPLE)
        set(added_definitions
 		-D__PX4_POSIX
 		-D__PX4_DARWIN
+		-D__DF_DARWIN
 		-DCLOCK_MONOTONIC=1
 		-Dnoreturn_function=__attribute__\(\(noreturn\)\)
 		-include ${PX4_INCLUDE_DIR}visibility.h
                )

+        set(added_exe_linker_flags
+		-lpthread
+		)
+
 else()

        set(added_definitions
 		-D__PX4_POSIX
 		-D__PX4_LINUX 
+		-D__DF_LINUX 
 		-DCLOCK_MONOTONIC=1
 		-Dnoreturn_function=__attribute__\(\(noreturn\)\)
 		-include ${PX4_INCLUDE_DIR}visibility.h
                )
-endif()

        set(added_exe_linker_flags
-		-lpthread
+		-lpthread -lrt
 		)
+
+endif()
 	

 	# Add the toolchain specific flags
@@ -172,6 +172,7 @@ function(px4_os_add_flags)
        set(added_definitions
                -D__PX4_QURT 
 		-D__PX4_POSIX
+		-D__DF_QURT
 		-include ${PX4_INCLUDE_DIR}visibility.h
                )

@@ -185,6 +186,8 @@ function(px4_os_add_flags)
 	set(CMAKE_SHARED_LIBRARY_LINK_C_FLAGS "")
 	set(CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "")

+	set(DF_TARGET "qurt" PARENT_SCOPE)
+
 	# output
 	foreach(var ${inout_vars})
 		string(TOLOWER ${var} lower_var)
@@ -5,6 +5,9 @@ uint8 INDEX_PITCH = 1
 uint8 INDEX_YAW = 2
 uint8 INDEX_THROTTLE = 3
 uint8 INDEX_FLAPS = 4
+uint8 INDEX_SPOILERS = 5
+uint8 INDEX_AIRBRAKES = 6
+uint8 INDEX_LANDING_GEAR = 7
 uint8 GROUP_INDEX_ATTITUDE = 0
 uint64 timestamp
 uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
@@ -10,6 +10,7 @@ float32 x_pos			# X position in local frame
 float32 y_pos			# Y position in local frame
 float32 z_pos			# z position in local frame
 float32 airspeed		# Airspeed, estimated
+bool airspeed_valid		# False: Non-finite values or non-updating sensor
 float32[3] vel_variance	# Variance in body velocity estimate
 float32[3] pos_variance	# Variance in local position estimate
 float32[4] q 			# Attitude Quaternion
@@ -0,0 +1,23 @@
+
+uint64 timestamp				# in microseconds since system start, is set whenever the writing thread stores new data
+
+float32 roll_body				# body angle in NED frame
+float32 pitch_body				# body angle in NED frame
+float32 yaw_body				# body angle in NED frame
+
+float32 yaw_sp_move_rate		# rad/s (commanded by user)
+
+float32[9] R_body				# Rotation matrix describing the setpoint as rotation from the current body frame
+bool R_valid					# Set to true if rotation matrix is valid
+
+# For quaternion-based attitude control
+float32[4] q_d					# Desired quaternion for quaternion control
+bool q_d_valid					# Set to true if quaternion vector is valid
+float32[4] q_e					# Attitude error in quaternion
+bool q_e_valid					# Set to true if quaternion error vector is valid
+
+float32 thrust					# Thrust in Newton the power system should generate
+
+bool roll_reset_integral			# Reset roll integral part (navigation logic change)
+bool pitch_reset_integral			# Reset pitch integral part (navigation logic change)
+bool yaw_reset_integral				# Reset yaw integral part (navigation logic change)
@@ -0,0 +1,23 @@
+
+uint64 timestamp				# in microseconds since system start, is set whenever the writing thread stores new data
+
+float32 roll_body				# body angle in NED frame
+float32 pitch_body				# body angle in NED frame
+float32 yaw_body				# body angle in NED frame
+
+float32 yaw_sp_move_rate		# rad/s (commanded by user)
+
+float32[9] R_body				# Rotation matrix describing the setpoint as rotation from the current body frame
+bool R_valid					# Set to true if rotation matrix is valid
+
+# For quaternion-based attitude control
+float32[4] q_d					# Desired quaternion for quaternion control
+bool q_d_valid					# Set to true if quaternion vector is valid
+float32[4] q_e					# Attitude error in quaternion
+bool q_e_valid					# Set to true if quaternion error vector is valid
+
+float32 thrust					# Thrust in Newton the power system should generate
+
+bool roll_reset_integral			# Reset roll integral part (navigation logic change)
+bool pitch_reset_integral			# Reset pitch integral part (navigation logic change)
+bool yaw_reset_integral				# Reset yaw integral part (navigation logic change)
@@ -3,3 +3,4 @@ float32 turn_distance		# the optimal distance to a waypoint to switch to the nex
 float32 landing_horizontal_slope_displacement
 float32 landing_slope_angle_rad
 float32 landing_flare_length
+bool abort_landing
@@ -26,3 +26,4 @@ uint8 channel_count						# Number of valid channels
 int8[20] function						# Functions mapping
 uint8 rssi							# Receive signal strength index
 bool signal_lost						# Control signal lost, should be checked together with topic timeout
+uint32 frame_drop_count						# Number of dropped frames
@@ -21,3 +21,7 @@ float32 thrust					# Thrust in Newton the power system should generate
 bool roll_reset_integral			# Reset roll integral part (navigation logic change)
 bool pitch_reset_integral			# Reset pitch integral part (navigation logic change)
 bool yaw_reset_integral				# Reset yaw integral part (navigation logic change)
+
+bool fw_control_yaw					# control heading with rudder (used for auto takeoff on runway)
+
+bool apply_flaps
@@ -18,3 +18,4 @@ float32 epv			# Standard deviation of position vertically
 float32 terrain_alt		# Terrain altitude in m, WGS84
 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
@@ -3,6 +3,7 @@ uint64 timestamp_position	# Timestamp for position information
 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

 uint64 timestamp_variance
 float32 s_variance_m_s		# speed accuracy estimate m/s 
@@ -9,7 +9,8 @@ uint8 MAIN_STATE_ACRO = 6
 uint8 MAIN_STATE_OFFBOARD = 7
 uint8 MAIN_STATE_STAB = 8
 uint8 MAIN_STATE_RATTITUDE = 9
-uint8 MAIN_STATE_MAX = 10
+uint8 MAIN_STATE_AUTO_TAKEOFF = 10
+uint8 MAIN_STATE_MAX = 11

 # If you change the order, add or remove arming_state_t states make sure to update the arrays
 # in state_machine_helper.cpp as well.
@@ -43,7 +44,8 @@ uint8 NAVIGATION_STATE_TERMINATION = 13		# Termination mode
 uint8 NAVIGATION_STATE_OFFBOARD = 14
 uint8 NAVIGATION_STATE_STAB = 15		# Stabilized mode
 uint8 NAVIGATION_STATE_RATTITUDE = 16		# Rattitude (aka "flip") mode
-uint8 NAVIGATION_STATE_MAX = 17
+uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17	# Takeoff
+uint8 NAVIGATION_STATE_MAX = 18

 # VEHICLE_MODE_FLAG, same as MAV_MODE_FLAG of MAVLink 1.0 protocol
 uint8 VEHICLE_MODE_FLAG_SAFETY_ARMED = 128
@@ -113,6 +115,7 @@ uint32 system_id			# system id, inspired by MAVLink's system ID field
 uint32 component_id			# subsystem / component id, inspired by MAVLink's component ID field

 bool is_rotary_wing			# True if system is in rotary wing configuration, so for a VTOL this is only true while flying as a multicopter
+bool vtol_in_transition		# True if VTOL is doing a transition
 bool is_vtol				# True if the system is VTOL capable
 bool vtol_fw_permanent_stab		# True if vtol should stabilize attitude for fw in manual mode
 bool in_transition_mode
@@ -141,7 +144,6 @@ bool rc_signal_lost_cmd				# true if RC lost mode is commanded
 bool rc_input_blocked				# set if RC input should be ignored temporarily
 uint8 rc_input_mode				# set to 1 to disable the RC input, 2 to enable manual control to RC in mapping.

-bool data_link_found_new			# new datalink to GCS found
 bool data_link_lost				# datalink to GCS lost
 bool data_link_lost_cmd				# datalink to GCS lost mode commanded
 uint8 data_link_lost_counter			# counts unique data link lost events
@@ -0,0 +1,299 @@
+/************************************************************************************
+ * configs/px4fmu/include/board.h
+ * include/arch/board/board.h
+ *
+ *   Copyright (C) 2009 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ************************************************************************************/
+
+#ifndef __ARCH_BOARD_BOARD_H
+#define __ARCH_BOARD_BOARD_H
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+#ifndef __ASSEMBLY__
+# include <stdint.h>
+#endif
+
+#include <stm32.h>
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/* Clocking *************************************************************************/
+/* The PX4FMUV2 uses a 24MHz crystal connected to the HSE.
+ *
+ * This is the "standard" configuration as set up by arch/arm/src/stm32f40xx_rcc.c:
+ *   System Clock source           : PLL (HSE)
+ *   SYSCLK(Hz)                    : 168000000    Determined by PLL configuration
+ *   HCLK(Hz)                      : 168000000    (STM32_RCC_CFGR_HPRE)
+ *   AHB Prescaler                 : 1            (STM32_RCC_CFGR_HPRE)
+ *   APB1 Prescaler                : 4            (STM32_RCC_CFGR_PPRE1)
+ *   APB2 Prescaler                : 2            (STM32_RCC_CFGR_PPRE2)
+ *   HSE Frequency(Hz)             : 24000000     (STM32_BOARD_XTAL)
+ *   PLLM                          : 24           (STM32_PLLCFG_PLLM)
+ *   PLLN                          : 336          (STM32_PLLCFG_PLLN)
+ *   PLLP                          : 2            (STM32_PLLCFG_PLLP)
+ *   PLLQ                          : 7            (STM32_PLLCFG_PPQ)
+ *   Main regulator output voltage : Scale1 mode  Needed for high speed SYSCLK
+ *   Flash Latency(WS)             : 5
+ *   Prefetch Buffer               : OFF
+ *   Instruction cache             : ON
+ *   Data cache                    : ON
+ *   Require 48MHz for USB OTG FS, : Enabled
+ *   SDIO and RNG clock
+ */
+
+/* HSI - 16 MHz RC factory-trimmed
+ * LSI - 32 KHz RC
+ * HSE - On-board crystal frequency is 24MHz
+ * LSE - not installed
+ */
+
+#define STM32_BOARD_XTAL        24000000ul
+
+#define STM32_HSI_FREQUENCY     16000000ul
+#define STM32_LSI_FREQUENCY     32000
+#define STM32_HSE_FREQUENCY     STM32_BOARD_XTAL
+//#define STM32_LSE_FREQUENCY     32768
+
+/* Main PLL Configuration.
+ *
+ * PLL source is HSE
+ * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN
+ *         = (25,000,000 / 25) * 336
+ *         = 336,000,000
+ * SYSCLK  = PLL_VCO / PLLP
+ *         = 336,000,000 / 2 = 168,000,000
+ * USB OTG FS, SDIO and RNG Clock
+ *         =  PLL_VCO / PLLQ
+ *         = 48,000,000
+ */
+
+#define STM32_PLLCFG_PLLM       RCC_PLLCFG_PLLM(24)
+#define STM32_PLLCFG_PLLN       RCC_PLLCFG_PLLN(336)
+#define STM32_PLLCFG_PLLP       RCC_PLLCFG_PLLP_2
+#define STM32_PLLCFG_PLLQ       RCC_PLLCFG_PLLQ(7)
+
+#define STM32_SYSCLK_FREQUENCY  168000000ul
+
+/* AHB clock (HCLK) is SYSCLK (168MHz) */
+
+#define STM32_RCC_CFGR_HPRE     RCC_CFGR_HPRE_SYSCLK  /* HCLK  = SYSCLK / 1 */
+#define STM32_HCLK_FREQUENCY    STM32_SYSCLK_FREQUENCY
+#define STM32_BOARD_HCLK        STM32_HCLK_FREQUENCY  /* same as above, to satisfy compiler */
+
+/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */
+
+#define STM32_RCC_CFGR_PPRE1    RCC_CFGR_PPRE1_HCLKd4     /* PCLK1 = HCLK / 4 */
+#define STM32_PCLK1_FREQUENCY   (STM32_HCLK_FREQUENCY/4)
+
+/* Timers driven from APB1 will be twice PCLK1 */
+
+#define STM32_APB1_TIM2_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM3_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM4_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM5_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM6_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM7_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM12_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM13_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM14_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+
+/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */
+
+#define STM32_RCC_CFGR_PPRE2    RCC_CFGR_PPRE2_HCLKd2     /* PCLK2 = HCLK / 2 */
+#define STM32_PCLK2_FREQUENCY   (STM32_HCLK_FREQUENCY/2)
+
+/* Timers driven from APB2 will be twice PCLK2 */
+
+#define STM32_APB2_TIM1_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM8_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM9_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM10_CLKIN  (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM11_CLKIN  (2*STM32_PCLK2_FREQUENCY)
+
+/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx
+ * otherwise frequency is 2xAPBx.
+ * Note: TIM1,8 are on APB2, others on APB1
+ */
+
+#define STM32_TIM18_FREQUENCY   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_TIM27_FREQUENCY   (2*STM32_PCLK1_FREQUENCY)
+
+/* SDIO dividers.  Note that slower clocking is required when DMA is disabled
+ * in order to avoid RX overrun/TX underrun errors due to delayed responses
+ * to service FIFOs in interrupt driven mode.  These values have not been
+ * tuned!!!
+ *
+ * HCLK=72MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(178+2)=400 KHz
+ */
+
+#define SDIO_INIT_CLKDIV        (178 << SDIO_CLKCR_CLKDIV_SHIFT)
+
+/* DMA ON:  HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(2+2)=18 MHz
+ * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz
+ */
+
+#ifdef CONFIG_SDIO_DMA
+#  define SDIO_MMCXFR_CLKDIV    (2 << SDIO_CLKCR_CLKDIV_SHIFT)
+#else
+#  define SDIO_MMCXFR_CLKDIV    (3 << SDIO_CLKCR_CLKDIV_SHIFT)
+#endif
+
+/* DMA ON:  HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(1+2)=24 MHz
+ * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz
+ */
+
+#ifdef CONFIG_SDIO_DMA
+#  define SDIO_SDXFR_CLKDIV     (1 << SDIO_CLKCR_CLKDIV_SHIFT)
+#else
+#  define SDIO_SDXFR_CLKDIV     (3 << SDIO_CLKCR_CLKDIV_SHIFT)
+#endif
+
+/* DMA Channl/Stream Selections *****************************************************/
+/* Stream selections are arbitrary for now but might become important in the future
+ * is we set aside more DMA channels/streams.
+ *
+ * SDIO DMA
+ *   DMAMAP_SDIO_1 = Channel 4, Stream 3 <- may later be used by SPI DMA
+ *   DMAMAP_SDIO_2 = Channel 4, Stream 6
+ */
+
+#define DMAMAP_SDIO DMAMAP_SDIO_1
+
+/* Alternate function pin selections ************************************************/
+
+/*
+ * UARTs.
+ */
+#define GPIO_USART1_RX	GPIO_USART1_RX_2	/* ESP8266 */
+#define GPIO_USART1_TX	GPIO_USART1_TX_2
+
+#define GPIO_USART2_RX	GPIO_USART2_RX_2
+#define GPIO_USART2_TX	GPIO_USART2_TX_2
+#define GPIO_USART2_RTS	GPIO_USART2_RTS_2
+#define GPIO_USART2_CTS	GPIO_USART2_CTS_2
+
+#define GPIO_USART3_RX	GPIO_USART3_RX_3
+#define GPIO_USART3_TX	GPIO_USART3_TX_3
+#define GPIO_USART3_RTS	GPIO_USART3_RTS_2
+#define GPIO_USART3_CTS	GPIO_USART3_CTS_2
+
+#define GPIO_UART4_RX	GPIO_UART4_RX_1
+#define GPIO_UART4_TX	GPIO_UART4_TX_1
+
+#define GPIO_USART6_RX	GPIO_USART6_RX_1
+#define GPIO_USART6_TX	GPIO_USART6_TX_1
+
+#define GPIO_UART7_RX	GPIO_UART7_RX_1
+#define GPIO_UART7_TX	GPIO_UART7_TX_1
+
+/* UART8 has no alternate pin config */
+
+/* UART RX DMA configurations */
+#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2
+#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2
+
+/*
+ * CAN
+ *
+ * CAN1 is routed to the onboard transceiver.
+ */
+#define GPIO_CAN1_RX	GPIO_CAN1_RX_3
+#define GPIO_CAN1_TX	GPIO_CAN1_TX_3
+
+/*
+ * I2C
+ *
+ * The optional _GPIO configurations allow the I2C driver to manually
+ * reset the bus to clear stuck slaves.  They match the pin configuration,
+ * but are normally-high GPIOs.
+ */
+#define GPIO_I2C1_SCL		GPIO_I2C1_SCL_2
+#define GPIO_I2C1_SDA		GPIO_I2C1_SDA_2
+#define GPIO_I2C1_SCL_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8)
+#define GPIO_I2C1_SDA_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9)
+
+
+/*
+ * SPI
+ *
+ * There are sensors on SPI1, and SPI2 is connected to the FRAM.
+ */
+#define GPIO_SPI1_MISO	(GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI1_MOSI	(GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI1_SCK	(GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz)
+
+#define GPIO_SPI2_MISO	(GPIO_SPI2_MISO_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI2_MOSI	(GPIO_SPI2_MOSI_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI2_SCK	(GPIO_SPI2_SCK_2|GPIO_SPEED_50MHz)
+
+/************************************************************************************
+ * Public Data
+ ************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+#undef EXTERN
+#if defined(__cplusplus)
+#define EXTERN extern "C"
+extern "C" {
+#else
+#define EXTERN extern
+#endif
+
+/************************************************************************************
+ * Public Function Prototypes
+ ************************************************************************************/
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+EXTERN void stm32_boardinitialize(void);
+
+#undef EXTERN
+#if defined(__cplusplus)
+}
+#endif
+
+#endif /* __ASSEMBLY__ */
+#endif  /* __ARCH_BOARD_BOARD_H */
@@ -0,0 +1,42 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * nsh_romfsetc.h
+ *
+ * This file is a stub for 'make export' purposes; the actual ROMFS
+ * must be supplied by the library client.
+ */
+
+extern unsigned char romfs_img[];
+extern unsigned int romfs_img_len;
@@ -0,0 +1,177 @@
+############################################################################
+# configs/px4fmu-v2/nsh/Make.defs
+#
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name NuttX nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+include ${TOPDIR}/.config
+include ${TOPDIR}/tools/Config.mk
+
+#
+# We only support building with the ARM bare-metal toolchain from
+# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS.
+#
+CONFIG_ARMV7M_TOOLCHAIN	:= GNU_EABI
+
+include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs
+
+CC			 = $(CROSSDEV)gcc
+CXX			 = $(CROSSDEV)g++
+CPP			 = $(CROSSDEV)gcc -E
+LD			 = $(CROSSDEV)ld
+AR			 = $(CROSSDEV)ar rcs
+NM			 = $(CROSSDEV)nm
+OBJCOPY			 = $(CROSSDEV)objcopy
+OBJDUMP			 = $(CROSSDEV)objdump
+
+MAXOPTIMIZATION		 = -Os
+ARCHCPUFLAGS		 = -mcpu=cortex-m4 \
+			   -mthumb \
+			   -march=armv7e-m \
+			   -mfpu=fpv4-sp-d16 \
+			   -mfloat-abi=hard
+
+
+# enable precise stack overflow tracking
+ifeq ($(CONFIG_ARMV7M_STACKCHECK),y)
+INSTRUMENTATIONDEFINES	 = -finstrument-functions -ffixed-r10
+endif
+
+# pull in *just* libm from the toolchain ... this is grody
+LIBM			 = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}"
+EXTRA_LIBS		+= $(LIBM)
+
+# use our linker script
+LDSCRIPT		 = ld.script
+
+ifeq ($(WINTOOL),y)
+  # Windows-native toolchains
+  DIRLINK		 = $(TOPDIR)/tools/copydir.sh
+  DIRUNLINK		 = $(TOPDIR)/tools/unlink.sh
+  MKDEP			 = $(TOPDIR)/tools/mknulldeps.sh
+  ARCHINCLUDES		 = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}"
+  ARCHXXINCLUDES	 = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}"
+  ARCHSCRIPT		 = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}"
+else
+  ifeq ($(PX4_WINTOOL),y)
+    # Windows-native toolchains (MSYS)
+    DIRLINK		 = $(TOPDIR)/tools/copydir.sh
+    DIRUNLINK		 = $(TOPDIR)/tools/unlink.sh
+    MKDEP		 = $(TOPDIR)/tools/mknulldeps.sh
+    ARCHINCLUDES	 = -I. -isystem $(TOPDIR)/include
+    ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+    ARCHSCRIPT		 = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)
+  else
+    # Linux/Cygwin-native toolchain
+    MKDEP		 = $(TOPDIR)/tools/mkdeps.sh
+    ARCHINCLUDES	 = -I. -isystem $(TOPDIR)/include
+    ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+    ARCHSCRIPT		 = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)
+  endif
+endif
+
+# tool versions
+ARCHCCVERSION		 = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'}
+ARCHCCMAJOR		 = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1}
+
+# optimisation flags
+ARCHOPTIMIZATION	 = $(MAXOPTIMIZATION) \
+			   -fno-strict-aliasing \
+			   -fno-strength-reduce \
+			   -fomit-frame-pointer \
+			   -funsafe-math-optimizations \
+			   -fno-builtin-printf \
+			   -ffunction-sections \
+			   -fdata-sections
+
+ifeq ("${CONFIG_DEBUG_SYMBOLS}","y")
+ARCHOPTIMIZATION	+= -g
+endif
+
+ARCHCFLAGS		 = -std=gnu99
+ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x
+ARCHWARNINGS		 = -Wall \
+			   -Wno-sign-compare \
+			   -Wextra \
+			   -Wdouble-promotion \
+			   -Wshadow \
+			   -Wframe-larger-than=1024 \
+			   -Wpointer-arith \
+			   -Wlogical-op \
+			   -Wpacked \
+			   -Wno-unused-parameter
+#   -Wcast-qual  - generates spurious noreturn attribute warnings, try again later
+#   -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code
+#   -Wcast-align - would help catch bad casts in some cases, but generates too many false positives
+
+ARCHCWARNINGS		 = $(ARCHWARNINGS) \
+			   -Wbad-function-cast \
+			   -Wstrict-prototypes \
+			   -Wold-style-declaration \
+			   -Wmissing-parameter-type \
+			   -Wnested-externs
+ARCHWARNINGSXX		 = $(ARCHWARNINGS) \
+			   -Wno-psabi
+ARCHDEFINES		 =
+ARCHPICFLAGS		 = -fpic -msingle-pic-base -mpic-register=r10
+
+# this seems to be the only way to add linker flags
+EXTRA_LIBS		+= --warn-common \
+			   --gc-sections
+
+CFLAGS			 = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common
+CPICFLAGS		 = $(ARCHPICFLAGS) $(CFLAGS)
+CXXFLAGS		 = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe
+CXXPICFLAGS		 = $(ARCHPICFLAGS) $(CXXFLAGS)
+CPPFLAGS		 = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES)
+AFLAGS			 = $(CFLAGS) -D__ASSEMBLY__
+
+NXFLATLDFLAGS1		 = -r -d -warn-common
+NXFLATLDFLAGS2		 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections
+LDNXFLATFLAGS		 = -e main -s 2048
+
+OBJEXT			 = .o
+LIBEXT			 = .a
+EXEEXT			 =
+
+
+# produce partially-linked $1 from files in $2
+define PRELINK
+	@echo "PRELINK: $1"
+	$(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1
+endef
+
+HOSTCC			 = gcc
+HOSTINCLUDES		 = -I.
+HOSTCFLAGS		 = -Wall -Wstrict-prototypes -Wshadow -g -pipe
+HOSTLDFLAGS		 =
+
@@ -1,6 +1,8 @@
 ############################################################################
+# configs/px4fmu/nsh/appconfig
 #
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -12,7 +14,7 @@
 #    notice, this list of conditions and the following disclaimer in
 #    the documentation and/or other materials provided with the
 #    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
+# 3. Neither the name NuttX nor the names of its contributors may be
 #    used to endorse or promote products derived from this software
 #    without specific prior written permission.
 #
@@ -31,16 +33,20 @@
 #
 ############################################################################

-#
-# Makefile to build the MPU6000 driver.
-#
+# Path to example in apps/examples containing the user_start entry point

-MODULE_COMMAND	= gyrosim
+CONFIGURED_APPS	+= examples/nsh

-SRCS		= gyrosim.cpp
+# The NSH application library
+CONFIGURED_APPS += nshlib
+CONFIGURED_APPS += system/readline

-MODULE_STACKSIZE	= 1200
+ifeq ($(CONFIG_CAN),y)
+#CONFIGURED_APPS += examples/can
+endif

-EXTRACXXFLAGS	= -Weffc++
-
-MAXOPTIMIZATION	 = -Os
+#ifeq ($(CONFIG_USBDEV),y)
+#ifeq ($(CONFIG_CDCACM),y)
+CONFIGURED_APPS += examples/cdcacm
+#endif
+#endif
@@ -0,0 +1,67 @@
+#!/bin/bash
+# configs/stm3240g-eval/nsh/setenv.sh
+#
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name NuttX nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+
+if [ "$_" = "$0" ] ; then
+  echo "You must source this script, not run it!" 1>&2
+  exit 1
+fi
+
+WD=`pwd`
+if [ ! -x "setenv.sh" ]; then
+  echo "This script must be executed from the top-level NuttX build directory"
+  exit 1
+fi
+
+if [ -z "${PATH_ORIG}" ]; then
+  export PATH_ORIG="${PATH}"
+fi
+
+# This the Cygwin path to the location where I installed the RIDE
+# toolchain under windows.  You will also have to edit this if you install
+# the RIDE toolchain in any other location
+#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin"
+
+# This the Cygwin path to the location where I installed the CodeSourcery
+# toolchain under windows.  You will also have to edit this if you install
+# the CodeSourcery toolchain in any other location
+export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin"
+
+# This the Cygwin path to the location where I build the buildroot
+# toolchain.
+#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin"
+
+# Add the path to the toolchain to the PATH varialble
+export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}"
+
+echo "PATH : ${PATH}"
@@ -0,0 +1,158 @@
+/****************************************************************************
+ * configs/px4fmu/common/ld.script
+ *
+ *   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* The STM32F427 has 2048Kb of FLASH beginning at address 0x0800:0000 and
+ * 256Kb of SRAM. SRAM is split up into three blocks:
+ *
+ * 1) 112Kb of SRAM beginning at address 0x2000:0000
+ * 2)  16Kb of SRAM beginning at address 0x2001:c000
+ * 3)  64Kb of SRAM beginning at address 0x2002:0000
+ * 4)  64Kb of TCM SRAM beginning at address 0x1000:0000
+ *
+ * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000
+ * where the code expects to begin execution by jumping to the entry point in
+ * the 0x0800:0000 address range.
+ *
+ * The first 0x4000 of flash is reserved for the bootloader.
+ */
+
+MEMORY
+{
+    flash (rx)   : ORIGIN = 0x08004000, LENGTH = 2032K
+    sram (rwx)   : ORIGIN = 0x20000000, LENGTH = 192K
+    ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
+}
+
+OUTPUT_ARCH(arm)
+
+ENTRY(__start)		/* treat __start as the anchor for dead code stripping */
+EXTERN(_vectors)	/* force the vectors to be included in the output */
+
+/* 
+ * Ensure that abort() is present in the final object.  The exception handling
+ * code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
+ */
+EXTERN(abort)
+EXTERN(_bootdelay_signature)
+
+SECTIONS
+{
+	.text : {
+		_stext = ABSOLUTE(.);
+		*(.vectors)
+                . = ALIGN(32);
+                /*
+                This signature provides the bootloader with a way to delay booting
+                */
+                _bootdelay_signature = ABSOLUTE(.);
+                FILL(0xffecc2925d7d05c5)
+                . += 8;
+		*(.text .text.*)
+		*(.fixup)
+		*(.gnu.warning)
+		*(.rodata .rodata.*)
+		*(.gnu.linkonce.t.*)
+		*(.got)
+		*(.gcc_except_table)
+		*(.gnu.linkonce.r.*)
+		_etext = ABSOLUTE(.);
+
+		/*
+		 * This is a hack to make the newlib libm __errno() call
+		 * use the NuttX get_errno_ptr() function.
+		 */
+		__errno = get_errno_ptr;
+	} > flash
+
+	/*
+	 * Init functions (static constructors and the like)
+	 */
+        .init_section : {
+                _sinit = ABSOLUTE(.);
+                KEEP(*(.init_array .init_array.*))
+                _einit = ABSOLUTE(.);
+        } > flash
+
+	/*
+	 * Construction data for parameters.
+	 */
+	__param ALIGN(4): {
+		__param_start = ABSOLUTE(.);
+		KEEP(*(__param*))
+		__param_end = ABSOLUTE(.);
+	} > flash
+
+	.ARM.extab : {
+		*(.ARM.extab*)
+	} > flash
+
+	__exidx_start = ABSOLUTE(.);
+	.ARM.exidx : {
+		*(.ARM.exidx*)
+	} > flash
+	__exidx_end = ABSOLUTE(.);
+
+	_eronly = ABSOLUTE(.);
+
+	.data : {
+		_sdata = ABSOLUTE(.);
+		*(.data .data.*)
+		*(.gnu.linkonce.d.*)
+		CONSTRUCTORS
+		_edata = ABSOLUTE(.);
+	} > sram AT > flash
+
+	.bss : {
+		_sbss = ABSOLUTE(.);
+		*(.bss .bss.*)
+		*(.gnu.linkonce.b.*)
+		*(COMMON)
+		_ebss = ABSOLUTE(.);
+	} > sram
+
+	/* Stabs debugging sections. */
+	.stab 0 : { *(.stab) }
+	.stabstr 0 : { *(.stabstr) }
+	.stab.excl 0 : { *(.stab.excl) }
+	.stab.exclstr 0 : { *(.stab.exclstr) }
+	.stab.index 0 : { *(.stab.index) }
+	.stab.indexstr 0 : { *(.stab.indexstr) }
+	.comment 0 : { *(.comment) }
+	.debug_abbrev 0 : { *(.debug_abbrev) }
+	.debug_info 0 : { *(.debug_info) }
+	.debug_line 0 : { *(.debug_line) }
+	.debug_pubnames 0 : { *(.debug_pubnames) }
+	.debug_aranges 0 : { *(.debug_aranges) }
+}
@@ -0,0 +1,84 @@
+############################################################################
+# configs/px4fmu/src/Makefile
+#
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name NuttX nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+-include $(TOPDIR)/Make.defs
+
+CFLAGS		+= -I$(TOPDIR)/sched
+
+ASRCS		=
+AOBJS		= $(ASRCS:.S=$(OBJEXT))
+
+CSRCS		= empty.c
+COBJS		= $(CSRCS:.c=$(OBJEXT))
+
+SRCS		= $(ASRCS) $(CSRCS)
+OBJS		= $(AOBJS) $(COBJS)
+
+ARCH_SRCDIR	= $(TOPDIR)/arch/$(CONFIG_ARCH)/src
+ifeq ($(WINTOOL),y)
+  CFLAGS	+= -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \
+					   -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \
+					   -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}"
+else
+  CFLAGS	+= -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m
+endif
+
+all: libboard$(LIBEXT)
+
+$(AOBJS): %$(OBJEXT): %.S
+	$(call ASSEMBLE, $<, $@)
+
+$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c
+	$(call COMPILE, $<, $@)
+
+libboard$(LIBEXT): $(OBJS)
+	$(call ARCHIVE, $@, $(OBJS))
+
+.depend: Makefile $(SRCS)
+	$(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
+	$(Q) touch $@
+
+depend: .depend
+
+clean:
+	$(call DELFILE, libboard$(LIBEXT))
+	$(call CLEAN)
+
+distclean: clean
+	$(call DELFILE, Make.dep)
+	$(call DELFILE, .depend)
+
+-include Make.dep
+
@@ -0,0 +1,4 @@
+/*
+ * There are no source files here, but libboard.a can't be empty, so
+ * we have this empty source file to keep it company.
+ */
@@ -5,10 +5,10 @@ param set MAV_TYPE 1
 param set SYS_AUTOSTART 3033
 param set SYS_RESTART_TYPE 2
 dataman start
-param set CAL_GYRO0_ID 2293760
-param set CAL_ACC0_ID 1376256
-param set CAL_ACC1_ID 1310720
-param set CAL_MAG0_ID 196608
+param set CAL_GYRO0_ID 2293768
+param set CAL_ACC0_ID 1376264
+param set CAL_ACC1_ID 1310728
+param set CAL_MAG0_ID 196616
 param set CAL_GYRO0_XOFF 0.01
 param set CAL_ACC0_XOFF 0.01
 param set CAL_ACC0_YOFF -0.01
@@ -21,7 +21,7 @@ param set CAL_MAG0_XOFF 0.01
 param set MPC_XY_P 0.4
 param set MPC_XY_VEL_P 0.2
 param set MPC_XY_VEL_D 0.005
-param set COM_RC_IN_MODE 2
+param set COM_RC_IN_MODE 1
 rgbled start
 tone_alarm start
 gyrosim start
@@ -9,10 +9,10 @@ param set MC_YAWRATE_P 0.35
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
 dataman start
-param set CAL_GYRO0_ID 2293760
-param set CAL_ACC0_ID 1376256
-param set CAL_ACC1_ID 1310720
-param set CAL_MAG0_ID 196608
+param set CAL_GYRO0_ID 2293768
+param set CAL_ACC0_ID 1376264
+param set CAL_ACC1_ID 1310728
+param set CAL_MAG0_ID 196616
 param set CAL_GYRO0_XOFF 0.01
 param set CAL_ACC0_XOFF 0.01
 param set CAL_ACC0_YOFF -0.01
@@ -27,8 +27,11 @@ param set MPC_XY_VEL_P 0.05
 param set MPC_XY_VEL_D 0.005
 param set MPC_XY_FF 0.1
 param set SENS_BOARD_ROT 8
-param set COM_RC_IN_MODE 2
-rgbled start
+param set COM_RC_IN_MODE 1
+param set NAV_ACC_RAD 2.0
+param set RTL_RETURN_ALT 30.0
+param set RTL_DESCEND_ALT 10.0
+rgbledsim start
 tone_alarm start
 gyrosim start
 accelsim start
@@ -0,0 +1,64 @@
+uorb start
+simulator start -s
+param load
+param set MAV_TYPE 20
+param set MC_PITCHRATE_P 0.3
+param set MC_ROLLRATE_P 0.3
+param set MC_YAW_P 2.0
+param set MC_YAWRATE_P 0.35
+param set VT_TYPE 0
+param set SYS_AUTOSTART 4010
+param set SYS_RESTART_TYPE 2
+dataman start
+param set CAL_GYRO0_ID 2293768
+param set CAL_ACC0_ID 1376264
+param set CAL_ACC1_ID 1310728
+param set CAL_MAG0_ID 196616
+param set CAL_GYRO0_XOFF 0.01
+param set CAL_ACC0_XOFF 0.01
+param set CAL_ACC0_YOFF -0.01
+param set CAL_ACC0_ZOFF 0.01
+param set CAL_ACC0_XSCALE 1.01
+param set CAL_ACC0_YSCALE 1.01
+param set CAL_ACC0_ZSCALE 1.01
+param set CAL_ACC1_XOFF 0.01
+param set CAL_MAG0_XOFF 0.01
+param set MPC_XY_P 0.15
+param set MPC_XY_VEL_P 0.05
+param set MPC_XY_VEL_D 0.005
+param set MPC_XY_FF 0.1
+param set SENS_BOARD_ROT 8
+param set COM_RC_IN_MODE 1
+rgbledsim start
+tone_alarm start
+gyrosim start
+accelsim start
+barosim start
+adcsim start
+gpssim start
+measairspeedsim start
+pwm_out_sim mode_pwm
+sleep 1
+sensors start
+commander start
+land_detector start multicopter
+navigator start
+attitude_estimator_q start
+position_estimator_inav start
+vtol_att_control start
+mc_pos_control start
+mc_att_control start
+fw_pos_control_l1 start
+fw_att_control start
+mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix
+mavlink start -u 14556 -r 2000000
+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
+mavlink stream -r 80 -s ATTITUDE -u 14556
+mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
+mavlink stream -r 20 -s RC_CHANNELS -u 14556
+mavlink stream -r 250 -s HIGHRES_IMU -u 14556
+mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
+mavlink boot_complete
+sdlog2 start -r 100 -e -t -a
@@ -3,16 +3,18 @@ simulator start -s
 param load
 param set MAV_TYPE 2
 param set MC_PITCHRATE_P 0.15
+param set MC_PITCH_P 7
+param set MC_ROLL_P 7
 param set MC_ROLLRATE_P 0.15
-param set MC_YAW_P 2.0
+param set MC_YAW_P 2.8
 param set MC_YAWRATE_P 0.35
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
 dataman start
-param set CAL_GYRO0_ID 2293760
-param set CAL_ACC0_ID 1376256
-param set CAL_ACC1_ID 1310720
-param set CAL_MAG0_ID 196608
+param set CAL_GYRO0_ID 2293768
+param set CAL_ACC0_ID 1376264
+param set CAL_ACC1_ID 1310728
+param set CAL_MAG0_ID 196616
 param set CAL_GYRO0_XOFF 0.01
 param set CAL_ACC0_XOFF 0.01
 param set CAL_ACC0_YOFF -0.01
@@ -26,8 +28,11 @@ param set MPC_XY_P 0.4
 param set MPC_XY_VEL_P 0.2
 param set MPC_XY_VEL_D 0.005
 param set SENS_BOARD_ROT 0
-param set COM_RC_IN_MODE 2
-rgbled start
+param set COM_RC_IN_MODE 1
+param set NAV_ACC_RAD 2.0
+param set RTL_RETURN_ALT 30.0
+param set RTL_DESCEND_ALT 10.0
+rgbledsim start
 tone_alarm start
 gyrosim start
 accelsim start
@@ -9,10 +9,10 @@ param set MC_YAWRATE_P 0.35
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
 dataman start
-param set CAL_GYRO0_ID 2293760
-param set CAL_ACC0_ID 1376256
-param set CAL_ACC1_ID 1310720
-param set CAL_MAG0_ID 196608
+param set CAL_GYRO0_ID 2293768
+param set CAL_ACC0_ID 1376264
+param set CAL_ACC1_ID 1310728
+param set CAL_MAG0_ID 196616
 param set CAL_GYRO0_XOFF 0.01
 param set CAL_ACC0_XOFF 1
 param set CAL_ACC0_YOFF 2
@@ -9,10 +9,10 @@ param set MC_YAWRATE_P 0.35
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
 dataman start
-param set CAL_GYRO0_ID 2293760
-param set CAL_ACC0_ID 1376256
-param set CAL_ACC1_ID 1310720
-param set CAL_MAG0_ID 196608
+param set CAL_GYRO0_ID 2293768
+param set CAL_ACC0_ID 1376264
+param set CAL_ACC1_ID 1310728
+param set CAL_MAG0_ID 196616
 param set CAL_GYRO0_XOFF 0.01
 param set CAL_ACC0_XOFF 0.01
 param set CAL_ACC0_YOFF -0.01
@@ -9,10 +9,10 @@ param set MC_YAWRATE_P 0.35
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
 dataman start
-param set CAL_GYRO0_ID 2293760
-param set CAL_ACC0_ID 1376256
-param set CAL_ACC1_ID 1310720
-param set CAL_MAG0_ID 196608
+param set CAL_GYRO0_ID 2293768
+param set CAL_ACC0_ID 1376264
+param set CAL_ACC1_ID 1310728
+param set CAL_MAG0_ID 196616
 param set CAL_GYRO0_XOFF 0.01
 param set CAL_ACC0_XOFF 0.01
 param set CAL_ACC0_YOFF -0.01
@@ -40,7 +40,7 @@ param set MP_ROLLRATE_I 0.001
 param set MP_ROLLRATE_D 0.001
 param set MP_PITCH_P 4
 param set MP_PITCHRATE_P 0.3
-param set COM_RC_IN_MODE 2
+param set COM_RC_IN_MODE 1
 rgbled start
 tone_alarm start
 gyrosim start
@@ -145,20 +145,6 @@ static struct spi_dev_s *spi3;

 #include <math.h>

-#if 0
-#ifdef __cplusplus
-__EXPORT int matherr(struct __exception *e)
-{
-	return 1;
-}
-#else
-__EXPORT int matherr(struct exception *e)
-{
-	return 1;
-}
-#endif
-#endif
-
 __EXPORT int nsh_archinitialize(void)
 {
 	int result;
@@ -44,4 +44,4 @@ px4_add_module(
 	DEPENDS
 		platforms__common
 	)
-# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :
@@ -212,19 +212,6 @@ static struct sdio_dev_s *sdio;

 #include <math.h>

-/* TODO XXX commented this out to get cmake build working */
-/*#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)
 {

@@ -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__boards__px4fmu-v4
+	COMPILE_FLAGS
+		-Os
+	SRCS
+		px4fmu_can.c
+		px4fmu_init.c
+		px4fmu_pwm_servo.c
+		px4fmu_spi.c
+		px4fmu_usb.c
+		px4fmu_led.c
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
@@ -0,0 +1,282 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file board_config.h
+ *
+ * PX4FMUv2 internal definitions
+ */
+
+#pragma once
+
+/****************************************************************************************************
+ * Included Files
+ ****************************************************************************************************/
+
+#include <px4_config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+/* these headers are not C++ safe */
+#include <stm32.h>
+#include <arch/board/board.h>
+
+#define UDID_START		0x1FFF7A10
+
+/****************************************************************************************************
+ * Definitions
+ ****************************************************************************************************/
+/* Configuration ************************************************************************************/
+//{GPIO_RSSI_IN,           0,                       0}, - pio Analog used as PWM
+//{0,                      GPIO_LED_SAFETY,         0},	pio replacement
+//{GPIO_SAFETY_SWITCH_IN,  0,                       0},   pio replacement
+//{0,                      GPIO_PERIPH_3V3_EN,      0},	Owned by the 8266 driver
+//{0,                      GPIO_SBUS_INV,           0},	https://github.com/PX4/Firmware/blob/master/src/modules/px4iofirmware/sbus.c
+//{GPIO_8266_GPIO0,        0,                       0},   Owned by the 8266 driver
+//{0,                      GPIO_SPEKTRUM_POWER,     0},	Owned Spektum driver input to auto pilot
+//{0,                      GPIO_8266_PD,            0},	Owned by the 8266 driver
+//{0,                      GPIO_8266_RST,           0},	Owned by the 8266 driver
+
+/* PX4FMU GPIOs ***********************************************************************************/
+/* LEDs */
+
+#define GPIO_LED1		(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN11)
+#define GPIO_LED2		(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
+#define GPIO_LED3		(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN3)
+
+#define GPIO_LED_RED 	GPIO_LED1
+#define GPIO_LED_GREEN 	GPIO_LED2
+#define GPIO_LED_BLUE   GPIO_LED3
+
+/*  Define the Chip Selects */
+
+#define GPIO_SPI_CS_MPU9250		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2)
+#define GPIO_SPI_CS_HMC5983		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN15)
+#define GPIO_SPI_CS_MS5611		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7)
+#define GPIO_SPI_CS_ICM_20608_G (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
+
+#define GPIO_SPI_CS_FRAM		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
+
+/*  Define the Ready interrupts */
+
+#define GPIO_DRDY_MPU9250		(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)
+#define GPIO_DRDY_HMC5983		(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTE|GPIO_PIN12)
+#define GPIO_DRDY_ICM_20608_G	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTC|GPIO_PIN14)
+
+
+/*
+ *  Define the ability to shut off off the sensor signals
+ *  by changing the signals to inputs
+ */
+
+#define _PIN_OFF(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz))
+
+#define GPIO_SPI_CS_OFF_MPU9250		_PIN_OFF(GPIO_SPI_CS_MPU9250)
+#define GPIO_SPI_CS_OFF_HMC5983		_PIN_OFF(GPIO_SPI_CS_HMC5983)
+#define GPIO_SPI_CS_OFF_MS5611		_PIN_OFF(GPIO_SPI_CS_MS5611)
+#define GPIO_SPI_CS_OFF_ICM_20608_G _PIN_OFF(GPIO_SPI_CS_ICM_20608_G)
+
+#define GPIO_DRDY_OFF_MPU9250		_PIN_OFF(GPIO_DRDY_MPU9250)
+#define GPIO_DRDY_OFF_HMC5983		_PIN_OFF(GPIO_DRDY_HMC5983)
+#define GPIO_DRDY_OFF_ICM_20608_G	_PIN_OFF(GPIO_DRDY_ICM_20608_G)
+
+
+/* SPI1 off */
+#define GPIO_SPI1_SCK_OFF	_PIN_OFF(GPIO_SPI1_SCK)
+#define GPIO_SPI1_MISO_OFF	_PIN_OFF(GPIO_SPI1_MISO)
+#define GPIO_SPI1_MOSI_OFF	_PIN_OFF(GPIO_SPI1_MOSI)
+
+#define PX4_SPI_BUS_SENSORS	1
+#define PX4_SPI_BUS_RAMTRON	2
+
+/* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */
+#define PX4_SPIDEV_GYRO			1
+#define PX4_SPIDEV_ACCEL_MAG	2
+#define PX4_SPIDEV_BARO			3
+#define PX4_SPIDEV_MPU			4
+#define PX4_SPIDEV_HMC			5
+#define PX4_SPIDEV_ICM			6
+
+/* I2C busses */
+#define PX4_I2C_BUS_EXPANSION	1
+#define PX4_I2C_BUS_LED			PX4_I2C_BUS_EXPANSION
+
+/* Devices on the external bus.
+ *
+ * Note that these are unshifted addresses.
+ */
+#define PX4_I2C_OBDEV_LED	0x55
+#define PX4_I2C_OBDEV_HMC5883	0x1e
+
+/*
+ * ADC channels
+ *
+ * These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4 Firmware in the adc driver
+ */
+#define ADC_CHANNELS (1 << 2) | (1 << 3) | (1 << 4) | (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15)
+
+// ADC defines to be used in sensors.cpp to read from a particular channel
+#define ADC_BATTERY_VOLTAGE_CHANNEL		2
+#define ADC_BATTERY_CURRENT_CHANNEL		3
+#define ADC_5V_RAIL_SENSE				4
+#define ADC_AIRSPEED_VOLTAGE_CHANNEL	15
+
+/* User GPIOs
+ *
+ * GPIO0-5 are the PWM servo outputs.
+ */
+#define GPIO_GPIO0_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN14)
+#define GPIO_GPIO1_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN13)
+#define GPIO_GPIO2_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11)
+#define GPIO_GPIO3_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN9)
+#define GPIO_GPIO4_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_GPIO5_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14)
+
+#define GPIO_GPIO0_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14)
+#define GPIO_GPIO1_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13)
+#define GPIO_GPIO2_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11)
+#define GPIO_GPIO3_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9)
+#define GPIO_GPIO4_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_GPIO5_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN14)
+
+/* Power supply control and monitoring GPIOs */
+#define GPIO_VDD_BRICK_VALID	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
+#define GPIO_VDD_3V3_SENSORS_EN	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3)
+
+/* Tone alarm output */
+#define TONE_ALARM_TIMER		2	/* timer 2 */
+#define TONE_ALARM_CHANNEL		1	/* channel 1 */
+#define GPIO_TONE_ALARM_IDLE	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15)
+#define GPIO_TONE_ALARM			(GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15)
+
+/* PWM
+ *
+ * Six PWM outputs are configured.
+ *
+ * Pins:
+ *
+ * CH1 : PE14 : TIM1_CH4
+ * CH2 : PE13 : TIM1_CH3
+ * CH3 : PE11 : TIM1_CH2
+ * CH4 : PE9  : TIM1_CH1
+ * CH5 : PD13 : TIM4_CH2
+ * CH6 : PD14 : TIM4_CH3
+ */
+#define GPIO_TIM1_CH1OUT	GPIO_TIM1_CH1OUT_2
+#define GPIO_TIM1_CH2OUT	GPIO_TIM1_CH2OUT_2
+#define GPIO_TIM1_CH3OUT	GPIO_TIM1_CH3OUT_2
+#define GPIO_TIM1_CH4OUT	GPIO_TIM1_CH4OUT_2
+#define GPIO_TIM4_CH2OUT	GPIO_TIM4_CH2OUT_2
+#define GPIO_TIM4_CH3OUT	GPIO_TIM4_CH3OUT_2
+
+/* USB OTG FS
+ *
+ * PA9  OTG_FS_VBUS VBUS sensing
+ */
+#define GPIO_OTGFS_VBUS		(GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
+
+/* High-resolution timer */
+#define HRT_TIMER		3	/* use timer8 for the HRT */
+#define HRT_TIMER_CHANNEL	4	/* use capture/compare channel */
+
+#define HRT_PPM_CHANNEL		3	/* use capture/compare channel 2 */
+#define GPIO_PPM_IN			(GPIO_ALT|GPIO_AF2|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0)
+
+#define SBUS_SERIAL_PORT	"/dev/ttyS4"
+
+/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */
+#define PWMIN_TIMER			4
+#define PWMIN_TIMER_CHANNEL	2
+#define GPIO_PWM_IN			GPIO_TIM4_CH2IN_2
+
+#define GPIO_RSSI_IN 			(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)
+#define GPIO_LED_SAFETY			(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN3)
+#define GPIO_SAFETY_SWITCH_IN	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4)
+#define GPIO_PERIPH_3V3_EN		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5)
+#define GPIO_SBUS_INV			(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
+
+#define GPIO_8266_GPIO0			(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN2)
+#define GPIO_SPEKTRUM_POWER		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)
+#define GPIO_8266_PD			(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN5)
+#define GPIO_8266_RST			(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN6)
+
+/****************************************************************************************************
+ * Public Types
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Public data
+ ****************************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+/****************************************************************************************************
+ * Public Functions
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ****************************************************************************************************/
+
+extern void stm32_spiinitialize(void);
+
+extern void stm32_usbinitialize(void);
+
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ *   Perform architecture specific initialization for NSH.
+ *
+ *   CONFIG_NSH_ARCHINIT=y :
+ *     Called from the NSH library
+ *
+ *   CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, &&
+ *   CONFIG_NSH_ARCHINIT=n :
+ *     Called from board_initialize().
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_NSH_LIBRARY
+int nsh_archinitialize(void);
+#endif
+
+#endif /* __ASSEMBLY__ */
+
+__END_DECLS
@@ -0,0 +1,144 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4fmu_can.c
+ *
+ * Board-specific CAN functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <px4_config.h>
+
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/can.h>
+#include <arch/board/board.h>
+
+#include "chip.h"
+#include "up_arch.h"
+
+#include "stm32.h"
+#include "stm32_can.h"
+#include "board_config.h"
+
+#ifdef CONFIG_CAN
+
+/************************************************************************************
+ * Pre-processor Definitions
+ ************************************************************************************/
+/* Configuration ********************************************************************/
+
+#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2)
+#  warning "Both CAN1 and CAN2 are enabled.  Assuming only CAN1."
+#  undef CONFIG_STM32_CAN2
+#endif
+
+#ifdef CONFIG_STM32_CAN1
+#  define CAN_PORT 1
+#else
+#  define CAN_PORT 2
+#endif
+
+/* Debug ***************************************************************************/
+/* Non-standard debug that may be enabled just for testing CAN */
+
+#ifdef CONFIG_DEBUG_CAN
+#  define candbg    dbg
+#  define canvdbg   vdbg
+#  define canlldbg  lldbg
+#  define canllvdbg llvdbg
+#else
+#  define candbg(x...)
+#  define canvdbg(x...)
+#  define canlldbg(x...)
+#  define canllvdbg(x...)
+#endif
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: can_devinit
+ *
+ * Description:
+ *   All STM32 architectures must provide the following interface to work with
+ *   examples/can.
+ *
+ ************************************************************************************/
+
+int can_devinit(void)
+{
+	static bool initialized = false;
+	struct can_dev_s *can;
+	int ret;
+
+	/* Check if we have already initialized */
+
+	if (!initialized) {
+		/* Call stm32_caninitialize() to get an instance of the CAN interface */
+
+		can = stm32_caninitialize(CAN_PORT);
+
+		if (can == NULL) {
+			candbg("ERROR:  Failed to get CAN interface\n");
+			return -ENODEV;
+		}
+
+		/* Register the CAN driver at "/dev/can0" */
+
+		ret = can_register("/dev/can0", can);
+
+		if (ret < 0) {
+			candbg("ERROR: can_register failed: %d\n", ret);
+			return ret;
+		}
+
+		/* Now we are initialized */
+
+		initialized = true;
+	}
+
+	return OK;
+}
+
+#endif
@@ -0,0 +1,335 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4fmu_init.c
+ *
+ * PX4FMU-specific early startup code.  This file implements the
+ * nsh_archinitialize() function that is called early by nsh during startup.
+ *
+ * Code here is run before the rcS script is invoked; it should start required
+ * subsystems and perform board-specific initialisation.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <px4_config.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/spi.h>
+#include <nuttx/i2c.h>
+#include <nuttx/sdio.h>
+#include <nuttx/mmcsd.h>
+#include <nuttx/analog/adc.h>
+#include <nuttx/gran.h>
+
+#include <stm32.h>
+#include "board_config.h"
+#include <stm32_uart.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_led.h>
+
+#include <systemlib/cpuload.h>
+#include <systemlib/perf_counter.h>
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+/* Configuration ************************************************************/
+
+/* Debug ********************************************************************/
+
+#ifdef CONFIG_CPP_HAVE_VARARGS
+#  ifdef CONFIG_DEBUG
+#    define message(...) lowsyslog(__VA_ARGS__)
+#  else
+#    define message(...) printf(__VA_ARGS__)
+#  endif
+#else
+#  ifdef CONFIG_DEBUG
+#    define message lowsyslog
+#  else
+#    define message printf
+#  endif
+#endif
+
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init(void);
+extern void led_on(int led);
+extern void led_off(int led);
+__END_DECLS
+
+/****************************************************************************
+ * Protected Functions
+ ****************************************************************************/
+
+#if defined(CONFIG_FAT_DMAMEMORY)
+# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY)
+#  error microSD DMA support requires CONFIG_GRAN
+# endif
+
+static GRAN_HANDLE dma_allocator;
+
+/*
+ * The DMA heap size constrains the total number of things that can be
+ * ready to do DMA at a time.
+ *
+ * For example, FAT DMA depends on one sector-sized buffer per filesystem plus
+ * one sector-sized buffer per file.
+ *
+ * We use a fundamental alignment / granule size of 64B; this is sufficient
+ * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits).
+ */
+static uint8_t g_dma_heap[8192] __attribute__((aligned(64)));
+static perf_counter_t g_dma_perf;
+
+static void
+dma_alloc_init(void)
+{
+	dma_allocator = gran_initialize(g_dma_heap,
+					sizeof(g_dma_heap),
+					7,  /* 128B granule - must be > alignment (XXX bug?) */
+					6); /* 64B alignment */
+
+	if (dma_allocator == NULL) {
+		message("[boot] DMA allocator setup FAILED");
+
+	} else {
+		g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations");
+	}
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/*
+ * DMA-aware allocator stubs for the FAT filesystem.
+ */
+
+__EXPORT void *fat_dma_alloc(size_t size);
+__EXPORT void fat_dma_free(FAR void *memory, size_t size);
+
+void *
+fat_dma_alloc(size_t size)
+{
+	perf_count(g_dma_perf);
+	return gran_alloc(dma_allocator, size);
+}
+
+void
+fat_dma_free(FAR void *memory, size_t size)
+{
+	gran_free(dma_allocator, memory, size);
+}
+
+#else
+
+# define dma_alloc_init()
+
+#endif
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void
+stm32_boardinitialize(void)
+{
+	/* configure SPI interfaces */
+	stm32_spiinitialize();
+
+	/* configure LEDs */
+	up_ledinit();
+}
+
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ *   Perform architecture specific initialization
+ *
+ ****************************************************************************/
+
+static struct spi_dev_s *spi1;
+static struct spi_dev_s *spi2;
+static struct sdio_dev_s *sdio;
+
+#include <math.h>
+
+__EXPORT int nsh_archinitialize(void)
+{
+
+	/* configure ADC pins */
+	stm32_configgpio(GPIO_ADC1_IN2);	/* BATT_VOLTAGE_SENS */
+	stm32_configgpio(GPIO_ADC1_IN3);	/* BATT_CURRENT_SENS */
+	stm32_configgpio(GPIO_ADC1_IN4);	/* VDD_5V_SENS */
+
+	stm32_configgpio(GPIO_ADC1_IN13);	/* FMU_AUX_ADC_1 */
+	stm32_configgpio(GPIO_ADC1_IN14);	/* FMU_AUX_ADC_2 */
+	stm32_configgpio(GPIO_ADC1_IN15);	/* PRESSURE_SENS */
+
+	/* configure power supply control/sense pins */
+	stm32_configgpio(GPIO_PERIPH_3V3_EN);
+	stm32_configgpio(GPIO_VDD_BRICK_VALID);
+	stm32_configgpio(GPIO_GPIO5_OUTPUT);
+
+	stm32_configgpio(GPIO_SBUS_INV);
+	stm32_configgpio(GPIO_8266_GPIO0);
+	stm32_configgpio(GPIO_SPEKTRUM_POWER);
+	stm32_configgpio(GPIO_8266_PD);
+	stm32_configgpio(GPIO_8266_RST);
+
+	/* configure the high-resolution time/callout interface */
+	hrt_init();
+
+	/* configure the DMA allocator */
+	dma_alloc_init();
+
+	/* configure CPU load estimation */
+#ifdef CONFIG_SCHED_INSTRUMENTATION
+	cpuload_initialize_once();
+#endif
+
+	/* set up the serial DMA polling */
+	static struct hrt_call serial_dma_call;
+	struct timespec ts;
+
+	/*
+	 * Poll at 1ms intervals for received bytes that have not triggered
+	 * a DMA event.
+	 */
+	ts.tv_sec = 0;
+	ts.tv_nsec = 1000000;
+
+	hrt_call_every(&serial_dma_call,
+		       ts_to_abstime(&ts),
+		       ts_to_abstime(&ts),
+		       (hrt_callout)stm32_serial_dma_poll,
+		       NULL);
+
+	/* initial LED state */
+	drv_led_start();
+	led_off(LED_RED);
+	led_off(LED_GREEN);
+	led_off(LED_BLUE);
+
+	/* Configure SPI-based devices */
+
+	spi1 = up_spiinitialize(1);
+
+	if (!spi1) {
+		message("[boot] FAILED to initialize SPI port 1\n");
+		up_ledon(LED_RED);
+		return -ENODEV;
+	}
+
+	/* Default SPI1 to 1MHz and de-assert the known chip selects. */
+	SPI_SETFREQUENCY(spi1, 10000000);
+	SPI_SETBITS(spi1, 8);
+	SPI_SETMODE(spi1, SPIDEV_MODE3);
+	SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
+	SPI_SELECT(spi1, PX4_SPIDEV_HMC, false);
+	SPI_SELECT(spi1, PX4_SPIDEV_BARO, false);
+	SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
+	up_udelay(20);
+
+	/* Get the SPI port for the FRAM */
+
+	spi2 = up_spiinitialize(2);
+
+	if (!spi2) {
+		message("[boot] FAILED to initialize SPI port 2\n");
+		up_ledon(LED_RED);
+		return -ENODEV;
+	}
+
+	/* Default SPI2 to 37.5 MHz (40 MHz rounded to nearest valid divider, F4 max)
+	 * and de-assert the known chip selects. */
+
+	// XXX start with 10.4 MHz in FRAM usage and go up to 37.5 once validated
+	SPI_SETFREQUENCY(spi2, 12 * 1000 * 1000);
+	SPI_SETBITS(spi2, 8);
+	SPI_SETMODE(spi2, SPIDEV_MODE3);
+	SPI_SELECT(spi2, SPIDEV_FLASH, false);
+
+#ifdef CONFIG_MMCSD
+	/* First, get an instance of the SDIO interface */
+
+	sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO);
+
+	if (!sdio) {
+		message("[boot] Failed to initialize SDIO slot %d\n",
+			CONFIG_NSH_MMCSDSLOTNO);
+		return -ENODEV;
+	}
+
+	/* Now bind the SDIO interface to the MMC/SD driver */
+	int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio);
+
+	if (ret != OK) {
+		message("[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret);
+		return ret;
+	}
+
+	/* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */
+	sdio_mediachange(sdio, true);
+
+#endif
+
+	return OK;
+}
@@ -0,0 +1,106 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4fmu2_led.c
+ *
+ * PX4FMU LED backend.
+ */
+
+#include <px4_config.h>
+
+#include <stdbool.h>
+
+#include "stm32.h"
+#include "board_config.h"
+
+#include <arch/board/board.h>
+
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init(void);
+extern void led_on(int led);
+extern void led_off(int led);
+extern void led_toggle(int led);
+__END_DECLS
+
+
+
+static uint32_t g_ledmap[] = {
+	GPIO_LED_BLUE,    // Indexed by LED_BLUE
+	GPIO_LED_RED,     // Indexed by LED_RED, LED_AMBER
+	GPIO_LED_SAFETY,  // Indexed by LED_SAFETY
+	GPIO_LED_GREEN,   // Indexed by LED_GREEN
+};
+
+__EXPORT void led_init(void)
+{
+	/* Configure LED GPIOs for output */
+	for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) {
+		stm32_configgpio(g_ledmap[l]);
+	}
+}
+
+static void phy_set_led(int led, bool state)
+{
+	/* Pull Down to switch on */
+	stm32_gpiowrite(g_ledmap[led], !state);
+}
+
+static bool phy_get_led(int led)
+{
+
+	return !stm32_gpioread(g_ledmap[led]);
+}
+
+__EXPORT void led_on(int led)
+{
+	phy_set_led(led, true);
+}
+
+__EXPORT void led_off(int led)
+{
+	phy_set_led(led, false);
+}
+
+__EXPORT void led_toggle(int led)
+{
+
+	phy_set_led(led, !phy_get_led(led));
+}
@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file px4fmu_pwm_servo.c
+ *
+ * Configuration data for the stm32 pwm_servo driver.
+ *
+ * Note that these arrays must always be fully-sized.
+ */
+
+#include <stdint.h>
+
+#include <stm32.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
+
+#include <drivers/stm32/drv_pwm_servo.h>
+#include <drivers/drv_pwm_output.h>
+
+#include "board_config.h"
+
+__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
+	{
+		.base = STM32_TIM1_BASE,
+		.clock_register = STM32_RCC_APB2ENR,
+		.clock_bit = RCC_APB2ENR_TIM1EN,
+		.clock_freq = STM32_APB2_TIM1_CLKIN
+	},
+	{
+		.base = STM32_TIM4_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM4EN,
+		.clock_freq = STM32_APB1_TIM4_CLKIN
+	}
+};
+
+__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
+	{
+		.gpio = GPIO_TIM1_CH4OUT,
+		.timer_index = 0,
+		.timer_channel = 4,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM1_CH3OUT,
+		.timer_index = 0,
+		.timer_channel = 3,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM1_CH2OUT,
+		.timer_index = 0,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM1_CH1OUT,
+		.timer_index = 0,
+		.timer_channel = 1,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM4_CH2OUT,
+		.timer_index = 1,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM4_CH3OUT,
+		.timer_index = 1,
+		.timer_channel = 3,
+		.default_value = 1000,
+	}
+};
@@ -0,0 +1,163 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4fmu_spi.c
+ *
+ * Board-specific SPI functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <px4_config.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/spi.h>
+#include <arch/board/board.h>
+
+#include <up_arch.h>
+#include <chip.h>
+#include <stm32.h>
+#include "board_config.h"
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_spiinitialize(void)
+{
+#ifdef CONFIG_STM32_SPI1
+	stm32_configgpio(GPIO_SPI_CS_MPU9250);
+	stm32_configgpio(GPIO_SPI_CS_HMC5983);
+	stm32_configgpio(GPIO_SPI_CS_MS5611);
+	stm32_configgpio(GPIO_SPI_CS_ICM_20608_G);
+
+	/* De-activate all peripherals,
+	 * required for some peripheral
+	 * state machines
+	 */
+	stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1);
+
+	stm32_configgpio(GPIO_DRDY_MPU9250);
+	stm32_configgpio(GPIO_DRDY_HMC5983);
+	stm32_configgpio(GPIO_DRDY_ICM_20608_G);
+#endif
+
+#ifdef CONFIG_STM32_SPI2
+	stm32_configgpio(GPIO_SPI_CS_FRAM);
+	stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1);
+#endif
+
+}
+
+__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* SPI select is active low, so write !selected to select the device */
+
+	switch (devid) {
+	case PX4_SPIDEV_ICM:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, !selected);
+		break;
+
+	case PX4_SPIDEV_ACCEL_MAG:
+		/* Making sure the other peripherals are not selected */
+		break;
+
+	case PX4_SPIDEV_BARO:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_MS5611, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1);
+		break;
+
+	case PX4_SPIDEV_HMC:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_HMC5983, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1);
+		break;
+
+	case PX4_SPIDEV_MPU:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_MPU9250, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1);
+		break;
+
+	default:
+		break;
+	}
+}
+
+__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	return SPI_STATUS_PRESENT;
+}
+
+
+#ifdef CONFIG_STM32_SPI2
+__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* there can only be one device on this bus, so always select it */
+	stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected);
+}
+
+__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	/* FRAM is always present */
+	return SPI_STATUS_PRESENT;
+}
+#endif
@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4fmu_usb.c
+ *
+ * Board-specific USB functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/usb/usbdev.h>
+#include <nuttx/usb/usbdev_trace.h>
+
+#include <up_arch.h>
+#include <stm32.h>
+#include "board_config.h"
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_usbinitialize
+ *
+ * Description:
+ *   Called to setup USB-related GPIO pins for the PX4FMU board.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbinitialize(void)
+{
+	/* The OTG FS has an internal soft pull-up */
+
+	/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
+
+#ifdef CONFIG_STM32_OTGFS
+	stm32_configgpio(GPIO_OTGFS_VBUS);
+	/* XXX We only support device mode
+	stm32_configgpio(GPIO_OTGFS_PWRON);
+	stm32_configgpio(GPIO_OTGFS_OVER);
+	*/
+#endif
+}
+
+/************************************************************************************
+ * Name:  stm32_usbsuspend
+ *
+ * Description:
+ *   Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is
+ *   used.  This function is called whenever the USB enters or leaves suspend mode.
+ *   This is an opportunity for the board logic to shutdown clocks, power, etc.
+ *   while the USB is suspended.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
+{
+	ulldbg("resume: %d\n", resume);
+}
+
@@ -65,6 +65,8 @@
 #define GPIO_LED3       (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\
 			 GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)

+#define GPIO_USART1_RX_SPEKTRUM		(GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10)
+
 /* Safety switch button *************************************************************/

 #define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5)
@@ -79,6 +79,8 @@
 #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
 #define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11)

+#define GPIO_USART1_RX_SPEKTRUM		(GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10)
+
 /* Safety switch button *******************************************************/

 #define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5)
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -30,13 +30,15 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-
-#
-# STM32 ADC driver
-#
-
-MODULE_COMMAND	= adcsim
-
-SRCS		= adcsim.cpp
-
-MAXOPTIMIZATION	 = -Os
+px4_add_module(
+	MODULE drivers__bst
+	MAIN bst
+	STACK 1200
+	COMPILE_FLAGS
+		-Os
+	SRCS
+		bst.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
@@ -0,0 +1,409 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file bst.cpp
+ *
+ * Black Sheep Telemetry driver
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <px4_config.h>
+#include <px4_getopt.h>
+#include <px4_workqueue.h>
+#include <drivers/device/i2c.h>
+#include <systemlib/err.h>
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/battery_status.h>
+
+#define BST_DEVICE_PATH "/dev/bst0"
+
+static const char commandline_usage[] = "usage: bst start|status|stop";
+
+extern "C" __EXPORT int bst_main(int argc, char *argv[]);
+
+#define BST_ADDR		0x76
+
+namespace px4
+{
+namespace bst
+{
+
+#pragma pack(push, 1)
+
+template <typename T>
+struct BSTPacket {
+	uint8_t length;
+	uint8_t type;
+	T payload;
+	uint8_t crc;
+};
+
+struct BSTDeviceInfoRequest {
+	uint8_t cmd;
+};
+
+struct BSTDeviceInfoReply {
+	uint32_t hw_id;
+	uint16_t fw_id;
+	uint8_t dev_name_len;
+	char dev_name[32];
+};
+
+struct BSTGPSPosition {
+	int32_t lat;
+	int32_t lon;
+	uint16_t gs;
+	uint16_t heading;
+	uint16_t alt;
+	uint8_t sats;
+};
+
+struct BSTAttitude {
+	int16_t pitch;
+	int16_t roll;
+	int16_t yaw;
+};
+
+struct BSTBattery {
+	uint16_t voltage;
+	uint16_t current;
+	uint8_t capacity[3];
+};
+
+#pragma pack(pop)
+
+class BST : public device::I2C
+{
+public:
+	BST(int bus);
+
+	virtual ~BST();
+
+	virtual int		init();
+
+	virtual int		probe();
+
+	virtual int		info() { return 0; }
+
+	virtual int		ioctl(device::file_t *filp, int cmd, unsigned long arg) { return 0; }
+
+	work_s *work_ptr() { return &_work; }
+
+	void stop();
+
+	static void		start_trampoline(void *arg);
+
+private:
+	work_s			_work = {};
+	bool			_should_run = false;
+	unsigned		_interval = 100;
+	int				_gps_sub;
+	int				_attitude_sub;
+	int				_battery_sub;
+
+	void			start();
+
+	static void		cycle_trampoline(void *arg);
+
+	void			cycle();
+
+	template <typename T>
+	void			send_packet(BSTPacket<T> &packet)
+	{
+		packet.length = sizeof(packet) - 1;	// Length
+		packet.crc = crc8(reinterpret_cast<uint8_t *>(&packet.type), sizeof(packet) - 2);
+
+		transfer(reinterpret_cast<uint8_t *>(&packet), sizeof(packet), nullptr, 0);
+	}
+
+	template <typename T_SEND, typename T_RECV>
+	void				send_packet(BSTPacket<T_SEND> &packet_send, BSTPacket<T_RECV> &packet_recv)
+	{
+		packet_send.length = sizeof(packet_send) - 1;	// Length
+		packet_send.crc = crc8(reinterpret_cast<uint8_t *>(&packet_send.type), sizeof(packet_send) - 2);
+		transfer(reinterpret_cast<uint8_t *>(&packet_send), sizeof(packet_send), reinterpret_cast<uint8_t *>(&packet_recv),
+			 sizeof(packet_recv));
+	}
+
+	static uint8_t	crc8(uint8_t *data, size_t len);
+
+	//! Byte swap unsigned short
+	uint16_t swap_uint16(uint16_t val)
+	{
+		return (val << 8) | (val >> 8);
+	}
+
+	//! Byte swap short
+	int16_t swap_int16(int16_t val)
+	{
+		return (val << 8) | ((val >> 8) & 0xFF);
+	}
+
+	//! Byte swap unsigned int
+	uint32_t swap_uint32(uint32_t val)
+	{
+		val = ((val << 8) & 0xFF00FF00) | ((val >> 8) & 0xFF00FF);
+		return (val << 16) | (val >> 16);
+	}
+
+	//! Byte swap int
+	int32_t swap_int32(int32_t val)
+	{
+		val = ((val << 8) & 0xFF00FF00) | ((val >> 8) & 0xFF00FF);
+		return (val << 16) | ((val >> 16) & 0xFFFF);
+	}
+};
+
+static BST *g_bst = nullptr;
+
+BST::BST(int bus) :
+	I2C("bst", BST_DEVICE_PATH, bus, BST_ADDR
+#ifdef __PX4_NUTTX
+	    , 100000 /* maximum speed supported */
+#endif
+	   )
+{
+}
+
+BST::~BST()
+{
+	_should_run = false;
+
+	work_cancel(LPWORK, &_work);
+}
+
+int BST::probe()
+{
+	int retries_prev = _retries;
+	_retries = 3;
+
+	BSTPacket<BSTDeviceInfoRequest> dev_info_req = {};
+	dev_info_req.type = 0x0A;
+	dev_info_req.payload.cmd = 0x04;
+	BSTPacket<BSTDeviceInfoReply> dev_info_reply = {};
+	send_packet(dev_info_req, dev_info_reply);
+
+	if (dev_info_reply.type != 0x05) {
+		warnx("no devices found");
+		return -EIO;
+	}
+
+	uint8_t *reply_raw = reinterpret_cast<uint8_t *>(&dev_info_reply);
+	uint8_t crc_calc = crc8(reinterpret_cast<uint8_t *>(&dev_info_reply.type), dev_info_reply.length - 1);
+	uint8_t crc_recv = reply_raw[dev_info_reply.length];
+
+	if (crc_recv != crc_calc) {
+		warnx("CRC error: got %02x, should be %02x", (int)crc_recv, (int)crc_calc);
+		return -EIO;
+	}
+
+	dev_info_reply.payload.dev_name[dev_info_reply.payload.dev_name_len] = '\0';
+	warnx("device info: hardware ID: 0x%08X, firmware ID: 0x%04X, device name: %s",
+	      (int)swap_uint32(dev_info_reply.payload.hw_id), (int)swap_uint16(dev_info_reply.payload.fw_id),
+	      dev_info_reply.payload.dev_name);
+
+	_retries = retries_prev;
+
+	return OK;
+}
+
+int BST::init()
+{
+	int ret;
+	ret = I2C::init();
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	work_queue(LPWORK, &_work, BST::start_trampoline, g_bst, 0);
+
+	return OK;
+}
+
+void BST::start_trampoline(void *arg)
+{
+	reinterpret_cast<BST *>(arg)->start();
+}
+
+void BST::start()
+{
+	_should_run = true;
+
+	_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	_battery_sub = orb_subscribe(ORB_ID(battery_status));
+
+	set_address(0x00);	// General call address
+
+	work_queue(LPWORK, &_work, BST::cycle_trampoline, this, 0);
+}
+
+void BST::cycle_trampoline(void *arg)
+{
+	reinterpret_cast<BST *>(arg)->cycle();
+}
+
+void BST::cycle()
+{
+	if (_should_run) {
+		bool updated = false;
+
+		orb_check(_attitude_sub, &updated);
+
+		if (updated) {
+			vehicle_attitude_s att;
+			orb_copy(ORB_ID(vehicle_attitude), _attitude_sub, &att);
+			BSTPacket<BSTAttitude> bst_att = {};
+			bst_att.type = 0x1E;
+			bst_att.payload.roll = swap_int32(att.roll * 10000);
+			bst_att.payload.pitch = swap_int32(att.pitch * 10000);
+			bst_att.payload.yaw = swap_int32(att.yaw * 10000);
+			send_packet(bst_att);
+		}
+
+		updated = false;
+		orb_check(_battery_sub, &updated);
+
+		if (updated) {
+			battery_status_s batt;
+			orb_copy(ORB_ID(battery_status), _battery_sub, &batt);
+			BSTPacket<BSTBattery> bst_batt = {};
+			bst_batt.type = 0x08;
+			bst_batt.payload.voltage = swap_uint16(batt.voltage_v * 10.0f);
+			bst_batt.payload.current = swap_uint16(batt.current_a * 10.0f);
+			uint32_t discharged = batt.discharged_mah;
+			bst_batt.payload.capacity[0] = static_cast<uint8_t>(discharged >> 16);
+			bst_batt.payload.capacity[1] = static_cast<uint8_t>(discharged >> 8);
+			bst_batt.payload.capacity[2] = static_cast<uint8_t>(discharged);
+			send_packet(bst_batt);
+		}
+
+		updated = false;
+		orb_check(_gps_sub, &updated);
+
+		if (updated) {
+			vehicle_gps_position_s gps;
+			orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps);
+
+			if (gps.fix_type >= 3 && gps.eph < 50.0f) {
+				BSTPacket<BSTGPSPosition> bst_gps = {};
+				bst_gps.type = 0x02;
+				bst_gps.payload.lat = swap_int32(gps.lat);
+				bst_gps.payload.lon = swap_int32(gps.lon);
+				bst_gps.payload.alt = swap_int16(gps.alt / 1000 + 1000);
+				bst_gps.payload.gs = swap_int16(gps.vel_m_s * 360.0f);
+				bst_gps.payload.heading = swap_int16(gps.cog_rad * 18000.0f / M_PI_F);
+				bst_gps.payload.sats = gps.satellites_used;
+				send_packet(bst_gps);
+			}
+		}
+
+		work_queue(LPWORK, &_work, BST::cycle_trampoline, this, _interval);
+	}
+}
+
+uint8_t BST::crc8(uint8_t *data, size_t len)
+{
+	uint8_t crc = 0x00;
+
+	while (len--) {
+		crc ^= *data++;
+
+		for (int i = 0; i < 8; i++) {
+			crc = crc & 0x80 ? (crc << 1) ^ 0xD5 : crc << 1;
+		}
+	}
+
+	return crc;
+}
+
+}
+}
+
+using namespace px4::bst;
+
+int bst_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		errx(1, "missing command\n%s", commandline_usage);
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (g_bst) {
+			warnx("already running");
+			exit(0);
+		}
+
+		g_bst = new BST(PX4_I2C_BUS_EXPANSION);
+
+		if (g_bst != nullptr && OK != g_bst->init()) {
+			delete g_bst;
+			g_bst = nullptr;
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		if (!g_bst) {
+			warnx("not running");
+			exit(0);
+		}
+
+		delete g_bst;
+		g_bst = nullptr;
+		warnx("stopped");
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (g_bst) {
+			warnx("is running");
+
+		} else {
+			warnx("is not running");
+		}
+
+		exit(0);
+	}
+
+	errx(1, "unrecognized command\n%s", commandline_usage);
+}
@@ -33,6 +33,7 @@
 px4_add_module(
 	MODULE drivers__camera_trigger
 	MAIN camera_trigger
+	STACK 1200
 	COMPILE_FLAGS
 		-Os
 	SRCS
@@ -96,7 +96,7 @@ Device::dev_write(unsigned offset, void *data, unsigned count)
 }

 int
-Device::dev_ioctl(unsigned operation, unsigned &arg)
+Device::dev_ioctl(unsigned operation, unsigned arg)
 {
 	switch (operation) {
 	case DEVIOCGDEVICEID:
@@ -45,6 +45,10 @@
 #include <stdio.h>
 #include <string.h>

+#include "DevMgr.hpp"
+
+using namespace DriverFramework;
+
 namespace device
 {

@@ -363,7 +367,6 @@ VDev::ioctl(file_t *filep, int cmd, unsigned long arg)

 	case DEVIOCGDEVICEID:
 		ret = (int)_device_id.devid;
-		PX4_INFO("IOCTL DEVIOCGDEVICEID %d", ret);
 		break;

 	default:
@@ -533,7 +536,7 @@ VDev *VDev::getDev(const char *path)
 void VDev::showDevices()
 {
 	int i = 0;
-	PX4_INFO("Devices:");
+	PX4_INFO("PX4 Devices:");

 	pthread_mutex_lock(&devmutex);

@@ -544,6 +547,23 @@ void VDev::showDevices()
 	}

 	pthread_mutex_unlock(&devmutex);
+
+#ifndef __PX4_UNIT_TESTS
+	PX4_INFO("DF Devices:");
+	const char *dev_path;
+	unsigned int index = 0;
+	i = 0;
+
+	do {
+		// Each look increments index and returns -1 if end reached
+		i = DevMgr::getNextDeviceName(index, &dev_path);
+
+		if (i == 0) {
+			PX4_INFO("   %s", dev_path);
+		}
+	} while (i == 0);
+
+#endif
 }

 void VDev::showTopics()
@@ -599,4 +619,4 @@ const char *VDev::devList(unsigned int *next)
 	return NULL;
 }

-} // namespace device
+} // namespace device
@@ -128,7 +128,7 @@ public:
 	 * @param arg           An argument to the operation.
 	 * @return              Negative errno on error, OK or positive value on success.
 	 */
-	virtual int     dev_ioctl(unsigned operation, unsigned &arg);
+	virtual int     dev_ioctl(unsigned operation, unsigned arg);

 	/*
 	  device bus types for DEVID
@@ -181,7 +181,7 @@ protected:
 	 */
 	void		lock()
 	{
-		DEVICE_DEBUG("lock");
+		//DEVICE_DEBUG("lock");
 		do {} while (px4_sem_wait(&_lock) != 0);
 	}

@@ -190,7 +190,7 @@ protected:
 	 */
 	void		unlock()
 	{
-		DEVICE_DEBUG("unlock");
+		//DEVICE_DEBUG("unlock");
 		px4_sem_post(&_lock);
 	}

@@ -56,13 +56,6 @@ pthread_mutex_t filemutex = PTHREAD_MUTEX_INITIALIZER;

 extern "C" {

-	static void timer_cb(void *data)
-	{
-		px4_sem_t *p_sem = (px4_sem_t *)data;
-		px4_sem_post(p_sem);
-		PX4_DEBUG("timer_handler: Timer expired");
-	}
-
 #define PX4_MAX_FD 200
 	static device::file_t *filemap[PX4_MAX_FD] = {};

@@ -106,7 +99,7 @@ extern "C" {
 		    strncmp(path, "/dev/", 5) != 0) {
 			va_list p;
 			va_start(p, flags);
-			mode = va_arg(p, mode_t);
+			mode = va_arg(p, int);
 			va_end(p);

 			// Create the file
@@ -240,15 +233,30 @@ extern "C" {

 	int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
 	{
+		if (nfds == 0) {
+			PX4_WARN("px4_poll with no fds");
+			return -1;
+		}
+
 		px4_sem_t sem;
 		int count = 0;
 		int ret = -1;
 		unsigned int i;

+		const unsigned NAMELEN = 32;
+		char thread_name[NAMELEN] = {};
+		int nret = pthread_getname_np(pthread_self(), thread_name, NAMELEN);
+
+		if (nret || thread_name[0] == 0) {
+			PX4_WARN("failed getting thread name");
+		}
+
 		PX4_DEBUG("Called px4_poll timeout = %d", timeout);
 		px4_sem_init(&sem, 0, 0);

-		// For each fd
+		// Go through all fds and check them for a pollable state
+		bool fd_pollable = false;
+
 		for (i = 0; i < nfds; ++i) {
 			fds[i].sem     = &sem;
 			fds[i].revents = 0;
@@ -258,42 +266,71 @@ extern "C" {

 			// If fd is valid
 			if (dev) {
-				PX4_DEBUG("px4_poll: VDev->poll(setup) %d", fds[i].fd);
+				PX4_DEBUG("%s: px4_poll: VDev->poll(setup) %d", thread_name, fds[i].fd);
 				ret = dev->poll(filemap[fds[i].fd], &fds[i], true);

 				if (ret < 0) {
+					PX4_WARN("%s: px4_poll() error: %s",
+						 thread_name, strerror(errno));
 					break;
 				}
+
+				if (ret >= 0) {
+					fd_pollable = true;
+				}
 			}
 		}

-		if (ret >= 0) {
+		// If any FD can be polled, lock the semaphore and
+		// check for new data
+		if (fd_pollable) {
 			if (timeout > 0) {
-				// Use a work queue task
-				work_s _hpwork;

-				hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)&sem, 1000 * timeout);
-				px4_sem_wait(&sem);
+				// Get the current time
+				struct timespec ts;
+				px4_clock_gettime(CLOCK_REALTIME, &ts);

-				// Make sure timer thread is killed before sem goes
-				// out of scope
-				hrt_work_cancel(&_hpwork);
+				// Calculate an absolute time in the future
+				const unsigned billion = (1000 * 1000 * 1000);
+				unsigned tdiff = timeout;
+				uint64_t nsecs = ts.tv_nsec + (tdiff * 1000 * 1000);
+				ts.tv_sec += nsecs / billion;
+				nsecs -= (nsecs / billion) * billion;
+				ts.tv_nsec = nsecs;
+
+				// Execute a blocking wait for that time in the future
+				errno = 0;
+				ret = px4_sem_timedwait(&sem, &ts);
+#ifndef __PX4_DARWIN
+				ret = errno;
+#endif
+
+				// Ensure ret is negative on failure
+				if (ret > 0) {
+					ret = -ret;
+				}
+
+				if (ret && ret != -ETIMEDOUT) {
+					PX4_WARN("%s: px4_poll() sem error", thread_name);
+				}

 			} else if (timeout < 0) {
 				px4_sem_wait(&sem);
 			}

-			// For each fd
+			// We have waited now (or not, depending on timeout),
+			// go through all fds and count how many have data
 			for (i = 0; i < nfds; ++i) {

 				VDev *dev = get_vdev(fds[i].fd);

 				// If fd is valid
 				if (dev) {
-					PX4_DEBUG("px4_poll: VDev->poll(teardown) %d", fds[i].fd);
+					PX4_DEBUG("%s: px4_poll: VDev->poll(teardown) %d", thread_name, fds[i].fd);
 					ret = dev->poll(filemap[fds[i].fd], &fds[i], false);

 					if (ret < 0) {
+						PX4_WARN("%s: px4_poll() 2nd poll fail", thread_name);
 						break;
 					}

@@ -306,7 +343,9 @@ extern "C" {

 		px4_sem_destroy(&sem);

-		return count;
+		// Return the positive count if present,
+		// return the negative error number if failed
+		return (count) ? count : ret;
 	}

 	int px4_fsync(int fd)
@@ -46,24 +46,7 @@
 #include "drv_sensor.h"
 #include "drv_orb_dev.h"

-/*
- * ioctl() definitions
- */
-
-#define _DEVICEIOCBASE		(0x100)
-#define _DEVICEIOC(_n)		(_PX4_IOC(_DEVICEIOCBASE, _n))
-
-/** ask device to stop publishing */
-#define DEVIOCSPUBBLOCK	_DEVICEIOC(0)
-
-/** check publication block status */
-#define DEVIOCGPUBBLOCK	_DEVICEIOC(1)
-
-/**
- * Return device ID, to enable matching of configuration parameters
- * (such as compass offsets) to specific sensors
- */
-#define DEVIOCGDEVICEID	_DEVICEIOC(2)
+#include "DevIOCTL.h"

 #ifdef __PX4_POSIX

@@ -94,6 +94,29 @@

 #endif

+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V4
+/*
+ * PX4FMUv3 GPIO numbers.
+ *
+ * There are no alternate functions on this board.
+ */
+# define GPIO_SERVO_1			(1<<0)		/**< servo 1 output */
+# define GPIO_SERVO_2			(1<<1)		/**< servo 2 output */
+# define GPIO_SERVO_3			(1<<2)		/**< servo 3 output */
+# define GPIO_SERVO_4			(1<<3)		/**< servo 4 output */
+# define GPIO_SERVO_5			(1<<4)		/**< servo 5 output */
+# define GPIO_SERVO_6			(1<<5)		/**< servo 6 output */
+
+# define GPIO_3V3_SENSORS_EN	(1<<7)		/**< PE3 - VDD_3V3_SENSORS_EN */
+# define GPIO_BRICK_VALID		(1<<8)		/**< PB5 - !VDD_BRICK_VALID */
+
+/**
+ * Device paths for things that support the GPIO ioctl protocol.
+ */
+# define PX4FMU_DEVICE_PATH	"/dev/px4fmu"
+
+#endif
+
 #ifdef CONFIG_ARCH_BOARD_AEROCORE
 /*
 * AeroCore GPIO numbers and configuration.
@@ -121,7 +144,7 @@
 #if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2)  && \
 	!defined(CONFIG_ARCH_BOARD_PX4FMU_V1) && !defined(CONFIG_ARCH_BOARD_PX4FMU_V2) && \
 	!defined(CONFIG_ARCH_BOARD_AEROCORE) && !defined(CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY) && \
-	!defined(CONFIG_ARCH_BOARD_SITL)
+	!defined(CONFIG_ARCH_BOARD_PX4FMU_V4) && !defined(CONFIG_ARCH_BOARD_SITL)
 # error No CONFIG_ARCH_BOARD_xxxx set
 #endif
 /*
@@ -43,6 +43,7 @@
 #include <stdint.h>
 #include <sys/ioctl.h>

+#define LED_BASE_DEVICE_PATH		"/dev/led"
 #define LED0_DEVICE_PATH		"/dev/led0"

 #define _LED_BASE		0x2800
@@ -52,6 +53,8 @@
 #define LED_RED			1	/* some boards have red rather than amber */
 #define LED_BLUE		0
 #define LED_SAFETY		2
+#define LED_GREEN		3
+

 #define LED_ON			_PX4_IOC(_LED_BASE, 0)
 #define LED_OFF			_PX4_IOC(_LED_BASE, 1)
@@ -76,7 +76,12 @@ __BEGIN_DECLS
 /**
 * Lowest minimum PWM in us
 */
-#define PWM_LOWEST_MIN 900
+#define PWM_LOWEST_MIN 90
+
+/**
+ * Default value for a shutdown motor
+ */
+#define PWM_MOTOR_OFF	800

 /**
 * Default minimum PWM in us
@@ -101,7 +106,7 @@ __BEGIN_DECLS
 /**
 * Lowest PWM allowed as the maximum PWM
 */
-#define PWM_LOWEST_MAX 950
+#define PWM_LOWEST_MAX 200

 /**
 * Do not output a channel with this value
@@ -42,6 +42,8 @@
 #include <stdint.h>
 #include <sys/ioctl.h>

+#define RGBLED_BASE_DEVICE_PATH "/dev/rgbled"
+
 /* more devices will be 1, 2, etc */
 #define RGBLED0_DEVICE_PATH "/dev/rgbled0"

@@ -37,6 +37,7 @@ px4_add_module(
 		-Os
 	SRCS
 		gimbal.cpp
+		gimbal_params.c
 	DEPENDS
 		platforms__common
 	)
--- a/Show More
+++ b/Show More