Merge pull request #1 from PX4/master

From original

98 files changed, 4531 insertions, 939 deletions

diff --git a/.gitignore b/.gitignore
index 611325444..0e553fa36 100644
--- a/.gitignore
+++ b/.gitignore
@@ -46,3 +46,5 @@ Firmware.zip
 unittests/build
 *.generated.h
 .vagrant
+*.pretty
+
diff --git a/.travis.yml b/.travis.yml
index a994d3471..d659bf650 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -37,6 +37,7 @@ env:
 # Email address: $PX4_EMAIL
     - secure: "ei3hKAw6Pk+vEkQBI5Y2Ak74BRAaXcK2UHVnVadviBHI4EVPwn1YGP6A4Y0wnLe4U7ETTl0UiijRoVxyDW0Mq896Pv0siw02amNpjSZZYu+RfN1+//MChB48OxsLDirUdHVrULhl/bOARM02h2Bg28jDE2g7IqmJwg3em3oMbjU="
     - PX4_REPORT=report.txt
+    - BUILD_URI=https://px4-travis.s3.amazonaws.com/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/Firmware.zip
 
 script:
   - arm-none-eabi-gcc --version
@@ -58,11 +59,14 @@ after_script:
 # upload newest build for this branch with s3 index
   - ./CI-Tools/s3cmd-put Images/px4*.px4 CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/
 # archive newest build by date with s3 index
-  - ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/`date "+%Y-%m-%d"`-$TRAVIS_BUILD_ID/
+  - ./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/
 # upload top level index.html and timestamp.html
   - ./CI-Tools/s3cmd-put CI-Tools/index.html index.html
   - ./CI-Tools/s3cmd-put CI-Tools/timestamp.html timestamp.html
+  - echo ""
+  - echo "Binaries have been posted to:"
+  - echo https://px4-travis.s3.amazonaws.com/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/Firmware.zip
 
 deploy:
   provider: releases
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0c81607b0..457a0bfa7 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -3,6 +3,7 @@ project(px4)
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
 add_definitions(-D__PX4_ROS)
 add_definitions(-D__EXPORT=)
+add_definitions(-DMAVLINK_DIALECT=common)
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
@@ -11,11 +12,14 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
   std_msgs
+  geometry_msgs
   message_generation
   cmake_modules
   gazebo_msgs
   sensor_msgs
   mav_msgs
+  libmavconn
+  tf
 )
 find_package(Eigen REQUIRED)
 
@@ -74,6 +78,8 @@ add_message_files(
   position_setpoint_triplet.msg
   vehicle_local_position_setpoint.msg
   vehicle_global_velocity_setpoint.msg
+  offboard_control_mode.msg
+  vehicle_force_setpoint.msg
 )
 
 ## Generate services in the 'srv' folder
@@ -109,7 +115,7 @@ generate_messages(
 catkin_package(
   INCLUDE_DIRS src/include
   LIBRARIES px4
-  CATKIN_DEPENDS message_runtime roscpp rospy std_msgs
+  CATKIN_DEPENDS message_runtime roscpp rospy std_msgs libmavconn
   DEPENDS system_lib
 )
 
@@ -128,6 +134,7 @@ include_directories(
   src/
   src/lib
   ${EIGEN_INCLUDE_DIRS}
+  integrationtests
 )
 
 ## generate multiplatform wrapper headers
@@ -231,15 +238,42 @@ target_link_libraries(mc_mixer
   px4
 )
 
-## Commander
+## Commander dummy
 add_executable(commander
 	src/platforms/ros/nodes/commander/commander.cpp)
-add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp)
+add_dependencies(commander ${PROJECT_NAME}_generate_messages_cpp_cpp)
 target_link_libraries(commander
   ${catkin_LIBRARIES}
   px4
 )
 
+## Mavlink dummy
+add_executable(mavlink
+	src/platforms/ros/nodes/mavlink/mavlink.cpp)
+add_dependencies(mavlink ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(mavlink
+  ${catkin_LIBRARIES}
+  px4
+)
+
+## Offboard Position Setpoint Demo
+add_executable(demo_offboard_position_setpoints
+	src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp)
+add_dependencies(demo_offboard_position_setpoints ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(demo_offboard_position_setpoints
+  ${catkin_LIBRARIES}
+  px4
+)
+
+## Offboard Attitude Setpoint Demo
+add_executable(demo_offboard_attitude_setpoints
+	src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp)
+add_dependencies(demo_offboard_attitude_setpoints ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(demo_offboard_attitude_setpoints
+  ${catkin_LIBRARIES}
+  px4
+)
+
 #############
 ## Install ##
 #############
@@ -287,3 +321,11 @@ install(TARGETS ${PROJECT_NAME}
 
 ## Add folders to be run by python nosetests
 # catkin_add_nosetests(test)
+
+if(CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+  add_rostest(integrationtests/demo_tests/direct_tests.launch)
+  add_rostest(integrationtests/demo_tests/mavros_tests.launch)
+endif()
+
+
diff --git a/Makefile b/Makefile
index ab8c614e1..201187e02 100644
--- a/Makefile
+++ b/Makefile
@@ -124,7 +124,7 @@ $(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4
 .PHONY: $(FIRMWARES)
 $(BUILD_DIR)%.build/firmware.px4: config   = $(patsubst $(BUILD_DIR)%.build/firmware.px4,%,$@)
 $(BUILD_DIR)%.build/firmware.px4: work_dir = $(BUILD_DIR)$(config).build/
-$(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4:	checksubmodules generateuorbtopicheaders
+$(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4:	generateuorbtopicheaders checksubmodules
 	@$(ECHO) %%%%
 	@$(ECHO) %%%% Building $(config) in $(work_dir)
 	@$(ECHO) %%%%
@@ -236,7 +236,7 @@ GENMSG_PYTHONPATH = $(PX4_BASE)Tools/genmsg/src
 GENCPP_PYTHONPATH = $(PX4_BASE)Tools/gencpp/src
 
 .PHONY: generateuorbtopicheaders
-generateuorbtopicheaders:
+generateuorbtopicheaders: checksubmodules
 	@$(ECHO) "Generating uORB topic headers"
 	$(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH):$(PYTHONPATH) $(PYTHON) \
 		$(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \
@@ -262,6 +262,10 @@ testbuild:
 tests:	generateuorbtopicheaders
 	$(Q) (mkdir -p $(PX4_BASE)/unittests/build && cd $(PX4_BASE)/unittests/build && cmake .. && $(MAKE) unittests)
 
+.PHONY: format check_format
+check_format:
+	$(Q) (./Tools/check_code_style.sh | sort -n)
+
 #
 # Cleanup targets.  'clean' should remove all built products and force
 # a complete re-compilation, 'distclean' should remove everything
diff --git a/README.md b/README.md
index 9441b341d..34c2e880e 100644
--- a/README.md
+++ b/README.md
@@ -13,7 +13,7 @@ This repository contains the PX4 Flight Core, with the main applications located
   * [Fixed wing](http://px4.io/platforms/planes/start)
   * [VTOL](http://px4.io/platforms/vtol/start)
 *   Binaries (always up-to-date from master):
-  * [Downloads](http://px4.io/downloads)
+  * [Downloads](http://px4.io/firmware/downloads)
 *   Mailing list: [Google Groups](http://groups.google.com/group/px4users)
 
 ### Users ###
diff --git a/ROMFS/px4fmu_common/init.d/50001_rover b/ROMFS/px4fmu_common/init.d/50001_rover
index e9df3904b..c2f94705c 100644
--- a/ROMFS/px4fmu_common/init.d/50001_rover
+++ b/ROMFS/px4fmu_common/init.d/50001_rover
@@ -3,8 +3,8 @@
 # Generic rover
 #
 
+#load some defaults e.g. PWM values
 sh /etc/init.d/rc.rover_defaults
 
+#choose a mixer, for rover control we need a plain passthrough to the servos
 set MIXER IO_pass
-
-set PWM_OUT 1234
diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps
index 2ecc104df..6517e026a 100644
--- a/ROMFS/px4fmu_common/init.d/rc.mc_apps
+++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps
@@ -4,9 +4,15 @@
 # att & pos estimator, att & pos control.
 #
 
-attitude_estimator_ekf start
-#ekf_att_pos_estimator start
-position_estimator_inav start
+# previously (2014) the system was relying on
+# INAV, which defaults to 0 now. 
+if param compare INAV_ENABLED 1
+then
+	attitude_estimator_ekf start
+	position_estimator_inav start
+else
+	ekf_att_pos_estimator start
+fi
 
 if mc_att_control start
 then
diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_defaults b/ROMFS/px4fmu_common/init.d/rc.rover_defaults
index fc68472a6..23bf47df1 100644
--- a/ROMFS/px4fmu_common/init.d/rc.rover_defaults
+++ b/ROMFS/px4fmu_common/init.d/rc.rover_defaults
@@ -2,12 +2,27 @@
 
 set VEHICLE_TYPE rover
 
-if [ $AUTOCNF == yes ]
-then
-	# param set MC_ROLL_P 7.0
-fi
+# This section can be enabled once tuning parameters for this particular
+# rover model are known. It allows to configure default gains via the GUI
+#if [ $AUTOCNF == yes ]
+#then
+#	# param set MC_ROLL_P 7.0
+#fi
 
+# PWM Hz - 50 Hz is the normal rate in RC cars, higher rates
+# may damage analog servos.
 set PWM_RATE 50
-set PWM_DISARMED 1100
-set PWM_MIN 1100
-set PWM_MAX 1900
+
+# PWM default value for "disarmed" mode
+# this centers the steering and throttle, which means no motion
+# for a rover
+set PWM_DISARMED 1500
+
+# PWM range
+set PWM_MIN 1200
+set PWM_MAX 1800
+
+# Enable servo output on pins 3 and 4 (steering and thrust)
+# but also include 1+2 as they form together one output group
+# and need to be set together.
+set PWM_OUT 1234
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 580043a1d..1d21d7772 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -198,8 +198,17 @@ then
 
 			tone_alarm MLL32CP8MB
 
-			px4io start
-			px4io safety_on
+			if px4io start
+			then
+				# try to safe px4 io so motor outputs dont go crazy
+				if px4io safety_on
+				then
+					# success! no-op
+				else
+					# px4io did not respond to the safety command
+					px4io stop
+				fi
+			fi
 
 			if px4io forceupdate 14662 ${IO_FILE}
 			then
@@ -432,7 +441,7 @@ then
 	then
 		if param compare SYS_COMPANION 921600
 		then
-			mavlink start -d /dev/ttyS2 -b 921600 -m onboard
+			mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 20000
 		fi
 	fi
 
diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh
new file mode 100755
index 000000000..8d1ab6363
--- /dev/null
+++ b/Tools/check_code_style.sh
@@ -0,0 +1,31 @@
+#!/usr/bin/env bash
+set -eu
+failed=0
+for fn in $(find . -path './src/lib/uavcan' -prune -o \
+                   -path './src/lib/mathlib/CMSIS' -prune -o \
+                   -path './src/modules/attitude_estimator_ekf/codegen/' -prune -o \
+                   -path './NuttX' -prune -o \
+                   -path './Build' -prune -o \
+                   -path './mavlink' -prune -o \
+                   -path './unittests/gtest' -prune -o \
+                   -path './unittests/build' -prune -o \
+                   -name '*.c' -o -name '*.cpp' -o -name '*.hpp' -o -name '*.h' -type f); do
+  if [ -f "$fn" ];
+  then
+    ./Tools/fix_code_style.sh --quiet < $fn > $fn.pretty
+    diffsize=$(diff -y --suppress-common-lines $fn $fn.pretty | wc -l)
+    rm -f $fn.pretty
+    if [ $diffsize -ne 0 ]; then
+      failed=1
+      echo $diffsize $fn
+    fi
+  fi
+done
+
+if [ $failed -eq 0 ]; then
+    echo "Format checks passed"
+    exit 0
+else
+    echo "Format checks failed; please run ./Tools/fix_code_style.sh on each file"
+    exit 1
+fi
diff --git a/Tools/fix_code_style.sh b/Tools/fix_code_style.sh
index 5995d428e..e73a5a8af 100755
--- a/Tools/fix_code_style.sh
+++ b/Tools/fix_code_style.sh
@@ -18,4 +18,5 @@ astyle \
     --exclude=EASTL		\
     --add-brackets		\
     --max-code-length=120	\
+    --preserve-date             \
     $*
diff --git a/Tools/pre-commit b/Tools/pre-commit
new file mode 100755
index 000000000..13cd4aadd
--- /dev/null
+++ b/Tools/pre-commit
@@ -0,0 +1,26 @@
+#!/bin/sh
+if git rev-parse --verify HEAD >/dev/null 2>&1
+then
+  against=HEAD
+else
+  # Initial commit: diff against an empty tree object
+  against=4b825dc642cb6eb9a060e54bf8d69288fbee4904
+fi
+
+# Redirect output to stderr.
+exec 1>&2
+
+CHANGED_FILES=`git diff --cached --name-only --diff-filter=ACM $against | grep '\.c\|\.cpp\|\.h\|\.hpp'`
+FAILED=0
+if [ ! -z "$CHANGED_FILES" -a "$CHANGED_FILES" != " " ]; then
+  for FILE in $CHANGED_FILES; do
+    ./Tools/fix_code_style.sh --quiet < $FILE > $FILE.pretty
+    diff -u $FILE $FILE.pretty || FAILED=1
+    rm -f $FILE.pretty
+    if [ $FAILED -ne 0 ]; then
+      echo "There are code formatting errors. Please fix them by running ./Tools/fix_code_style.sh $FILE"
+      exit $FAILED
+    fi
+  done
+fi
+exit 0
diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py
index 95a3d4046..859c821e5 100755
--- a/Tools/px_uploader.py
+++ b/Tools/px_uploader.py
@@ -169,8 +169,8 @@ class uploader(object):
         INFO_BOARD_REV  = b'\x03'        # board revision
         INFO_FLASH_SIZE = b'\x04'        # max firmware size in bytes
 
-        PROG_MULTI_MAX  = 60            # protocol max is 255, must be multiple of 4
-        READ_MULTI_MAX  = 60            # protocol max is 255, something overflows with >= 64
+        PROG_MULTI_MAX  = 252            # protocol max is 255, must be multiple of 4
+        READ_MULTI_MAX  = 252            # protocol max is 255
         
         NSH_INIT        = bytearray(b'\x0d\x0d\x0d')
         NSH_REBOOT_BL   = b"reboot -b\n"
@@ -416,7 +416,7 @@ class uploader(object):
         def upload(self, fw):
                 # Make sure we are doing the right thing
                 if self.board_type != fw.property('board_id'):
-                        raise RuntimeError("Firmware not suitable for this board")
+                        raise IOError("Firmware not suitable for this board")
                 if self.fw_maxsize < fw.property('image_size'):
                         raise RuntimeError("Firmware image is too large for this board")
 
@@ -501,75 +501,87 @@ print("Loaded firmware for %x,%x, size: %d bytes, waiting for the bootloader..."
 print("If the board does not respond within 1-2 seconds, unplug and re-plug the USB connector.")
 
 # Spin waiting for a device to show up
-while True:
-        portlist = []
-        patterns = args.port.split(",")
-        # on unix-like platforms use glob to support wildcard ports. This allows
-        # the use of /dev/serial/by-id/usb-3D_Robotics on Linux, which prevents the upload from
-        # causing modem hangups etc
-        if "linux" in _platform or "darwin" in _platform:
-                import glob
-                for pattern in patterns:
-                        portlist += glob.glob(pattern)
-        else:
-                portlist = patterns
-
-        for port in portlist:
-
-                #print("Trying %s" % port)
-
-                # create an uploader attached to the port
-                try:
-                        if "linux" in _platform:
-                        # Linux, don't open Mac OS and Win ports
-                                if not "COM" in port and not "tty.usb" in port:
-                                        up = uploader(port, args.baud)
-                        elif "darwin" in _platform:
-                                # OS X, don't open Windows and Linux ports
-                                if not "COM" in port and not "ACM" in port:
-                                        up = uploader(port, args.baud)
-                        elif "win" in _platform:
-                                # Windows, don't open POSIX ports
-                                if not "/" in port:
-                                        up = uploader(port, args.baud)
-                except Exception:
-                        # open failed, rate-limit our attempts
-                        time.sleep(0.05)
-
-                        # and loop to the next port
-                        continue
-
-                # port is open, try talking to it
-                try:
-                        # identify the bootloader
-                        up.identify()
-                        print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port))
+try:
+    while True:
+            portlist = []
+            patterns = args.port.split(",")
+            # on unix-like platforms use glob to support wildcard ports. This allows
+            # the use of /dev/serial/by-id/usb-3D_Robotics on Linux, which prevents the upload from
+            # causing modem hangups etc
+            if "linux" in _platform or "darwin" in _platform:
+                    import glob
+                    for pattern in patterns:
+                            portlist += glob.glob(pattern)
+            else:
+                    portlist = patterns
+
+            for port in portlist:
+
+                    #print("Trying %s" % port)
+
+                    # create an uploader attached to the port
+                    try:
+                            if "linux" in _platform:
+                            # Linux, don't open Mac OS and Win ports
+                                    if not "COM" in port and not "tty.usb" in port:
+                                            up = uploader(port, args.baud)
+                            elif "darwin" in _platform:
+                                    # OS X, don't open Windows and Linux ports
+                                    if not "COM" in port and not "ACM" in port:
+                                            up = uploader(port, args.baud)
+                            elif "win" in _platform:
+                                    # Windows, don't open POSIX ports
+                                    if not "/" in port:
+                                            up = uploader(port, args.baud)
+                    except Exception:
+                            # open failed, rate-limit our attempts
+                            time.sleep(0.05)
+
+                            # and loop to the next port
+                            continue
+
+                    # port is open, try talking to it
+                    try:
+                            # identify the bootloader
+                            up.identify()
+                            print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port))
 
-                except Exception:
-                        # most probably a timeout talking to the port, no bootloader, try to reboot the board
-                        print("attempting reboot on %s..." % port)
-                        print("if the board does not respond, unplug and re-plug the USB connector.")
-                        up.send_reboot()
+                    except Exception:
+                            # most probably a timeout talking to the port, no bootloader, try to reboot the board
+                            print("attempting reboot on %s..." % port)
+                            print("if the board does not respond, unplug and re-plug the USB connector.")
+                            up.send_reboot()
 
-                        # wait for the reboot, without we might run into Serial I/O Error 5 
-                        time.sleep(0.5)
+                            # wait for the reboot, without we might run into Serial I/O Error 5
+                            time.sleep(0.5)
 
-                        # always close the port
-                        up.close()
-                        continue
+                            # always close the port
+                            up.close()
+                            continue
 
-                try:
-                        # ok, we have a bootloader, try flashing it
-                        up.upload(fw)
+                    try:
+                            # ok, we have a bootloader, try flashing it
+                            up.upload(fw)
+
+                    except RuntimeError as ex:
+                            # print the error
+                            print("\nERROR: %s" % ex.args)
+
+                    except IOError as e:
+                            up.close();
+                            continue
 
-                except RuntimeError as ex:
+                    finally:
+                            # always close the port
+                            up.close()
 
-                        # print the error
-                        print("\nERROR: %s" % ex.args)
+                    # we could loop here if we wanted to wait for more boards...
+                    sys.exit(0)
 
-                finally:
-                        # always close the port
-                        up.close()
+            # Delay retries to < 20 Hz to prevent spin-lock from hogging the CPU
+            time.sleep(0.05)
 
-                # we could loop here if we wanted to wait for more boards...
-                sys.exit(0)
+# CTRL+C aborts the upload/spin-lock by interrupt mechanics
+except KeyboardInterrupt:
+    print("\n Upload aborted by user.")
+    sys.exit(0)
diff --git a/integrationtests/demo_tests/direct_manual_input_test.py b/integrationtests/demo_tests/direct_manual_input_test.py
new file mode 100755
index 000000000..6d115316b
--- /dev/null
+++ b/integrationtests/demo_tests/direct_manual_input_test.py
@@ -0,0 +1,87 @@
+#!/usr/bin/env python
+#***************************************************************************
+#
+#   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.
+#
+#***************************************************************************/
+
+#
+# @author Andreas Antener <andreas@uaventure.com>
+#
+PKG = 'px4'
+
+import sys
+import unittest
+import rospy
+
+from px4.msg import actuator_armed
+from px4.msg import vehicle_control_mode
+from manual_input import ManualInput
+
+#
+# Tests if commander reacts to manual input and sets control flags accordingly
+#
+class ManualInputTest(unittest.TestCase):
+
+    #
+    # General callback functions used in tests
+    #
+    def actuator_armed_callback(self, data):
+        self.actuatorStatus = data
+
+    def vehicle_control_mode_callback(self, data):
+        self.controlMode = data
+    
+    #
+    # Test arming
+    #
+    def test_manual_input(self):
+        rospy.init_node('test_node', anonymous=True)
+        rospy.Subscriber('px4_multicopter/actuator_armed', actuator_armed, self.actuator_armed_callback)
+        rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback)
+
+        man = ManualInput()
+
+        # Test arming
+        man.arm()
+        self.assertEquals(self.actuatorStatus.armed, True, "did not arm")
+
+        # Test posctl
+        man.posctl()
+        self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set")
+
+        # Test offboard
+        man.offboard()
+        self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set")
+    
+
+if __name__ == '__main__':
+    import rostest
+    rostest.rosrun(PKG, 'direct_manual_input_test', ManualInputTest)
diff --git a/integrationtests/demo_tests/direct_offboard_posctl_test.py b/integrationtests/demo_tests/direct_offboard_posctl_test.py
new file mode 100755
index 000000000..e09550bbc
--- /dev/null
+++ b/integrationtests/demo_tests/direct_offboard_posctl_test.py
@@ -0,0 +1,167 @@
+#!/usr/bin/env python
+#***************************************************************************
+#
+#   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.
+#
+#***************************************************************************/
+
+#
+# @author Andreas Antener <andreas@uaventure.com>
+#
+PKG = 'px4'
+
+import sys
+import unittest
+import rospy
+
+from numpy import linalg
+import numpy as np
+
+from px4.msg import vehicle_local_position
+from px4.msg import vehicle_control_mode
+from px4.msg import actuator_armed
+from px4.msg import position_setpoint_triplet
+from px4.msg import position_setpoint
+from sensor_msgs.msg import Joy
+from std_msgs.msg import Header
+
+from manual_input import ManualInput
+from flight_path_assertion import FlightPathAssertion
+
+#
+# Tests flying a path in offboard control by directly sending setpoints
+# to the position controller (position_setpoint_triplet).
+#
+# For the test to be successful it needs to stay on the predefined path
+# and reach all setpoints in a certain time.
+#
+class DirectOffboardPosctlTest(unittest.TestCase):
+
+    def setUp(self):
+        rospy.init_node('test_node', anonymous=True)
+        rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback)
+        rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback)
+        self.pubSpt = rospy.Publisher('px4_multicopter/position_setpoint_triplet', position_setpoint_triplet, queue_size=10)
+        self.rate = rospy.Rate(10) # 10hz
+
+    def tearDown(self):
+        if (self.fpa):
+            self.fpa.stop()
+
+    #
+    # General callback functions used in tests
+    #
+    def position_callback(self, data):
+        self.hasPos = True
+        self.localPosition = data
+
+    def vehicle_control_mode_callback(self, data):
+        self.controlMode = data
+
+
+    #
+    # Helper methods
+    #
+    def is_at_position(self, x, y, z, offset):
+        rospy.logdebug("current position %f, %f, %f" % (self.localPosition.x, self.localPosition.y, self.localPosition.z))
+        desired = np.array((x, y, z))
+        pos = np.array((self.localPosition.x, self.localPosition.y, self.localPosition.z))
+        return linalg.norm(desired - pos) < offset
+
+    def reach_position(self, x, y, z, timeout):
+        # set a position setpoint
+        pos = position_setpoint()
+        pos.valid = True
+        pos.x = x
+        pos.y = y
+        pos.z = z
+        pos.position_valid = True
+        stp = position_setpoint_triplet()
+        stp.current = pos
+        self.pubSpt.publish(stp)
+
+        # does it reach the position in X seconds?
+        count = 0
+        while(count < timeout):
+            if(self.is_at_position(pos.x, pos.y, pos.z, 0.5)):
+                break
+            count = count + 1
+            self.rate.sleep()
+
+        self.assertTrue(count < timeout, "took too long to get to position")
+
+    #
+    # Test offboard position control
+    #
+    def test_posctl(self):
+        manIn = ManualInput()
+
+        # arm and go into offboard
+        manIn.arm()
+        manIn.offboard()
+        self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set")
+        self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set")
+        self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set")
+
+        # prepare flight path
+        positions = (
+            (0,0,0),
+            (2,2,-2),
+            (2,-2,-2),
+            (-2,-2,-2),
+            (2,2,-2))
+
+        # flight path assertion
+        self.fpa = FlightPathAssertion(positions, 1, 0)
+        self.fpa.start()
+
+        for i in range(0, len(positions)):
+            self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120)
+            self.assertFalse(self.fpa.failed, "breached flight path tunnel (%d)" % i)
+        
+        # does it hold the position for Y seconds?
+        positionHeld = True
+        count = 0
+        timeout = 50
+        while(count < timeout):
+            if(not self.is_at_position(2, 2, -2, 0.5)):
+                positionHeld = False
+                break
+            count = count + 1
+            self.rate.sleep()
+
+        self.assertTrue(count == timeout, "position could not be held")
+        self.fpa.stop()
+    
+
+if __name__ == '__main__':
+    import rostest
+    rostest.rosrun(PKG, 'direct_offboard_posctl_test', DirectOffboardPosctlTest)
+    #unittest.main()
diff --git a/integrationtests/demo_tests/direct_tests.launch b/integrationtests/demo_tests/direct_tests.launch
new file mode 100644
index 000000000..1a7d843fd
--- /dev/null
+++ b/integrationtests/demo_tests/direct_tests.launch
@@ -0,0 +1,18 @@
+<launch>
+    <arg name="headless" default="true"/>
+    <arg name="gui" default="false"/>
+    <arg name="enable_logging" default="false"/>
+    <arg name="enable_ground_truth" default="false"/>
+    <arg name="log_file" default="iris"/>
+
+    <include file="$(find px4)/launch/gazebo_iris_empty_world.launch">
+        <arg name="headless" value="$(arg headless)"/>
+        <arg name="gui" value="$(arg gui)"/>
+        <arg name="enable_logging" value="$(arg enable_logging)" />
+        <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
+        <arg name="log_file" value="$(arg log_file)"/>
+    </include>
+
+    <test test-name="direct_manual_input_test" pkg="px4" type="direct_manual_input_test.py" />
+    <test test-name="direct_offboard_posctl_test" pkg="px4" type="direct_offboard_posctl_test.py" />
+</launch>
diff --git a/integrationtests/demo_tests/flight_path_assertion.py b/integrationtests/demo_tests/flight_path_assertion.py
new file mode 100644
index 000000000..485de8c41
--- /dev/null
+++ b/integrationtests/demo_tests/flight_path_assertion.py
@@ -0,0 +1,171 @@
+#!/usr/bin/env python
+#***************************************************************************
+#
+#   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.
+#
+#***************************************************************************/
+
+#
+# @author Andreas Antener <andreas@uaventure.com>
+#
+import sys
+import rospy
+import threading
+
+from px4.msg import vehicle_local_position
+from gazebo_msgs.srv import SpawnModel
+from gazebo_msgs.srv import SetModelState
+from gazebo_msgs.srv import DeleteModel
+from geometry_msgs.msg import Pose
+from geometry_msgs.msg import Twist
+
+from numpy import linalg
+import numpy as np
+import math
+
+#
+# Helper to test if vehicle stays on expected flight path.
+#
+class FlightPathAssertion(threading.Thread):
+
+    #
+    # Arguments
+    # - positions: tuple of tuples in the form (x, y, z, heading)
+    #
+    # TODO: yaw validation
+    # TODO: fail main test thread
+    #
+    def __init__(self, positions, tunnelRadius = 1, yawOffset = 0.2):
+        threading.Thread.__init__(self)
+        rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback)
+        self.spawnModel = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel)
+        self.setModelState = rospy.ServiceProxy('gazebo/set_model_state', SetModelState)
+        self.deleteModel = rospy.ServiceProxy('gazebo/delete_model', DeleteModel)
+
+        self.positions = positions
+        self.tunnelRadius = tunnelRadius
+        self.yawOffset = yawOffset
+        self.hasPos = False
+        self.shouldStop = False
+        self.center = positions[0]
+        self.endOfSegment = False
+        self.failed = False
+
+    def position_callback(self, data):
+        self.hasPos = True
+        self.localPosition = data
+
+    def spawn_indicator(self):
+        self.deleteModel("indicator")
+        xml = "<?xml version='1.0'?><sdf version='1.4'><model name='indicator'><static>true</static><link name='link'><visual name='visual'><transparency>0.7</transparency><geometry><sphere><radius>%f</radius></sphere></geometry><material><ambient>1 0 0 0.5</ambient><diffuse>1 0 0 0.5</diffuse></material></visual></link></model></sdf>" % self.tunnelRadius
+        self.spawnModel("indicator", xml, "", Pose(), "")
+
+    def position_indicator(self):
+        state = SetModelState()
+        state.model_name = "indicator"
+        pose = Pose()
+        pose.position.x = self.center[0]
+        pose.position.y = (-1) * self.center[1]
+        pose.position.z = (-1) * self.center[2]
+        state.pose = pose
+        state.twist = Twist()
+        state.reference_frame = ""
+        self.setModelState(state)
+
+    def distance_to_line(self, a, b, pos):
+        v = b - a
+        w = pos - a
+
+        c1 = np.dot(w, v)
+        if c1 <= 0: # before a
+            self.center = a
+            return linalg.norm(pos - a)
+
+        c2 = np.dot(v, v)
+        if c2 <= c1: # after b
+            self.center = b
+            self.endOfSegment = True
+            return linalg.norm(pos - b)
+
+        x = c1 / c2
+        l = a + x * v
+        self.center = l
+        return linalg.norm(pos - l)
+    
+    def stop(self):
+        self.shouldStop = True
+
+    def run(self):
+        rate = rospy.Rate(10) # 10hz
+        self.spawn_indicator()
+
+        current = 0
+
+        while not self.shouldStop:
+            if (self.hasPos):
+                # calculate distance to line segment between first two points
+                # if distances > tunnelRadius
+                #   exit with error
+                # advance current pos if not on the line anymore or distance to next point < tunnelRadius
+                # exit if current pos is now the last position
+
+                self.position_indicator()
+
+                pos = np.array((self.localPosition.x,
+                    self.localPosition.y, 
+                    self.localPosition.z))
+                aPos = np.array((self.positions[current][0],
+                    self.positions[current][1], 
+                    self.positions[current][2]))
+                bPos = np.array((self.positions[current + 1][0],
+                    self.positions[current + 1][1], 
+                    self.positions[current + 1][2]))
+
+                dist = self.distance_to_line(aPos, bPos, pos)
+                bDist = linalg.norm(pos - bPos)
+
+                rospy.logdebug("distance to line: %f, distance to end: %f" % (dist, bDist))
+
+                if (dist > self.tunnelRadius):
+                    msg = "left tunnel at position (%f, %f, %f)" % (self.localPosition.x, self.localPosition.y, self.localPosition.z)
+                    rospy.logerr(msg)
+                    self.failed = True
+                    break
+
+                if (self.endOfSegment or bDist < self.tunnelRadius):
+                    rospy.loginfo("next segment")
+                    self.endOfSegment = False
+                    current = current + 1
+
+                if (current == len(self.positions) - 1):
+                    rospy.loginfo("no more positions")
+                    break
+
+            rate.sleep()
diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/demo_tests/manual_input.py
new file mode 100755
index 000000000..9b2471a00
--- /dev/null
+++ b/integrationtests/demo_tests/manual_input.py
@@ -0,0 +1,175 @@
+#!/usr/bin/env python
+#***************************************************************************
+#
+#   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.
+#
+#***************************************************************************/
+
+#
+# @author Andreas Antener <andreas@uaventure.com>
+#
+import sys
+import rospy
+
+from px4.msg import manual_control_setpoint
+from px4.msg import offboard_control_mode
+from mav_msgs.msg import CommandAttitudeThrust
+from std_msgs.msg import Header
+
+#
+# Manual input control helper
+#
+# FIXME: this is not the way to do it! ATM it fakes input to iris/command/attitude because else
+# the simulator does not instantiate our controller. Probably this whole class will disappear once
+# arming works correctly.
+#
+class ManualInput:
+
+    def __init__(self):
+        rospy.init_node('test_node', anonymous=True)
+        self.pubMcsp = rospy.Publisher('px4_multicopter/manual_control_setpoint', manual_control_setpoint, queue_size=10)
+        self.pubOff = rospy.Publisher('px4_multicopter/offboard_control_mode', offboard_control_mode, queue_size=10)
+        self.pubAtt = rospy.Publisher('iris/command/attitude', CommandAttitudeThrust, queue_size=10)
+
+    def arm(self):
+        rate = rospy.Rate(10) # 10hz
+
+        att = CommandAttitudeThrust()
+        att.header = Header()
+
+        pos = manual_control_setpoint()
+        pos.x = 0
+        pos.z = 0
+        pos.y = 0
+        pos.r = 0
+        pos.mode_switch = 3
+        pos.return_switch = 3
+        pos.posctl_switch = 3
+        pos.loiter_switch = 3
+        pos.acro_switch = 0
+        pos.offboard_switch = 3
+
+        count = 0
+        while not rospy.is_shutdown() and count < 5:
+            rospy.loginfo("zeroing")
+            time = rospy.get_rostime().now()
+            pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
+            self.pubMcsp.publish(pos)
+            # Fake input to iris commander
+            self.pubAtt.publish(att)
+            rate.sleep()
+            count = count + 1 
+
+        pos.r = 1
+        count = 0
+        while not rospy.is_shutdown() and count < 5:
+            rospy.loginfo("arming")
+            time = rospy.get_rostime().now()
+            pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
+            self.pubMcsp.publish(pos)
+            rate.sleep()
+            count = count + 1
+
+    def posctl(self):
+        rate = rospy.Rate(10) # 10hz
+
+        # triggers posctl
+        pos = manual_control_setpoint()
+        pos.x = 0
+        pos.z = 0
+        pos.y = 0
+        pos.r = 0
+        pos.mode_switch = 2
+        pos.return_switch = 3
+        pos.posctl_switch = 1
+        pos.loiter_switch = 3
+        pos.acro_switch = 0
+        pos.offboard_switch = 3
+
+        count = 0
+        while not rospy.is_shutdown() and count < 5:
+            rospy.loginfo("triggering posctl")
+            time = rospy.get_rostime().now()
+            pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
+            self.pubMcsp.publish(pos)
+            rate.sleep()
+            count = count + 1
+
+
+    def offboard_attctl(self):
+        self.offboard(False, False, True, True, True, True)
+
+    def offboard_posctl(self):
+        self.offboard(False, False, True, False, True, True)
+
+    # Trigger offboard and set offboard control mode before
+    def offboard(self, ignore_thrust=False, ignore_attitude=False, ignore_bodyrate=True,
+                        ignore_position=False, ignore_velocity=True, ignore_acceleration_force=True):
+        rate = rospy.Rate(10) # 10hz
+
+        mode = offboard_control_mode()
+        mode.ignore_thrust = ignore_thrust
+        mode.ignore_attitude = ignore_attitude
+        mode.ignore_bodyrate = ignore_bodyrate
+        mode.ignore_position = ignore_position
+        mode.ignore_velocity = ignore_velocity
+        mode.ignore_acceleration_force = ignore_acceleration_force
+
+        count = 0
+        while not rospy.is_shutdown() and count < 5:
+            rospy.loginfo("setting offboard mode")
+            time = rospy.get_rostime().now()
+            mode.timestamp = time.secs * 1e6 + time.nsecs / 1000
+            self.pubOff.publish(mode)
+            rate.sleep()
+            count = count + 1
+
+        # triggers offboard
+        pos = manual_control_setpoint()
+        pos.x = 0
+        pos.z = 0
+        pos.y = 0
+        pos.r = 0
+        pos.mode_switch = 3
+        pos.return_switch = 3
+        pos.posctl_switch = 3
+        pos.loiter_switch = 3
+        pos.acro_switch = 0
+        pos.offboard_switch = 1
+
+        count = 0
+        while not rospy.is_shutdown() and count < 5:
+            rospy.loginfo("triggering offboard")
+            time = rospy.get_rostime().now()
+            pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
+            self.pubMcsp.publish(pos)
+            rate.sleep()
+            count = count + 1
+
diff --git a/integrationtests/demo_tests/mavros_offboard_attctl_test.py b/integrationtests/demo_tests/mavros_offboard_attctl_test.py
new file mode 100755
index 000000000..a52f7ffc1
--- /dev/null
+++ b/integrationtests/demo_tests/mavros_offboard_attctl_test.py
@@ -0,0 +1,143 @@
+#!/usr/bin/env python
+#***************************************************************************
+#
+#   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.
+#
+#***************************************************************************/
+
+#
+# @author Andreas Antener <andreas@uaventure.com>
+#
+PKG = 'px4'
+
+import sys
+import unittest
+import rospy
+import math
+
+from numpy import linalg
+import numpy as np
+
+from px4.msg import vehicle_control_mode
+from std_msgs.msg import Header
+from std_msgs.msg import Float64
+from geometry_msgs.msg import PoseStamped, Quaternion
+from tf.transformations import quaternion_from_euler
+from mavros.srv import CommandBool
+
+from manual_input import ManualInput
+
+#
+# Tests flying a path in offboard control by sending position setpoints
+# over MAVROS.
+#
+# For the test to be successful it needs to reach all setpoints in a certain time.
+# FIXME: add flight path assertion (needs transformation from ROS frame to NED)
+#
+class MavrosOffboardAttctlTest(unittest.TestCase):
+
+    def setUp(self):
+        rospy.init_node('test_node', anonymous=True)
+        rospy.wait_for_service('mavros/cmd/arming', 30)
+        rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback)
+        rospy.Subscriber("mavros/position/local", PoseStamped, self.position_callback)
+        self.pubAtt = rospy.Publisher('mavros/setpoint/attitude', PoseStamped, queue_size=10)
+        self.pubThr = rospy.Publisher('mavros/setpoint/att_throttle', Float64, queue_size=10)
+        self.cmdArm = rospy.ServiceProxy("mavros/cmd/arming", CommandBool)
+        self.rate = rospy.Rate(10) # 10hz
+        self.rateSec = rospy.Rate(1)
+        self.hasPos = False
+        self.controlMode = vehicle_control_mode()
+
+    #
+    # General callback functions used in tests
+    #
+    def position_callback(self, data):
+        self.hasPos = True
+        self.localPosition = data
+
+    def vehicle_control_mode_callback(self, data):
+        self.controlMode = data
+
+
+    #
+    # Helper methods
+    #
+    def arm(self):
+        return self.cmdArm(value=True)
+
+    #
+    # Test offboard position control
+    #
+    def test_attctl(self):
+        # FIXME: this must go ASAP when arming is implemented
+        manIn = ManualInput()
+        manIn.arm()
+        manIn.offboard_attctl()
+
+        self.assertTrue(self.arm(), "Could not arm")
+        self.rateSec.sleep()
+        self.rateSec.sleep()
+        self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set after 2 seconds")
+
+        # set some attitude and thrust
+        att = PoseStamped()
+        att.header = Header() 
+        att.header.frame_id = "base_footprint"
+        att.header.stamp = rospy.Time.now()
+        quaternion = quaternion_from_euler(0.15, 0.15, 0)
+        att.pose.orientation = Quaternion(*quaternion)
+
+        throttle = Float64()
+        throttle.data = 0.6
+
+        # does it cross expected boundaries in X seconds?
+        count = 0
+        timeout = 120
+        while(count < timeout):
+            # update timestamp for each published SP
+            att.header.stamp = rospy.Time.now()
+            self.pubAtt.publish(att)
+            self.pubThr.publish(throttle)
+            
+            if (self.localPosition.pose.position.x > 5
+                and self.localPosition.pose.position.z > 5
+                and self.localPosition.pose.position.y < -5):
+                break
+            count = count + 1
+            self.rate.sleep()
+
+        self.assertTrue(count < timeout, "took too long to cross boundaries")
+    
+
+if __name__ == '__main__':
+    import rostest
+    rostest.rosrun(PKG, 'mavros_offboard_attctl_test', MavrosOffboardAttctlTest)
+    #unittest.main()
diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py
new file mode 100755
index 000000000..a3739ae5c
--- /dev/null
+++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py
@@ -0,0 +1,174 @@
+#!/usr/bin/env python
+#***************************************************************************
+#
+#   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.
+#
+#***************************************************************************/
+
+#
+# @author Andreas Antener <andreas@uaventure.com>
+#
+PKG = 'px4'
+
+import sys
+import unittest
+import rospy
+import math
+
+from numpy import linalg
+import numpy as np
+
+from px4.msg import vehicle_control_mode
+from std_msgs.msg import Header
+from geometry_msgs.msg import PoseStamped, Quaternion
+from tf.transformations import quaternion_from_euler
+from mavros.srv import CommandBool
+
+from manual_input import ManualInput
+
+#
+# Tests flying a path in offboard control by sending position setpoints
+# over MAVROS.
+#
+# For the test to be successful it needs to reach all setpoints in a certain time.
+# FIXME: add flight path assertion (needs transformation from ROS frame to NED)
+#
+class MavrosOffboardPosctlTest(unittest.TestCase):
+
+    def setUp(self):
+        rospy.init_node('test_node', anonymous=True)
+        rospy.wait_for_service('mavros/cmd/arming', 30)
+        rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback)
+        rospy.Subscriber("mavros/position/local", PoseStamped, self.position_callback)
+        self.pubSpt = rospy.Publisher('mavros/setpoint/local_position', PoseStamped, queue_size=10)
+        self.cmdArm = rospy.ServiceProxy("mavros/cmd/arming", CommandBool)
+        self.rate = rospy.Rate(10) # 10hz
+        self.rateSec = rospy.Rate(1)
+        self.hasPos = False
+        self.controlMode = vehicle_control_mode()
+
+    #
+    # General callback functions used in tests
+    #
+    def position_callback(self, data):
+        self.hasPos = True
+        self.localPosition = data
+
+    def vehicle_control_mode_callback(self, data):
+        self.controlMode = data
+
+
+    #
+    # Helper methods
+    #
+    def is_at_position(self, x, y, z, offset):
+        if(not self.hasPos):
+            return False
+
+        rospy.logdebug("current position %f, %f, %f" % (self.localPosition.pose.position.x, self.localPosition.pose.position.y, self.localPosition.pose.position.z))
+        desired = np.array((x, y, z))
+        pos = np.array((self.localPosition.pose.position.x, self.localPosition.pose.position.y, self.localPosition.pose.position.z))
+        return linalg.norm(desired - pos) < offset
+
+    def reach_position(self, x, y, z, timeout):
+        # set a position setpoint
+        pos = PoseStamped()
+        pos.header = Header() 
+        pos.header.frame_id = "base_footprint"
+        pos.pose.position.x = x
+        pos.pose.position.y = y
+        pos.pose.position.z = z
+
+        # For demo purposes we will lock yaw/heading to north.
+        yaw_degrees = 0  # North
+        yaw = math.radians(yaw_degrees)
+        quaternion = quaternion_from_euler(0, 0, yaw)
+        pos.pose.orientation = Quaternion(*quaternion)
+
+        # does it reach the position in X seconds?
+        count = 0
+        while(count < timeout):
+            # update timestamp for each published SP
+            pos.header.stamp = rospy.Time.now()
+            self.pubSpt.publish(pos)
+            
+            if(self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5)):
+                break
+            count = count + 1
+            self.rate.sleep()
+
+        self.assertTrue(count < timeout, "took too long to get to position")
+
+    def arm(self):
+        return self.cmdArm(value=True)
+
+    #
+    # Test offboard position control
+    #
+    def test_posctl(self):
+        # FIXME: this must go ASAP when arming is implemented
+        manIn = ManualInput()
+        manIn.arm()
+        manIn.offboard_posctl()
+
+        self.assertTrue(self.arm(), "Could not arm")
+        self.rateSec.sleep()
+        self.rateSec.sleep()
+        self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set after 2 seconds")
+
+        # prepare flight path
+        positions = (
+            (0,0,0),
+            (2,2,2),
+            (2,-2,2),
+            (-2,-2,2),
+            (2,2,2))
+
+        for i in range(0, len(positions)):
+            self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120)
+        
+        # does it hold the position for Y seconds?
+        positionHeld = True
+        count = 0
+        timeout = 50
+        while(count < timeout):
+            if(not self.is_at_position(2, 2, 2, 0.5)):
+                positionHeld = False
+                break
+            count = count + 1
+            self.rate.sleep()
+
+        self.assertTrue(count == timeout, "position could not be held")
+    
+
+if __name__ == '__main__':
+    import rostest
+    rostest.rosrun(PKG, 'mavros_offboard_posctl_test', MavrosOffboardPosctlTest)
+    #unittest.main()
diff --git a/integrationtests/demo_tests/mavros_tests.launch b/integrationtests/demo_tests/mavros_tests.launch
new file mode 100644
index 000000000..4651f0dc9
--- /dev/null
+++ b/integrationtests/demo_tests/mavros_tests.launch
@@ -0,0 +1,19 @@
+<launch>
+    <arg name="headless" default="true"/>
+    <arg name="gui" default="false"/>
+    <arg name="enable_logging" default="false"/>
+    <arg name="enable_ground_truth" default="false"/>
+    <arg name="log_file" default="iris"/>
+
+    <include file="$(find px4)/launch/gazebo_iris_empty_world.launch">
+        <arg name="headless" value="$(arg headless)"/>
+        <arg name="gui" value="$(arg gui)"/>
+        <arg name="enable_logging" value="$(arg enable_logging)" />
+        <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
+        <arg name="log_file" value="$(arg log_file)"/>
+    </include>
+    <include file="$(find px4)/launch/mavros_sitl.launch" />
+
+    <test test-name="mavros_offboard_posctl_test" pkg="px4" type="mavros_offboard_posctl_test.py" />
+    <test test-name="mavros_offboard_attctl_test" pkg="px4" type="mavros_offboard_attctl_test.py" />
+</launch>
diff --git a/launch/gazebo_ardrone_empty_world.launch b/launch/gazebo_ardrone_empty_world.launch
index 9d19fe5f9..22bfb0c79 100644
--- a/launch/gazebo_ardrone_empty_world.launch
+++ b/launch/gazebo_ardrone_empty_world.launch
@@ -3,7 +3,7 @@
     <arg name="headless" default="false"/>
     <arg name="gui" default="true"/>
     <arg name="enable_logging" default="false"/>
-    <arg name="enable_ground_truth" default="true"/>
+    <arg name="enable_ground_truth" default="false"/>
     <arg name="log_file" default="ardrone"/>
 
     <include file="$(find rotors_gazebo)/launch/ardrone_empty_world_with_joy.launch">
diff --git a/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch b/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch
new file mode 100644
index 000000000..717244abf
--- /dev/null
+++ b/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch
@@ -0,0 +1,9 @@
+
+<launch>
+
+<include file="$(find px4)/launch/gazebo_ardrone_empty_world.launch" />
+<include file="$(find px4)/launch/mavros_sitl.launch" />
+
+<node pkg="px4" name="demo_offboard_attitude_setpoints" type="demo_offboard_attitude_setpoints"/>
+
+</launch>
diff --git a/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch b/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch
new file mode 100644
index 000000000..9ff7f7d1f
--- /dev/null
+++ b/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch
@@ -0,0 +1,9 @@
+
+<launch>
+
+<include file="$(find px4)/launch/gazebo_ardrone_empty_world.launch" />
+<include file="$(find px4)/launch/mavros_sitl.launch" />
+
+<node pkg="px4" name="demo_offboard_position_setpoints" type="demo_offboard_position_setpoints"/>
+
+</launch>
diff --git a/launch/mavros_sitl.launch b/launch/mavros_sitl.launch
new file mode 100644
index 000000000..40010a273
--- /dev/null
+++ b/launch/mavros_sitl.launch
@@ -0,0 +1,21 @@
+<launch>
+	<!-- vim: set ft=xml noet : -->
+	<!-- example launch script for PX4 based FCU's -->
+
+	<arg name="fcu_url" default="udp://localhost:14560@localhost:14565" />
+	<arg name="gcs_url" default="" />
+	<arg name="tgt_system" default="1" />
+	<arg name="tgt_component" default="50" />
+
+	<param name="mavros/setpoint/attitude/listen_twist" type="bool" value="false" />
+
+	<include file="$(find mavros)/launch/node.launch">
+		<arg name="blacklist_yaml" value="$(find mavros)/launch/px4_blacklist.yaml" />
+		<arg name="config_yaml" value="$(find mavros)/launch/px4_config.yaml" />
+
+		<arg name="fcu_url" value="$(arg fcu_url)" />
+		<arg name="gcs_url" value="$(arg gcs_url)" />
+		<arg name="tgt_system" value="$(arg tgt_system)" />
+		<arg name="tgt_component" value="$(arg tgt_component)" />
+	</include>
+</launch>
diff --git a/launch/multicopter.launch b/launch/multicopter.launch
index 95400bd82..bc0e37771 100644
--- a/launch/multicopter.launch
+++ b/launch/multicopter.launch
@@ -9,6 +9,7 @@
 	<node pkg="px4" name="position_estimator" type="position_estimator"/>
 	<node pkg="px4" name="mc_att_control" type="mc_att_control"/>
 	<node pkg="px4" name="mc_pos_control" type="mc_pos_control"/>
+	<node pkg="px4" name="mavlink" type="mavlink"/>
 	<!-- <node pkg="rosbag" type="record" name="record" output="screen" args="-a -O px4_multicopter"/> -->
 </group>
 
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index 0347492eb..86925f9ff 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -43,6 +43,7 @@ MODULES		+= drivers/frsky_telemetry
 MODULES		+= modules/sensors
 MODULES		+= drivers/mkblctrl
 MODULES		+= drivers/px4flow
+MODULES		+= drivers/oreoled
 
 #
 # System commands
diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0
-Subproject 68a88824741ac26838ec0c8c38d67b51dc84b79
+Subproject 7ae438b86ea983e95af5f092e45a5d0f9d093c7
diff --git a/msg/offboard_control_mode.msg b/msg/offboard_control_mode.msg
new file mode 100644
index 000000000..e2d06963b
--- /dev/null
+++ b/msg/offboard_control_mode.msg
@@ -0,0 +1,9 @@
+# Off-board control mode
+uint64 timestamp
+
+bool ignore_thrust
+bool ignore_attitude
+bool ignore_bodyrate
+bool ignore_position
+bool ignore_velocity
+bool ignore_acceleration_force
diff --git a/msg/pwm_input.msg b/msg/pwm_input.msg
new file mode 100644
index 000000000..beb82021d
--- /dev/null
+++ b/msg/pwm_input.msg
@@ -0,0 +1,5 @@
+
+uint64 timestamp	# Microseconds since system boot
+uint64 error_count
+uint32 pulse_width	# Pulse width, timer counts 
+uint32 period		# Period, timer counts
diff --git a/msg/vehicle_force_setpoint.msg b/msg/vehicle_force_setpoint.msg
new file mode 100644
index 000000000..9e2322005
--- /dev/null
+++ b/msg/vehicle_force_setpoint.msg
@@ -0,0 +1,8 @@
+# Definition of force (NED) setpoint uORB topic. Typically this can be used
+# by a position control app together with an attitude control app.
+
+
+float32 x		# in N NED
+float32 y		# in N NED
+float32 z		# in N NED
+float32 yaw		# right-hand rotation around downward axis (rad, equivalent to Tait-Bryan yaw)
diff --git a/nuttx-configs/aerocore/nsh/defconfig b/nuttx-configs/aerocore/nsh/defconfig
index 29dff64aa..08ff4a988 100644
--- a/nuttx-configs/aerocore/nsh/defconfig
+++ b/nuttx-configs/aerocore/nsh/defconfig
@@ -332,7 +332,7 @@ CONFIG_BOARD_LOOPSPERMSEC=16717
 CONFIG_DRAM_START=0x20000000
 CONFIG_DRAM_SIZE=262144
 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y
-CONFIG_ARCH_INTERRUPTSTACK=4096
+CONFIG_ARCH_INTERRUPTSTACK=1500
 
 #
 # Boot options
@@ -415,8 +415,8 @@ CONFIG_PREALLOC_TIMERS=50
 #
 # Stack and heap information
 #
-CONFIG_IDLETHREAD_STACKSIZE=6000
-CONFIG_USERMAIN_STACKSIZE=4096
+CONFIG_IDLETHREAD_STACKSIZE=1000
+CONFIG_USERMAIN_STACKSIZE=3000
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_PTHREAD_STACK_DEFAULT=2048
 
diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig
index 86ed041ff..cd410051c 100644
--- a/nuttx-configs/px4fmu-v1/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v1/nsh/defconfig
@@ -325,7 +325,8 @@ CONFIG_BOARD_LOOPSPERMSEC=16717
 CONFIG_DRAM_START=0x20000000
 CONFIG_DRAM_SIZE=196608
 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y
-CONFIG_ARCH_INTERRUPTSTACK=4096
+# The actual usage is 420 bytes
+CONFIG_ARCH_INTERRUPTSTACK=1500
 
 #
 # Boot options
@@ -416,8 +417,8 @@ CONFIG_PREALLOC_TIMERS=50
 #
 # Stack and heap information
 #
-CONFIG_IDLETHREAD_STACKSIZE=3500
-CONFIG_USERMAIN_STACKSIZE=2600
+CONFIG_IDLETHREAD_STACKSIZE=1000
+CONFIG_USERMAIN_STACKSIZE=3000
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_PTHREAD_STACK_DEFAULT=2048
 
diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig
index 6a1aec22b..4ccc5dacb 100644
--- a/nuttx-configs/px4fmu-v2/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v2/nsh/defconfig
@@ -367,7 +367,7 @@ CONFIG_BOARD_LOOPSPERMSEC=16717
 CONFIG_DRAM_START=0x20000000
 CONFIG_DRAM_SIZE=262144
 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y
-CONFIG_ARCH_INTERRUPTSTACK=4096
+CONFIG_ARCH_INTERRUPTSTACK=1500
 
 #
 # Boot options
@@ -451,8 +451,8 @@ CONFIG_PREALLOC_TIMERS=50
 #
 # Stack and heap information
 #
-CONFIG_IDLETHREAD_STACKSIZE=3500
-CONFIG_USERMAIN_STACKSIZE=2600
+CONFIG_IDLETHREAD_STACKSIZE=1000
+CONFIG_USERMAIN_STACKSIZE=3000
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_PTHREAD_STACK_DEFAULT=2048
 
diff --git a/package.xml b/package.xml
index 666200390..2da49096f 100644
--- a/package.xml
+++ b/package.xml
@@ -44,10 +44,15 @@
   <build_depend>rospy</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>eigen</build_depend>
+  <build_depend>libmavconn</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>rostest</build_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>rospy</run_depend>
   <run_depend>std_msgs</run_depend>
   <run_depend>eigen</run_depend>
+  <run_depend>libmavconn</run_depend>
+  <run_depend>tf</run_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->
diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp
index 604ce35c5..a958fc60d 100644
--- a/src/drivers/batt_smbus/batt_smbus.cpp
+++ b/src/drivers/batt_smbus/batt_smbus.cpp
@@ -452,10 +452,7 @@ BATT_SMBUS::read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_
 	uint8_t pec = get_PEC(reg, true, buff, bufflen + 1);
 
 	if (pec != buff[bufflen + 1]) {
-		// debug
-		warnx("CurrPEC:%x vs MyPec:%x", (int)buff[bufflen + 1], (int)pec);
 		return 0;
-
 	}
 
 	// copy data
diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h
index 2fd8bc724..273af1023 100644
--- a/src/drivers/boards/px4fmu-v2/board_config.h
+++ b/src/drivers/boards/px4fmu-v2/board_config.h
@@ -207,6 +207,11 @@ __BEGIN_DECLS
 #define HRT_TIMER		8	/* use timer8 for the HRT */
 #define HRT_TIMER_CHANNEL	1	/* use capture/compare channel */
 
+/* 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
+
 /****************************************************************************************************
  * Public Types
  ****************************************************************************************************/
diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h
index bacafe1dc..be9604b6e 100644
--- a/src/drivers/drv_gpio.h
+++ b/src/drivers/drv_gpio.h
@@ -159,4 +159,6 @@
 
 #define GPIO_SENSOR_RAIL_RESET	GPIOC(13)
 
+#define GPIO_PERIPHERAL_RAIL_RESET	GPIOC(14)
+
 #endif /* _DRV_GPIO_H */
diff --git a/src/drivers/drv_oreoled.h b/src/drivers/drv_oreoled.h
new file mode 100644
index 000000000..0dcb10a7b
--- /dev/null
+++ b/src/drivers/drv_oreoled.h
@@ -0,0 +1,149 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file drv_oreoled.h
+ *
+ * Oreo led device API
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+/* oreoled device path */
+#define OREOLED0_DEVICE_PATH "/dev/oreoled0"
+
+/*
+ * ioctl() definitions
+ */
+
+#define _OREOLEDIOCBASE		(0x2d00)
+#define _OREOLEDIOC(_n)		(_IOC(_OREOLEDIOCBASE, _n))
+
+/** set constant RGB values */
+#define OREOLED_SET_RGB			_OREOLEDIOC(1)
+
+/** run macro */
+#define OREOLED_RUN_MACRO		_OREOLEDIOC(2)
+
+/** send bytes */
+#define OREOLED_SEND_BYTES		_OREOLEDIOC(3)
+
+/* Oreo LED driver supports up to 4 leds */
+#define OREOLED_NUM_LEDS		4
+
+/* instance of 0xff means apply to all instances */
+#define OREOLED_ALL_INSTANCES	0xff
+
+/* maximum command length that can be sent to LEDs */
+#define OREOLED_CMD_LENGTH_MAX	24
+
+/* enum passed to OREOLED_SET_MODE ioctl()
+ *	defined by hardware */
+enum oreoled_pattern {
+	OREOLED_PATTERN_OFF = 0,
+	OREOLED_PATTERN_SINE = 1,
+	OREOLED_PATTERN_SOLID = 2,
+	OREOLED_PATTERN_SIREN = 3,
+	OREOLED_PATTERN_STROBE = 4,
+	OREOLED_PATTERN_FADEIN = 5,
+	OREOLED_PATTERN_FADEOUT = 6,
+	OREOLED_PATTERN_PARAMUPDATE = 7,
+	OREOLED_PATTERN_ENUM_COUNT
+};
+
+/* enum passed to OREOLED_SET_MODE ioctl()
+ *	defined by hardware */
+enum oreoled_param {
+	OREOLED_PARAM_BIAS_RED = 0,
+	OREOLED_PARAM_BIAS_GREEN = 1,
+	OREOLED_PARAM_BIAS_BLUE = 2,
+	OREOLED_PARAM_AMPLITUDE_RED = 3,
+	OREOLED_PARAM_AMPLITUDE_GREEN = 4,
+	OREOLED_PARAM_AMPLITUDE_BLUE = 5,
+	OREOLED_PARAM_PERIOD = 6,
+	OREOLED_PARAM_REPEAT = 7,
+	OREOLED_PARAM_PHASEOFFSET = 8,
+	OREOLED_PARAM_MACRO = 9,
+	OREOLED_PARAM_ENUM_COUNT
+};
+
+/* enum of available macros
+ * 	defined by hardware */
+enum oreoled_macro {
+	OREOLED_PARAM_MACRO_RESET = 0,
+	OREOLED_PARAM_MACRO_COLOUR_CYCLE = 1,
+	OREOLED_PARAM_MACRO_BREATH = 2,
+	OREOLED_PARAM_MACRO_STROBE = 3,
+	OREOLED_PARAM_MACRO_FADEIN = 4,
+	OREOLED_PARAM_MACRO_FADEOUT = 5,
+	OREOLED_PARAM_MACRO_RED = 6,
+	OREOLED_PARAM_MACRO_GREEN = 7,
+	OREOLED_PARAM_MACRO_BLUE = 8,
+	OREOLED_PARAM_MACRO_YELLOW = 9,
+	OREOLED_PARAM_MACRO_WHITE = 10,
+	OREOLED_PARAM_MACRO_ENUM_COUNT
+};
+
+/* 
+  structure passed to OREOLED_SET_RGB ioctl()
+  Note that the driver scales the brightness to 0 to 255, regardless
+  of the hardware scaling
+ */
+typedef struct {
+	uint8_t instance;
+	oreoled_pattern pattern;
+	uint8_t red;
+	uint8_t green;
+	uint8_t blue;
+} oreoled_rgbset_t;
+
+/*
+  structure passed to OREOLED_RUN_MACRO ioctl()
+ */
+typedef struct {
+	uint8_t instance;
+	oreoled_macro macro;
+} oreoled_macrorun_t;
+
+/*
+  structure passed to send_bytes method (only used for testing)
+ */
+typedef struct {
+	uint8_t led_num;
+	uint8_t num_bytes;
+	uint8_t buff[OREOLED_CMD_LENGTH_MAX];
+} oreoled_cmd_t;
+
diff --git a/src/drivers/drv_pwm_input.h b/src/drivers/drv_pwm_input.h
new file mode 100644
index 000000000..778d9fcf5
--- /dev/null
+++ b/src/drivers/drv_pwm_input.h
@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <sys/types.h>
+#include <stdbool.h>
+
+#include <time.h>
+#include <queue.h>
+
+#define PWMIN0_DEVICE_PATH		"/dev/pwmin0"
+
+__BEGIN_DECLS
+
+/*
+ * Initialise the timer
+ */
+__EXPORT extern int	pwm_input_main(int argc, char * argv[]);
+
+__END_DECLS
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index e197059db..f42c968d3 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -45,7 +45,7 @@
  *   (rework, add ubx7+ compatibility)
  *
  * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf
- * @see http://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8-V15_ReceiverDescriptionProtocolSpec_Public_%28UBX-13003221%29.pdf
+ * @see http://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8_ReceiverDescriptionProtocolSpec_%28UBX-13003221%29_Public.pdf
  */
 
 #include <assert.h>
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 4aa05a980..b48ea8577 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -1881,6 +1881,7 @@ MPU6000	*g_dev_int; // on internal bus
 MPU6000	*g_dev_ext; // on external bus
 
 void	start(bool, enum Rotation);
+void	stop(bool);
 void	test(bool);
 void	reset(bool);
 void	info(bool);
@@ -1946,6 +1947,20 @@ fail:
 	errx(1, "driver start failed");
 }
 
+void
+stop(bool external_bus)
+{
+	MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int;
+	if (*g_dev_ptr != nullptr) {
+		delete *g_dev_ptr;
+		*g_dev_ptr = nullptr;
+	} else {
+		/* warn, but not an error */
+		warnx("already stopped.");
+	}
+	exit(0);
+}
+
 /**
  * Perform some basic functional tests on the driver;
  * make sure we can collect data from the sensor in polled
@@ -2111,7 +2126,7 @@ factorytest(bool external_bus)
 void
 usage()
 {
-	warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'factorytest', 'testerror'");
+	warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'factorytest', 'testerror'");
 	warnx("options:");
 	warnx("    -X    (external bus)");
 	warnx("    -R rotation");
@@ -2147,38 +2162,50 @@ mpu6000_main(int argc, char *argv[])
 	 * Start/load the driver.
 
 	 */
-	if (!strcmp(verb, "start"))
+	if (!strcmp(verb, "start")) {
 		mpu6000::start(external_bus, rotation);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		mpu6000::stop(external_bus);
+	}
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(verb, "test"))
+	if (!strcmp(verb, "test")) {
 		mpu6000::test(external_bus);
+	}
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(verb, "reset"))
+	if (!strcmp(verb, "reset")) {
 		mpu6000::reset(external_bus);
+	}
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(verb, "info"))
+	if (!strcmp(verb, "info")) {
 		mpu6000::info(external_bus);
+	}
 
 	/*
 	 * Print register information.
 	 */
-	if (!strcmp(verb, "regdump"))
+	if (!strcmp(verb, "regdump")) {
 		mpu6000::regdump(external_bus);
+	}
 
-	if (!strcmp(verb, "factorytest"))
+	if (!strcmp(verb, "factorytest")) {
 		mpu6000::factorytest(external_bus);
+	}
 
-	if (!strcmp(verb, "testerror"))
+	if (!strcmp(verb, "testerror")) {
 		mpu6000::testerror(external_bus);
+	}
 
-	errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'regdump', 'factorytest' or 'testerror'");
+	mpu6000::usage();
+	exit(1);
 }
diff --git a/src/drivers/oreoled/module.mk b/src/drivers/oreoled/module.mk
new file mode 100644
index 000000000..96afdd5fd
--- /dev/null
+++ b/src/drivers/oreoled/module.mk
@@ -0,0 +1,8 @@
+#
+# Oreo LED driver
+#
+
+MODULE_COMMAND	 = oreoled
+SRCS		 = oreoled.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp
new file mode 100644
index 000000000..b44c4b720
--- /dev/null
+++ b/src/drivers/oreoled/oreoled.cpp
@@ -0,0 +1,700 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Randy Mackay <rmackay9@yahoo.com>
+ *
+ * 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 oreoled.cpp
+ *
+ * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C.
+ *
+ */
+
+#include <nuttx/config.h>
+
+#include <drivers/device/i2c.h>
+#include <drivers/drv_hrt.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <ctype.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+#include <board_config.h>
+
+#include <drivers/drv_oreoled.h>
+#include <drivers/device/ringbuffer.h>
+
+#define OREOLED_NUM_LEDS		4			///< maximum number of LEDs the oreo led driver can support
+#define OREOLED_BASE_I2C_ADDR	0x68		///< base i2c address (7-bit)
+#define OREOLED_TIMEOUT_MS		10000000U	///< timeout looking for battery 10seconds after startup
+#define OREOLED_GENERALCALL_US	4000000U	///< general call sent every 4 seconds
+#define OREOLED_GENERALCALL_CMD	0x00		///< general call command sent at regular intervals
+
+#define OREOLED_STARTUP_INTERVAL_US		(1000000U / 10U)	///< time in microseconds, measure at 10hz
+#define OREOLED_UPDATE_INTERVAL_US		(1000000U / 50U)	///< time in microseconds, measure at 10hz
+
+#define OREOLED_CMD_QUEUE_SIZE	10		///< up to 10 messages can be queued up to send to the LEDs
+
+class OREOLED : public device::I2C
+{
+public:
+	OREOLED(int bus, int i2c_addr);
+	virtual ~OREOLED();
+
+	virtual int		init();
+	virtual int		probe();
+	virtual int		info();
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/* send general call on I2C bus to syncronise all LEDs */
+	int				send_general_call();
+
+	/* send cmd to an LEDs (used for testing only) */
+	int				send_cmd(oreoled_cmd_t sb);
+
+private:
+
+	/**
+	 * Start periodic updates to the LEDs
+	 */
+	void			start();
+
+	/**
+	 * Stop periodic updates to the LEDs
+	 */
+	void			stop();
+
+	/**
+	 * static function that is called by worker queue
+	 */
+	static void		cycle_trampoline(void *arg);
+
+	/**
+	 * update the colours displayed by the LEDs
+	 */
+	void			cycle();
+
+	/* internal variables */
+	work_s			_work;							///< work queue for scheduling reads
+	bool			_healthy[OREOLED_NUM_LEDS];		///< health of each LED
+	uint8_t			_num_healthy;					///< number of healthy LEDs
+	RingBuffer		*_cmd_queue;					///< buffer of commands to send to LEDs
+	uint64_t		_last_gencall;
+	uint64_t		_start_time;					///< system time we first attempt to communicate with battery
+};
+
+/* for now, we only support one OREOLED */
+namespace
+{
+OREOLED *g_oreoled = nullptr;
+}
+
+void oreoled_usage();
+
+extern "C" __EXPORT int oreoled_main(int argc, char *argv[]);
+
+/* constructor */
+OREOLED::OREOLED(int bus, int i2c_addr) :
+	I2C("oreoled", OREOLED0_DEVICE_PATH, bus, i2c_addr, 100000),
+	_work{},
+	_num_healthy(0),
+	_cmd_queue(nullptr),
+	_last_gencall(0)
+{
+	/* initialise to unhealthy */
+	memset(_healthy, 0, sizeof(_healthy));
+
+	/* capture startup time */
+	_start_time = hrt_absolute_time();
+}
+
+/* destructor */
+OREOLED::~OREOLED()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* clear command queue */
+	if (_cmd_queue != nullptr) {
+		delete _cmd_queue;
+	}
+}
+
+int
+OREOLED::init()
+{
+	int ret;
+
+	/* initialise I2C bus */
+	ret = I2C::init();
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	/* allocate command queue */
+	_cmd_queue = new RingBuffer(OREOLED_CMD_QUEUE_SIZE, sizeof(oreoled_cmd_t));
+
+	if (_cmd_queue == nullptr) {
+		return ENOTTY;
+
+	} else {
+		/* start work queue */
+		start();
+	}
+
+	return OK;
+}
+
+int
+OREOLED::probe()
+{
+	/* always return true */
+	return OK;
+}
+
+int
+OREOLED::info()
+{
+	/* print health info on each LED */
+	for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
+		if (!_healthy[i]) {
+			log("oreo %u: BAD", (int)i);
+
+		} else {
+			log("oreo %u: OK", (int)i);
+		}
+	}
+
+	return OK;
+}
+
+void
+OREOLED::start()
+{
+	/* schedule a cycle to start things */
+	work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, 1);
+}
+
+void
+OREOLED::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+OREOLED::cycle_trampoline(void *arg)
+{
+	OREOLED *dev = (OREOLED *)arg;
+
+	/* check global oreoled and cycle */
+	if (g_oreoled != nullptr) {
+		dev->cycle();
+	}
+}
+
+void
+OREOLED::cycle()
+{
+	/* check time since startup */
+	uint64_t now = hrt_absolute_time();
+	bool startup_timeout = (now - _start_time > OREOLED_TIMEOUT_MS);
+
+	/* if not leds found during start-up period, exit without rescheduling */
+	if (startup_timeout && _num_healthy == 0) {
+		warnx("did not find oreoled");
+		return;
+	}
+
+	/* during startup period keep searching for unhealthy LEDs */
+	if (!startup_timeout && _num_healthy < OREOLED_NUM_LEDS) {
+		/* prepare command to turn off LED*/
+		uint8_t msg[] = {OREOLED_PATTERN_OFF};
+
+		/* attempt to contact each unhealthy LED */
+		for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
+			if (!_healthy[i]) {
+				/* set I2C address */
+				set_address(OREOLED_BASE_I2C_ADDR + i);
+
+				/* send I2C command and record health*/
+				if (transfer(msg, sizeof(msg), nullptr, 0) == OK) {
+					_healthy[i] = true;
+					_num_healthy++;
+					warnx("oreoled %d ok", (unsigned)i);
+				}
+			}
+		}
+
+		/* schedule another attempt in 0.1 sec */
+		work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this,
+			   USEC2TICK(OREOLED_STARTUP_INTERVAL_US));
+		return;
+	}
+
+	/* get next command from queue */
+	oreoled_cmd_t next_cmd;
+
+	while (_cmd_queue->get(&next_cmd, sizeof(oreoled_cmd_t))) {
+		/* send valid messages to healthy LEDs */
+		if ((next_cmd.led_num < OREOLED_NUM_LEDS) && _healthy[next_cmd.led_num]
+		    && (next_cmd.num_bytes <= OREOLED_CMD_LENGTH_MAX)) {
+			/* set I2C address */
+			set_address(OREOLED_BASE_I2C_ADDR + next_cmd.led_num);
+			/* send I2C command */
+			transfer(next_cmd.buff, next_cmd.num_bytes, nullptr, 0);
+		}
+	}
+
+	/* send general call every 4 seconds*/
+	if ((now - _last_gencall) > OREOLED_GENERALCALL_US) {
+		send_general_call();
+	}
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this,
+		   USEC2TICK(OREOLED_UPDATE_INTERVAL_US));
+}
+
+int
+OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	int ret = -ENODEV;
+	oreoled_cmd_t new_cmd;
+
+	switch (cmd) {
+	case OREOLED_SET_RGB:
+		/* set the specified color */
+		new_cmd.led_num = ((oreoled_rgbset_t *) arg)->instance;
+		new_cmd.buff[0] = ((oreoled_rgbset_t *) arg)->pattern;
+		new_cmd.buff[1] = OREOLED_PARAM_BIAS_RED;
+		new_cmd.buff[2] = ((oreoled_rgbset_t *) arg)->red;
+		new_cmd.buff[3] = OREOLED_PARAM_BIAS_GREEN;
+		new_cmd.buff[4] = ((oreoled_rgbset_t *) arg)->green;
+		new_cmd.buff[5] = OREOLED_PARAM_BIAS_BLUE;
+		new_cmd.buff[6] = ((oreoled_rgbset_t *) arg)->blue;
+		new_cmd.num_bytes = 7;
+
+		/* special handling for request to set all instances rgb values */
+		if (new_cmd.led_num == OREOLED_ALL_INSTANCES) {
+			for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
+				/* add command to queue for all healthy leds */
+				if (_healthy[i]) {
+					new_cmd.led_num = i;
+					_cmd_queue->force(&new_cmd);
+					ret = OK;
+				}
+			}
+
+		} else if (new_cmd.led_num < OREOLED_NUM_LEDS) {
+			/* request to set individual instance's rgb value */
+			if (_healthy[new_cmd.led_num]) {
+				_cmd_queue->force(&new_cmd);
+				ret = OK;
+			}
+		}
+
+		return ret;
+
+	case OREOLED_RUN_MACRO:
+		/* run a macro */
+		new_cmd.led_num = ((oreoled_macrorun_t *) arg)->instance;
+		new_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE;
+		new_cmd.buff[1] = OREOLED_PARAM_MACRO;
+		new_cmd.buff[2] = ((oreoled_macrorun_t *) arg)->macro;
+		new_cmd.num_bytes = 3;
+
+		/* special handling for request to set all instances */
+		if (new_cmd.led_num == OREOLED_ALL_INSTANCES) {
+			for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
+				/* add command to queue for all healthy leds */
+				if (_healthy[i]) {
+					new_cmd.led_num = i;
+					_cmd_queue->force(&new_cmd);
+					ret = OK;
+				}
+			}
+
+		} else if (new_cmd.led_num < OREOLED_NUM_LEDS) {
+			/* request to set individual instance's rgb value */
+			if (_healthy[new_cmd.led_num]) {
+				_cmd_queue->force(&new_cmd);
+				ret = OK;
+			}
+		}
+
+		return ret;
+
+	case OREOLED_SEND_BYTES:
+		/* send bytes */
+		new_cmd = *((oreoled_cmd_t *) arg);
+
+		/* special handling for request to set all instances */
+		if (new_cmd.led_num == OREOLED_ALL_INSTANCES) {
+			for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
+				/* add command to queue for all healthy leds */
+				if (_healthy[i]) {
+					new_cmd.led_num = i;
+					_cmd_queue->force(&new_cmd);
+					ret = OK;
+				}
+			}
+
+		} else if (new_cmd.led_num < OREOLED_NUM_LEDS) {
+			/* request to set individual instance's rgb value */
+			if (_healthy[new_cmd.led_num]) {
+				_cmd_queue->force(&new_cmd);
+				ret = OK;
+			}
+		}
+
+		return ret;
+
+	default:
+		/* see if the parent class can make any use of it */
+		ret = CDev::ioctl(filp, cmd, arg);
+		break;
+	}
+
+	return ret;
+}
+
+/* send general call on I2C bus to syncronise all LEDs */
+int
+OREOLED::send_general_call()
+{
+	int ret = -ENODEV;
+
+	/* set I2C address to zero */
+	set_address(0);
+
+	/* prepare command : 0x01 = general hardware call, 0x00 = I2C address of master (but we don't act as a slave so set to zero)*/
+	uint8_t msg[] = {0x01, 0x00};
+
+	/* send I2C command */
+	if (transfer(msg, sizeof(msg), nullptr, 0) == OK) {
+		ret = OK;
+	}
+
+	/* record time */
+	_last_gencall = hrt_absolute_time();
+
+	return ret;
+}
+
+/* send a cmd to an LEDs (used for testing only) */
+int
+OREOLED::send_cmd(oreoled_cmd_t new_cmd)
+{
+	int ret = -ENODEV;
+
+	/* sanity check led number, health and cmd length */
+	if ((new_cmd.led_num < OREOLED_NUM_LEDS) && _healthy[new_cmd.led_num] && (new_cmd.num_bytes < OREOLED_CMD_LENGTH_MAX)) {
+		/* set I2C address */
+		set_address(OREOLED_BASE_I2C_ADDR + new_cmd.led_num);
+
+		/* add to queue */
+		_cmd_queue->force(&new_cmd);
+		ret = OK;
+	}
+
+	return ret;
+}
+
+void
+oreoled_usage()
+{
+	warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50' 'macro 4' 'gencall' 'bytes <lednum> 7 9 6'");
+	warnx("options:");
+	warnx("    -b i2cbus (%d)", PX4_I2C_BUS_LED);
+	warnx("    -a addr (0x%x)", OREOLED_BASE_I2C_ADDR);
+}
+
+int
+oreoled_main(int argc, char *argv[])
+{
+	int i2cdevice = -1;
+	int i2c_addr = OREOLED_BASE_I2C_ADDR; /* 7bit */
+
+	int ch;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = getopt(argc, argv, "a:b:")) != EOF) {
+		switch (ch) {
+		case 'a':
+			i2c_addr = (int)strtol(optarg, NULL, 0);
+			break;
+
+		case 'b':
+			i2cdevice = (int)strtol(optarg, NULL, 0);
+			break;
+
+		default:
+			oreoled_usage();
+			exit(0);
+		}
+	}
+
+	if (optind >= argc) {
+		oreoled_usage();
+		exit(1);
+	}
+
+	const char *verb = argv[optind];
+
+	int ret;
+
+	/* start driver */
+	if (!strcmp(verb, "start")) {
+		if (g_oreoled != nullptr) {
+			errx(1, "already started");
+		}
+
+		/* by default use LED bus */
+		if (i2cdevice == -1) {
+			i2cdevice = PX4_I2C_BUS_LED;
+		}
+
+		/* instantiate driver */
+		g_oreoled = new OREOLED(i2cdevice, i2c_addr);
+
+		/* check if object was created */
+		if (g_oreoled == nullptr) {
+			errx(1, "failed to allocated memory for driver");
+		}
+
+		/* check object was created successfully */
+		if (g_oreoled->init() != OK) {
+			delete g_oreoled;
+			g_oreoled = nullptr;
+			errx(1, "failed to start driver");
+		}
+
+		exit(0);
+	}
+
+	/* need the driver past this point */
+	if (g_oreoled == nullptr) {
+		warnx("not started");
+		oreoled_usage();
+		exit(1);
+	}
+
+	if (!strcmp(verb, "test")) {
+		int fd = open(OREOLED0_DEVICE_PATH, O_RDWR);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " OREOLED0_DEVICE_PATH);
+		}
+
+		/* structure to hold desired colour */
+		oreoled_rgbset_t rgb_set_red = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_SOLID, 0xFF, 0x0, 0x0};
+		oreoled_rgbset_t rgb_set_blue = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_SOLID, 0x0, 0x0, 0xFF};
+		oreoled_rgbset_t rgb_set_off = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_OFF, 0x0, 0x0, 0x0};
+
+		/* flash red and blue for 3 seconds */
+		for (uint8_t i = 0; i < 30; i++) {
+			/* red */
+			if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_red)) != OK) {
+				errx(1, " failed to update rgb");
+			}
+
+			/* sleep for 0.05 seconds */
+			usleep(50000);
+
+			/* blue */
+			if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_blue)) != OK) {
+				errx(1, " failed to update rgb");
+			}
+
+			/* sleep for 0.05 seconds */
+			usleep(50000);
+		}
+
+		/* turn off LED */
+		if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_off)) != OK) {
+			errx(1, " failed to turn off led");
+		}
+
+		close(fd);
+		exit(ret);
+	}
+
+	/* display driver status */
+	if (!strcmp(verb, "info")) {
+		g_oreoled->info();
+		exit(0);
+	}
+
+	if (!strcmp(verb, "off") || !strcmp(verb, "stop")) {
+		int fd = open(OREOLED0_DEVICE_PATH, 0);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " OREOLED0_DEVICE_PATH);
+		}
+
+		/* turn off LED */
+		oreoled_rgbset_t rgb_set_off = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_OFF, 0x0, 0x0, 0x0};
+		ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_off);
+
+		close(fd);
+
+		/* delete the oreoled object if stop was requested, in addition to turning off the LED. */
+		if (!strcmp(verb, "stop")) {
+			OREOLED *tmp_oreoled = g_oreoled;
+			g_oreoled = nullptr;
+			delete tmp_oreoled;
+			exit(0);
+		}
+
+		exit(ret);
+	}
+
+	/* send rgb request to all LEDS */
+	if (!strcmp(verb, "rgb")) {
+		if (argc < 5) {
+			errx(1, "Usage: oreoled rgb <red> <green> <blue>");
+		}
+
+		int fd = open(OREOLED0_DEVICE_PATH, 0);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " OREOLED0_DEVICE_PATH);
+		}
+
+		uint8_t red = (uint8_t)strtol(argv[2], NULL, 0);
+		uint8_t green = (uint8_t)strtol(argv[3], NULL, 0);
+		uint8_t blue = (uint8_t)strtol(argv[4], NULL, 0);
+		oreoled_rgbset_t rgb_set = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_SOLID, red, green, blue};
+
+		if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set)) != OK) {
+			errx(1, "failed to set rgb");
+		}
+
+		close(fd);
+		exit(ret);
+	}
+
+	/* send macro request to all LEDS */
+	if (!strcmp(verb, "macro")) {
+		if (argc < 3) {
+			errx(1, "Usage: oreoled macro <macro_num>");
+		}
+
+		int fd = open(OREOLED0_DEVICE_PATH, 0);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " OREOLED0_DEVICE_PATH);
+		}
+
+		uint8_t macro = (uint8_t)strtol(argv[2], NULL, 0);
+
+		/* sanity check macro number */
+		if (macro > OREOLED_PARAM_MACRO_ENUM_COUNT) {
+			errx(1, "invalid macro number %d", (int)macro);
+			exit(ret);
+		}
+
+		oreoled_macrorun_t macro_run = {OREOLED_ALL_INSTANCES, (enum oreoled_macro)macro};
+
+		if ((ret = ioctl(fd, OREOLED_RUN_MACRO, (unsigned long)&macro_run)) != OK) {
+			errx(1, "failed to run macro");
+		}
+
+		close(fd);
+		exit(ret);
+	}
+
+	/* send general hardware call to all LEDS */
+	if (!strcmp(verb, "gencall")) {
+		ret = g_oreoled->send_general_call();
+		warnx("sent general call");
+		exit(ret);
+	}
+
+	/* send a string of bytes to an LED using send_bytes function */
+	if (!strcmp(verb, "bytes")) {
+		if (argc < 3) {
+			errx(1, "Usage: oreoled bytes <led_num> <byte1> <byte2> <byte3> ...");
+		}
+
+		/* structure to be sent */
+		oreoled_cmd_t sendb;
+
+		/* maximum of 20 bytes can be sent */
+		if (argc > 20 + 3) {
+			errx(1, "Max of 20 bytes can be sent");
+		}
+
+		/* check led num */
+		sendb.led_num = (uint8_t)strtol(argv[2], NULL, 0);
+
+		if (sendb.led_num > 3) {
+			errx(1, "led number must be between 0 ~ 3");
+		}
+
+		/* get bytes */
+		sendb.num_bytes = argc - 3;
+		uint8_t byte_count;
+
+		for (byte_count = 0; byte_count < sendb.num_bytes; byte_count++) {
+			sendb.buff[byte_count] = (uint8_t)strtol(argv[byte_count + 3], NULL, 0);
+		}
+
+		/* send bytes */
+		if ((ret = g_oreoled->send_cmd(sendb)) != OK) {
+			errx(1, "failed to send command");
+
+		} else {
+			warnx("sent %d bytes", (int)sendb.num_bytes);
+		}
+
+		exit(ret);
+	}
+
+	oreoled_usage();
+	exit(0);
+}
diff --git a/src/drivers/pwm_input/module.mk b/src/drivers/pwm_input/module.mk
new file mode 100644
index 000000000..04f04d6cb
--- /dev/null
+++ b/src/drivers/pwm_input/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Makefile to build the PWM input driver.
+#
+
+MODULE_COMMAND	= pwm_input
+
+SRCS		= pwm_input.cpp
+
diff --git a/src/drivers/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp
new file mode 100644
index 000000000..2d771266c
--- /dev/null
+++ b/src/drivers/pwm_input/pwm_input.cpp
@@ -0,0 +1,603 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 Andrew Tridgell. 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 pwm_input.cpp 
+ *
+ * PWM input driver based on earlier driver from Evan Slatyer,
+ * which in turn was based on drv_hrt.c
+ */
+
+#include <nuttx/config.h>
+#include <nuttx/arch.h>
+#include <nuttx/irq.h>
+
+#include <sys/types.h>
+#include <stdbool.h>
+
+#include <assert.h>
+#include <debug.h>
+#include <time.h>
+#include <queue.h>
+#include <errno.h>
+#include <string.h>
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <board_config.h>
+#include <drivers/drv_pwm_input.h>
+#include <drivers/drv_hrt.h>
+
+#include "chip.h"
+#include "up_internal.h"
+#include "up_arch.h"
+
+#include "stm32.h"
+#include "stm32_gpio.h"
+#include "stm32_tim.h"
+#include <systemlib/err.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/pwm_input.h>
+#include <uORB/topics/subsystem_info.h>
+
+#include <drivers/drv_device.h>
+#include <drivers/device/device.h>
+#include <drivers/device/ringbuffer.h>
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+
+#if HRT_TIMER == PWMIN_TIMER
+#error cannot share timer between HRT and PWMIN
+#endif
+
+#if !defined(GPIO_PWM_IN) || !defined(PWMIN_TIMER) || !defined(PWMIN_TIMER_CHANNEL)
+#error PWMIN defines are needed in board_config.h for this board
+#endif
+
+/* PWMIN configuration */
+#if   PWMIN_TIMER == 1
+# define PWMIN_TIMER_BASE	STM32_TIM1_BASE
+# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB2ENR
+# define PWMIN_TIMER_POWER_BIT	RCC_APB2ENR_TIM1EN
+# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM1CC
+# define PWMIN_TIMER_CLOCK	STM32_APB2_TIM1_CLKIN
+#elif PWMIN_TIMER == 2
+# define PWMIN_TIMER_BASE	STM32_TIM2_BASE
+# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define PWMIN_TIMER_POWER_BIT	RCC_APB2ENR_TIM2EN
+# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM2
+# define PWMIN_TIMER_CLOCK	STM32_APB1_TIM2_CLKIN
+#elif PWMIN_TIMER == 3
+# define PWMIN_TIMER_BASE	STM32_TIM3_BASE
+# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define PWMIN_TIMER_POWER_BIT	RCC_APB1ENR_TIM3EN
+# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM3
+# define PWMIN_TIMER_CLOCK	STM32_APB1_TIM3_CLKIN
+#elif PWMIN_TIMER == 4
+# define PWMIN_TIMER_BASE	STM32_TIM4_BASE
+# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define PWMIN_TIMER_POWER_BIT	RCC_APB1ENR_TIM4EN
+# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM4
+# define PWMIN_TIMER_CLOCK	STM32_APB1_TIM4_CLKIN
+#elif PWMIN_TIMER == 5
+# define PWMIN_TIMER_BASE	STM32_TIM5_BASE
+# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define PWMIN_TIMER_POWER_BIT	RCC_APB2ENR_TIM5EN
+# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM5
+# define PWMIN_TIMER_CLOCK	STM32_APB1_TIM5_CLKIN
+#elif PWMIN_TIMER == 8
+# define PWMIN_TIMER_BASE	STM32_TIM8_BASE
+# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB2ENR
+# define PWMIN_TIMER_POWER_BIT	RCC_APB2ENR_TIM8EN
+# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM8CC
+# define PWMIN_TIMER_CLOCK	STM32_APB2_TIM8_CLKIN
+#elif PWMIN_TIMER == 9
+# define PWMIN_TIMER_BASE	STM32_TIM9_BASE
+# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define PWMIN_TIMER_POWER_BIT	RCC_APB2ENR_TIM9EN
+# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM1BRK
+# define PWMIN_TIMER_CLOCK	STM32_APB1_TIM9_CLKIN
+#elif PWMIN_TIMER == 10
+# define PWMIN_TIMER_BASE	STM32_TIM10_BASE
+# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define PWMIN_TIMER_POWER_BIT	RCC_APB2ENR_TIM10EN
+# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM1UP
+# define PWMIN_TIMER_CLOCK	STM32_APB2_TIM10_CLKIN
+#elif PWMIN_TIMER == 11
+# define PWMIN_TIMER_BASE	STM32_TIM11_BASE
+# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define PWMIN_TIMER_POWER_BIT	RCC_APB2ENR_TIM11EN
+# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM1TRGCOM
+# define PWMIN_TIMER_CLOCK	STM32_APB2_TIM11_CLKIN
+#elif PWMIN_TIMER == 12
+# define PWMIN_TIMER_BASE	STM32_TIM12_BASE
+# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define PWMIN_TIMER_POWER_BIT	RCC_APB2ENR_TIM12EN
+# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM1TRGCOM
+# define PWMIN_TIMER_CLOCK	STM32_APB2_TIM12_CLKIN
+#else
+# error PWMIN_TIMER must be a value between 1 and 12
+#endif
+
+/*
+ * HRT clock must be at least 1MHz
+ */
+#if PWMIN_TIMER_CLOCK <= 1000000
+# error PWMIN_TIMER_CLOCK must be greater than 1MHz
+#endif
+
+/*
+ * Timer register accessors
+ */
+#define REG(_reg)	(*(volatile uint32_t *)(PWMIN_TIMER_BASE + _reg))
+
+#define rCR1		REG(STM32_GTIM_CR1_OFFSET)
+#define rCR2		REG(STM32_GTIM_CR2_OFFSET)
+#define rSMCR		REG(STM32_GTIM_SMCR_OFFSET)
+#define rDIER		REG(STM32_GTIM_DIER_OFFSET)
+#define rSR		REG(STM32_GTIM_SR_OFFSET)
+#define rEGR		REG(STM32_GTIM_EGR_OFFSET)
+#define rCCMR1		REG(STM32_GTIM_CCMR1_OFFSET)
+#define rCCMR2		REG(STM32_GTIM_CCMR2_OFFSET)
+#define rCCER		REG(STM32_GTIM_CCER_OFFSET)
+#define rCNT		REG(STM32_GTIM_CNT_OFFSET)
+#define rPSC		REG(STM32_GTIM_PSC_OFFSET)
+#define rARR		REG(STM32_GTIM_ARR_OFFSET)
+#define rCCR1		REG(STM32_GTIM_CCR1_OFFSET)
+#define rCCR2		REG(STM32_GTIM_CCR2_OFFSET)
+#define rCCR3		REG(STM32_GTIM_CCR3_OFFSET)
+#define rCCR4		REG(STM32_GTIM_CCR4_OFFSET)
+#define rDCR		REG(STM32_GTIM_DCR_OFFSET)
+#define rDMAR		REG(STM32_GTIM_DMAR_OFFSET)
+
+/*
+ * Specific registers and bits used by HRT sub-functions
+ */
+#if PWMIN_TIMER_CHANNEL == 1
+ #define rCCR_PWMIN_A		rCCR1			/* compare register for PWMIN */
+ #define DIER_PWMIN_A		(GTIM_DIER_CC1IE) 	/* interrupt enable for PWMIN */
+ #define SR_INT_PWMIN_A		GTIM_SR_CC1IF		/* interrupt status for PWMIN */
+ #define rCCR_PWMIN_B		rCCR2 			/* compare register for PWMIN */
+ #define SR_INT_PWMIN_B		GTIM_SR_CC2IF		/* interrupt status for PWMIN */
+ #define CCMR1_PWMIN		((0x02 << GTIM_CCMR1_CC2S_SHIFT) | (0x01 << GTIM_CCMR1_CC1S_SHIFT))
+ #define CCMR2_PWMIN		0
+ #define CCER_PWMIN		(GTIM_CCER_CC2P | GTIM_CCER_CC1E | GTIM_CCER_CC2E)
+ #define SR_OVF_PWMIN		(GTIM_SR_CC1OF | GTIM_SR_CC2OF)
+ #define SMCR_PWMIN_1		(0x05 << GTIM_SMCR_TS_SHIFT)
+ #define SMCR_PWMIN_2		((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1)
+#elif PWMIN_TIMER_CHANNEL == 2
+ #define rCCR_PWMIN_A		rCCR2			/* compare register for PWMIN */
+ #define DIER_PWMIN_A		(GTIM_DIER_CC2IE)	/* interrupt enable for PWMIN */
+ #define SR_INT_PWMIN_A		GTIM_SR_CC2IF		/* interrupt status for PWMIN */
+ #define rCCR_PWMIN_B		rCCR1			/* compare register for PWMIN */
+ #define DIER_PWMIN_B		GTIM_DIER_CC1IE		/* interrupt enable for PWMIN */
+ #define SR_INT_PWMIN_B		GTIM_SR_CC1IF		/* interrupt status for PWMIN */
+ #define CCMR1_PWMIN		((0x01 << GTIM_CCMR1_CC2S_SHIFT) | (0x02 << GTIM_CCMR1_CC1S_SHIFT))
+ #define CCMR2_PWMIN		0
+ #define CCER_PWMIN		(GTIM_CCER_CC1P | GTIM_CCER_CC1E | GTIM_CCER_CC2E)
+ #define SR_OVF_PWMIN		(GTIM_SR_CC1OF | GTIM_SR_CC2OF)
+ #define SMCR_PWMIN_1		(0x06 << GTIM_SMCR_TS_SHIFT)
+ #define SMCR_PWMIN_2		((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1)
+#else
+ #error PWMIN_TIMER_CHANNEL must be either 1 and 2.
+#endif
+
+
+class PWMIN : device::CDev
+{
+public:
+	PWMIN();
+	virtual ~PWMIN();
+
+	virtual int init();
+	virtual int open(struct file *filp);
+	virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
+	virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	void _publish(uint16_t status, uint32_t period, uint32_t pulse_width);
+	void _print_info(void);
+
+private:
+	uint32_t error_count;
+	uint32_t pulses_captured;
+	uint32_t last_period;
+	uint32_t last_width;
+	RingBuffer *reports;
+	bool timer_started;
+
+	void timer_init(void);
+};
+
+static int pwmin_tim_isr(int irq, void *context);
+static void pwmin_start(void);
+static void pwmin_info(void);
+static void pwmin_test(void);
+static void pwmin_reset(void);
+
+static PWMIN *g_dev;
+
+PWMIN::PWMIN() :
+	CDev("pwmin", PWMIN0_DEVICE_PATH),
+	error_count(0),
+	pulses_captured(0),
+	last_period(0),
+	last_width(0),
+	reports(nullptr),
+	timer_started(false)
+{
+}
+
+PWMIN::~PWMIN()
+{
+	if (reports != nullptr)
+		delete reports;
+}
+
+/*
+  initialise the driver. This doesn't actually start the timer (that
+  is done on open). We don't start the timer to allow for this driver
+  to be started in init scripts when the user may be using the input
+  pin as PWM output
+ */
+int
+PWMIN::init()
+{
+	// we just register the device in /dev, and only actually
+	// activate the timer when requested to when the device is opened
+	CDev::init();
+
+	reports = new RingBuffer(2, sizeof(struct pwm_input_s));
+	if (reports == nullptr) {
+		return -ENOMEM;
+	}
+
+	return OK;
+}
+
+/*
+ * Initialise the timer we are going to use.
+ */
+void PWMIN::timer_init(void) 
+{
+	// run with interrupts disabled in case the timer is already
+	// setup. We don't want it firing while we are doing the setup
+	irqstate_t flags = irqsave();
+	stm32_configgpio(GPIO_PWM_IN);
+
+	/* claim our interrupt vector */
+	irq_attach(PWMIN_TIMER_VECTOR, pwmin_tim_isr);
+
+	/* Clear no bits, set timer enable bit.*/
+	modifyreg32(PWMIN_TIMER_POWER_REG, 0, PWMIN_TIMER_POWER_BIT); 
+
+	/* disable and configure the timer */
+	rCR1 = 0;
+	rCR2 = 0;
+	rSMCR = 0;
+	rDIER = DIER_PWMIN_A;
+	rCCER = 0;		/* unlock CCMR* registers */
+	rCCMR1 = CCMR1_PWMIN;
+	rCCMR2 = CCMR2_PWMIN;
+	rSMCR = SMCR_PWMIN_1;	/* Set up mode */
+	rSMCR = SMCR_PWMIN_2;	/* Enable slave mode controller */
+	rCCER = CCER_PWMIN;
+	rDCR = 0;
+
+	// for simplicity scale by the clock in MHz. This gives us
+	// readings in microseconds which is typically what is needed
+	// for a PWM input driver
+	uint32_t prescaler = PWMIN_TIMER_CLOCK/1000000UL;
+
+	/* 
+	 * define the clock speed. We want the highest possible clock 
+	 * speed that avoids overflows.
+	 */
+	rPSC = prescaler - 1;
+
+	/* run the full span of the counter. All timers can handle
+	 * uint16 */
+	rARR = UINT16_MAX;
+
+	/* generate an update event; reloads the counter, all registers */
+	rEGR = GTIM_EGR_UG;
+
+	/* enable the timer */
+	rCR1 = GTIM_CR1_CEN;
+
+	/* enable interrupts */
+	up_enable_irq(PWMIN_TIMER_VECTOR);
+
+	irqrestore(flags);
+
+	timer_started = true;
+}
+
+/*
+  hook for open of the driver. We start the timer at this point, then
+  leave it running
+ */
+int
+PWMIN::open(struct file *filp)
+{
+	if (g_dev == nullptr) {
+		return -EIO;
+	}
+	int ret = CDev::open(filp);	
+	if (ret == OK && !timer_started) {
+		g_dev->timer_init();
+	}
+	return ret;
+}
+
+
+/*
+  handle ioctl requests
+ */
+int
+PWMIN::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+	case SENSORIOCSQUEUEDEPTH: {
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 1) || (arg > 500))
+				return -EINVAL;
+
+			irqstate_t flags = irqsave();
+			if (!reports->resize(arg)) {
+				irqrestore(flags);
+				return -ENOMEM;
+			}
+			irqrestore(flags);
+
+			return OK;
+		}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return reports->size();
+
+	case SENSORIOCRESET:
+		/* user has asked for the timer to be reset. This may
+		   be needed if the pin was used for a different
+		   purpose (such as PWM output)
+		*/
+		timer_init();
+		return OK;
+
+	default:
+		/* give it to the superclass */
+		return CDev::ioctl(filp, cmd, arg);
+	}
+}
+
+
+/*
+  read some samples from the device
+ */
+ssize_t
+PWMIN::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct pwm_input_s);
+	struct pwm_input_s *buf = reinterpret_cast<struct pwm_input_s *>(buffer);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1)
+		return -ENOSPC;
+
+	while (count--) {
+		if (reports->get(buf)) {
+			ret += sizeof(struct pwm_input_s);
+			buf++;
+		}
+	}
+
+	/* if there was no data, warn the caller */
+	return ret ? ret : -EAGAIN;
+}
+
+/*
+  publish some data from the ISR in the ring buffer
+ */
+void PWMIN::_publish(uint16_t status, uint32_t period, uint32_t pulse_width)
+{
+	/* if we missed an edge, we have to give up */
+	if (status & SR_OVF_PWMIN) {
+		error_count++;
+		return;
+	}
+
+	struct pwm_input_s pwmin_report;
+	pwmin_report.timestamp = hrt_absolute_time();
+	pwmin_report.error_count = error_count;
+	pwmin_report.period = period;
+	pwmin_report.pulse_width = pulse_width;
+
+	reports->force(&pwmin_report);
+
+	last_period = period;
+	last_width = pulse_width;
+	pulses_captured++;
+}
+
+/*
+  print information on the last captured
+ */
+void PWMIN::_print_info(void)
+{
+	if (!timer_started) {
+		printf("timer not started - try the 'test' command\n");
+	} else {
+		printf("count=%u period=%u width=%u\n",
+		       (unsigned)pulses_captured,
+		       (unsigned)last_period,
+		       (unsigned)last_width);
+	}
+}
+
+
+/*
+  Handle the interupt, gathering pulse data
+ */
+static int pwmin_tim_isr(int irq, void *context)
+{
+	uint16_t status = rSR;
+	uint32_t period = rCCR_PWMIN_A;
+	uint32_t pulse_width = rCCR_PWMIN_B;
+
+	/* ack the interrupts we just read */
+	rSR = 0;
+
+	if (g_dev != nullptr) {
+		g_dev->_publish(status, period, pulse_width);
+	}
+
+	return OK;
+}
+
+/*
+  start the driver
+ */
+static void pwmin_start(void)
+{
+	if (g_dev != nullptr) {
+		printf("driver already started\n");
+		exit(1);
+	}
+	g_dev = new PWMIN();
+	if (g_dev == nullptr) {
+		errx(1, "driver allocation failed");
+	}
+	if (g_dev->init() != OK) {
+		errx(1, "driver init failed");
+	}
+	exit(0);
+}
+
+/*
+  test the driver
+ */
+static void pwmin_test(void)
+{
+	int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY);
+	if (fd == -1) {
+		errx(1, "Failed to open device");
+	}
+	uint64_t start_time = hrt_absolute_time();
+
+	printf("Showing samples for 5 seconds\n");
+
+	while (hrt_absolute_time() < start_time+5U*1000UL*1000UL) {
+		struct pwm_input_s buf;
+		if (::read(fd, &buf, sizeof(buf)) == sizeof(buf)) {
+			printf("period=%u width=%u error_count=%u\n",
+			       (unsigned)buf.period,
+			       (unsigned)buf.pulse_width,
+			       (unsigned)buf.error_count);
+		}
+	}
+	close(fd);
+	exit(0);
+}
+
+/*
+  reset the timer
+ */
+static void pwmin_reset(void)
+{
+	int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY);
+	if (fd == -1) {
+		errx(1, "Failed to open device");
+	}
+	if (ioctl(fd, SENSORIOCRESET, 0) != OK) {
+		errx(1, "reset failed");
+	}
+	close(fd);
+	exit(0);
+}
+
+/*
+  show some information on the driver
+ */
+static void pwmin_info(void)
+{
+	if (g_dev == nullptr) {
+		printf("driver not started\n");
+		exit(1);
+	}
+	g_dev->_print_info();
+	exit(0);
+}
+
+
+/*
+  driver entry point
+ */
+int pwm_input_main(int argc, char * argv[]) 
+{
+	const char *verb = argv[1];
+
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(verb, "start")) {
+		pwmin_start();
+	}
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(verb, "info")) {
+		pwmin_info();
+	}
+
+	/*
+	 * print test results
+	 */
+	if (!strcmp(verb, "test")) {
+		pwmin_test();
+	}
+
+	/*
+	 * reset the timer
+	 */
+	if (!strcmp(verb, "reset")) {
+		pwmin_reset();
+	}
+
+	errx(1, "unrecognized command, try 'start', 'info', 'reset' or 'test'");
+	return 0;
+}
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 3a9246bf2..7b09a4676 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -182,6 +182,7 @@ private:
 
 	void		gpio_reset(void);
 	void		sensor_reset(int ms);
+	void		peripheral_reset(int ms);
 	void		gpio_set_function(uint32_t gpios, int function);
 	void		gpio_write(uint32_t gpios, int function);
 	uint32_t	gpio_read(void);
@@ -1376,6 +1377,29 @@ PX4FMU::sensor_reset(int ms)
 #endif
 }
 
+void
+PX4FMU::peripheral_reset(int ms)
+{
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+
+	if (ms < 1) {
+		ms = 10;
+	}
+
+	/* set the peripheral rails off */
+	stm32_configgpio(GPIO_VDD_5V_PERIPH_EN);
+	stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 1);
+
+	/* wait for the peripheral rail to reach GND */
+	usleep(ms * 1000);
+	warnx("reset done, %d ms", ms);
+
+	/* re-enable power */
+
+	/* switch the peripheral rail back on */
+	stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 0);
+#endif
+}
 
 void
 PX4FMU::gpio_reset(void)
@@ -1488,6 +1512,10 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
 		sensor_reset(arg);
 		break;
 
+	case GPIO_PERIPHERAL_RAIL_RESET:
+		peripheral_reset(arg);
+		break;
+
 	case GPIO_SET_OUTPUT:
 	case GPIO_SET_INPUT:
 	case GPIO_SET_ALT_1:
@@ -1529,6 +1557,7 @@ enum PortMode {
 	PORT_GPIO_AND_SERIAL,
 	PORT_PWM_AND_SERIAL,
 	PORT_PWM_AND_GPIO,
+	PORT_PWM4,
 };
 
 PortMode g_port_mode;
@@ -1564,6 +1593,13 @@ fmu_new_mode(PortMode new_mode)
 #endif
 		break;
 
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+	case PORT_PWM4:
+		/* select 4-pin PWM mode */
+		servo_mode = PX4FMU::MODE_4PWM;
+		break;
+#endif
+
 		/* mixed modes supported on v1 board only */
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 
@@ -1667,6 +1703,24 @@ sensor_reset(int ms)
 }
 
 void
+peripheral_reset(int ms)
+{
+	int	 fd;
+
+	fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
+
+	if (fd < 0) {
+		errx(1, "open fail");
+	}
+
+	if (ioctl(fd, GPIO_PERIPHERAL_RAIL_RESET, ms) < 0) {
+		warnx("peripheral rail reset failed");
+	}
+
+	close(fd);
+}
+
+void
 test(void)
 {
 	int	 fd;
@@ -1836,6 +1890,10 @@ fmu_main(int argc, char *argv[])
 	} else if (!strcmp(verb, "mode_pwm")) {
 		new_mode = PORT_FULL_PWM;
 
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+	} else if (!strcmp(verb, "mode_pwm4")) {
+		new_mode = PORT_PWM4;
+#endif
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 
 	} else if (!strcmp(verb, "mode_serial")) {
@@ -1883,6 +1941,19 @@ fmu_main(int argc, char *argv[])
 		exit(0);
 	}
 
+	if (!strcmp(verb, "peripheral_reset")) {
+		if (argc > 2) {
+			int reset_time = strtol(argv[2], 0, 0);
+			peripheral_reset(reset_time);
+
+		} else {
+			peripheral_reset(0);
+			warnx("resettet default time");
+		}
+
+		exit(0);
+	}
+
 	if (!strcmp(verb, "i2c")) {
 		if (argc > 3) {
 			int bus = strtol(argv[2], 0, 0);
diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp
index bf6e3365d..d28966fca 100644
--- a/src/lib/mathlib/math/Quaternion.hpp
+++ b/src/lib/mathlib/math/Quaternion.hpp
@@ -116,6 +116,35 @@ public:
 	}
 
 	/**
+	 * inverse of quaternion
+	 */
+	math::Quaternion inverse() {
+		Quaternion res;
+		memcpy(res.data,data,sizeof(res.data));
+		res.data[1] = -res.data[1];
+		res.data[2] = -res.data[2];
+		res.data[3] = -res.data[3];
+		return res;
+	}
+
+
+	/**
+	* rotate vector by quaternion
+	*/
+	Vector<3> rotate(const Vector<3> &w) {
+		Quaternion q_w;	// extend vector to quaternion
+		Quaternion q = {data[0],data[1],data[2],data[3]};
+		Quaternion q_rotated;	// quaternion representation of rotated vector
+		q_w(0) = 0;
+		q_w(1) = w.data[0];
+		q_w(2) = w.data[1];
+		q_w(3) = w.data[2];
+		q_rotated = q*q_w*q.inverse();
+		Vector<3> res = {q_rotated.data[1],q_rotated.data[2],q_rotated.data[3]};
+		return res;
+	}
+
+	/**
 	 * set quaternion to rotation defined by euler angles
 	 */
 	void from_euler(float roll, float pitch, float yaw) {
@@ -135,12 +164,38 @@ public:
 		data[3] = static_cast<float>(cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2);
 	}
 
-	void from_dcm(const Matrix<3, 3> &m) {
-		// avoiding singularities by not using division equations
-		data[0] = 0.5f * sqrtf(1.0f + m.data[0][0] + m.data[1][1] + m.data[2][2]);
-		data[1] = 0.5f * sqrtf(1.0f + m.data[0][0] - m.data[1][1] - m.data[2][2]);
-		data[2] = 0.5f * sqrtf(1.0f - m.data[0][0] + m.data[1][1] - m.data[2][2]);
-		data[3] = 0.5f * sqrtf(1.0f - m.data[0][0] - m.data[1][1] + m.data[2][2]);
+	/**
+	 * set quaternion to rotation by DCM
+	 * Reference: Shoemake, Quaternions, http://www.cs.ucr.edu/~vbz/resources/quatut.pdf
+	 */
+	void from_dcm(const Matrix<3, 3> &dcm) {
+		float tr = dcm.data[0][0] + dcm.data[1][1] + dcm.data[2][2];
+		if (tr > 0.0f) {
+			float s = sqrtf(tr + 1.0f);
+			data[0] = s * 0.5f;
+			s = 0.5f / s;
+			data[1] = (dcm.data[2][1] - dcm.data[1][2]) * s;
+			data[2] = (dcm.data[0][2] - dcm.data[2][0]) * s;
+			data[3] = (dcm.data[1][0] - dcm.data[0][1]) * s;
+		} else {
+			/* Find maximum diagonal element in dcm
+			* store index in dcm_i */
+			int dcm_i = 0;
+			for (int i = 1; i < 3; i++) {
+				if (dcm.data[i][i] > dcm.data[dcm_i][dcm_i]) {
+					dcm_i = i;
+				}
+			}
+			int dcm_j = (dcm_i + 1) % 3;
+			int dcm_k = (dcm_i + 2) % 3;
+			float s = sqrtf((dcm.data[dcm_i][dcm_i] - dcm.data[dcm_j][dcm_j] -
+			dcm.data[dcm_k][dcm_k]) + 1.0f);
+			data[dcm_i + 1] = s * 0.5f;
+			s = 0.5f / s;
+			data[dcm_j + 1] = (dcm.data[dcm_i][dcm_j] + dcm.data[dcm_j][dcm_i]) * s;
+			data[dcm_k + 1] = (dcm.data[dcm_k][dcm_i] + dcm.data[dcm_i][dcm_k]) * s;
+			data[0] = (dcm.data[dcm_k][dcm_j] - dcm.data[dcm_j][dcm_k]) * s;
+		}
 	}
 
 	/**
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 87065b56f..d70e05000 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -358,7 +358,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se
 
 		if (orient < 0) {
 			mavlink_and_console_log_info(mavlink_fd, "invalid motion, hold still...");
-			sleep(3);
+			sleep(2);
 			continue;
 		}
 
@@ -372,6 +372,8 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se
 		mavlink_and_console_log_info(mavlink_fd, "Hold still, starting to measure %s side", orientation_strs[orient]);
 		sleep(1);
 		read_accelerometer_avg(subs, accel_ref, orient, samples_num);
+		mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]);
+		usleep(100000);
 		mavlink_and_console_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient],
 				 (double)accel_ref[0][orient][0],
 				 (double)accel_ref[0][orient][1],
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 242d8a486..f832f761e 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -65,7 +65,7 @@
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/offboard_control_setpoint.h>
+#include <uORB/topics/offboard_control_mode.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
@@ -180,7 +180,7 @@ static struct vehicle_status_s status;
 static struct actuator_armed_s armed;
 static struct safety_s safety;
 static struct vehicle_control_mode_s control_mode;
-static struct offboard_control_setpoint_s sp_offboard;
+static struct offboard_control_mode_s offboard_control_mode;
 
 /* tasks waiting for low prio thread */
 typedef enum {
@@ -966,7 +966,7 @@ int commander_thread_main(int argc, char *argv[])
 
 	pthread_attr_t commander_low_prio_attr;
 	pthread_attr_init(&commander_low_prio_attr);
-	pthread_attr_setstacksize(&commander_low_prio_attr, 2900);
+	pthread_attr_setstacksize(&commander_low_prio_attr, 2400);
 
 	struct sched_param param;
 	(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
@@ -1016,8 +1016,8 @@ int commander_thread_main(int argc, char *argv[])
 	memset(&sp_man, 0, sizeof(sp_man));
 
 	/* Subscribe to offboard control data */
-	int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
-	memset(&sp_offboard, 0, sizeof(sp_offboard));
+	int offboard_control_mode_sub = orb_subscribe(ORB_ID(offboard_control_mode));
+	memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));
 
 	/* Subscribe to telemetry status topics */
 	int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM];
@@ -1227,14 +1227,14 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
 		}
 
-		orb_check(sp_offboard_sub, &updated);
+		orb_check(offboard_control_mode_sub, &updated);
 
 		if (updated) {
-			orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
+			orb_copy(ORB_ID(offboard_control_mode), offboard_control_mode_sub, &offboard_control_mode);
 		}
 
-		if (sp_offboard.timestamp != 0 &&
-		    sp_offboard.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) {
+		if (offboard_control_mode.timestamp != 0 &&
+		    offboard_control_mode.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) {
 			if (status.offboard_control_signal_lost) {
 				status.offboard_control_signal_lost = false;
 				status_changed = true;
@@ -2051,7 +2051,7 @@ int commander_thread_main(int argc, char *argv[])
 	led_deinit();
 	buzzer_deinit();
 	close(sp_man_sub);
-	close(sp_offboard_sub);
+	close(offboard_control_mode_sub);
 	close(local_position_sub);
 	close(global_position_sub);
 	close(gps_sub);
@@ -2426,56 +2426,30 @@ set_control_mode()
 		control_mode.flag_control_auto_enabled = false;
 		control_mode.flag_control_offboard_enabled = true;
 
-		switch (sp_offboard.mode) {
-		case OFFBOARD_CONTROL_MODE_DIRECT_RATES:
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = false;
-			control_mode.flag_control_altitude_enabled = false;
-			control_mode.flag_control_climb_rate_enabled = false;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-			break;
+		/*
+		 * The control flags depend on what is ignored according to the offboard control mode topic
+		 * Inner loop flags (e.g. attitude) also depend on outer loop ignore flags (e.g. position)
+		 */
+		control_mode.flag_control_rates_enabled = !offboard_control_mode.ignore_bodyrate ||
+			!offboard_control_mode.ignore_attitude ||
+			!offboard_control_mode.ignore_position ||
+			!offboard_control_mode.ignore_velocity ||
+			!offboard_control_mode.ignore_acceleration_force;
 
-		case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = true;
-			control_mode.flag_control_altitude_enabled = false;
-			control_mode.flag_control_climb_rate_enabled = false;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-			break;
+		control_mode.flag_control_attitude_enabled = !offboard_control_mode.ignore_attitude ||
+			!offboard_control_mode.ignore_position ||
+			!offboard_control_mode.ignore_velocity ||
+			!offboard_control_mode.ignore_acceleration_force;
 
-		case OFFBOARD_CONTROL_MODE_DIRECT_FORCE:
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = false;
-			control_mode.flag_control_force_enabled = true;
-			control_mode.flag_control_altitude_enabled = false;
-			control_mode.flag_control_climb_rate_enabled = false;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-			break;
+		control_mode.flag_control_velocity_enabled = !offboard_control_mode.ignore_velocity ||
+			!offboard_control_mode.ignore_position;
 
-		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED:
-		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED:
-		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED:
-		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED:
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = true;
-			control_mode.flag_control_altitude_enabled = true;
-			control_mode.flag_control_climb_rate_enabled = true;
-			control_mode.flag_control_position_enabled = true;
-			control_mode.flag_control_velocity_enabled = true;
-			//XXX: the flags could depend on sp_offboard.ignore
-			break;
+		control_mode.flag_control_climb_rate_enabled = !offboard_control_mode.ignore_velocity ||
+			!offboard_control_mode.ignore_position;
 
-		default:
-			control_mode.flag_control_rates_enabled = false;
-			control_mode.flag_control_attitude_enabled = false;
-			control_mode.flag_control_altitude_enabled = false;
-			control_mode.flag_control_climb_rate_enabled = false;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-		}
+		control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position;
+
+		control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_position;
 
 		break;
 
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index a94b478c4..e0786db79 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -137,6 +137,9 @@ int do_mag_calibration(int mavlink_fd)
 	}
 
 	if (calibrated_ok) {
+
+		mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100);
+		usleep(100000);
 		mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
 
 		/* auto-save to EEPROM */
@@ -155,7 +158,7 @@ int do_mag_calibration(int mavlink_fd)
 int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id)
 {
 	/* 45 seconds */
-	uint64_t calibration_interval = 45 * 1000 * 1000;
+	uint64_t calibration_interval = 25 * 1000 * 1000;
 
 	/* maximum 500 values */
 	const unsigned int calibration_maxcount = 240;
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 40aa4a2f0..0154f235f 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-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
@@ -35,7 +35,7 @@
  * @file state_machine_helper.cpp
  * State machine helper functions implementations
  *
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Thomas Gubler <thomas@px4.io>
  * @author Julian Oes <julian@oes.ch>
  */
 
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 1c79cb61d..903158129 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -457,9 +457,7 @@ int AttitudePositionEstimatorEKF::check_filter_state()
 			rep.states[i] = ekf_report.states[i];
 		}
 
-		for (size_t i = 0; i < rep.n_states; i++) {
-			rep.states[i] = ekf_report.states[i];
-		}
+
 
 		if (_estimator_status_pub > 0) {
 			orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep);
@@ -774,14 +772,14 @@ void AttitudePositionEstimatorEKF::publishAttitude()
 	_att.pitch = euler(1);
 	_att.yaw = euler(2);
 
-	_att.rollspeed = _ekf->angRate.x - _ekf->states[10];
-	_att.pitchspeed = _ekf->angRate.y - _ekf->states[11];
-	_att.yawspeed = _ekf->angRate.z - _ekf->states[12];
+	_att.rollspeed = _ekf->angRate.x - _ekf->states[10] / _ekf->dtIMU;
+	_att.pitchspeed = _ekf->angRate.y - _ekf->states[11] / _ekf->dtIMU;
+	_att.yawspeed = _ekf->angRate.z - _ekf->states[12] / _ekf->dtIMU;
 
 	// gyro offsets
-	_att.rate_offsets[0] = _ekf->states[10];
-	_att.rate_offsets[1] = _ekf->states[11];
-	_att.rate_offsets[2] = _ekf->states[12];
+	_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMU;
+	_att.rate_offsets[1] = _ekf->states[11] / _ekf->dtIMU;
+	_att.rate_offsets[2] = _ekf->states[12] / _ekf->dtIMU;
 
 	/* lazily publish the attitude only once available */
 	if (_att_pub > 0) {
@@ -1056,7 +1054,7 @@ void AttitudePositionEstimatorEKF::print_status()
 	// 4-6: Velocity - m/sec (North, East, Down)
 	// 7-9: Position - m (North, East, Down)
 	// 10-12: Delta Angle bias - rad (X,Y,Z)
-	// 13:    Accelerometer offset
+	// 13:    Delta Velocity Bias - m/s (Z)
 	// 14-15: Wind Vector  - m/sec (North,East)
 	// 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
 	// 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 6e49bd1d1..5daeae477 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -520,7 +520,7 @@ FixedwingAttitudeControl::vehicle_control_mode_poll()
 	orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated);
 
 	if (vcontrol_mode_updated) {
-
+		
 		orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode);
 	}
 }
@@ -795,10 +795,10 @@ FixedwingAttitudeControl::task_main()
 			/* Simple handling of failsafe: deploy parachute if failsafe is on */
 			if (_vcontrol_mode.flag_control_termination_enabled) {
 				_actuators_airframe.control[7] = 1.0f;
-//				warnx("_actuators_airframe.control[1] = 1.0f;");
+				//warnx("_actuators_airframe.control[1] = 1.0f;");
 			} else {
 				_actuators_airframe.control[7] = 0.0f;
-//				warnx("_actuators_airframe.control[1] = -1.0f;");
+				//warnx("_actuators_airframe.control[1] = -1.0f;");
 			}
 
 			/* decide if in stabilized or full manual control */
@@ -1077,20 +1077,25 @@ FixedwingAttitudeControl::task_main()
 			_actuators_airframe.timestamp = hrt_absolute_time();
 			_actuators_airframe.timestamp_sample = _att.timestamp;
 
-			/* publish the actuator controls */
-			if (_actuators_0_pub > 0) {
-				orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
-			} else if (_actuators_id) {
-				_actuators_0_pub= orb_advertise(_actuators_id, &_actuators);
-			}
+			/* Only publish if any of the proper modes are enabled */
+			if(_vcontrol_mode.flag_control_rates_enabled ||
+			   _vcontrol_mode.flag_control_attitude_enabled)
+			{
+				/* publish the actuator controls */
+				if (_actuators_0_pub > 0) {
+					orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
+				} else if (_actuators_id) {
+					_actuators_0_pub= orb_advertise(_actuators_id, &_actuators);
+				}
 
-			if (_actuators_2_pub > 0) {
-				/* publish the actuator controls*/
-				orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe);
+				if (_actuators_2_pub > 0) {
+					/* publish the actuator controls*/
+					orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe);
 
-			} else {
-				/* advertise and publish */
-				_actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe);
+				} else {
+					/* advertise and publish */
+					_actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe);
+				}
 			}
 		}
 
diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c
index a61108c4c..6ab3ddbfc 100644
--- a/src/modules/fw_att_control/fw_att_control_params.c
+++ b/src/modules/fw_att_control/fw_att_control_params.c
@@ -302,10 +302,10 @@ PARAM_DEFINE_INT32(FW_YCO_METHOD, 0);
  *
  * @unit m/s
  * @min 0.0
- * @max 30.0
+ * @max 40
  * @group FW Attitude Control
  */
-PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 13.0f);
+PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f);
 
 /**
  * Trim Airspeed
@@ -314,10 +314,10 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 13.0f);
  *
  * @unit m/s
  * @min 0.0
- * @max 30.0
+ * @max 40
  * @group FW Attitude Control
  */
-PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f);
+PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f);
 
 /**
  * Maximum Airspeed
@@ -327,10 +327,10 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f);
  *
  * @unit m/s
  * @min 0.0
- * @max 30.0
+ * @max 40
  * @group FW Attitude Control
  */
-PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 50.0f);
+PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f);
 
 /**
  * Roll Setpoint Offset
diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp
index 8e5bcf7ba..e26954d1a 100644
--- a/src/modules/land_detector/FixedwingLandDetector.cpp
+++ b/src/modules/land_detector/FixedwingLandDetector.cpp
@@ -66,6 +66,8 @@ void FixedwingLandDetector::initialize()
 	// Subscribe to local position and airspeed data
 	_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
 	_airspeedSub = orb_subscribe(ORB_ID(airspeed));
+
+	updateParameterCache(true);
 }
 
 void FixedwingLandDetector::updateSubscriptions()
diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp
index 1e43e7ad5..011567e57 100644
--- a/src/modules/land_detector/land_detector_main.cpp
+++ b/src/modules/land_detector/land_detector_main.cpp
@@ -139,7 +139,7 @@ static int land_detector_start(const char *mode)
 	_landDetectorTaskID = task_spawn_cmd("land_detector",
 					     SCHED_DEFAULT,
 					     SCHED_PRIORITY_DEFAULT,
-					     1200,
+					     1000,
 					     (main_t)&land_detector_deamon_thread,
 					     nullptr);
 
@@ -179,8 +179,7 @@ int land_detector_main(int argc, char *argv[])
 {
 
 	if (argc < 1) {
-		warnx("usage: land_detector {start|stop|status} [mode]\nmode can either be 'fixedwing' or 'multicopter'");
-		exit(0);
+		goto exiterr;
 	}
 
 	if (argc >= 2 && !strcmp(argv[1], "start")) {
@@ -209,6 +208,8 @@ int land_detector_main(int argc, char *argv[])
 		}
 	}
 
-	warn("usage: land_detector {start|stop|status} [mode]\nmode can either be 'fixedwing' or 'multicopter'");
+exiterr:
+	warnx("usage: land_detector {start|stop|status} [mode]");
+	warnx("mode can either be 'fixedwing' or 'multicopter'");
 	return 1;
 }
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 024dfd6fb..f8e819ce7 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1621,7 +1621,7 @@ Mavlink::start(int argc, char *argv[])
 	task_spawn_cmd(buf,
 		       SCHED_DEFAULT,
 		       SCHED_PRIORITY_DEFAULT,
-		       2800,
+		       2400,
 		       (main_t)&Mavlink::start_helper,
 		       (char * const *)argv);
 
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 9711d8fc3..7d6b60e22 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -51,7 +51,6 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/offboard_control_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_vicon_position.h>
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 90c3cb47f..0c34fc58a 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -108,7 +108,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_cmd_pub(-1),
 	_flow_pub(-1),
 	_range_pub(-1),
-	_offboard_control_sp_pub(-1),
+	_offboard_control_mode_pub(-1),
+	_actuator_controls_pub(-1),
 	_global_vel_sp_pub(-1),
 	_att_sp_pub(-1),
 	_rates_sp_pub(-1),
@@ -170,6 +171,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_set_attitude_target(msg);
 		break;
 
+	case MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET:
+		handle_message_set_actuator_control_target(msg);
+		break;
+
 	case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
 		handle_message_vision_position_estimate(msg);
 		break;
@@ -517,8 +522,8 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 	mavlink_set_position_target_local_ned_t set_position_target_local_ned;
 	mavlink_msg_set_position_target_local_ned_decode(msg, &set_position_target_local_ned);
 
-	struct offboard_control_setpoint_s offboard_control_sp;
-	memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));//XXX breaks compatibility with multiple setpoints
+	struct offboard_control_mode_s offboard_control_mode;
+	memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints
 
 	/* Only accept messages which are intended for this system */
 	if ((mavlink_system.sysid == set_position_target_local_ned.target_system ||
@@ -527,64 +532,22 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 			 set_position_target_local_ned.target_component == 0)) {
 
 		/* convert mavlink type (local, NED) to uORB offboard control struct */
-		switch (set_position_target_local_ned.coordinate_frame) {
-			case MAV_FRAME_LOCAL_NED:
-				offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED;
-				break;
-			case MAV_FRAME_LOCAL_OFFSET_NED:
-				offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED;
-				break;
-			case MAV_FRAME_BODY_NED:
-				offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED;
-				break;
-			case MAV_FRAME_BODY_OFFSET_NED:
-				offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED;
-				break;
-			default:
-				/* invalid setpoint, avoid publishing */
-				return;
-		}
-		offboard_control_sp.position[0] = set_position_target_local_ned.x;
-		offboard_control_sp.position[1] = set_position_target_local_ned.y;
-		offboard_control_sp.position[2] = set_position_target_local_ned.z;
-		offboard_control_sp.velocity[0] = set_position_target_local_ned.vx;
-		offboard_control_sp.velocity[1] = set_position_target_local_ned.vy;
-		offboard_control_sp.velocity[2] = set_position_target_local_ned.vz;
-		offboard_control_sp.acceleration[0] = set_position_target_local_ned.afx;
-		offboard_control_sp.acceleration[1] = set_position_target_local_ned.afy;
-		offboard_control_sp.acceleration[2] = set_position_target_local_ned.afz;
-		offboard_control_sp.yaw = set_position_target_local_ned.yaw;
-		offboard_control_sp.yaw_rate = set_position_target_local_ned.yaw_rate;
-		offboard_control_sp.isForceSetpoint = (bool)(set_position_target_local_ned.type_mask & (1 << 9));
-
-		/* If we are in force control mode, for now set offboard mode to force control */
-		if (offboard_control_sp.isForceSetpoint) {
-			offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_FORCE;
-		}
-
-		/* set ignore flags */
-		for (int i = 0; i < 9; i++) {
-			offboard_control_sp.ignore &=  ~(1 << i);
-			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << i));
-		}
-
-		offboard_control_sp.ignore &=  ~(1 << OFB_IGN_BIT_YAW);
-		if (set_position_target_local_ned.type_mask & (1 << 10)) {
-			offboard_control_sp.ignore |=  (1 << OFB_IGN_BIT_YAW);
-		}
-
-		offboard_control_sp.ignore &=  ~(1 << OFB_IGN_BIT_YAWRATE);
-		if (set_position_target_local_ned.type_mask & (1 << 11)) {
-			offboard_control_sp.ignore |=  (1 << OFB_IGN_BIT_YAWRATE);
-		}
+		offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7);
+		offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask & 0x38);
+		offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask & 0x1C0);
+		bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9));
+		/* yaw ignore flag mapps to ignore_attitude */
+		offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400);
+		/* yawrate ignore flag mapps to ignore_bodyrate */
+		offboard_control_mode.ignore_bodyrate = (bool)(set_position_target_local_ned.type_mask & 0x800);
 
-		offboard_control_sp.timestamp = hrt_absolute_time();
+		offboard_control_mode.timestamp = hrt_absolute_time();
 
-		if (_offboard_control_sp_pub < 0) {
-			_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
+		if (_offboard_control_mode_pub < 0) {
+			_offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode);
 
 		} else {
-			orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
+			orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode);
 		}
 
 		/* If we are in offboard control mode and offboard control loop through is enabled
@@ -596,15 +559,14 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 				orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
 			}
 			if (_control_mode.flag_control_offboard_enabled) {
-				if (offboard_control_sp.isForceSetpoint &&
-						offboard_control_sp_ignore_position_all(offboard_control_sp) &&
-						offboard_control_sp_ignore_velocity_all(offboard_control_sp)) {
+				if (is_force_sp && offboard_control_mode.ignore_position &&
+						offboard_control_mode.ignore_velocity) {
 					/* The offboard setpoint is a force setpoint only, directly writing to the force
 					 * setpoint topic and not publishing the setpoint triplet topic */
 					struct vehicle_force_setpoint_s	force_sp;
-					force_sp.x = offboard_control_sp.acceleration[0];
-					force_sp.y = offboard_control_sp.acceleration[1];
-					force_sp.z = offboard_control_sp.acceleration[2];
+					force_sp.x = set_position_target_local_ned.afx;
+					force_sp.y = set_position_target_local_ned.afy;
+					force_sp.z = set_position_target_local_ned.afz;
 					//XXX: yaw
 					if (_force_sp_pub < 0) {
 						_force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp);
@@ -619,62 +581,53 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 					pos_sp_triplet.current.valid = true;
 					pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; //XXX support others
 
-					/* set the local pos values if the setpoint type is 'local pos' and none
-					 * of the local pos fields is set to 'ignore' */
-					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
-							!offboard_control_sp_ignore_position_some(offboard_control_sp)) {
-					pos_sp_triplet.current.position_valid = true;
-					pos_sp_triplet.current.x = offboard_control_sp.position[0];
-					pos_sp_triplet.current.y = offboard_control_sp.position[1];
-					pos_sp_triplet.current.z = offboard_control_sp.position[2];
+					/* set the local pos values */
+					if (!offboard_control_mode.ignore_position) {
+						pos_sp_triplet.current.position_valid = true;
+						pos_sp_triplet.current.x = set_position_target_local_ned.x;
+						pos_sp_triplet.current.y = set_position_target_local_ned.y;
+						pos_sp_triplet.current.z = set_position_target_local_ned.z;
 					} else {
 						pos_sp_triplet.current.position_valid = false;
 					}
 
-					/* set the local vel values if the setpoint type is 'local pos' and none
-					 * of the local vel fields is set to 'ignore' */
-					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
-							!offboard_control_sp_ignore_velocity_some(offboard_control_sp)) {
+					/* set the local vel values */
+					if (!offboard_control_mode.ignore_velocity) {
 						pos_sp_triplet.current.velocity_valid = true;
-						pos_sp_triplet.current.vx = offboard_control_sp.velocity[0];
-						pos_sp_triplet.current.vy = offboard_control_sp.velocity[1];
-						pos_sp_triplet.current.vz = offboard_control_sp.velocity[2];
+						pos_sp_triplet.current.vx = set_position_target_local_ned.vx;
+						pos_sp_triplet.current.vy = set_position_target_local_ned.vy;
+						pos_sp_triplet.current.vz = set_position_target_local_ned.vz;
 					} else {
 						pos_sp_triplet.current.velocity_valid = false;
 					}
 
 					/* set the local acceleration values if the setpoint type is 'local pos' and none
 					 * of the accelerations fields is set to 'ignore' */
-					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
-							!offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) {
+					if (!offboard_control_mode.ignore_acceleration_force) {
 						pos_sp_triplet.current.acceleration_valid = true;
-						pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0];
-						pos_sp_triplet.current.a_y = offboard_control_sp.acceleration[1];
-						pos_sp_triplet.current.a_z = offboard_control_sp.acceleration[2];
+						pos_sp_triplet.current.a_x = set_position_target_local_ned.afx;
+						pos_sp_triplet.current.a_y = set_position_target_local_ned.afy;
+						pos_sp_triplet.current.a_z = set_position_target_local_ned.afz;
 						pos_sp_triplet.current.acceleration_is_force =
-						offboard_control_sp.isForceSetpoint;
+						is_force_sp;
 
 					} else {
 						pos_sp_triplet.current.acceleration_valid = false;
 					}
 
-					/* set the yaw sp value if the setpoint type is 'local pos' and the yaw
-					 * field is not set to 'ignore' */
-					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
-							!offboard_control_sp_ignore_yaw(offboard_control_sp)) {
+					/* set the yaw sp value */
+					if (!offboard_control_mode.ignore_attitude) {
 						pos_sp_triplet.current.yaw_valid = true;
-						pos_sp_triplet.current.yaw = offboard_control_sp.yaw;
+						pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw;
 
 					} else {
 						pos_sp_triplet.current.yaw_valid = false;
 					}
 
-					/* set the yawrate sp value if the setpoint type is 'local pos' and the yawrate
-					 * field is not set to 'ignore' */
-					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
-							!offboard_control_sp_ignore_yawrate(offboard_control_sp)) {
+					/* set the yawrate sp value */
+					if (!offboard_control_mode.ignore_bodyrate) {
 						pos_sp_triplet.current.yawspeed_valid = true;
-						pos_sp_triplet.current.yawspeed = offboard_control_sp.yaw_rate;
+						pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate;
 
 					} else {
 						pos_sp_triplet.current.yawspeed_valid = false;
@@ -699,6 +652,66 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 }
 
 void
+MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *msg)
+{
+	mavlink_set_actuator_control_target_t set_actuator_control_target;
+	mavlink_msg_set_actuator_control_target_decode(msg, &set_actuator_control_target);
+
+	struct offboard_control_mode_s offboard_control_mode;
+	memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints
+
+	struct actuator_controls_s actuator_controls;
+	memset(&actuator_controls, 0, sizeof(actuator_controls));//XXX breaks compatibility with multiple setpoints
+
+	if ((mavlink_system.sysid == set_actuator_control_target.target_system ||
+		    set_actuator_control_target.target_system == 0) &&
+		(mavlink_system.compid == set_actuator_control_target.target_component ||
+		    set_actuator_control_target.target_component == 0)) {
+
+		/* ignore all since we are setting raw actuators here */
+		offboard_control_mode.ignore_thrust             = true;
+		offboard_control_mode.ignore_attitude           = true;
+		offboard_control_mode.ignore_bodyrate           = true;
+		offboard_control_mode.ignore_position           = true;
+		offboard_control_mode.ignore_velocity           = true;
+		offboard_control_mode.ignore_acceleration_force = true;
+
+		offboard_control_mode.timestamp = hrt_absolute_time();
+
+		if (_offboard_control_mode_pub < 0) {
+			_offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode);
+		} else {
+			orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode);
+		}
+
+
+		/* If we are in offboard control mode, publish the actuator controls */
+		bool updated;
+		orb_check(_control_mode_sub, &updated);
+		if (updated) {
+			orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
+		}
+
+		if (_control_mode.flag_control_offboard_enabled) {
+
+			actuator_controls.timestamp = hrt_absolute_time();
+
+			/* Set duty cycles for the servos in actuator_controls_0 */
+			for(size_t i = 0; i < 8; i++) {
+				actuator_controls.control[i] = set_actuator_control_target.controls[i];
+			}
+
+			if (_actuator_controls_pub < 0) {
+				_actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls);
+			} else {
+				orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pub, &actuator_controls);
+			}
+		}
+	}
+
+}
+
+void
 MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 {
 	mavlink_vision_position_estimate_t pos;
@@ -743,42 +756,52 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 	mavlink_set_attitude_target_t set_attitude_target;
 	mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target);
 
-	struct offboard_control_setpoint_s offboard_control_sp;
-	memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); //XXX breaks compatibility with multiple setpoints
+	static struct offboard_control_mode_s offboard_control_mode = {};
 
 	/* Only accept messages which are intended for this system */
 	if ((mavlink_system.sysid == set_attitude_target.target_system ||
 				set_attitude_target.target_system == 0) &&
 			(mavlink_system.compid == set_attitude_target.target_component ||
 			 set_attitude_target.target_component == 0)) {
-		for (int i = 0; i < 4; i++) {
-			offboard_control_sp.attitude[i] = set_attitude_target.q[i];
-		}
-		offboard_control_sp.attitude_rate[0] = set_attitude_target.body_roll_rate;
-		offboard_control_sp.attitude_rate[1] = set_attitude_target.body_pitch_rate;
-		offboard_control_sp.attitude_rate[2] = set_attitude_target.body_yaw_rate;
-
-		/* set correct ignore flags for body rate fields: copy from mavlink message */
-		for (int i = 0; i < 3; i++) {
-			offboard_control_sp.ignore &=  ~(1 << (i + OFB_IGN_BIT_BODYRATE_X));
-			offboard_control_sp.ignore |=  (set_attitude_target.type_mask & (1 << i)) << OFB_IGN_BIT_BODYRATE_X;
-		}
+
 		/* set correct ignore flags for thrust field: copy from mavlink message */
-		offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_THRUST);
-		offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 6)) << OFB_IGN_BIT_THRUST);
-		/* set correct ignore flags for attitude field: copy from mavlink message */
-		offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_ATT);
-		offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 7)) << OFB_IGN_BIT_ATT);
+		offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6));
+
+		/*
+		 * The tricky part in parsing this message is that the offboard sender *can* set attitude and thrust
+		 * using different messages. Eg.: First send set_attitude_target containing the attitude and ignore
+		 * bits set for everything else and then send set_attitude_target containing the thrust and ignore bits
+		 * set for everything else.
+		 */
+
+		/*
+		 * if attitude or body rate have been used (not ignored) previously and this message only sends
+		 * throttle and has the ignore bits set for attitude and rates don't change the flags for attitude and
+		 * body rates to keep the controllers running
+		 */
+		bool ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7);
+		bool ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7));
+
+		if (ignore_bodyrate && ignore_attitude && !offboard_control_mode.ignore_thrust) {
+			/* Message want's us to ignore everything except thrust: only ignore if previously ignored */
+			offboard_control_mode.ignore_bodyrate = ignore_bodyrate && offboard_control_mode.ignore_bodyrate;
+			offboard_control_mode.ignore_attitude = ignore_attitude && offboard_control_mode.ignore_attitude;
+		} else {
+			offboard_control_mode.ignore_bodyrate = ignore_bodyrate;
+			offboard_control_mode.ignore_attitude = ignore_attitude;
+		}
 
+		offboard_control_mode.ignore_position = true;
+		offboard_control_mode.ignore_velocity = true;
+		offboard_control_mode.ignore_acceleration_force = true;
 
-		offboard_control_sp.timestamp = hrt_absolute_time();
-		offboard_control_sp.mode =OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; //XXX handle rate control mode
+		offboard_control_mode.timestamp = hrt_absolute_time();
 
-		if (_offboard_control_sp_pub < 0) {
-			_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
+		if (_offboard_control_mode_pub < 0) {
+			_offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode);
 
 		} else {
-			orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
+			orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode);
 		}
 
 		/* If we are in offboard control mode and offboard control loop through is enabled
@@ -793,15 +816,16 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 			if (_control_mode.flag_control_offboard_enabled) {
 
 				/* Publish attitude setpoint if attitude and thrust ignore bits are not set */
-				if (!(offboard_control_sp_ignore_attitude(offboard_control_sp) ||
-							offboard_control_sp_ignore_thrust(offboard_control_sp))) {
-					struct vehicle_attitude_setpoint_s att_sp;
+				if (!(offboard_control_mode.ignore_attitude)) {
+					static struct vehicle_attitude_setpoint_s att_sp = {};
 					att_sp.timestamp = hrt_absolute_time();
 					mavlink_quaternion_to_euler(set_attitude_target.q,
 							&att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body);
 					mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body);
 					att_sp.R_valid = true;
-					att_sp.thrust = set_attitude_target.thrust;
+					if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
+						att_sp.thrust = set_attitude_target.thrust;
+					}
 					att_sp.yaw_sp_move_rate = 0.0;
 					memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d));
 					att_sp.q_d_valid = true;
@@ -814,14 +838,15 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 
 				/* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */
 				///XXX add support for ignoring individual axes
-				if (!(offboard_control_sp_ignore_bodyrates_some(offboard_control_sp) ||
-							offboard_control_sp_ignore_thrust(offboard_control_sp))) {
-					struct vehicle_rates_setpoint_s rates_sp;
+				if (!(offboard_control_mode.ignore_bodyrate)) {
+					static struct vehicle_rates_setpoint_s rates_sp = {};
 					rates_sp.timestamp = hrt_absolute_time();
 					rates_sp.roll = set_attitude_target.body_roll_rate;
 					rates_sp.pitch = set_attitude_target.body_pitch_rate;
 					rates_sp.yaw = set_attitude_target.body_yaw_rate;
-					rates_sp.thrust = set_attitude_target.thrust;
+					if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
+						rates_sp.thrust = set_attitude_target.thrust;
+					}
 
 					if (_att_sp_pub < 0) {
 						_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
@@ -1524,7 +1549,7 @@ MavlinkReceiver::receive_start(Mavlink *parent)
 	param.sched_priority = SCHED_PRIORITY_MAX - 80;
 	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
 
-	pthread_attr_setstacksize(&receiveloop_attr, 2900);
+	pthread_attr_setstacksize(&receiveloop_attr, 2100);
 	pthread_t thread;
 	pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent);
 
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 699996860..4886bbd0a 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -53,7 +53,7 @@
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/offboard_control_setpoint.h>
+#include <uORB/topics/offboard_control_mode.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_global_velocity_setpoint.h>
@@ -122,6 +122,7 @@ private:
 	void handle_message_vision_position_estimate(mavlink_message_t *msg);
 	void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
 	void handle_message_set_position_target_local_ned(mavlink_message_t *msg);
+	void handle_message_set_actuator_control_target(mavlink_message_t *msg);
 	void handle_message_set_attitude_target(mavlink_message_t *msg);
 	void handle_message_radio_status(mavlink_message_t *msg);
 	void handle_message_manual_control(mavlink_message_t *msg);
@@ -142,7 +143,7 @@ private:
 	/**
 	* Exponential moving average filter to smooth time offset
 	*/
-	void smooth_time_offset(uint64_t offset_ns);	
+	void smooth_time_offset(uint64_t offset_ns);
 
 	mavlink_status_t status;
 	struct vehicle_local_position_s hil_local_pos;
@@ -162,7 +163,8 @@ private:
 	orb_advert_t _cmd_pub;
 	orb_advert_t _flow_pub;
 	orb_advert_t _range_pub;
-	orb_advert_t _offboard_control_sp_pub;
+	orb_advert_t _offboard_control_mode_pub;
+	orb_advert_t _actuator_controls_pub;
 	orb_advert_t _global_vel_sp_pub;
 	orb_advert_t _att_sp_pub;
 	orb_advert_t _rates_sp_pub;
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index f9d30dcbe..e82b8bd93 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -51,7 +51,7 @@ INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
 
 MAXOPTIMIZATION	 = -Os
 
-MODULE_STACKSIZE = 1024
+MODULE_STACKSIZE = 1200
 
 EXTRACXXFLAGS	= -Weffc++ -Wno-attributes -Wno-packed
 
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index a0d76b0a6..0243dc2f7 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -835,7 +835,7 @@ MulticopterAttitudeControl::start()
 	_control_task = task_spawn_cmd("mc_att_control",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       2000,
+				       1800,
 				       (main_t)&MulticopterAttitudeControl::task_main_trampoline,
 				       nullptr);
 
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp
index ecbf98c9e..b5a551109 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp
@@ -89,9 +89,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 			.yaw_rate_d	= px4::ParameterFloat("MC_YAWRATE_D", PARAM_MC_YAWRATE_D_DEFAULT),
 			.yaw_ff		= px4::ParameterFloat("MC_YAW_FF", PARAM_MC_YAW_FF_DEFAULT),
 			.yaw_rate_max	= px4::ParameterFloat("MC_YAWRATE_MAX", PARAM_MC_YAWRATE_MAX_DEFAULT),
-			.man_roll_max	= px4::ParameterFloat("MC_MAN_R_MAX", PARAM_MC_MAN_R_MAX_DEFAULT),
-			.man_pitch_max	= px4::ParameterFloat("MC_MAN_P_MAX", PARAM_MC_MAN_P_MAX_DEFAULT),
-			.man_yaw_max	= px4::ParameterFloat("MC_MAN_Y_MAX", PARAM_MC_MAN_Y_MAX_DEFAULT),
 			.acro_roll_max	= px4::ParameterFloat("MC_ACRO_R_MAX", PARAM_MC_ACRO_R_MAX_DEFAULT),
 			.acro_pitch_max	= px4::ParameterFloat("MC_ACRO_P_MAX", PARAM_MC_ACRO_P_MAX_DEFAULT),
 			.acro_yaw_max	= px4::ParameterFloat("MC_ACRO_Y_MAX", PARAM_MC_ACRO_Y_MAX_DEFAULT)
@@ -146,11 +143,6 @@ MulticopterAttitudeControl::parameters_update()
 	_params.yaw_ff = _params_handles.yaw_ff.update();
 	_params.yaw_rate_max = math::radians(_params_handles.yaw_rate_max.update());
 
-	/* manual control scale */
-	_params.man_roll_max = math::radians(_params_handles.man_roll_max.update());
-	_params.man_pitch_max = math::radians(_params_handles.man_pitch_max.update());
-	_params.man_yaw_max = math::radians(_params_handles.man_yaw_max.update());
-
 	/* acro control scale */
 	_params.acro_rate_max(0) = math::radians(_params_handles.acro_roll_max.update());
 	_params.acro_rate_max(1) = math::radians(_params_handles.acro_pitch_max.update());
@@ -186,18 +178,6 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const px4_vehicle_atti
 	if (_v_control_mode->data().flag_control_attitude_enabled) {
 		control_attitude(dt);
 
-		/* publish the attitude setpoint if needed */
-		if (_publish_att_sp && _v_status->data().is_rotary_wing) {
-			_v_att_sp_mod.data().timestamp = px4::get_time_micros();
-
-			if (_att_sp_pub != nullptr) {
-				_att_sp_pub->publish(_v_att_sp_mod);
-
-			} else {
-				_att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>();
-			}
-		}
-
 		/* publish attitude rates setpoint */
 		_v_rates_sp_mod.data().roll = _rates_sp(0);
 		_v_rates_sp_mod.data().pitch = _rates_sp(1);
@@ -224,9 +204,6 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const px4_vehicle_atti
 						    _manual_control_sp->data().r).emult(_params.acro_rate_max);
 			_thrust_sp = _manual_control_sp->data().z;
 
-			/* reset yaw setpoint after ACRO */
-			_reset_yaw_sp = true;
-
 			/* publish attitude rates setpoint */
 			_v_rates_sp_mod.data().roll = _rates_sp(0);
 			_v_rates_sp_mod.data().pitch = _rates_sp(1);
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.h b/src/modules/mc_att_control_multiplatform/mc_att_control.h
index e44317316..52ba369c1 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control.h
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control.h
@@ -110,9 +110,6 @@ private:
 		px4::ParameterFloat yaw_ff;
 		px4::ParameterFloat yaw_rate_max;
 
-		px4::ParameterFloat man_roll_max;
-		px4::ParameterFloat man_pitch_max;
-		px4::ParameterFloat man_yaw_max;
 		px4::ParameterFloat acro_roll_max;
 		px4::ParameterFloat acro_pitch_max;
 		px4::ParameterFloat acro_yaw_max;
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
index e779239ba..5db8f77ac 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
@@ -55,7 +55,6 @@ using namespace std;
 #endif
 
 MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
-	_v_att_sp_mod(),
 	_v_rates_sp_mod(),
 	_actuators(),
 	_publish_att_sp(false)
@@ -67,9 +66,6 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
 	_params.rate_d.zero();
 	_params.yaw_ff = 0.0f;
 	_params.yaw_rate_max = 0.0f;
-	_params.man_roll_max = 0.0f;
-	_params.man_pitch_max = 0.0f;
-	_params.man_yaw_max = 0.0f;
 	_params.acro_rate_max.zero();
 
 	_rates_prev.zero();
@@ -87,101 +83,11 @@ MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase()
 
 void MulticopterAttitudeControlBase::control_attitude(float dt)
 {
-	float yaw_sp_move_rate = 0.0f;
-	_publish_att_sp = false;
-
-
-	if (_v_control_mode->data().flag_control_manual_enabled) {
-		/* manual input, set or modify attitude setpoint */
-
-		if (_v_control_mode->data().flag_control_velocity_enabled
-		    || _v_control_mode->data().flag_control_climb_rate_enabled) {
-			/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
-			//XXX get rid of memcpy
-			memcpy(&(_v_att_sp_mod.data()), &_v_att_sp->data(), sizeof(_v_att_sp_mod.data()));
-		}
-
-		if (!_v_control_mode->data().flag_control_climb_rate_enabled) {
-			/* pass throttle directly if not in altitude stabilized mode */
-			_v_att_sp_mod.data().thrust = _manual_control_sp->data().z;
-			_publish_att_sp = true;
-		}
-
-		if (!_armed->data().armed) {
-			/* reset yaw setpoint when disarmed */
-			_reset_yaw_sp = true;
-		}
-
-		/* reset yaw setpoint to current position if needed */
-		if (_reset_yaw_sp) {
-			_reset_yaw_sp = false;
-			_v_att_sp_mod.data().yaw_body = _v_att->data().yaw;
-			_v_att_sp_mod.data().R_valid = false;
-			// _publish_att_sp = true;
-		}
-
-		if (!_v_control_mode->data().flag_control_velocity_enabled) {
-
-			if (_v_att_sp_mod.data().thrust < 0.1f) {
-				// TODO
-				//if (_status.condition_landed) {
-				/* reset yaw setpoint if on ground */
-				//	reset_yaw_sp = true;
-				//}
-			} else {
-				/* move yaw setpoint */
-				yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max;
-				_v_att_sp_mod.data().yaw_body = _wrap_pi(
-							     _v_att_sp_mod.data().yaw_body + yaw_sp_move_rate * dt);
-				float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
-				float yaw_offs = _wrap_pi(_v_att_sp_mod.data().yaw_body - _v_att->data().yaw);
-
-				if (yaw_offs < -yaw_offs_max) {
-					_v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw - yaw_offs_max);
-
-				} else if (yaw_offs > yaw_offs_max) {
-					_v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw + yaw_offs_max);
-				}
-
-				_v_att_sp_mod.data().R_valid = false;
-				// _publish_att_sp = true;
-			}
-			/* update attitude setpoint if not in position control mode */
-			_v_att_sp_mod.data().roll_body = _manual_control_sp->data().y * _params.man_roll_max;
-			_v_att_sp_mod.data().pitch_body = -_manual_control_sp->data().x
-					       * _params.man_pitch_max;
-			_v_att_sp_mod.data().R_valid = false;
-			// _publish_att_sp = true;
-		}
-
-	} else {
-		/* in non-manual mode use 'vehicle_attitude_setpoint' topic */
-		//XXX get rid of memcpy
-		memcpy(&(_v_att_sp_mod.data()), &_v_att_sp->data(), sizeof(_v_att_sp_mod.data()));
-
-		/* reset yaw setpoint after non-manual control mode */
-		_reset_yaw_sp = true;
-	}
-
-	_thrust_sp = _v_att_sp_mod.data().thrust;
+	_thrust_sp = _v_att_sp->data().thrust;
 
 	/* construct attitude setpoint rotation matrix */
 	math::Matrix<3, 3> R_sp;
-
-	if (_v_att_sp_mod.data().R_valid) {
-		/* rotation matrix in _att_sp is valid, use it */
-		R_sp.set(&_v_att_sp_mod.data().R_body[0]);
-
-	} else {
-		/* rotation matrix in _att_sp is not valid, use euler angles instead */
-		R_sp.from_euler(_v_att_sp_mod.data().roll_body, _v_att_sp_mod.data().pitch_body,
-				_v_att_sp_mod.data().yaw_body);
-
-		/* copy rotation matrix back to setpoint struct */
-		memcpy(&_v_att_sp_mod.data().R_body[0], &R_sp.data[0][0],
-		       sizeof(_v_att_sp_mod.data().R_body));
-		_v_att_sp_mod.data().R_valid = true;
-	}
+	R_sp.set(_v_att_sp->data().R_body);
 
 	/* rotation matrix for current state */
 	math::Matrix<3, 3> R;
@@ -190,11 +96,11 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 	/* all input data is ready, run controller itself */
 
 	/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
-	math::Vector < 3 > R_z(R(0, 2), R(1, 2), R(2, 2));
-	math::Vector < 3 > R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
+	math::Vector <3> R_z(R(0, 2), R(1, 2), R(2, 2));
+	math::Vector <3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
 
 	/* axis and sin(angle) of desired rotation */
-	math::Vector < 3 > e_R = R.transposed() * (R_z % R_sp_z);
+	math::Vector <3> e_R = R.transposed() * (R_z % R_sp_z);
 
 	/* calculate angle error */
 	float e_R_z_sin = e_R.length();
@@ -209,7 +115,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 	if (e_R_z_sin > 0.0f) {
 		/* get axis-angle representation */
 		float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
-		math::Vector < 3 > e_R_z_axis = e_R / e_R_z_sin;
+		math::Vector <3> e_R_z_axis = e_R / e_R_z_sin;
 
 		e_R = e_R_z_axis * e_R_z_angle;
 
@@ -224,9 +130,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 		e_R_cp(2, 1) = e_R_z_axis(0);
 
 		/* rotation matrix for roll/pitch only rotation */
-		R_rp = R
-		       * (_I + e_R_cp * e_R_z_sin
-			  + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
+		R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
 
 	} else {
 		/* zero roll/pitch rotation */
@@ -234,8 +138,8 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 	}
 
 	/* R_rp and R_sp has the same Z axis, calculate yaw error */
-	math::Vector < 3 > R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
-	math::Vector < 3 > R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
+	math::Vector <3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
+	math::Vector <3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
 	e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
 
 	if (e_R_z_cos < 0.0f) {
@@ -243,7 +147,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 		 * calculate angle and axis for R -> R_sp rotation directly */
 		math::Quaternion q;
 		q.from_dcm(R.transposed() * R_sp);
-		math::Vector < 3 > e_R_d = q.imag();
+		math::Vector <3> e_R_d = q.imag();
 		e_R_d.normalize();
 		e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));
 
@@ -260,7 +164,8 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 				       _params.yaw_rate_max);
 
 	/* feed forward yaw setpoint rate */
-	_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;
+	_rates_sp(2) += _v_att_sp->data().yaw_sp_move_rate * yaw_w * _params.yaw_ff;
+
 }
 
 void MulticopterAttitudeControlBase::control_attitude_rates(float dt)
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.h b/src/modules/mc_att_control_multiplatform/mc_att_control_base.h
index 124147079..4e33018b4 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control_base.h
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.h
@@ -97,8 +97,6 @@ protected:
 	px4::Subscriber<px4_actuator_armed> *_armed;			    /**< actuator arming status */
 	px4::Subscriber<px4_vehicle_status> *_v_status;			    /**< vehicle status */
 
-	px4_vehicle_attitude_setpoint	_v_att_sp_mod;	/**< modified vehicle attitude setpoint
-								  that gets published eventually */
 	px4_vehicle_rates_setpoint	_v_rates_sp_mod;	/**< vehicle rates setpoint
 								  that gets published eventually*/
 	px4_actuator_controls_0		_actuators;	/**< actuator controls */
@@ -121,9 +119,6 @@ protected:
 		float yaw_ff; /**< yaw control feed-forward */
 		float yaw_rate_max; /**< max yaw rate */
 
-		float man_roll_max;
-		float man_pitch_max;
-		float man_yaw_max;
 		math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */
 
 		int32_t autostart_id;
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_params.c b/src/modules/mc_att_control_multiplatform/mc_att_control_params.c
index a0b1706f2..395ae83b0 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control_params.c
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_params.c
@@ -190,35 +190,6 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAW_FF);
 PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX);
 
 /**
- * Max manual roll
- *
- * @unit deg
- * @min 0.0
- * @max 90.0
- * @group Multicopter Attitude Control
- */
-PX4_PARAM_DEFINE_FLOAT(MC_MAN_R_MAX);
-
-/**
- * Max manual pitch
- *
- * @unit deg
- * @min 0.0
- * @max 90.0
- * @group Multicopter Attitude Control
- */
-PX4_PARAM_DEFINE_FLOAT(MC_MAN_P_MAX);
-
-/**
- * Max manual yaw rate
- *
- * @unit deg/s
- * @min 0.0
- * @group Multicopter Attitude Control
- */
-PX4_PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX);
-
-/**
  * Max acro roll rate
  *
  * @unit deg/s
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_params.h b/src/modules/mc_att_control_multiplatform/mc_att_control_params.h
index 59dd5240f..ff962dbf1 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control_params.h
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_params.h
@@ -57,9 +57,6 @@
 #define PARAM_MC_YAWRATE_D_DEFAULT 0.0f
 #define PARAM_MC_YAW_FF_DEFAULT 0.5f
 #define PARAM_MC_YAWRATE_MAX_DEFAULT 120.0f
-#define PARAM_MC_MAN_R_MAX_DEFAULT 35.0f
-#define PARAM_MC_MAN_P_MAX_DEFAULT 35.0f
-#define PARAM_MC_MAN_Y_MAX_DEFAULT 120.0f
 #define PARAM_MC_ACRO_R_MAX_DEFAULT 35.0f
 #define PARAM_MC_ACRO_P_MAX_DEFAULT 35.0f
 #define PARAM_MC_ACRO_Y_MAX_DEFAULT 120.0f
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp
index c2b847075..40eb498b4 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp
@@ -71,7 +71,7 @@ int mc_att_control_m_main(int argc, char *argv[])
 		daemon_task = task_spawn_cmd("mc_att_control_m",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       3000,
+				       1900,
 				       main,
 					(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
 
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 7f87e3532..d993692ab 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -761,8 +761,8 @@ void MulticopterPositionControl::control_auto(float dt)
 		orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
 
 		//Make sure that the position setpoint is valid
-		if (!isfinite(_pos_sp_triplet.current.lat) || 
-			!isfinite(_pos_sp_triplet.current.lon) || 
+		if (!isfinite(_pos_sp_triplet.current.lat) ||
+			!isfinite(_pos_sp_triplet.current.lon) ||
 			!isfinite(_pos_sp_triplet.current.alt)) {
 			_pos_sp_triplet.current.valid = false;
 		}
@@ -1367,7 +1367,7 @@ MulticopterPositionControl::task_main()
 
 				} else if (yaw_offs > YAW_OFFSET_MAX) {
 					_att_sp.yaw_body = _wrap_pi(_att.yaw + YAW_OFFSET_MAX);
-				}				
+				}
 			}
 
 			//Control roll and pitch directly if we no aiding velocity controller is active
@@ -1388,15 +1388,22 @@ MulticopterPositionControl::task_main()
 			_att_sp.timestamp = hrt_absolute_time();
 		}
 		else {
-			reset_yaw_sp = true;			
+			reset_yaw_sp = true;
 		}
 
-		// publish attitude setpoint
-		if (_att_sp_pub > 0) {
-			orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
+		/* publish attitude setpoint
+		 * Do not publish if offboard is enabled but position/velocity control is disabled,
+		 * in this case the attitude setpoint is published by the mavlink app
+		 */
+		if (!(_control_mode.flag_control_offboard_enabled &&
+					!(_control_mode.flag_control_position_enabled ||
+						_control_mode.flag_control_velocity_enabled))) {
+			if (_att_sp_pub > 0) {
+				orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
 
-		} else {
-			_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
+			} else {
+				_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
+			}
 		}
 
 		/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
@@ -1419,7 +1426,7 @@ MulticopterPositionControl::start()
 	_control_task = task_spawn_cmd("mc_pos_control",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       2000,
+				       1800,
 				       (main_t)&MulticopterPositionControl::task_main_trampoline,
 				       nullptr);
 
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
index 391558dcc..40268358a 100644
--- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
@@ -41,6 +41,9 @@
 
 #include "mc_pos_control.h"
 #include "mc_pos_control_params.h"
+/* The following inclue is needed because the pos controller depens on a parameter from attitude control to set a
+ * reasonable yaw setpoint in manual mode */
+#include <mc_att_control_multiplatform/mc_att_control_params.h>
 
 #define TILT_COS_MAX	0.7f
 #define SIGMA		0.000001f
@@ -82,7 +85,11 @@ MulticopterPositionControl::MulticopterPositionControl() :
 		.xy_ff		    = px4::ParameterFloat("MPC_XY_FF", PARAM_MPC_XY_FF_DEFAULT),
 		.tilt_max_air	    = px4::ParameterFloat("MPC_TILTMAX_AIR", PARAM_MPC_TILTMAX_AIR_DEFAULT),
 		.land_speed	    = px4::ParameterFloat("MPC_LAND_SPEED", PARAM_MPC_LAND_SPEED_DEFAULT),
-		.tilt_max_land	    = px4::ParameterFloat("MPC_TILTMAX_LND", PARAM_MPC_TILTMAX_LND_DEFAULT)
+		.tilt_max_land	    = px4::ParameterFloat("MPC_TILTMAX_LND", PARAM_MPC_TILTMAX_LND_DEFAULT),
+		.man_roll_max	    = px4::ParameterFloat("MPC_MAN_R_MAX", PARAM_MPC_MAN_R_MAX_DEFAULT),
+		.man_pitch_max	    = px4::ParameterFloat("MPC_MAN_P_MAX", PARAM_MPC_MAN_P_MAX_DEFAULT),
+		.man_yaw_max	    = px4::ParameterFloat("MPC_MAN_Y_MAX", PARAM_MPC_MAN_Y_MAX_DEFAULT),
+		.mc_att_yaw_p	    = px4::ParameterFloat("MC_YAW_P", PARAM_MC_YAW_P_DEFAULT)
 	}),
 	_ref_alt(0.0f),
 	_ref_timestamp(0),
@@ -99,14 +106,13 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	 * Do subscriptions
 	 */
 	_att = _n.subscribe<px4_vehicle_attitude>(&MulticopterPositionControl::handle_vehicle_attitude, this, 0);
-	_v_att_sp = _n.subscribe<px4_vehicle_attitude_setpoint>(0);
 	_control_mode = _n.subscribe<px4_vehicle_control_mode>(0);
 	_parameter_update = _n.subscribe<px4_parameter_update>(
 			&MulticopterPositionControl::handle_parameter_update, this, 1000);
 	_manual_control_sp = _n.subscribe<px4_manual_control_setpoint>(0);
 	_armed = _n.subscribe<px4_actuator_armed>(0);
 	_local_pos = _n.subscribe<px4_vehicle_local_position>(0);
-	_pos_sp_triplet = _n.subscribe<px4_position_setpoint_triplet>(0);
+	_pos_sp_triplet = _n.subscribe<px4_position_setpoint_triplet>(&MulticopterPositionControl::handle_position_setpoint_triplet, this, 0);
 	_local_pos_sp = _n.subscribe<px4_vehicle_local_position_setpoint>(0);
 	_global_vel_sp = _n.subscribe<px4_vehicle_global_velocity_setpoint>(0);
 
@@ -146,6 +152,13 @@ MulticopterPositionControl::parameters_update()
 	_params.land_speed = _params_handles.land_speed.update();
 	_params.tilt_max_land = math::radians(_params_handles.tilt_max_land.update());
 
+	/* manual control scale */
+	_params.man_roll_max = math::radians(_params_handles.man_roll_max.update());
+	_params.man_pitch_max = math::radians(_params_handles.man_pitch_max.update());
+	_params.man_yaw_max = math::radians(_params_handles.man_yaw_max.update());
+
+	_params.mc_att_yaw_p = _params_handles.mc_att_yaw_p.update();
+
 	_params.pos_p(0) = _params.pos_p(1) = _params_handles.xy_p.update();
 	_params.pos_p(2) = _params_handles.z_p.update();
 	_params.vel_p(0) = _params.vel_p(1) = _params_handles.xy_vel_p.update();
@@ -287,22 +300,6 @@ MulticopterPositionControl::control_manual(float dt)
 		_sp_move_rate /= sp_move_norm;
 	}
 
-	/* move yaw setpoint */
-	//XXX hardcoded hack until #1741 is in/ported (the values stem
-	//from default param values, see how yaw setpoint is moved in the attitude controller)
-	float yaw_sp_move_rate = _manual_control_sp->data().r * 120.0f * M_DEG_TO_RAD_F;
-	_att_sp_msg.data().yaw_body = _wrap_pi(
-			_att_sp_msg.data().yaw_body + yaw_sp_move_rate * dt);
-	float yaw_offs_max = 120.0f * M_DEG_TO_RAD_F / 2.0f;
-	float yaw_offs = _wrap_pi(_att_sp_msg.data().yaw_body - _att->data().yaw);
-
-	if (yaw_offs < -yaw_offs_max) {
-		_att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw - yaw_offs_max);
-
-	} else if (yaw_offs > yaw_offs_max) {
-		_att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw + yaw_offs_max);
-	}
-
 	/* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */
 	math::Matrix<3, 3> R_yaw_sp;
 	R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp_msg.data().yaw_body);
@@ -554,11 +551,22 @@ void MulticopterPositionControl::handle_parameter_update(const px4_parameter_upd
 	parameters_update();
 }
 
+void MulticopterPositionControl::handle_position_setpoint_triplet(const px4_position_setpoint_triplet &msg)
+{
+	/* Make sure that the position setpoint is valid */
+	if (!isfinite(_pos_sp_triplet->data().current.lat) ||
+			!isfinite(_pos_sp_triplet->data().current.lon) ||
+			!isfinite(_pos_sp_triplet->data().current.alt)) {
+		_pos_sp_triplet->data().current.valid = false;
+	}
+}
+
 void  MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_attitude &msg)
 {
 	static bool reset_int_z = true;
 	static bool reset_int_z_manual = false;
 	static bool reset_int_xy = true;
+	static bool reset_yaw_sp = true;
 	static bool was_armed = false;
 	static uint64_t t_prev = 0;
 
@@ -572,8 +580,10 @@ void  MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti
 		_reset_alt_sp = true;
 		reset_int_z = true;
 		reset_int_xy = true;
+		reset_yaw_sp = true;
 	}
 
+	/* Update previous arming state */
 	was_armed = _control_mode->data().flag_armed;
 
 	update_ref();
@@ -610,22 +620,6 @@ void  MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti
 			control_auto(dt);
 		}
 
-		/* fill local position setpoint */
-		_local_pos_sp_msg.data().timestamp = get_time_micros();
-		_local_pos_sp_msg.data().x = _pos_sp(0);
-		_local_pos_sp_msg.data().y = _pos_sp(1);
-		_local_pos_sp_msg.data().z = _pos_sp(2);
-		_local_pos_sp_msg.data().yaw = _att_sp_msg.data().yaw_body;
-
-		/* publish local position setpoint */
-		if (_local_pos_sp_pub != nullptr) {
-			_local_pos_sp_pub->publish(_local_pos_sp_msg);
-
-		} else {
-			_local_pos_sp_pub = _n.advertise<px4_vehicle_local_position_setpoint>();
-		}
-
-
 		if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid && _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_IDLE) {
 			/* idle state, don't run controller and set zero thrust */
 			_R.identity();
@@ -633,7 +627,8 @@ void  MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti
 			_att_sp_msg.data().R_valid = true;
 
 			_att_sp_msg.data().roll_body = 0.0f;
-			// _att_sp_msg.data().yaw_body = _att->data().yaw;
+			_att_sp_msg.data().pitch_body = 0.0f;
+			_att_sp_msg.data().yaw_body = _att->data().yaw;
 			_att_sp_msg.data().thrust = 0.0f;
 
 			_att_sp_msg.data().timestamp = get_time_micros();
@@ -925,6 +920,11 @@ void  MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti
 
 				_att_sp_msg.data().thrust = thrust_abs;
 
+				/* save thrust setpoint for logging */
+				_local_pos_sp_msg.data().acc_x = thrust_sp(0);
+				_local_pos_sp_msg.data().acc_x = thrust_sp(1);
+				_local_pos_sp_msg.data().acc_x = thrust_sp(2);
+
 				_att_sp_msg.data().timestamp = get_time_micros();
 
 				/* publish attitude setpoint */
@@ -940,6 +940,25 @@ void  MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti
 			}
 		}
 
+		/* fill local position setpoint */
+		_local_pos_sp_msg.data().timestamp = get_time_micros();
+		_local_pos_sp_msg.data().x = _pos_sp(0);
+		_local_pos_sp_msg.data().y = _pos_sp(1);
+		_local_pos_sp_msg.data().z = _pos_sp(2);
+		_local_pos_sp_msg.data().yaw = _att_sp_msg.data().yaw_body;
+		_local_pos_sp_msg.data().vx = _vel_sp(0);
+		_local_pos_sp_msg.data().vy = _vel_sp(1);
+		_local_pos_sp_msg.data().vz = _vel_sp(2);
+
+		/* publish local position setpoint */
+		if (_local_pos_sp_pub != nullptr) {
+			_local_pos_sp_pub->publish(_local_pos_sp_msg);
+
+		} else {
+			_local_pos_sp_pub = _n.advertise<px4_vehicle_local_position_setpoint>();
+		}
+
+
 	} else {
 		/* position controller disabled, reset setpoints */
 		_reset_alt_sp = true;
@@ -949,6 +968,68 @@ void  MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti
 		reset_int_xy = true;
 	}
 
+	/* generate attitude setpoint from manual controls */
+	if(_control_mode->data().flag_control_manual_enabled && _control_mode->data().flag_control_attitude_enabled) {
+
+		/* reset yaw setpoint to current position if needed */
+		if (reset_yaw_sp) {
+			reset_yaw_sp = false;
+			_att_sp_msg.data().yaw_body = _att->data().yaw;
+		}
+
+		/* do not move yaw while arming */
+		else if (_manual_control_sp->data().z > 0.1f)
+		{
+			const float YAW_OFFSET_MAX = _params.man_yaw_max / _params.mc_att_yaw_p;
+
+			_att_sp_msg.data().yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max;
+			_att_sp_msg.data().yaw_body = _wrap_pi(_att_sp_msg.data().yaw_body + _att_sp_msg.data().yaw_sp_move_rate * dt);
+			float yaw_offs = _wrap_pi(_att_sp_msg.data().yaw_body - _att->data().yaw);
+			if (yaw_offs < - YAW_OFFSET_MAX) {
+				_att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw - YAW_OFFSET_MAX);
+
+			} else if (yaw_offs > YAW_OFFSET_MAX) {
+				_att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw + YAW_OFFSET_MAX);
+			}
+		}
+
+		/* Control roll and pitch directly if we no aiding velocity controller is active */
+		if(!_control_mode->data().flag_control_velocity_enabled) {
+			_att_sp_msg.data().roll_body = _manual_control_sp->data().y * _params.man_roll_max;
+			_att_sp_msg.data().pitch_body = -_manual_control_sp->data().x * _params.man_pitch_max;
+		}
+
+		/* Control climb rate directly if no aiding altitude controller is active */
+		if(!_control_mode->data().flag_control_climb_rate_enabled) {
+			_att_sp_msg.data().thrust = _manual_control_sp->data().z;
+		}
+
+		/* Construct attitude setpoint rotation matrix */
+		math::Matrix<3,3> R_sp;
+		R_sp.from_euler(_att_sp_msg.data().roll_body,_att_sp_msg.data().pitch_body,_att_sp_msg.data().yaw_body);
+		_att_sp_msg.data().R_valid = true;
+		memcpy(&_att_sp_msg.data().R_body[0], R_sp.data, sizeof(_att_sp_msg.data().R_body));
+		_att_sp_msg.data().timestamp = get_time_micros();
+	}
+	else {
+		reset_yaw_sp = true;
+	}
+
+	/* publish attitude setpoint
+	 * Do not publish if offboard is enabled but position/velocity control is disabled, in this case the attitude setpoint
+	 * is published by the mavlink app
+	 */
+	if (!(_control_mode->data().flag_control_offboard_enabled &&
+				!(_control_mode->data().flag_control_position_enabled ||
+					_control_mode->data().flag_control_velocity_enabled))) {
+		if (_att_sp_pub != nullptr) {
+			_att_sp_pub->publish(_att_sp_msg);
+
+		} else {
+			_att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>();
+		}
+	}
+
 	/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
 	reset_int_z_manual = _control_mode->data().flag_armed && _control_mode->data().flag_control_manual_enabled && !_control_mode->data().flag_control_climb_rate_enabled;
 }
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h
index 245b5f5a9..54c6de155 100644
--- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h
@@ -75,6 +75,7 @@ public:
 	/* Callbacks for topics */
 	void handle_vehicle_attitude(const px4_vehicle_attitude &msg);
 	void handle_parameter_update(const px4_parameter_update &msg);
+	void handle_position_setpoint_triplet(const px4_position_setpoint_triplet &msg);
 
 	void spin() { _n.spin(); }
 
@@ -87,19 +88,18 @@ protected:
 
 	Publisher<px4_vehicle_attitude_setpoint>	*_att_sp_pub;			/**< attitude setpoint publication */
 	Publisher<px4_vehicle_local_position_setpoint>	*_local_pos_sp_pub;		/**< vehicle local position setpoint publication */
-	Publisher<px4_vehicle_global_velocity_setpoint>		*_global_vel_sp_pub;		/**< vehicle global velocity setpoint publication */
+	Publisher<px4_vehicle_global_velocity_setpoint>	*_global_vel_sp_pub;		/**< vehicle global velocity setpoint publication */
 
 
-	Subscriber<px4_vehicle_attitude> *_att;			    /**< vehicle attitude */
-	Subscriber<px4_vehicle_attitude_setpoint> *_v_att_sp;	    /**< vehicle attitude setpoint */
-	Subscriber<px4_vehicle_control_mode> *_control_mode;	    /**< vehicle control mode */
-	Subscriber<px4_parameter_update> *_parameter_update;	    /**< parameter update */
-	Subscriber<px4_manual_control_setpoint> *_manual_control_sp;   /**< manual control setpoint */
-	Subscriber<px4_actuator_armed> *_armed;			    /**< actuator arming status */
-	Subscriber<px4_vehicle_local_position> *_local_pos;			    /**< local position */
-	Subscriber<px4_position_setpoint_triplet> *_pos_sp_triplet;			    /**< local position */
-	Subscriber<px4_vehicle_local_position_setpoint> *_local_pos_sp;			    /**< local position */
-	Subscriber<px4_vehicle_global_velocity_setpoint> *_global_vel_sp;			    /**< local position */
+	Subscriber<px4_vehicle_attitude> *_att;				    /**< vehicle attitude */
+	Subscriber<px4_vehicle_control_mode> *_control_mode;		    /**< vehicle control mode */
+	Subscriber<px4_parameter_update> *_parameter_update;		    /**< parameter update */
+	Subscriber<px4_manual_control_setpoint> *_manual_control_sp;	    /**< manual control setpoint */
+	Subscriber<px4_actuator_armed> *_armed;				    /**< actuator arming status */
+	Subscriber<px4_vehicle_local_position> *_local_pos;		    /**< local position */
+	Subscriber<px4_position_setpoint_triplet> *_pos_sp_triplet;	    /**< local position */
+	Subscriber<px4_vehicle_local_position_setpoint> *_local_pos_sp;	    /**< local position */
+	Subscriber<px4_vehicle_global_velocity_setpoint> *_global_vel_sp;   /**< local position */
 
 	px4_vehicle_attitude_setpoint _att_sp_msg;
 	px4_vehicle_local_position_setpoint _local_pos_sp_msg;
@@ -125,6 +125,10 @@ protected:
 		px4::ParameterFloat tilt_max_air;
 		px4::ParameterFloat land_speed;
 		px4::ParameterFloat tilt_max_land;
+		px4::ParameterFloat man_roll_max;
+		px4::ParameterFloat man_pitch_max;
+		px4::ParameterFloat man_yaw_max;
+		px4::ParameterFloat mc_att_yaw_p;   // needed for calculating reasonable attitude setpoints in manual
 	}		_params_handles;		/**< handles for interesting parameters */
 
 	struct {
@@ -133,6 +137,10 @@ protected:
 		float tilt_max_air;
 		float land_speed;
 		float tilt_max_land;
+		float man_roll_max;
+		float man_pitch_max;
+		float man_yaw_max;
+		float mc_att_yaw_p;
 
 		math::Vector<3> pos_p;
 		math::Vector<3> vel_p;
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c
index c741a7f0a..709085662 100644
--- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c
@@ -210,3 +210,32 @@ PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND);
  */
 PX4_PARAM_DEFINE_FLOAT(MPC_LAND_SPEED);
 
+/**
+ * Max manual roll
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_MAN_R_MAX);
+
+/**
+ * Max manual pitch
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_MAN_P_MAX);
+
+/**
+ * Max manual yaw rate
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_MAN_Y_MAX);
+
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h
index fec3bcb7c..8c8b707ae 100644
--- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h
@@ -58,4 +58,7 @@
 #define PARAM_MPC_TILTMAX_AIR_DEFAULT 45.0f
 #define PARAM_MPC_TILTMAX_LND_DEFAULT 15.0f
 #define PARAM_MPC_LAND_SPEED_DEFAULT 1.0f
+#define PARAM_MPC_MAN_R_MAX_DEFAULT 35.0f
+#define PARAM_MPC_MAN_P_MAX_DEFAULT 35.0f
+#define PARAM_MPC_MAN_Y_MAX_DEFAULT 120.0f
 
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp
index 87996d93b..1082061f6 100644
--- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp
@@ -71,7 +71,7 @@ int mc_pos_control_m_main(int argc, char *argv[])
 		daemon_task = task_spawn_cmd("mc_pos_control_m",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       3000,
+				       2500,
 				       main,
 					(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
 
diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c
index 1568233b0..10394fed1 100644
--- a/src/modules/navigator/rtl_params.c
+++ b/src/modules/navigator/rtl_params.c
@@ -83,7 +83,7 @@ PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 60);
  * @max 100
  * @group RTL
  */
-PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20);
+PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 30);
 
 /**
  * RTL delay
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c
index 5387b7e87..91915fb53 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -288,6 +288,20 @@ PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f);
  */
 PARAM_DEFINE_INT32(CBRK_NO_VISION, 0);
 
+/**
+ * INAV enabled
+ *
+ * If set to 1, use INAV for position estimation
+ * the system uses the compined attitude / position
+ * filter framework.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @unit s
+ * @group Position Estimator INAV
+ */
+PARAM_DEFINE_INT32(INAV_ENABLED, 0);
+
 int parameters_init(struct position_estimator_inav_param_handles *h)
 {
 	h->w_z_baro = param_find("INAV_W_Z_BARO");
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 3e21ec2a9..272e4b14f 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -130,7 +130,7 @@ PARAM_DEFINE_INT32(CAL_MAG0_ID, 0);
  * @max 30
  * @group Sensor Calibration
  */
-PARAM_DEFINE_INT32(CAL_MAG0_ROT, 0);
+PARAM_DEFINE_INT32(CAL_MAG0_ROT, -1);
 
 /**
  * Magnetometer X-axis offset
@@ -308,7 +308,7 @@ PARAM_DEFINE_INT32(CAL_MAG1_ID, 0);
  * @max 30
  * @group Sensor Calibration
  */
-PARAM_DEFINE_INT32(CAL_MAG1_ROT, 0);
+PARAM_DEFINE_INT32(CAL_MAG1_ROT, -1);
 
 /**
  * Magnetometer X-axis offset
@@ -486,7 +486,7 @@ PARAM_DEFINE_INT32(CAL_MAG2_ID, 0);
  * @max 30
  * @group Sensor Calibration
  */
-PARAM_DEFINE_INT32(CAL_MAG2_ROT, 0);
+PARAM_DEFINE_INT32(CAL_MAG2_ROT, -1);
 
 /**
  * Magnetometer X-axis offset
@@ -994,6 +994,36 @@ PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124);	/* scaling for 3DR power brick */
 
 
 /**
+ * RC channel count
+ *
+ * This parameter is used by Ground Station software to save the number
+ * of channels which were used during RC calibration. It is only meant
+ * for ground station use.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+
+PARAM_DEFINE_INT32(RC_CHAN_CNT, 0);
+
+/**
+ * RC mode switch threshold automaic distribution
+ *
+ * This parameter is used by Ground Station software to specify whether
+ * the threshold values for flight mode switches were automatically calculated.
+ * 0 indicates that the threshold values were set by the user. Any other value
+ * indicates that the threshold value where automatically set by the ground
+ * station software. It is only meant for ground station use.
+ *
+ * @min 0
+ * @max 1
+ * @group Radio Calibration
+ */
+
+PARAM_DEFINE_INT32(RC_TH_USER, 1);
+
+/**
  * Roll control channel mapping.
  *
  * The channel index (starting from 1 for channel 1) indicates
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 9a90c84e0..527ca2210 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1511,14 +1511,21 @@ Sensors::parameter_update_poll(bool forced)
 					if (ioctl(fd, MAGIOCGEXTERNAL, 0) <= 0) {
 						/* mag is internal */
 						_mag_rotation[s] = _board_rotation;
-						/* reset param to -1 to indicate external mag */
+						/* reset param to -1 to indicate internal mag */
 						int32_t minus_one = MAG_ROT_VAL_INTERNAL;
 						param_set_no_notification(param_find(str), &minus_one);
 					} else {
 
-						int32_t mag_rot = 0;
+						int32_t mag_rot;
 						param_get(param_find(str), &mag_rot);
 
+						/* check if this mag is still set as internal */
+						if (mag_rot < 0) {
+							/* it was marked as internal, change to external with no rotation */
+							mag_rot = 0;
+							param_set_no_notification(param_find(str), &mag_rot);
+						}
+
 						/* handling of old setups, will be removed later (noted Feb 2015) */
 						int32_t deprecated_mag_rot = 0;
 						param_get(param_find("SENS_EXT_MAG_ROT"), &deprecated_mag_rot);
@@ -1536,6 +1543,9 @@ Sensors::parameter_update_poll(bool forced)
 						if ((deprecated_mag_rot != 0) && (mag_rot <= 0)) {
 							mag_rot = deprecated_mag_rot;
 							param_set_no_notification(param_find(str), &mag_rot);
+							/* clear the old param, not supported in GUI anyway */
+							deprecated_mag_rot = 0;
+							param_set_no_notification(param_find("SENS_EXT_MAG_ROT"), &deprecated_mag_rot);
 						}
 
 						/* handling of transition from internal to external */
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index f60aa8d86..dbed29774 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -163,8 +163,8 @@ ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s);
 #include "topics/vehicle_control_debug.h"
 ORB_DEFINE(vehicle_control_debug, struct vehicle_control_debug_s);
 
-#include "topics/offboard_control_setpoint.h"
-ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s);
+#include "topics/offboard_control_mode.h"
+ORB_DEFINE(offboard_control_mode, struct offboard_control_mode_s);
 
 #include "topics/optical_flow.h"
 ORB_DEFINE(optical_flow, struct optical_flow_s);
diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h
deleted file mode 100644
index 72a28e501..000000000
--- a/src/modules/uORB/topics/offboard_control_setpoint.h
+++ /dev/null
@@ -1,276 +0,0 @@
-/****************************************************************************
- *
- *	 Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *	 Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *
- * 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 offboard_control_setpoint.h
- * Definition of the manual_control_setpoint uORB topic.
- */
-
-#ifndef TOPIC_OFFBOARD_CONTROL_SETPOINT_H_
-#define TOPIC_OFFBOARD_CONTROL_SETPOINT_H_
-
-#include <stdint.h>
-#include "../uORB.h"
-
-/**
- * Off-board control inputs.
- *
- * Typically sent by a ground control station / joystick or by
- * some off-board controller via C or SIMULINK.
- */
-enum OFFBOARD_CONTROL_MODE {
-	OFFBOARD_CONTROL_MODE_DIRECT = 0,
-	OFFBOARD_CONTROL_MODE_DIRECT_RATES = 1,
-	OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE = 2,
-	OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED = 3,
-	OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED = 4,
-	OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED = 5,
-	OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED = 6,
-	OFFBOARD_CONTROL_MODE_DIRECT_GLOBAL = 7,
-	OFFBOARD_CONTROL_MODE_DIRECT_FORCE = 8,
-	OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 9,
-	OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 10,
-	OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 11 /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
-};
-
-enum OFFBOARD_CONTROL_FRAME {
-	OFFBOARD_CONTROL_FRAME_LOCAL_NED = 0,
-	OFFBOARD_CONTROL_FRAME_LOCAL_OFFSET_NED = 1,
-	OFFBOARD_CONTROL_FRAME_BODY_NED = 2,
-	OFFBOARD_CONTROL_FRAME_BODY_OFFSET_NED = 3,
-	OFFBOARD_CONTROL_FRAME_GLOBAL = 4
-};
-
-/* mappings for the ignore bitmask */
-enum {OFB_IGN_BIT_POS_X,
-	OFB_IGN_BIT_POS_Y,
-	OFB_IGN_BIT_POS_Z,
-	OFB_IGN_BIT_VEL_X,
-	OFB_IGN_BIT_VEL_Y,
-	OFB_IGN_BIT_VEL_Z,
-	OFB_IGN_BIT_ACC_X,
-	OFB_IGN_BIT_ACC_Y,
-	OFB_IGN_BIT_ACC_Z,
-	OFB_IGN_BIT_BODYRATE_X,
-	OFB_IGN_BIT_BODYRATE_Y,
-	OFB_IGN_BIT_BODYRATE_Z,
-	OFB_IGN_BIT_ATT,
-	OFB_IGN_BIT_THRUST,
-	OFB_IGN_BIT_YAW,
-	OFB_IGN_BIT_YAWRATE,
-};
-
-/**
- * @addtogroup topics
- * @{
- */
-
-struct offboard_control_setpoint_s {
-	uint64_t timestamp;
-
-	enum OFFBOARD_CONTROL_MODE mode;		 /**< The current control inputs mode */
-
-	double position[3];	/**< lat, lon, alt / x, y, z */
-	float velocity[3];	/**< x vel, y vel, z vel */
-	float acceleration[3];	/**< x acc, y acc, z acc */
-	float attitude[4];	/**< attitude of vehicle (quaternion) */
-	float attitude_rate[3];	/**< body angular rates (x, y, z) */
-	float thrust;	/**< thrust */
-	float yaw;	/**< yaw: this is the yaw from the position_target message
-			  (not from the full attitude_target message) */
-	float yaw_rate;	/**< yaw rate: this is the yaw from the position_target message
-			  (not from the full attitude_target message) */
-
-	uint16_t ignore; /**< if field i is set to true, the value should be ignored, see definition at top of file
-			   for mapping */
-
-	bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */
-
-	float override_mode_switch;
-
-	float aux1_cam_pan_flaps;
-	float aux2_cam_tilt;
-	float aux3_cam_zoom;
-	float aux4_cam_roll;
-
-}; /**< offboard control inputs */
-/**
- * @}
- */
-
-/**
- * Returns true if the position setpoint at index should be ignored
- */
-inline bool offboard_control_sp_ignore_position(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
-	return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_POS_X +  index)));
-}
-
-/**
- * Returns true if all position setpoints should be ignored
- */
-inline bool offboard_control_sp_ignore_position_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	for (int i = 0; i < 3; i++) {
-		if (!offboard_control_sp_ignore_position(offboard_control_sp, i))	{
-			return false;
-		}
-	}
-	return true;
-}
-
-/**
- * Returns true if some position setpoints should be ignored
- */
-inline bool offboard_control_sp_ignore_position_some(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	for (int i = 0; i < 3; i++) {
-		if (offboard_control_sp_ignore_position(offboard_control_sp, i))	{
-			return true;
-		}
-	}
-	return false;
-}
-
-/**
- * Returns true if the velocity setpoint at index should be ignored
- */
-inline bool offboard_control_sp_ignore_velocity(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
-	return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_VEL_X + index)));
-}
-
-/**
- * Returns true if all velocity setpoints should be ignored
- */
-inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	for (int i = 0; i < 3; i++) {
-		if (!offboard_control_sp_ignore_velocity(offboard_control_sp, i))	{
-			return false;
-		}
-	}
-	return true;
-}
-
-/**
- * Returns true if some velocity setpoints should be ignored
- */
-inline bool offboard_control_sp_ignore_velocity_some(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	for (int i = 0; i < 3; i++) {
-		if (offboard_control_sp_ignore_velocity(offboard_control_sp, i))	{
-			return true;
-		}
-	}
-	return false;
-}
-
-/**
- * Returns true if the acceleration setpoint at index should be ignored
- */
-inline bool offboard_control_sp_ignore_acceleration(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
-	return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_ACC_X + index)));
-}
-
-/**
- * Returns true if all acceleration setpoints should be ignored
- */
-inline bool offboard_control_sp_ignore_acceleration_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	for (int i = 0; i < 3; i++) {
-		if (!offboard_control_sp_ignore_acceleration(offboard_control_sp, i))	{
-			return false;
-		}
-	}
-	return true;
-}
-
-/**
- * Returns true if some acceleration setpoints should be ignored
- */
-inline bool offboard_control_sp_ignore_acceleration_some(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	for (int i = 0; i < 3; i++) {
-		if (offboard_control_sp_ignore_acceleration(offboard_control_sp, i))	{
-			return true;
-		}
-	}
-	return false;
-}
-
-/**
- * Returns true if the bodyrate setpoint at index should be ignored
- */
-inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
-	return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_BODYRATE_X + index)));
-}
-
-/**
- * Returns true if some of the bodyrate setpoints should be ignored
- */
-inline bool offboard_control_sp_ignore_bodyrates_some(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	for (int i = 0; i < 3; i++) {
-		if (offboard_control_sp_ignore_bodyrates(offboard_control_sp, i))	{
-			return true;
-		}
-	}
-	return false;
-}
-
-/**
- * Returns true if the attitude setpoint should be ignored
- */
-inline bool offboard_control_sp_ignore_attitude(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_ATT));
-}
-
-/**
- * Returns true if the thrust setpoint should be ignored
- */
-inline bool offboard_control_sp_ignore_thrust(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_THRUST));
-}
-
-/**
- * Returns true if the yaw setpoint should be ignored
- */
-inline bool offboard_control_sp_ignore_yaw(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_YAW));
-}
-
-/**
- * Returns true if the yaw rate setpoint should be ignored
- */
-inline bool offboard_control_sp_ignore_yawrate(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_YAWRATE));
-}
-
-
-/* register this as object request broker structure */
-ORB_DECLARE(offboard_control_setpoint);
-
-#endif
diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp
index 764e33179..b4f81d429 100644
--- a/src/modules/uORB/uORB.cpp
+++ b/src/modules/uORB/uORB.cpp
@@ -684,6 +684,8 @@ namespace
 
 ORBDevMaster	*g_dev;
 bool pubsubtest_passed = false;
+bool pubsubtest_print = false;
+int pubsubtest_res = OK;
 
 struct orb_test {
 	int val;
@@ -693,6 +695,22 @@ struct orb_test {
 ORB_DEFINE(orb_test, struct orb_test);
 ORB_DEFINE(orb_multitest, struct orb_test);
 
+struct orb_test_medium {
+	int val;
+	hrt_abstime time;
+	char junk[64];
+};
+
+ORB_DEFINE(orb_test_medium, struct orb_test_medium);
+
+struct orb_test_large {
+	int val;
+	hrt_abstime time;
+	char junk[512];
+};
+
+ORB_DEFINE(orb_test_large, struct orb_test_large);
+
 int
 test_fail(const char *fmt, ...)
 {
@@ -727,21 +745,44 @@ int pubsublatency_main(void)
 	float latency_integral = 0.0f;
 
 	/* wakeup source(s) */
-	struct pollfd fds[1];
+	struct pollfd fds[3];
 
-	int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_multitest), 0);
+	int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_test), 0);
+	int test_multi_sub_medium = orb_subscribe_multi(ORB_ID(orb_test_medium), 0);
+	int test_multi_sub_large = orb_subscribe_multi(ORB_ID(orb_test_large), 0);
+
+	struct orb_test_large t;
+
+	/* clear all ready flags */
+	orb_copy(ORB_ID(orb_test), test_multi_sub, &t);
+	orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t);
+	orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t);
 
 	fds[0].fd = test_multi_sub;
 	fds[0].events = POLLIN;
+	fds[1].fd = test_multi_sub_medium;
+	fds[1].events = POLLIN;
+	fds[2].fd = test_multi_sub_large;
+	fds[2].events = POLLIN;
 
-	struct orb_test t;
+	const unsigned maxruns = 1000;
+	unsigned timingsgroup = 0;
 
-	const unsigned maxruns = 10;
+	unsigned *timings = new unsigned[maxruns];
 
 	for (unsigned i = 0; i < maxruns; i++) {
 		/* wait for up to 500ms for data */
 		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
-		orb_copy(ORB_ID(orb_multitest), test_multi_sub, &t);
+		if (fds[0].revents & POLLIN) {
+			orb_copy(ORB_ID(orb_test), test_multi_sub, &t);
+			timingsgroup = 0;
+		} else if (fds[1].revents & POLLIN) {
+			orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t);
+			timingsgroup = 1;
+		} else if (fds[2].revents & POLLIN) {
+			orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t);
+			timingsgroup = 2;
+		}
 
 		if (pret < 0) {
 			warn("poll error %d, %d", pret, errno);
@@ -750,17 +791,46 @@ int pubsublatency_main(void)
 
 		hrt_abstime elt = hrt_elapsed_time(&t.time);
 		latency_integral += elt;
+		timings[i] = elt;
 	}
 
 	orb_unsubscribe(test_multi_sub);
+	orb_unsubscribe(test_multi_sub_medium);
+	orb_unsubscribe(test_multi_sub_large);
+
+	if (pubsubtest_print) {
+		char fname[32];
+		sprintf(fname, "/fs/microsd/timings%u.txt", timingsgroup);
+		FILE *f = fopen(fname, "w");
+		if (f == NULL) {
+			warnx("Error opening file!\n");
+			return ERROR;
+    		}
+
+		for (unsigned i = 0; i < maxruns; i++) {
+			fprintf(f, "%u\n", timings[i]);
+		}
+
+		fclose(f);
+	}
+
+	delete[] timings;
 
 	warnx("mean: %8.4f", static_cast<double>(latency_integral / maxruns));
 
 	pubsubtest_passed = true;
 
-	return OK;
+	if (static_cast<float>(latency_integral / maxruns) > 30.0f) {
+		pubsubtest_res = ERROR;
+	} else {
+		pubsubtest_res = OK;
+	}
+
+	return pubsubtest_res;
 }
 
+template<typename S> int latency_test(orb_id_t T, bool print);
+
 int
 test()
 {
@@ -874,6 +944,25 @@ test()
 	if (prio != ORB_PRIO_MIN)
 		return test_fail("prio: %d", prio);
 
+	if (OK != latency_test<struct orb_test>(ORB_ID(orb_test), false))
+		return test_fail("latency test failed");
+
+	return test_note("PASS");
+}
+
+template<typename S> int
+latency_test(orb_id_t T, bool print)
+{
+	S t;
+	t.val = 308;
+	t.time = hrt_absolute_time();
+
+	int pfd0 = orb_advertise(T, &t);
+
+	pubsubtest_print = print;
+
+	pubsubtest_passed = false;
+
 	/* test pub / sub latency */
 
 	int pubsub_task = task_spawn_cmd("uorb_latency",
@@ -885,20 +974,22 @@ test()
 
 	/* give the test task some data */
 	while (!pubsubtest_passed) {
-		t.val = 303;
+		t.val = 308;
 		t.time = hrt_absolute_time();
-		if (OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t))
+		if (OK != orb_publish(T, pfd0, &t))
 			return test_fail("mult. pub0 timing fail");
 
 		/* simulate >800 Hz system operation */
 		usleep(1000);
 	}
 
+	close(pfd0);
+
 	if (pubsub_task < 0) {
 		return test_fail("failed launching task");
 	}
 
-	return test_note("PASS");
+	return pubsubtest_res;
 }
 
 int
@@ -956,12 +1047,26 @@ uorb_main(int argc, char *argv[])
 		return test();
 
 	/*
+	 * Test the latency.
+	 */
+	if (!strcmp(argv[1], "latency_test")) {
+
+		if (argc > 2 && !strcmp(argv[2], "medium")) {
+			return latency_test<struct orb_test_medium>(ORB_ID(orb_test_medium), true);
+		} else if (argc > 2 && !strcmp(argv[2], "large")) {
+			return latency_test<struct orb_test_large>(ORB_ID(orb_test_large), true);
+		} else {
+			return latency_test<struct orb_test>(ORB_ID(orb_test), true);
+		}
+	}
+
+	/*
 	 * Print driver information.
 	 */
 	if (!strcmp(argv[1], "status"))
 		return info();
 
-	errx(-EINVAL, "unrecognized command, try 'start', 'test' or 'status'");
+	errx(-EINVAL, "unrecognized command, try 'start', 'test', 'latency_test' or 'status'");
 }
 
 /*
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index 288ba2ebe..74e1efd6c 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -782,18 +782,23 @@ void VtolAttitudeControl::task_main()
 				fill_mc_att_control_output();
 				fill_mc_att_rates_sp();
 
-				if (_actuators_0_pub > 0) {
-					orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
-
-				} else {
-					_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
-				}
-
-				if (_actuators_1_pub > 0) {
-					orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
-
-				} else {
-					_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
+				/* Only publish if the proper mode(s) are enabled */
+				if(_v_control_mode.flag_control_attitude_enabled ||
+				   _v_control_mode.flag_control_rates_enabled)
+				{
+					if (_actuators_0_pub > 0) {
+						orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
+
+					} else {
+						_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
+					}
+
+					if (_actuators_1_pub > 0) {
+						orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
+
+					} else {
+						_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
+					}
 				}
 			}
 		}
@@ -813,18 +818,23 @@ void VtolAttitudeControl::task_main()
 				fill_fw_att_control_output();
 				fill_fw_att_rates_sp();
 
-				if (_actuators_0_pub > 0) {
-					orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
+				/* Only publish if the proper mode(s) are enabled */
+				if(_v_control_mode.flag_control_attitude_enabled ||
+				   _v_control_mode.flag_control_rates_enabled)
+				{
+					if (_actuators_0_pub > 0) {
+						orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
 
-				} else {
-					_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
-				}
+					} else {
+						_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
+					}
 
-				if (_actuators_1_pub > 0) {
-					orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
+					if (_actuators_1_pub > 0) {
+						orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
 
-				} else {
-					_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
+					} else {
+						_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
+					}
 				}
 			}
 		}
diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
index f8561fa3b..0e98783fd 100644
--- a/src/platforms/px4_includes.h
+++ b/src/platforms/px4_includes.h
@@ -66,6 +66,8 @@
 #include <px4_vehicle_global_velocity_setpoint.h>
 #include <px4_vehicle_local_position.h>
 #include <px4_position_setpoint_triplet.h>
+#include <px4_offboard_control_mode.h>
+#include <px4_vehicle_force_setpoint.h>
 #endif
 
 #else
@@ -93,6 +95,8 @@
 #include <platforms/nuttx/px4_messages/px4_vehicle_global_velocity_setpoint.h>
 #include <platforms/nuttx/px4_messages/px4_vehicle_local_position.h>
 #include <platforms/nuttx/px4_messages/px4_position_setpoint_triplet.h>
+#include <platforms/nuttx/px4_messages/px4_offboard_control_mode.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_force_setpoint.h>
 #endif
 #include <systemlib/err.h>
 #include <systemlib/param/param.h>
diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp
index 2673122c7..0c32026f3 100644
--- a/src/platforms/ros/nodes/commander/commander.cpp
+++ b/src/platforms/ros/nodes/commander/commander.cpp
@@ -45,18 +45,25 @@
 Commander::Commander() :
 	_n(),
 	_man_ctrl_sp_sub(_n.subscribe("manual_control_setpoint", 10, &Commander::ManualControlInputCallback, this)),
+	_offboard_control_mode_sub(_n.subscribe("offboard_control_mode", 10, &Commander::OffboardControlModeCallback, this)),
 	_vehicle_control_mode_pub(_n.advertise<px4::vehicle_control_mode>("vehicle_control_mode", 10)),
 	_actuator_armed_pub(_n.advertise<px4::actuator_armed>("actuator_armed", 10)),
 	_vehicle_status_pub(_n.advertise<px4::vehicle_status>("vehicle_status", 10)),
 	_parameter_update_pub(_n.advertise<px4::parameter_update>("parameter_update", 10)),
 	_msg_parameter_update(),
 	_msg_actuator_armed(),
-	_msg_vehicle_control_mode()
+	_msg_vehicle_control_mode(),
+	_msg_offboard_control_mode(),
+	_got_manual_control(false)
 {
+
+	/* Default to offboard control: when no joystick is connected offboard control should just work */
+
 }
 
 void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg)
 {
+	_got_manual_control = true;
 	px4::vehicle_status msg_vehicle_status;
 
 	/* fill vehicle control mode based on (faked) stick positions*/
@@ -101,12 +108,51 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon
 	}
 }
 
+void Commander::SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode,
+			px4::vehicle_control_mode &msg_vehicle_control_mode)
+{
+	msg_vehicle_control_mode.flag_control_manual_enabled = false;
+	msg_vehicle_control_mode.flag_control_offboard_enabled = true;
+	msg_vehicle_control_mode.flag_control_auto_enabled = false;
+
+	msg_vehicle_control_mode.flag_control_rates_enabled = !msg_offboard_control_mode.ignore_bodyrate ||
+		!msg_offboard_control_mode.ignore_attitude ||
+		!msg_offboard_control_mode.ignore_position ||
+		!msg_offboard_control_mode.ignore_velocity ||
+		!msg_offboard_control_mode.ignore_acceleration_force;
+
+	msg_vehicle_control_mode.flag_control_attitude_enabled = !msg_offboard_control_mode.ignore_attitude ||
+		!msg_offboard_control_mode.ignore_position ||
+		!msg_offboard_control_mode.ignore_velocity ||
+		!msg_offboard_control_mode.ignore_acceleration_force;
+
+
+	msg_vehicle_control_mode.flag_control_velocity_enabled = !msg_offboard_control_mode.ignore_velocity ||
+		!msg_offboard_control_mode.ignore_position;
+
+	msg_vehicle_control_mode.flag_control_climb_rate_enabled = !msg_offboard_control_mode.ignore_velocity ||
+		!msg_offboard_control_mode.ignore_position;
+
+	msg_vehicle_control_mode.flag_control_position_enabled = !msg_offboard_control_mode.ignore_position;
+
+	msg_vehicle_control_mode.flag_control_altitude_enabled = !msg_offboard_control_mode.ignore_position;
+}
+
 void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg,
 		px4::vehicle_status &msg_vehicle_status,
 		px4::vehicle_control_mode &msg_vehicle_control_mode) {
 	// XXX this is a minimal implementation. If more advanced functionalities are
 	// needed consider a full port of the commander
 
+
+	if (msg->offboard_switch == px4::manual_control_setpoint::SWITCH_POS_ON)
+	{
+		SetOffboardControl(_msg_offboard_control_mode, msg_vehicle_control_mode);
+		return;
+	}
+
+	msg_vehicle_control_mode.flag_control_offboard_enabled = false;
+
 	switch (msg->mode_switch) {
 		case px4::manual_control_setpoint::SWITCH_POS_NONE:
 		ROS_WARN("Joystick button mapping error, main mode not set");
@@ -152,6 +198,35 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg,
 
 }
 
+void Commander::OffboardControlModeCallback(const px4::offboard_control_modeConstPtr &msg)
+{
+	_msg_offboard_control_mode = *msg;
+
+	/* Force system into offboard control mode */
+	if (!_got_manual_control) {
+		SetOffboardControl(_msg_offboard_control_mode, _msg_vehicle_control_mode);
+		
+		px4::vehicle_status msg_vehicle_status;
+		msg_vehicle_status.timestamp = px4::get_time_micros();
+		msg_vehicle_status.hil_state = msg_vehicle_status.HIL_STATE_OFF;
+		msg_vehicle_status.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR;
+		msg_vehicle_status.is_rotary_wing = true;
+		msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED;
+		
+
+		_msg_actuator_armed.armed = true;
+		_msg_actuator_armed.timestamp = px4::get_time_micros();
+
+		_msg_vehicle_control_mode.timestamp = px4::get_time_micros();
+		_msg_vehicle_control_mode.flag_armed = true;
+		
+
+		_vehicle_control_mode_pub.publish(_msg_vehicle_control_mode);
+		_actuator_armed_pub.publish(_msg_actuator_armed);
+		_vehicle_status_pub.publish(msg_vehicle_status);
+	}
+}
+
 int main(int argc, char **argv)
 {
 	ros::init(argc, argv, "commander");
diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h
index 58b7257b7..c537c8419 100644
--- a/src/platforms/ros/nodes/commander/commander.h
+++ b/src/platforms/ros/nodes/commander/commander.h
@@ -44,6 +44,7 @@
 #include <px4/vehicle_status.h>
 #include <px4/parameter_update.h>
 #include <px4/actuator_armed.h>
+#include <px4/offboard_control_mode.h>
 
 class Commander
 {
@@ -59,14 +60,26 @@ protected:
 	void ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg);
 
 	/**
+	 * Stores the offboard control mode
+	 */
+	void OffboardControlModeCallback(const px4::offboard_control_modeConstPtr &msg);
+
+	/**
 	 * Set control mode flags based on stick positions (equiv to code in px4 commander)
 	 */
 	void EvalSwitches(const px4::manual_control_setpointConstPtr &msg,
 			px4::vehicle_status &msg_vehicle_status,
 			px4::vehicle_control_mode &msg_vehicle_control_mode);
 
+	/**
+	 * Sets offboard controll flags in msg_vehicle_control_mode
+	 */
+	void SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode,
+			px4::vehicle_control_mode &msg_vehicle_control_mode);
+
 	ros::NodeHandle _n;
 	ros::Subscriber _man_ctrl_sp_sub;
+	ros::Subscriber _offboard_control_mode_sub;
 	ros::Publisher _vehicle_control_mode_pub;
 	ros::Publisher _actuator_armed_pub;
 	ros::Publisher _vehicle_status_pub;
@@ -75,5 +88,8 @@ protected:
 	px4::parameter_update _msg_parameter_update;
 	px4::actuator_armed _msg_actuator_armed;
 	px4::vehicle_control_mode _msg_vehicle_control_mode;
+	px4::offboard_control_mode _msg_offboard_control_mode;
+
+	bool _got_manual_control;
 
 };
diff --git a/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp
new file mode 100644
index 000000000..fb0b09de1
--- /dev/null
+++ b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp
@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   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 demo_offboard_position_Setpoints.cpp
+ *
+ * Demo for sending offboard position setpoints to mavros to show offboard position control in SITL
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "demo_offboard_attitude_setpoints.h"
+
+#include <platforms/px4_middleware.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <std_msgs/Float64.h>
+#include <math.h>
+#include <tf/transform_datatypes.h>
+
+DemoOffboardAttitudeSetpoints::DemoOffboardAttitudeSetpoints() :
+	_n(),
+	_attitude_sp_pub(_n.advertise<geometry_msgs::PoseStamped>("mavros/setpoint/attitude", 1)),
+	_thrust_sp_pub(_n.advertise<std_msgs::Float64>("mavros/setpoint/att_throttle", 1))
+{
+}
+
+
+int DemoOffboardAttitudeSetpoints::main()
+{
+	px4::Rate loop_rate(10);
+
+	while (ros::ok()) {
+		loop_rate.sleep();
+		ros::spinOnce();
+
+		/* Publish example offboard attitude setpoint */
+		geometry_msgs::PoseStamped pose;
+		tf::Quaternion q = tf::createQuaternionFromRPY(0.0, 0.1 * (sinf(0.5 * (float)px4::get_time_micros() / 1000000.0f)) , 0.0);
+		quaternionTFToMsg(q, pose.pose.orientation);
+
+		_attitude_sp_pub.publish(pose);
+
+		std_msgs::Float64 thrust;
+		thrust.data = 0.4f + 0.25 * (sinf((float)px4::get_time_micros() / 1000000.0f)); // just some example throttle input that makes the quad 'jump'
+		_thrust_sp_pub.publish(thrust);
+	}
+	return 0;
+}
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "demo_offboard_position_setpoints");
+	DemoOffboardAttitudeSetpoints d;
+	return d.main();
+}
diff --git a/src/modules/uORB/topics/vehicle_force_setpoint.h b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.h
index e3a7360b2..d7b7a37ba 100644
--- a/src/modules/uORB/topics/vehicle_force_setpoint.h
+++ b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2014 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
@@ -32,34 +32,27 @@
  ****************************************************************************/
 
 /**
- * @file vehicle_force_setpoint.h
+ * @file demo_offboard_attitude_Setpoints.h
+ *
+ * Demo for sending offboard position setpoints to mavros to show offboard position control in SITL
+ *
  * @author Thomas Gubler <thomasgubler@gmail.com>
- * Definition of force (NED) setpoint uORB topic. Typically this can be used
- * by a position control app together with an attitude control app.
- */
-
-#ifndef TOPIC_VEHICLE_FORCE_SETPOINT_H_
-#define TOPIC_VEHICLE_FORCE_SETPOINT_H_
+*/
 
-#include "../uORB.h"
+#include "ros/ros.h"
+#include <px4/manual_control_setpoint.h>
 
-/**
- * @addtogroup topics
- * @{
- */
-
-struct vehicle_force_setpoint_s {
-	float x;		/**< in N NED			  		*/
-	float y;		/**< in N NED			  		*/
-	float z;		/**< in N NED			  		*/
-	float yaw;		/**< right-hand rotation around downward axis (rad, equivalent to Tait-Bryan yaw) */
-}; /**< Desired force in NED frame */
+class DemoOffboardAttitudeSetpoints
+{
+public:
+	DemoOffboardAttitudeSetpoints();
 
-/**
- * @}
- */
+	~DemoOffboardAttitudeSetpoints() {}
 
-/* register this as object request broker structure */
-ORB_DECLARE(vehicle_force_setpoint);
+	int main();
 
-#endif
+protected:
+	ros::NodeHandle _n;
+	ros::Publisher _attitude_sp_pub;
+	ros::Publisher _thrust_sp_pub;
+};
diff --git a/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp
new file mode 100644
index 000000000..7366d7fc6
--- /dev/null
+++ b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp
@@ -0,0 +1,77 @@
+/****************************************************************************
+ *
+ *   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 demo_offboard_position_Setpoints.cpp
+ *
+ * Demo for sending offboard position setpoints to mavros to show offboard position control in SITL
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "demo_offboard_position_setpoints.h"
+
+#include <platforms/px4_middleware.h>
+#include <geometry_msgs/PoseStamped.h>
+
+DemoOffboardPositionSetpoints::DemoOffboardPositionSetpoints() :
+	_n(),
+	_local_position_sp_pub(_n.advertise<geometry_msgs::PoseStamped>("mavros/setpoint/local_position", 1))
+{
+}
+
+
+int DemoOffboardPositionSetpoints::main()
+{
+	px4::Rate loop_rate(10);
+
+	while (ros::ok()) {
+		loop_rate.sleep();
+		ros::spinOnce();
+
+		/* Publish example offboard position setpoint */
+		geometry_msgs::PoseStamped pose;
+		pose.pose.position.x = 0;
+		pose.pose.position.y = 0;
+		pose.pose.position.z = 1;
+		_local_position_sp_pub.publish(pose);
+	}
+	return 0;
+}
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "demo_offboard_position_setpoints");
+	DemoOffboardPositionSetpoints d;
+	return d.main();
+}
diff --git a/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.h b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.h
new file mode 100644
index 000000000..7d39690f4
--- /dev/null
+++ b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.h
@@ -0,0 +1,57 @@
+/****************************************************************************
+ *
+ *   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 demo_offboard_position_Setpoints.h
+ *
+ * Demo for sending offboard position setpoints to mavros to show offboard position control in SITL
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "ros/ros.h"
+#include <px4/manual_control_setpoint.h>
+
+class DemoOffboardPositionSetpoints
+{
+public:
+	DemoOffboardPositionSetpoints();
+
+	~DemoOffboardPositionSetpoints() {}
+
+	int main();
+
+protected:
+	ros::NodeHandle _n;
+	ros::Publisher _local_position_sp_pub;
+};
diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp
index 72f6e252f..8488c518f 100644
--- a/src/platforms/ros/nodes/manual_input/manual_input.cpp
+++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp
@@ -76,7 +76,8 @@ ManualInput::ManualInput() :
 	_n.param("map_posctl", _param_buttons_map[2], 2);
 	_n.param("map_auto_mission", _param_buttons_map[3], 3);
 	_n.param("map_auto_loiter", _param_buttons_map[4], 4);
-	_n.param("map_auto_rtl", _param_buttons_map[5], 4);
+	_n.param("map_auto_rtl", _param_buttons_map[5], 5);
+	_n.param("map_offboard", _param_buttons_map[6], 6);
 
 	/* Default to manual */
 	_msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
@@ -84,6 +85,8 @@ ManualInput::ManualInput() :
 	_msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
 	_msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
 	_msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+	_msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+	_msg_mc_sp.acro_switch = px4::manual_control_setpoint::SWITCH_POS_NONE;
 
 }
 
@@ -120,7 +123,6 @@ void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, do
 
 void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_control_setpoint &msg_mc_sp) {
 	msg_mc_sp.acro_switch = px4::manual_control_setpoint::SWITCH_POS_NONE;
-	msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_NONE;
 
 	if (_buttons_state[MAIN_STATE_MANUAL] != msg->buttons[_param_buttons_map[MAIN_STATE_MANUAL]] == true) {
 		msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
@@ -128,6 +130,7 @@ void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_co
 		msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
 		msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
 		msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
 		return;
 
 	} else if (_buttons_state[MAIN_STATE_ALTCTL] != msg->buttons[_param_buttons_map[MAIN_STATE_ALTCTL]] == true) {
@@ -136,6 +139,7 @@ void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_co
 		msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
 		msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
 		msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
 		return;
 
 	} else if (_buttons_state[MAIN_STATE_POSCTL] != msg->buttons[_param_buttons_map[MAIN_STATE_POSCTL]] == true) {
@@ -144,6 +148,15 @@ void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_co
 		msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_ON;
 		msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
 		msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		return;
+	} else if (_buttons_state[MAIN_STATE_OFFBOARD] != msg->buttons[_param_buttons_map[MAIN_STATE_OFFBOARD]] == true) {
+		msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_MIDDLE;
+		msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_ON;
+		msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_ON;
 		return;
 	}
 
diff --git a/src/platforms/ros/nodes/manual_input/manual_input.h b/src/platforms/ros/nodes/manual_input/manual_input.h
index bf704f675..2bafcca2e 100644
--- a/src/platforms/ros/nodes/manual_input/manual_input.h
+++ b/src/platforms/ros/nodes/manual_input/manual_input.h
@@ -77,6 +77,8 @@ protected:
 		MAIN_STATE_AUTO_MISSION,
 		MAIN_STATE_AUTO_LOITER,
 		MAIN_STATE_AUTO_RTL,
+		// MAIN_STATE_ACRO,
+		MAIN_STATE_OFFBOARD,
 		MAIN_STATE_MAX
 	};
 
diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp
new file mode 100644
index 000000000..5459fcffd
--- /dev/null
+++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp
@@ -0,0 +1,296 @@
+/****************************************************************************
+ *
+ *   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 mavlink.cpp
+ * Dummy mavlink node that interfaces to a mavros node via UDP
+ * This simulates the onboard mavlink app to some degree. It should be possible to
+ * send offboard setpoints via mavros to the SITL setup the same way as on the real system
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "mavlink.h"
+
+#include <platforms/px4_middleware.h>
+
+using namespace px4;
+
+Mavlink::Mavlink() :
+	_n(),
+	_v_att_sub(_n.subscribe("vehicle_attitude", 1, &Mavlink::VehicleAttitudeCallback, this)),
+	_v_local_pos_sub(_n.subscribe("vehicle_local_position", 1, &Mavlink::VehicleLocalPositionCallback, this)),
+	_v_att_sp_pub(_n.advertise<vehicle_attitude_setpoint>("vehicle_attitude_setpoint", 1)),
+	_pos_sp_triplet_pub(_n.advertise<position_setpoint_triplet>("position_setpoint_triplet", 1)),
+	_offboard_control_mode_pub(_n.advertise<offboard_control_mode>("offboard_control_mode", 1)),
+	_force_sp_pub(_n.advertise<vehicle_force_setpoint>("vehicle_force_setpoint", 1))
+{
+	_link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560");
+	_link->message_received.connect(boost::bind(&Mavlink::handle_msg, this, _1, _2, _3));
+
+}
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "mavlink");
+	Mavlink m;
+	ros::spin();
+	return 0;
+}
+
+void Mavlink::VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg)
+{
+	mavlink_message_t msg_m;
+	mavlink_msg_attitude_quaternion_pack_chan(
+			_link->get_system_id(),
+			_link->get_component_id(),
+			_link->get_channel(),
+			&msg_m,
+			get_time_micros() / 1000,
+			msg->q[0],
+			msg->q[1],
+			msg->q[2],
+			msg->q[3],
+			msg->rollspeed,
+			msg->pitchspeed,
+			msg->yawspeed);
+	_link->send_message(&msg_m);
+}
+
+void Mavlink::VehicleLocalPositionCallback(const vehicle_local_positionConstPtr &msg)
+{
+	mavlink_message_t msg_m;
+	mavlink_msg_local_position_ned_pack_chan(
+			_link->get_system_id(),
+			_link->get_component_id(),
+			_link->get_channel(),
+			&msg_m,
+			get_time_micros() / 1000,
+			msg->x,
+			msg->y,
+			msg->z,
+			msg->vx,
+			msg->vy,
+			msg->vz);
+	_link->send_message(&msg_m);
+}
+
+void Mavlink::handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid) {
+	(void)sysid;
+	(void)compid;
+
+	switch(mmsg->msgid) {
+		case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
+			handle_msg_set_attitude_target(mmsg);
+			break;
+		case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
+			handle_msg_set_position_target_local_ned(mmsg);
+			break;
+		default:
+			break;
+	}
+
+}
+
+void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg)
+{
+	mavlink_set_attitude_target_t set_attitude_target;
+	mavlink_msg_set_attitude_target_decode(mmsg, &set_attitude_target);
+
+	static offboard_control_mode offboard_control_mode;
+
+	/* set correct ignore flags for thrust field: copy from mavlink message */
+	offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6));
+
+	/*
+	 * if attitude or body rate have been used (not ignored) previously and this message only sends
+	 * throttle and has the ignore bits set for attitude and rates don't change the flags for attitude and
+	 * body rates to keep the controllers running
+	 */
+	bool ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7);
+	bool ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7));
+
+	if (ignore_bodyrate && ignore_attitude && !offboard_control_mode.ignore_thrust) {
+		/* Message want's us to ignore everything except thrust: only ignore if previously ignored */
+		offboard_control_mode.ignore_bodyrate = ignore_bodyrate && offboard_control_mode.ignore_bodyrate;
+		offboard_control_mode.ignore_attitude = ignore_attitude && offboard_control_mode.ignore_attitude;
+	} else {
+		offboard_control_mode.ignore_bodyrate = ignore_bodyrate;
+		offboard_control_mode.ignore_attitude = ignore_attitude;
+	}
+	offboard_control_mode.ignore_position = true;
+	offboard_control_mode.ignore_velocity = true;
+	offboard_control_mode.ignore_acceleration_force = true;
+
+	offboard_control_mode.timestamp = get_time_micros();
+	_offboard_control_mode_pub.publish(offboard_control_mode);
+
+	static vehicle_attitude_setpoint att_sp = {};
+
+	/* The real mavlink app has a ckeck at this location which makes sure that the attitude setpoint
+	 * gets published only if in offboard mode. We leave that out for now.
+	 */
+
+	att_sp.timestamp = get_time_micros();
+	mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body,
+			&att_sp.yaw_body);
+	mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body.data());
+	att_sp.R_valid = true;
+
+	if (!offboard_control_mode.ignore_thrust) {
+		att_sp.thrust = set_attitude_target.thrust;
+	}
+
+	if (!offboard_control_mode.ignore_attitude) {
+		for (ssize_t i = 0; i < 4; i++) {
+			att_sp.q_d[i] = set_attitude_target.q[i];
+		}
+		att_sp.q_d_valid = true;
+	}
+
+	_v_att_sp_pub.publish(att_sp);
+
+
+	//XXX real mavlink publishes rate sp here
+
+}
+
+void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t *mmsg)
+{
+
+	mavlink_set_position_target_local_ned_t set_position_target_local_ned;
+	mavlink_msg_set_position_target_local_ned_decode(mmsg, &set_position_target_local_ned);
+
+	offboard_control_mode offboard_control_mode;
+	// memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints
+
+	/* Only accept messages which are intended for this system */
+	// XXX removed for sitl, makes maybe sense to re-introduce at some point
+	// if ((mavlink_system.sysid == set_position_target_local_ned.target_system ||
+				// set_position_target_local_ned.target_system == 0) &&
+			// (mavlink_system.compid == set_position_target_local_ned.target_component ||
+			 // set_position_target_local_ned.target_component == 0)) {
+
+	/* convert mavlink type (local, NED) to uORB offboard control struct */
+	offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7);
+	offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask & 0x38);
+	offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask & 0x1C0);
+	bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9));
+	/* yaw ignore flag mapps to ignore_attitude */
+	offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400);
+	/* yawrate ignore flag mapps to ignore_bodyrate */
+	offboard_control_mode.ignore_bodyrate = (bool)(set_position_target_local_ned.type_mask & 0x800);
+
+
+
+	offboard_control_mode.timestamp = get_time_micros();
+	_offboard_control_mode_pub.publish(offboard_control_mode);
+
+	/* The real mavlink app has a ckeck at this location which makes sure that the position setpoint triplet
+	 * gets published only if in offboard mode. We leave that out for now.
+	 */
+	if (is_force_sp && offboard_control_mode.ignore_position &&
+			offboard_control_mode.ignore_velocity) {
+		/* The offboard setpoint is a force setpoint only, directly writing to the force
+		 * setpoint topic and not publishing the setpoint triplet topic */
+		vehicle_force_setpoint	force_sp;
+		force_sp.x = set_position_target_local_ned.afx;
+		force_sp.y = set_position_target_local_ned.afy;
+		force_sp.z = set_position_target_local_ned.afz;
+		//XXX: yaw
+
+		_force_sp_pub.publish(force_sp);
+	} else {
+		/* It's not a pure force setpoint: publish to setpoint triplet  topic */
+		position_setpoint_triplet pos_sp_triplet;
+		pos_sp_triplet.previous.valid = false;
+		pos_sp_triplet.next.valid = false;
+		pos_sp_triplet.current.valid = true;
+		pos_sp_triplet.current.type = position_setpoint::SETPOINT_TYPE_POSITION; //XXX support others
+
+		/* set the local pos values */
+		if (!offboard_control_mode.ignore_position) {
+			pos_sp_triplet.current.position_valid = true;
+			pos_sp_triplet.current.x = set_position_target_local_ned.x;
+			pos_sp_triplet.current.y = set_position_target_local_ned.y;
+			pos_sp_triplet.current.z = set_position_target_local_ned.z;
+		} else {
+			pos_sp_triplet.current.position_valid = false;
+		}
+
+		/* set the local vel values */
+		if (!offboard_control_mode.ignore_velocity) {
+			pos_sp_triplet.current.velocity_valid = true;
+			pos_sp_triplet.current.vx = set_position_target_local_ned.vx;
+			pos_sp_triplet.current.vy = set_position_target_local_ned.vy;
+			pos_sp_triplet.current.vz = set_position_target_local_ned.vz;
+		} else {
+			pos_sp_triplet.current.velocity_valid = false;
+		}
+
+		/* set the local acceleration values if the setpoint type is 'local pos' and none
+		 * of the accelerations fields is set to 'ignore' */
+		if (!offboard_control_mode.ignore_acceleration_force) {
+			pos_sp_triplet.current.acceleration_valid = true;
+			pos_sp_triplet.current.a_x = set_position_target_local_ned.afx;
+			pos_sp_triplet.current.a_y = set_position_target_local_ned.afy;
+			pos_sp_triplet.current.a_z = set_position_target_local_ned.afz;
+			pos_sp_triplet.current.acceleration_is_force =
+				is_force_sp;
+
+		} else {
+			pos_sp_triplet.current.acceleration_valid = false;
+		}
+
+		/* set the yaw sp value */
+		if (!offboard_control_mode.ignore_attitude) {
+			pos_sp_triplet.current.yaw_valid = true;
+			pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw;
+
+		} else {
+			pos_sp_triplet.current.yaw_valid = false;
+		}
+
+		/* set the yawrate sp value */
+		if (!offboard_control_mode.ignore_bodyrate) {
+			pos_sp_triplet.current.yawspeed_valid = true;
+			pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate;
+
+		} else {
+			pos_sp_triplet.current.yawspeed_valid = false;
+		}
+		//XXX handle global pos setpoints (different MAV frames)
+
+		_pos_sp_triplet_pub.publish(pos_sp_triplet);
+	}
+}
diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h
new file mode 100644
index 000000000..acb2408f3
--- /dev/null
+++ b/src/platforms/ros/nodes/mavlink/mavlink.h
@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   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 mavlink.h
+ * Dummy mavlink node that interfaces to a mavros node via UDP
+ * This simulates the onboard mavlink app to some degree. It should be possible to
+ * send offboard setpoints via mavros to the SITL setup the same way as on the real system
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "ros/ros.h"
+#include <mavconn/interface.h>
+#include <px4/vehicle_attitude.h>
+#include <px4/vehicle_local_position.h>
+#include <px4/vehicle_attitude_setpoint.h>
+#include <px4/position_setpoint_triplet.h>
+#include <px4/vehicle_force_setpoint.h>
+#include <px4/offboard_control_mode.h>
+
+namespace px4
+{
+
+class Mavlink
+{
+public:
+	Mavlink();
+
+	~Mavlink() {}
+
+protected:
+
+	ros::NodeHandle _n;
+	mavconn::MAVConnInterface::Ptr _link;
+	ros::Subscriber _v_att_sub;
+	ros::Subscriber _v_local_pos_sub;
+	ros::Publisher _v_att_sp_pub;
+	ros::Publisher _pos_sp_triplet_pub;
+	ros::Publisher _offboard_control_mode_pub;
+	ros::Publisher _force_sp_pub;
+
+	/**
+	 *
+	 * Simulates output of attitude data from the FCU
+	 * Equivalent to the mavlink stream ATTITUDE_QUATERNION
+	 *
+	 * */
+	void VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg);
+
+	/**
+	 *
+	 * Simulates output of local position data from the FCU
+	 * Equivalent to the mavlink stream LOCAL_POSITION_NED
+	 *
+	 * */
+	void VehicleLocalPositionCallback(const vehicle_local_positionConstPtr &msg);
+
+
+	/**
+	 *
+	 * Handle incoming mavlink messages ant publish them to ROS ("Mavlink Receiver")
+	 *
+	 * */
+	void handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid);
+
+	/**
+	 *
+	 * Handle SET_ATTITUDE_TARGET mavlink messages
+	 *
+	 * */
+	void handle_msg_set_attitude_target(const mavlink_message_t *mmsg);
+
+	/**
+	 *
+	 * Handle SET_POSITION_TARGET_LOCAL_NED mavlink messages
+	 *
+	 * */
+	void handle_msg_set_position_target_local_ned(const mavlink_message_t *mmsg);
+
+};
+
+}
diff --git a/src/systemcmds/bl_update/bl_update.c b/src/systemcmds/bl_update/bl_update.c
index ec9269d39..02d2d6f3c 100644
--- a/src/systemcmds/bl_update/bl_update.c
+++ b/src/systemcmds/bl_update/bl_update.c
@@ -74,7 +74,7 @@ bl_update_main(int argc, char *argv[])
 
 	struct stat s;
 
-	if (!stat(argv[1], &s))
+	if (stat(argv[1], &s) != 0)
 		err(1, "stat %s", argv[1]);
 
 	/* sanity-check file size */
@@ -214,4 +214,4 @@ setopt(void)
 
 	errx(1, "option bits setting failed; readback 0x%04x", *optcr);
 
-}
\ No newline at end of file
+}
diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index 0c984d69f..620b0a6f6 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -279,7 +279,14 @@ pwm_main(int argc, char *argv[])
 		if (pwm_value == 0)
 			usage("no PWM value provided");
 
-		struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
+		struct pwm_output_values pwm_values;
+		memset(&pwm_values, 0, sizeof(pwm_values));
+		pwm_values.channel_count = servo_count;
+		/* first get current state before modifying it */
+		ret = ioctl(fd, PWM_SERVO_GET_MIN_PWM, (long unsigned int)&pwm_values);
+		if (ret != OK) {
+			errx(ret, "failed get min values");
+		}
 
 		for (unsigned i = 0; i < servo_count; i++) {
 			if (set_mask & 1<<i) {
@@ -287,7 +294,6 @@ pwm_main(int argc, char *argv[])
 				if (print_verbose)
 					warnx("Channel %d: min PWM: %d", i+1, pwm_value);
 			}
-			pwm_values.channel_count++;
 		}
 
 		if (pwm_values.channel_count == 0) {
@@ -308,7 +314,14 @@ pwm_main(int argc, char *argv[])
 		if (pwm_value == 0)
 			usage("no PWM value provided");
 
-		struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
+		struct pwm_output_values pwm_values;
+		memset(&pwm_values, 0, sizeof(pwm_values));
+		pwm_values.channel_count = servo_count;
+		/* first get current state before modifying it */
+		ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, (long unsigned int)&pwm_values);
+		if (ret != OK) {
+			errx(ret, "failed get max values");
+		}
 
 		for (unsigned i = 0; i < servo_count; i++) {
 			if (set_mask & 1<<i) {
@@ -316,7 +329,6 @@ pwm_main(int argc, char *argv[])
 				if (print_verbose)
 					warnx("Channel %d: max PWM: %d", i+1, pwm_value);
 			}
-			pwm_values.channel_count++;
 		}
 
 		if (pwm_values.channel_count == 0) {
@@ -337,15 +349,22 @@ pwm_main(int argc, char *argv[])
 		if (pwm_value == 0)
 			warnx("reading disarmed value of zero, disabling disarmed PWM");
 
-		struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
+		struct pwm_output_values pwm_values;
+		memset(&pwm_values, 0, sizeof(pwm_values));
+		pwm_values.channel_count = servo_count;
+		/* first get current state before modifying it */
+		ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (long unsigned int)&pwm_values);
+		if (ret != OK) {
+			errx(ret, "failed get disarmed values");
+		}
 
 		for (unsigned i = 0; i < servo_count; i++) {
 			if (set_mask & 1<<i) {
 				pwm_values.values[i] = pwm_value;
-				if (print_verbose)
-					warnx("channel %d: disarmed PWM: %d", i+1, pwm_value);
+				if (print_verbose) {
+					warnx("chan %d: disarmed PWM: %d", i+1, pwm_value);
+				}
 			}
-			pwm_values.channel_count++;
 		}
 
 		if (pwm_values.channel_count == 0) {
@@ -353,8 +372,9 @@ pwm_main(int argc, char *argv[])
 		} else {
 
 			ret = ioctl(fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_values);
-			if (ret != OK)
+			if (ret != OK) {
 				errx(ret, "failed setting disarmed values");
+			}
 		}
 		exit(0);
 
@@ -366,7 +386,14 @@ pwm_main(int argc, char *argv[])
 		if (pwm_value == 0)
 			usage("no PWM provided");
 
-		struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
+		struct pwm_output_values pwm_values;
+		memset(&pwm_values, 0, sizeof(pwm_values));
+		pwm_values.channel_count = servo_count;
+		/* first get current state before modifying it */
+		ret = ioctl(fd, PWM_SERVO_GET_FAILSAFE_PWM, (long unsigned int)&pwm_values);
+		if (ret != OK) {
+			errx(ret, "failed get failsafe values");
+		}
 
 		for (unsigned i = 0; i < servo_count; i++) {
 			if (set_mask & 1<<i) {
@@ -374,7 +401,6 @@ pwm_main(int argc, char *argv[])
 				if (print_verbose)
 					warnx("Channel %d: failsafe PWM: %d", i+1, pwm_value);
 			}
-			pwm_values.channel_count++;
 		}
 
 		if (pwm_values.channel_count == 0) {
diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp
index 9fa52aaaa..682514cb7 100644
--- a/src/systemcmds/tests/test_mathlib.cpp
+++ b/src/systemcmds/tests/test_mathlib.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-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
@@ -158,12 +158,13 @@ int test_mathlib(int argc, char *argv[])
 	}
 
 	{
+		warnx("Nonsymmetric matrix operations test");
 		// test nonsymmetric +, -, +=, -=
 
-		float data1[2][3] = {{1,2,3},{4,5,6}};
-		float data2[2][3] = {{2,4,6},{8,10,12}};
-		float data3[2][3] = {{3,6,9},{12,15,18}};
-		
+		float data1[2][3] = {{1, 2, 3}, {4, 5, 6}};
+		float data2[2][3] = {{2, 4, 6}, {8, 10, 12}};
+		float data3[2][3] = {{3, 6, 9}, {12, 15, 18}};
+
 		Matrix<2, 3> m1(data1);
 		Matrix<2, 3> m2(data2);
 		Matrix<2, 3> m3(data3);
@@ -185,6 +186,7 @@ int test_mathlib(int argc, char *argv[])
 		}
 
 		m1 += m2;
+
 		if (m1 != m3) {
 			warnx("Matrix<2, 3> += Matrix<2, 3> failed!");
 			m1.print();
@@ -195,6 +197,7 @@ int test_mathlib(int argc, char *argv[])
 
 		m1 -= m2;
 		Matrix<2, 3> m1_orig(data1);
+
 		if (m1 != m1_orig) {
 			warnx("Matrix<2, 3> -= Matrix<2, 3> failed!");
 			m1.print();
@@ -202,7 +205,81 @@ int test_mathlib(int argc, char *argv[])
 			m1_orig.print();
 			rc = 1;
 		}
-	
+
+	}
+
+	{
+		// test conversion rotation matrix to quaternion and back
+		math::Matrix<3, 3> R_orig;
+		math::Matrix<3, 3> R;
+		math::Quaternion q;
+		float diff = 0.1f;
+		float tol = 0.00001f;
+
+		warnx("Quaternion transformation methods test.");
+
+		for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
+			for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
+				for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
+					R_orig.from_euler(roll, pitch, yaw);
+					q.from_dcm(R_orig);
+					R = q.to_dcm();
+
+					for (int i = 0; i < 3; i++) {
+						for (int j = 0; j < 3; j++) {
+							if (fabsf(R_orig.data[i][j] - R.data[i][j]) > 0.00001f) {
+								warnx("Quaternion method 'from_dcm' or 'to_dcm' outside tolerance!");
+								rc = 1;
+							}
+						}
+					}
+				}
+			}
+		}
+
+		// test against some known values
+		tol = 0.0001f;
+		math::Quaternion q_true = {1.0f, 0.0f, 0.0f, 0.0f};
+		R_orig.identity();
+		q.from_dcm(R_orig);
+
+		for (unsigned i = 0; i < 4; i++) {
+			if (fabsf(q.data[i] - q_true.data[i]) > tol) {
+				warnx("Quaternion method 'from_dcm()' outside tolerance!");
+				rc = 1;
+			}
+		}
+
+		q_true.from_euler(0.3f, 0.2f, 0.1f);
+		q = {0.9833f, 0.1436f, 0.1060f, 0.0343f};
+
+		for (unsigned i = 0; i < 4; i++) {
+			if (fabsf(q.data[i] - q_true.data[i]) > tol) {
+				warnx("Quaternion method 'from_euler()' outside tolerance!");
+				rc = 1;
+			}
+		}
+
+		q_true.from_euler(-1.5f, -0.2f, 0.5f);
+		q = {0.7222f, -0.6391f, -0.2386f, 0.1142f};
+
+		for (unsigned i = 0; i < 4; i++) {
+			if (fabsf(q.data[i] - q_true.data[i]) > tol) {
+				warnx("Quaternion method 'from_euler()' outside tolerance!");
+				rc = 1;
+			}
+		}
+
+		q_true.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3);
+		q = {0.6830f, 0.1830f, -0.6830f, 0.1830f};
+
+		for (unsigned i = 0; i < 4; i++) {
+			if (fabsf(q.data[i] - q_true.data[i]) > tol) {
+				warnx("Quaternion method 'from_euler()' outside tolerance!");
+				rc = 1;
+			}
+		}
+
 	}
 
 	return rc;