Merge ROS into master

189 files changed, 10094 insertions, 1859 deletions

diff --git a/.gitignore b/.gitignore
index 8ea2698e7..611325444 100644
--- a/.gitignore
+++ b/.gitignore
@@ -17,6 +17,7 @@
 .~lock.*
 Archives/*
 Build/*
+build/*
 core
 cscope.out
 Firmware.sublime-workspace
@@ -38,6 +39,10 @@ tags
 .pydevproject
 .ropeproject
 *.orig
+src/modules/uORB/topics/*
+src/platforms/nuttx/px4_messages/*
+src/platforms/ros/px4_messages/*
 Firmware.zip
 unittests/build
 *.generated.h
+.vagrant
diff --git a/.gitmodules b/.gitmodules
index 0c8c91da4..c869803b7 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -7,6 +7,12 @@
 [submodule "uavcan"]
 	path = src/lib/uavcan
 	url = git://github.com/pavel-kirienko/uavcan.git
+[submodule "Tools/genmsg"]
+	path = Tools/genmsg
+	url = https://github.com/ros/genmsg.git
+[submodule "Tools/gencpp"]
+	path = Tools/gencpp
+	url = https://github.com/ros/gencpp.git
 [submodule "unittests/gtest"]
 	path = unittests/gtest
 	url = https://github.com/sjwilks/gtest.git
diff --git a/.travis.yml b/.travis.yml
index 8677bf91b..a994d3471 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -12,7 +12,7 @@ before_script:
   - sudo apt-get install s3cmd grep zip mailutils
 # General toolchain dependencies
   - sudo apt-get install libc6:i386 libgcc1:i386 gcc-4.6-base:i386 libstdc++5:i386 libstdc++6:i386
-  - sudo apt-get install python-serial python-argparse
+  - sudo apt-get install python-serial python-argparse python-empy
   - sudo apt-get install flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget cmake
   - pushd .
   - cd ~
diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 000000000..0c81607b0
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,289 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(px4)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+add_definitions(-D__PX4_ROS)
+add_definitions(-D__EXPORT=)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+  message_generation
+  cmake_modules
+  gazebo_msgs
+  sensor_msgs
+  mav_msgs
+)
+find_package(Eigen REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependencies might have been
+##     pulled in transitively but can be declared for certainty nonetheless:
+##     * add a build_depend tag for "message_generation"
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+add_message_files(
+  FILES
+  rc_channels.msg
+  vehicle_attitude.msg
+  vehicle_attitude_setpoint.msg
+  manual_control_setpoint.msg
+  actuator_controls.msg
+  actuator_controls_0.msg
+  actuator_controls_virtual_mc.msg
+  vehicle_rates_setpoint.msg
+  mc_virtual_rates_setpoint.msg
+  vehicle_attitude.msg
+  vehicle_control_mode.msg
+  actuator_armed.msg
+  parameter_update.msg
+  vehicle_status.msg
+  vehicle_local_position.msg
+  position_setpoint.msg
+  position_setpoint_triplet.msg
+  vehicle_local_position_setpoint.msg
+  vehicle_global_velocity_setpoint.msg
+)
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+  gazebo_msgs
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS src/include
+  LIBRARIES px4
+  CATKIN_DEPENDS message_runtime roscpp rospy std_msgs
+  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+  src/platforms
+  src/platforms/ros/px4_messages
+  src/include
+  src/modules
+  src/
+  src/lib
+  ${EIGEN_INCLUDE_DIRS}
+)
+
+## generate multiplatform wrapper headers
+## note that the message header files are generated as in any ROS project with generate_messages()
+set(MULTIPLATFORM_HEADER_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src/platforms/ros/px4_messages)
+set(MULTIPLATFORM_TEMPLATE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/msg/templates/px4/ros)
+set(TOPICHEADER_TEMP_DIR ${CMAKE_BINARY_DIR}/topics_temporary)
+set(MULTIPLATFORM_PREFIX px4_)
+add_custom_target(multiplatform_message_headers ALL ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/Tools/px_generate_uorb_topic_headers.py
+	-d ${CMAKE_CURRENT_SOURCE_DIR}/msg -o ${MULTIPLATFORM_HEADER_DIR} -e ${MULTIPLATFORM_TEMPLATE_DIR}
+	-t ${TOPICHEADER_TEMP_DIR} -p ${MULTIPLATFORM_PREFIX})
+
+## Declare a cpp library
+add_library(px4
+  src/platforms/ros/px4_ros_impl.cpp
+  src/platforms/ros/perf_counter.cpp
+  src/platforms/ros/geo.cpp
+  src/lib/mathlib/math/Limits.cpp
+  src/modules/systemlib/circuit_breaker.cpp
+)
+add_dependencies(px4 ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers)
+
+target_link_libraries(px4
+  ${catkin_LIBRARIES}
+)
+
+## Declare a test publisher
+add_executable(publisher
+	src/examples/publisher/publisher_main.cpp
+	src/examples/publisher/publisher_example.cpp)
+add_dependencies(publisher ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers)
+target_link_libraries(publisher
+  ${catkin_LIBRARIES}
+  px4
+)
+
+## Declare a test subscriber
+add_executable(subscriber
+	src/examples/subscriber/subscriber_main.cpp
+	src/examples/subscriber/subscriber_example.cpp)
+add_dependencies(subscriber ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers)
+target_link_libraries(subscriber
+  ${catkin_LIBRARIES}
+  px4
+)
+
+## MC Attitude Control
+add_executable(mc_att_control
+	src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
+	src/modules/mc_att_control_multiplatform/mc_att_control.cpp
+	src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp)
+add_dependencies(mc_att_control ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(mc_att_control
+  ${catkin_LIBRARIES}
+  px4
+)
+
+## MC Position Control
+add_executable(mc_pos_control
+	src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp
+	src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp)
+add_dependencies(mc_pos_control ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(mc_pos_control
+  ${catkin_LIBRARIES}
+  px4
+)
+
+## Attitude Estimator dummy
+add_executable(attitude_estimator
+	src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp)
+add_dependencies(attitude_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(attitude_estimator
+  ${catkin_LIBRARIES}
+  px4
+)
+
+## Position Estimator dummy
+add_executable(position_estimator
+	src/platforms/ros/nodes/position_estimator/position_estimator.cpp)
+add_dependencies(position_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(position_estimator
+  ${catkin_LIBRARIES}
+  px4
+)
+
+## Manual input
+add_executable(manual_input
+	src/platforms/ros/nodes/manual_input/manual_input.cpp)
+add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(manual_input
+  ${catkin_LIBRARIES}
+  px4
+)
+
+## Multicopter Mixer dummy
+add_executable(mc_mixer
+  src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp)
+add_dependencies(mc_mixer ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(mc_mixer
+  ${catkin_LIBRARIES}
+  px4
+)
+
+## Commander
+add_executable(commander
+	src/platforms/ros/nodes/commander/commander.cpp)
+add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(commander
+  ${catkin_LIBRARIES}
+  px4
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_px4test.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/Makefile b/Makefile
index b17859705..af93504ee 100644
--- a/Makefile
+++ b/Makefile
@@ -104,13 +104,13 @@ DESIRED_FIRMWARES 	 = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4)
 STAGED_FIRMWARES	 = $(foreach config,$(KNOWN_CONFIGS),$(IMAGE_DIR)$(config).px4)
 FIRMWARES		 = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4)
 
-all:			checksubmodules $(DESIRED_FIRMWARES)
+all:	$(DESIRED_FIRMWARES)
 
 #
 # Copy FIRMWARES into the image directory.
 #
-# XXX copying the .bin files is a hack to work around the PX4IO uploader 
-#     not supporting .px4 files, and it should be deprecated onced that 
+# XXX copying the .bin files is a hack to work around the PX4IO uploader
+#     not supporting .px4 files, and it should be deprecated onced that
 #     is taken care of.
 #
 $(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4
@@ -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:
+$(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4:	checksubmodules generateuorbtopicheaders
 	@$(ECHO) %%%%
 	@$(ECHO) %%%% Building $(config) in $(work_dir)
 	@$(ECHO) %%%%
@@ -152,7 +152,7 @@ $(foreach config,$(FMU_CONFIGS),$(eval $(call FMU_DEP,$(config))))
 # Build the NuttX export archives.
 #
 # Note that there are no explicit dependencies extended from these
-# archives. If NuttX is updated, the user is expected to rebuild the 
+# archives. If NuttX is updated, the user is expected to rebuild the
 # archives/build area manually. Likewise, when the 'archives' target is
 # invoked, all archives are always rebuilt.
 #
@@ -226,6 +226,29 @@ updatesubmodules:
 	$(Q) (git submodule init)
 	$(Q) (git submodule update)
 
+MSG_DIR = $(PX4_BASE)msg
+UORB_TEMPLATE_DIR = $(PX4_BASE)msg/templates/uorb
+MULTIPLATFORM_TEMPLATE_DIR = $(PX4_BASE)msg/templates/px4/uorb
+TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics
+MULTIPLATFORM_HEADER_DIR = $(PX4_BASE)src/platforms/nuttx/px4_messages
+MULTIPLATFORM_PREFIX = px4_
+TOPICHEADER_TEMP_DIR = $(BUILD_DIR)topics_temporary
+GENMSG_PYTHONPATH = $(PX4_BASE)Tools/genmsg/src
+GENCPP_PYTHONPATH = $(PX4_BASE)Tools/gencpp/src
+
+.PHONY: generateuorbtopicheaders
+generateuorbtopicheaders:
+	@$(ECHO) "Generating uORB topic headers"
+	$(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH) $(PYTHON) \
+		$(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \
+		-d $(MSG_DIR) -o $(TOPICS_DIR) -e $(UORB_TEMPLATE_DIR) -t $(TOPICHEADER_TEMP_DIR))
+	@$(ECHO) "Generating multiplatform uORB topic wrapper headers"
+	$(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH) $(PYTHON) \
+		$(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \
+		-d $(MSG_DIR) -o $(MULTIPLATFORM_HEADER_DIR) -e $(MULTIPLATFORM_TEMPLATE_DIR) -t $(TOPICHEADER_TEMP_DIR) -p $(MULTIPLATFORM_PREFIX))
+# clean up temporary files
+	$(Q) (rm -r $(TOPICHEADER_TEMP_DIR))
+
 #
 # Testing targets
 #
@@ -237,12 +260,12 @@ testbuild:
 # Unittest targets. Builds and runs the host-level
 # unit tests.
 .PHONY: tests
-tests:
+tests:	generateuorbtopicheaders
 	$(Q) (mkdir -p $(PX4_BASE)/unittests/build && cd $(PX4_BASE)/unittests/build && cmake .. && $(MAKE) unittests)
 
 #
 # Cleanup targets.  'clean' should remove all built products and force
-# a complete re-compilation, 'distclean' should remove everything 
+# a complete re-compilation, 'distclean' should remove everything
 # that's generated leaving only files that are in source control.
 #
 .PHONY:	clean
diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps
index 033b3b640..2ecc104df 100644
--- a/ROMFS/px4fmu_common/init.d/rc.mc_apps
+++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps
@@ -8,8 +8,19 @@ attitude_estimator_ekf start
 #ekf_att_pos_estimator start
 position_estimator_inav start
 
-mc_att_control start
-mc_pos_control start
+if mc_att_control start
+then
+else
+	# try the multiplatform version
+	mc_att_control_m start
+fi
+
+if mc_pos_control start
+then
+else
+	# try the multiplatform version
+	mc_pos_control_m start
+fi
 
 #
 # Start Land Detector
diff --git a/Tools/gencpp b/Tools/gencpp
new file mode 160000
+Subproject 26a86f04bcec0018af6652b3ddd3f680e6e3fa2
diff --git a/Tools/genmsg b/Tools/genmsg
new file mode 160000
+Subproject 72f0383f0e6a489214c51802ae12d6e271b1e45
diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py
new file mode 100755
index 000000000..4bcab4d54
--- /dev/null
+++ b/Tools/px_generate_uorb_topic_headers.py
@@ -0,0 +1,159 @@
+#!/usr/bin/env python
+#############################################################################
+#
+#   Copyright (C) 2013-2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+#############################################################################
+
+"""
+px_generate_uorb_topic_headers.py
+Generates c/cpp header files for uorb topics from .msg (ROS syntax)
+message files
+"""
+from __future__ import print_function
+import os
+import shutil
+import filecmp
+import argparse
+import genmsg.template_tools
+
+__author__ = "Thomas Gubler"
+__copyright__ = "Copyright (C) 2013-2014 PX4 Development Team."
+__license__ = "BSD"
+__email__ = "thomasgubler@gmail.com"
+
+
+msg_template_map = {'msg.h.template': '@NAME@.h'}
+srv_template_map = {}
+incl_default = ['std_msgs:./msg/std_msgs']
+package = 'px4'
+
+
+def convert_file(filename, outputdir, templatedir, includepath):
+        """
+        Converts a single .msg file to a uorb header
+        """
+        print("Generating headers from {0}".format(filename))
+        genmsg.template_tools.generate_from_file(filename,
+                                                 package,
+                                                 outputdir,
+                                                 templatedir,
+                                                 includepath,
+                                                 msg_template_map,
+                                                 srv_template_map)
+
+
+def convert_dir(inputdir, outputdir, templatedir):
+        """
+        Converts all .msg files in inputdir to uORB header files
+        """
+        includepath = incl_default + [':'.join([package, inputdir])]
+        for f in os.listdir(inputdir):
+                fn = os.path.join(inputdir, f)
+                if os.path.isfile(fn):
+                        convert_file(
+                            fn,
+                            outputdir,
+                            templatedir,
+                            includepath)
+
+
+def copy_changed(inputdir, outputdir, prefix=''):
+        """
+        Copies files from inputdir to outputdir if they don't exist in
+        ouputdir or if their content changed
+        """
+
+        # Make sure output directory exists:
+        if not os.path.isdir(outputdir):
+                os.makedirs(outputdir)
+
+        for f in os.listdir(inputdir):
+                fni = os.path.join(inputdir, f)
+                if os.path.isfile(fni):
+                        # Check if f exists in outpoutdir, copy the file if not
+                        fno = os.path.join(outputdir, prefix + f)
+                        if not os.path.isfile(fno):
+                                shutil.copy(fni, fno)
+                                print("{0}: new header file".format(f))
+                                continue
+                        # The file exists in inputdir and outputdir
+                        # only copy if contents do not match
+                        if not filecmp.cmp(fni, fno):
+                                shutil.copy(fni, fno)
+                                print("{0}: updated".format(f))
+                                continue
+
+                        print("{0}: unchanged".format(f))
+
+
+def convert_dir_save(inputdir, outputdir, templatedir, temporarydir, prefix):
+        """
+        Converts all .msg files in inputdir to uORB header files
+        Unchanged existing files are not overwritten.
+        """
+        # Create new headers in temporary output directory
+        convert_dir(inputdir, temporarydir, templatedir)
+
+        # Copy changed headers from temporary dir to output dir
+        copy_changed(temporarydir, outputdir, prefix)
+
+if __name__ == "__main__":
+        parser = argparse.ArgumentParser(
+            description='Convert msg files to uorb headers')
+        parser.add_argument('-d', dest='dir', help='directory with msg files')
+        parser.add_argument('-f', dest='file',
+                            help="files to convert (use only without -d)",
+                            nargs="+")
+        parser.add_argument('-e', dest='templatedir',
+                            help='directory with template files',)
+        parser.add_argument('-o', dest='outputdir',
+                            help='output directory for header files')
+        parser.add_argument('-t', dest='temporarydir',
+                            help='temporary directory')
+        parser.add_argument('-p', dest='prefix', default='',
+                            help='string added as prefix to the output file '
+                            ' name when converting directories')
+        args = parser.parse_args()
+
+        if args.file is not None:
+                for f in args.file:
+                        convert_file(
+                            f,
+                            args.outputdir,
+                            args.templatedir,
+                            incl_default)
+        elif args.dir is not None:
+                convert_dir_save(
+                    args.dir,
+                    args.outputdir,
+                    args.templatedir,
+                    args.temporarydir,
+                    args.prefix)
diff --git a/Tools/ros/docker/px4-ros-full/Dockerfile b/Tools/ros/docker/px4-ros-full/Dockerfile
new file mode 100644
index 000000000..1242b56b5
--- /dev/null
+++ b/Tools/ros/docker/px4-ros-full/Dockerfile
@@ -0,0 +1,57 @@
+#
+# PX4 full ROS container
+#
+# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
+
+FROM ubuntu:14.04.1
+MAINTAINER Andreas Antener <andreas@uaventure.com>
+
+# Install basics
+## Use the "noninteractive" debconf frontend
+ENV DEBIAN_FRONTEND noninteractive
+
+RUN apt-get update \
+	&& apt-get -y install wget git mercurial
+
+# Main ROS Setup
+# Following http://wiki.ros.org/indigo/Installation/Ubuntu
+# Also adding dependencies for gazebo http://gazebosim.org/tutorials?tut=drcsim_install
+
+## add ROS repositories and keys
+## install main ROS pacakges
+RUN echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list \
+	&& wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | apt-key add - \
+	&& apt-get update \
+	&& apt-get -y install ros-indigo-desktop-full
+
+RUN rosdep init \
+	&& rosdep update
+
+## setup environment variables
+RUN echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc
+
+## get rosinstall
+RUN apt-get -y install python-rosinstall
+
+## additional dependencies
+RUN apt-get -y install ros-indigo-octomap-msgs ros-indigo-joy
+
+## install drcsim
+RUN echo "deb http://packages.osrfoundation.org/drc/ubuntu trusty main" > /etc/apt/sources.list.d/drc-latest.list \
+	&& wget http://packages.osrfoundation.org/drc.key -O - | apt-key add - \
+	&& apt-get update \
+	&& apt-get -y install drcsim
+
+# Install x11-utils to get xdpyinfo, for X11 display debugging
+# mesa-utils provides glxinfo, handy for understanding the 3D support
+RUN apt-get -y install x11-utils mesa-utils
+
+# Some QT-Apps/Gazebo don't not show controls without this
+ENV QT_X11_NO_MITSHM 1
+
+# FIXME: this doesn't work when building from vagrant
+COPY scripts/setup-workspace.sh /root/scripts/
+RUN chmod +x -R /root/scripts/*
+RUN chown -R root:root /root/scripts/*
+
+CMD ["/usr/bin/xterm"]
diff --git a/Tools/ros/docker/px4-ros-full/README.md b/Tools/ros/docker/px4-ros-full/README.md
new file mode 100644
index 000000000..af5170c70
--- /dev/null
+++ b/Tools/ros/docker/px4-ros-full/README.md
@@ -0,0 +1,12 @@
+# PX4 ROS #
+
+Full desktop ROS container.
+
+License: according to LICENSE.md in the root directory of the PX4 Firmware repository
+
+**TODO:**
+
+- use https://github.com/phusion/baseimage-docker as base
+- add user, best synced with host
+- configure ssh to work with vagrant out of the box
+
diff --git a/Tools/ros/docker/px4-ros-full/scripts/setup-workspace.sh b/Tools/ros/docker/px4-ros-full/scripts/setup-workspace.sh
new file mode 100644
index 000000000..2de5f8bec
--- /dev/null
+++ b/Tools/ros/docker/px4-ros-full/scripts/setup-workspace.sh
@@ -0,0 +1,45 @@
+#!/bin/sh
+#
+# Create workspace at current location and fetch source repositories
+#
+
+# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
+
+WDIR=`pwd`
+WORKSPACE=$WDIR/catkin_ws
+
+# Setup workspace
+mkdir -p $WORKSPACE/src
+cd $WORKSPACE/src
+catkin_init_workspace
+cd $WORKSPACE
+catkin_make
+echo "source $WORKSPACE/devel/setup.bash" >> ~/.bashrc
+
+# PX4 Firmware
+cd $WORKSPACE/src
+git clone https://github.com/PX4/Firmware.git \
+	&& cd Firmware \
+	&& git checkout ros
+
+# euroc simulator
+cd $WORKSPACE/src
+git clone https://github.com/PX4/euroc_simulator.git \
+	&& cd euroc_simulator \
+	&& git checkout px4_nodes
+
+# mav comm
+cd $WORKSPACE/src
+git clone https://github.com/PX4/mav_comm.git
+
+# glog catkin
+cd $WORKSPACE/src
+git clone https://github.com/ethz-asl/glog_catkin.git
+
+# catkin simple
+cd $WORKSPACE/src
+git clone https://github.com/catkin/catkin_simple.git
+
+cd $WORKSPACE
+catkin_make
+
diff --git a/Tools/ros/px4_ros_installation_ubuntu.sh b/Tools/ros/px4_ros_installation_ubuntu.sh
new file mode 100755
index 000000000..7efc400cd
--- /dev/null
+++ b/Tools/ros/px4_ros_installation_ubuntu.sh
@@ -0,0 +1,41 @@
+#!/bin/sh
+# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
+
+# main ROS Setup
+# following http://wiki.ros.org/indigo/Installation/Ubuntu
+# also adding drcsim http://gazebosim.org/tutorials?tut=drcsim_install
+# run this file with . ./px4_ros_setup_ubuntu.sh
+
+## add ROS repository
+sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
+
+## add key
+wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | \
+    sudo apt-key add -
+
+## Install main ROS pacakges
+sudo apt-get update
+sudo apt-get -y install ros-indigo-desktop-full
+sudo rosdep init
+rosdep update
+
+## Setup environment variables
+echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc
+. ~/.bashrc
+
+# get rosinstall
+sudo apt-get -y install python-rosinstall
+
+# additional dependencies
+sudo apt-get -y install ros-indigo-octomap-msgs ros-indigo-joy
+
+## drcsim setup (for models)
+### add osrf repository
+sudo sh -c 'echo "deb http://packages.osrfoundation.org/drc/ubuntu trusty main" > /etc/apt/sources.list.d/drc-latest.list'
+
+### add key
+wget http://packages.osrfoundation.org/drc.key -O - | sudo apt-key add -
+
+### install drcsim
+sudo apt-get update
+sudo apt-get -y install drcsim
diff --git a/Tools/ros/px4_workspace_create.sh b/Tools/ros/px4_workspace_create.sh
new file mode 100755
index 000000000..cf80bcf8d
--- /dev/null
+++ b/Tools/ros/px4_workspace_create.sh
@@ -0,0 +1,9 @@
+#!/bin/sh
+# this script creates a catkin_ws in the current folder
+# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
+
+mkdir -p catkin_ws/src
+cd catkin_ws/src
+catkin_init_workspace
+cd ..
+catkin_make
diff --git a/Tools/ros/px4_workspace_setup.sh b/Tools/ros/px4_workspace_setup.sh
new file mode 100755
index 000000000..53568b4fb
--- /dev/null
+++ b/Tools/ros/px4_workspace_setup.sh
@@ -0,0 +1,33 @@
+#!/bin/bash
+# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
+
+# run this script from the root of your catkin_ws
+source devel/setup.bash
+cd src
+
+# PX4 Firmware
+git clone https://github.com/PX4/Firmware.git
+cd Firmware
+git checkout ros
+cd ..
+
+# euroc simulator
+git clone https://github.com/PX4/euroc_simulator.git
+cd euroc_simulator
+git checkout px4_nodes
+cd ..
+
+# mav comm
+git clone https://github.com/PX4/mav_comm.git
+
+# glog catkin
+git clone https://github.com/ethz-asl/glog_catkin.git
+
+# catkin simple
+git clone https://github.com/catkin/catkin_simple.git
+
+# drcsim (for scenery and models)
+
+cd ..
+
+catkin_make
diff --git a/Tools/ros/vagrant/docker-host-base/Vagrantfile b/Tools/ros/vagrant/docker-host-base/Vagrantfile
new file mode 100644
index 000000000..06e4e897d
--- /dev/null
+++ b/Tools/ros/vagrant/docker-host-base/Vagrantfile
@@ -0,0 +1,58 @@
+# -*- mode: ruby -*-
+# vi: set ft=ruby :
+
+#
+# Vagrantfile to create docker-host-base
+#
+# Maintainer: Andreas Antener <andreas@uaventure.com>
+#
+# After build, do "vagrant package --base docker-host-base" to package,
+# and import as box: "vagrant box add --name uaventure/docker-host-base package.box"
+#
+# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
+
+Vagrant.configure(2) do |config|
+  config.vm.box = "ubuntu/trusty64"
+
+  config.vm.define "docker-host-base"
+
+  config.vm.provider "virtualbox" do |vb|
+    vb.name = "docker-host-base"
+    vb.gui = true
+    vb.memory = "1024"
+  end
+
+  config.vm.provision "file", source: "config/docker-default", destination: "/home/vagrant/docker-default"
+  config.vm.provision "file", source: "config/xsessionrc", destination: "/home/vagrant/.xsessionrc"
+
+  config.vm.provision "shell", inline: <<-SHELL
+    # Update and install apps
+    export DEBIAN_FRONTEND=noninteractive
+    sudo apt-get update
+    sudo apt-get upgrade -y
+    sudo apt-get install -y --no-install-recommends ubuntu-desktop
+    sudo apt-get install -y gnome-terminal unity-lens-applications
+
+    # Reset the ssh key (because vagrant regenerates it during provisioning)
+    sudo wget --no-check-certificate https://raw.github.com/mitchellh/vagrant/master/keys/vagrant.pub -O /home/vagrant/.ssh/authorized_keys
+    sudo chmod 0700 /home/vagrant/.ssh
+    sudo chmod 0600 /home/vagrant/.ssh/authorized_keys
+    sudo chown -R vagrant /home/vagrant/.ssh
+
+    # Copy docker config
+    sudo mv /home/vagrant/docker-default /etc/default/docker
+
+    # Enable autologin so docker can start GUI apps
+    sudo echo "autologin-user=vagrant" >> /usr/share/lightdm/lightdm.conf.d/50-unity-greeter.conf
+    sudo echo "autologin-user-timeout=0" >> /usr/share/lightdm/lightdm.conf.d/50-unity-greeter.conf
+
+    # X session RC
+    chmod +x /home/vagrant/.xsessionrc
+  SHELL
+
+  config.vm.provision "docker"
+
+  # Shutdown after provisioning. "vagrant halt" doesn't recognize the original ssh key anymore
+  # and would just kill the VM. This might lead to FS inconsistencies (e.g. in the docker DB).
+  config.vm.provision "shell", inline: "sudo shutdown -h now"
+end
diff --git a/Tools/ros/vagrant/docker-host-base/config/docker-default b/Tools/ros/vagrant/docker-host-base/config/docker-default
new file mode 100644
index 000000000..19c466b6b
--- /dev/null
+++ b/Tools/ros/vagrant/docker-host-base/config/docker-default
@@ -0,0 +1,29 @@
+#
+# Default config for docker /etc/default/docker
+# Copied from a provisioned vagrant box
+#
+# Modifications:
+# - listen to TCP port
+# - removing deprecated "-r=true" option which apparently doesn't work anymore
+#   > use restart policies for specific containers if necessary
+#
+
+# Docker Upstart and SysVinit configuration file
+
+# Customize location of Docker binary (especially for development testing).
+#DOCKER="/usr/local/bin/docker"
+
+# Use DOCKER_OPTS to modify the daemon startup options.
+#DOCKER_OPTS="--dns 8.8.8.8 --dns 8.8.4.4"
+
+# If you need Docker to use an HTTP proxy, it can also be specified here.
+#export http_proxy="http://127.0.0.1:3128/"
+
+# This is also a handy place to tweak where Docker's temporary files go.
+#export TMPDIR="/mnt/bigdrive/docker-tmp"
+
+# Expose TCP port in addition to socket
+
+# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
+
+DOCKER_OPTS="${DOCKER_OPTS} -H unix:///var/run/docker.sock -H 0.0.0.0:2375"
diff --git a/Tools/ros/vagrant/docker-host-base/config/xsessionrc b/Tools/ros/vagrant/docker-host-base/config/xsessionrc
new file mode 100644
index 000000000..1bd4f8d3f
--- /dev/null
+++ b/Tools/ros/vagrant/docker-host-base/config/xsessionrc
@@ -0,0 +1,6 @@
+#!/bin/sh
+#
+# Disable X access control so we can easily start GUI apps
+#
+# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
+xhost +
diff --git a/Tools/ros/vagrant/docker-host/Vagrantfile b/Tools/ros/vagrant/docker-host/Vagrantfile
new file mode 100644
index 000000000..2a0a02271
--- /dev/null
+++ b/Tools/ros/vagrant/docker-host/Vagrantfile
@@ -0,0 +1,38 @@
+# -*- mode: ruby -*-
+# vi: set ft=ruby :
+
+#
+# Actual docker host VM to run.
+#
+# Maintainer: Andreas Antener <andreas@uaventure.com>
+#
+# To add local docker images into the docker host, configure your local
+# docker client to control the docker daemon on the running "docker-host" VM.
+# The box ("docker-host-base") configures docker to listen on any IP on port 2375.
+# You can then load an existing image, e.g.:
+# "docker load -i container-image.tar"
+#
+# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
+
+Vagrant.configure(2) do |config|
+  config.vm.box = "uaventure/docker-host-base"
+
+  config.vm.define "docker-host"
+
+  config.vm.provider "virtualbox" do |vb|
+    vb.name = "docker-host"
+    vb.gui = true
+    vb.memory = "4096"
+    vb.cpus = 2
+    vb.customize ["modifyvm", :id, "--graphicscontroller", "vboxvga"]
+    vb.customize ["modifyvm", :id, "--accelerate3d", "on"]
+    vb.customize ["modifyvm", :id, "--ioapic", "on"]
+    vb.customize ["modifyvm", :id, "--vram", "128"]
+    vb.customize ["modifyvm", :id, "--hwvirtex", "on"]
+  end
+
+  config.vm.network "private_network", ip: "192.168.59.104"
+
+  # TBD: would it be better to provision docker here instead of in the base box?
+  #config.vm.provision "docker"
+end
diff --git a/Tools/ros/vagrant/px4-ros/Vagrantfile b/Tools/ros/vagrant/px4-ros/Vagrantfile
new file mode 100644
index 000000000..5b372a94d
--- /dev/null
+++ b/Tools/ros/vagrant/px4-ros/Vagrantfile
@@ -0,0 +1,61 @@
+# -*- mode: ruby -*-
+# vi: set ft=ruby :
+
+#
+# Boot docker SITL environment
+#
+# Maintainer: Andreas Antener <andreas@uaventure.com>
+#
+# "vagrant up" will build the images. Should eventually start "xterm" from within the docker container.
+#
+# Notes:
+# (will change, need proper docs)
+#
+# Build with multiple dependent docker containers:
+# Use the "--no-parallel" option so the containers will be built/started in order.
+# e.g.: "vagrant up --no-parallel"
+#
+# Running apps directly:
+# "vagrant docker-run ros -- <cmd>"
+# Attention: will loose all data when stopped, vagrant runs docker always with "--rm"
+#
+# TODO
+# - do not run the docker container with "--rm" (vagrant default). is that even possible?
+# - maybe map a local working directory to compile stuff without loosing it in side the docker container
+#
+# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
+
+Vagrant.configure(2) do |config|
+  # Configure docker host
+  config.vm.provider "docker" do |d|
+    d.vagrant_machine = "docker-host"
+    d.vagrant_vagrantfile = "../docker-host/Vagrantfile"
+  end
+
+  # Configure docker apps to run
+  config.vm.define "ros" do |app|
+    app.vm.provider "docker" do |d|
+      d.name = "ros"
+      d.image = "uaventure/px4-ros-full"
+      #d.build_dir = "../../docker/px4-ros-full"
+      #d.build_args = ["-t=uaventure/px4-ros-full"]
+
+      # Share docker host x11 socket
+      # Run privileged to support 3d acceleration
+      d.volumes = [
+        "/tmp/.X11-unix:/tmp/.X11-unix:ro"
+      ]
+      d.create_args = ["--privileged"]
+
+      # TODO: get display number from host system
+      d.env = {
+        "DISPLAY" => ":0"
+      }
+
+      d.remains_running = true
+      d.cmd = ["xterm"]
+      #d.has_ssh = true
+    end
+  end
+
+end
diff --git a/launch/ardrone.launch b/launch/ardrone.launch
new file mode 100644
index 000000000..d53333b11
--- /dev/null
+++ b/launch/ardrone.launch
@@ -0,0 +1,14 @@
+<launch>
+
+<include file="$(find px4)/launch/multicopter_x.launch" />
+
+<group ns="px4_multicopter">
+	<param name="MPC_XY_P" type="double" value="1.0" />
+	<param name="MPC_XY_FF" type="double" value="0.0" />
+	<param name="MPC_XY_VEL_P" type="double" value="0.01" />
+	<param name="MPC_XY_VEL_I" type="double" value="0.0" />
+	<param name="MPC_XY_VEL_D" type="double" value="0.01" />
+	<param name="MPC_XY_VEL_MAX" type="double" value="2.0" />
+</group>
+
+</launch>
diff --git a/launch/example.launch b/launch/example.launch
new file mode 100644
index 000000000..39da4b3d4
--- /dev/null
+++ b/launch/example.launch
@@ -0,0 +1,8 @@
+<launch>
+
+<group ns="px4_example">
+	<node pkg="px4" name="subscriber" type="subscriber"/>
+	<node pkg="px4" name="publisher" type="publisher"/>
+</group>
+
+</launch>
diff --git a/launch/gazebo_ardrone_empty_world.launch b/launch/gazebo_ardrone_empty_world.launch
new file mode 100644
index 000000000..395e70b00
--- /dev/null
+++ b/launch/gazebo_ardrone_empty_world.launch
@@ -0,0 +1,6 @@
+<launch>
+
+<include file="$(find mav_gazebo)/launch/ardrone_empty_world_with_joy.launch" />
+<include file="$(find px4)/launch/ardrone.launch" />
+
+</launch>
diff --git a/launch/gazebo_ardrone_house_world.launch b/launch/gazebo_ardrone_house_world.launch
new file mode 100644
index 000000000..f43c16b50
--- /dev/null
+++ b/launch/gazebo_ardrone_house_world.launch
@@ -0,0 +1,6 @@
+<launch>
+
+<include file="$(find mav_gazebo)/launch/ardrone_house_world_with_joy.launch" />
+<include file="$(find px4)/launch/ardrone.launch" />
+
+</launch>
diff --git a/launch/gazebo_iris_empty_world.launch b/launch/gazebo_iris_empty_world.launch
new file mode 100644
index 000000000..833aaf07a
--- /dev/null
+++ b/launch/gazebo_iris_empty_world.launch
@@ -0,0 +1,6 @@
+<launch>
+
+<include file="$(find mav_gazebo)/launch/iris_empty_world_with_joy.launch" />
+<include file="$(find px4)/launch/iris.launch" />
+
+</launch>
diff --git a/launch/gazebo_iris_house_world.launch b/launch/gazebo_iris_house_world.launch
new file mode 100644
index 000000000..bfdf9fe31
--- /dev/null
+++ b/launch/gazebo_iris_house_world.launch
@@ -0,0 +1,6 @@
+<launch>
+
+<include file="$(find mav_gazebo)/launch/iris_house_world_with_joy.launch" />
+<include file="$(find px4)/launch/iris.launch" />
+
+</launch>
diff --git a/launch/gazebo_iris_outdoor_world.launch b/launch/gazebo_iris_outdoor_world.launch
new file mode 100644
index 000000000..c2975a050
--- /dev/null
+++ b/launch/gazebo_iris_outdoor_world.launch
@@ -0,0 +1,6 @@
+<launch>
+
+<include file="$(find mav_gazebo)/launch/iris_outdoor_world_with_joy.launch" />
+<include file="$(find px4)/launch/iris.launch" />
+
+</launch>
diff --git a/launch/iris.launch b/launch/iris.launch
new file mode 100644
index 000000000..7b5b13250
--- /dev/null
+++ b/launch/iris.launch
@@ -0,0 +1,15 @@
+<launch>
+
+<include file="$(find px4)/launch/multicopter_w.launch" />
+
+<group ns="px4_multicopter">
+	<param name="mixer" type="string" value="i" />
+	<param name="MPC_XY_P" type="double" value="1.0" />
+	<param name="MPC_XY_FF" type="double" value="0.0" />
+	<param name="MPC_XY_VEL_P" type="double" value="0.01" />
+	<param name="MPC_XY_VEL_I" type="double" value="0.0" />
+	<param name="MPC_XY_VEL_D" type="double" value="0.01" />
+	<param name="MPC_XY_VEL_MAX" type="double" value="2.0" />
+</group>
+
+</launch>
diff --git a/launch/multicopter.launch b/launch/multicopter.launch
new file mode 100644
index 000000000..95400bd82
--- /dev/null
+++ b/launch/multicopter.launch
@@ -0,0 +1,15 @@
+<launch>
+
+<group ns="px4_multicopter">
+	<node pkg="joy" name="joy_node" type="joy_node"/>
+	<node pkg="px4" name="manual_input" type="manual_input"/>
+	<node pkg="px4" name="commander" type="commander"/>
+	<node pkg="px4" name="mc_mixer" type="mc_mixer"/>
+	<node pkg="px4" name="attitude_estimator" type="attitude_estimator"/>
+	<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="rosbag" type="record" name="record" output="screen" args="-a -O px4_multicopter"/> -->
+</group>
+
+</launch>
diff --git a/launch/multicopter_w.launch b/launch/multicopter_w.launch
new file mode 100644
index 000000000..f124082aa
--- /dev/null
+++ b/launch/multicopter_w.launch
@@ -0,0 +1,9 @@
+<launch>
+
+<include file="$(find px4)/launch/multicopter.launch" />
+
+<group ns="px4_multicopter">
+	<param name="mixer" type="string" value="w" />
+</group>
+
+</launch>
diff --git a/launch/multicopter_x.launch b/launch/multicopter_x.launch
new file mode 100644
index 000000000..6355b87be
--- /dev/null
+++ b/launch/multicopter_x.launch
@@ -0,0 +1,9 @@
+<launch>
+
+<include file="$(find px4)/launch/multicopter.launch" />
+
+<group ns="px4_multicopter">
+	<param name="mixer" type="string" value="x" />
+</group>
+
+</launch>
diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index 8d2733051..f30733694 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -111,6 +111,7 @@ MODULES		+= lib/geo
 MODULES		+= lib/geo_lookup
 MODULES		+= lib/conversion
 MODULES		+= lib/launchdetection
+MODULES		+= platforms/nuttx
 
 #
 # Demo apps
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index 3e2d94236..256c2a636 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -116,6 +116,7 @@ MODULES		+= lib/geo
 MODULES		+= lib/geo_lookup
 MODULES		+= lib/conversion
 MODULES		+= lib/launchdetection
+MODULES		+= platforms/nuttx
 
 #
 # OBC challenge
diff --git a/makefiles/config_px4fmu-v2_multiplatform.mk b/makefiles/config_px4fmu-v2_multiplatform.mk
new file mode 100644
index 000000000..29eb68096
--- /dev/null
+++ b/makefiles/config_px4fmu-v2_multiplatform.mk
@@ -0,0 +1,170 @@
+#
+# Makefile for the px4fmu_default configuration
+#
+
+#
+# Use the configuration's ROMFS, copy the px4iov2 firmware into
+# the ROMFS if it's available
+#
+ROMFS_ROOT	 = $(PX4_BASE)/ROMFS/px4fmu_common
+ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_default.bin
+
+#
+# Board support modules
+#
+MODULES		+= drivers/device
+MODULES		+= drivers/stm32
+MODULES		+= drivers/stm32/adc
+MODULES		+= drivers/stm32/tone_alarm
+MODULES		+= drivers/led
+MODULES		+= drivers/px4fmu
+MODULES		+= drivers/px4io
+MODULES		+= drivers/boards/px4fmu-v2
+MODULES		+= drivers/rgbled
+MODULES		+= drivers/mpu6000
+MODULES		+= drivers/lsm303d
+MODULES		+= drivers/l3gd20
+MODULES		+= drivers/hmc5883
+MODULES		+= drivers/ms5611
+MODULES		+= drivers/mb12xx
+# MODULES		+= drivers/sf0x
+MODULES		+= drivers/ll40ls
+# MODULES		+= drivers/trone
+MODULES		+= drivers/gps
+MODULES		+= drivers/hil
+MODULES		+= drivers/hott
+MODULES		+= drivers/hott/hott_telemetry
+MODULES		+= drivers/hott/hott_sensors
+# MODULES		+= drivers/blinkm
+MODULES		+= drivers/airspeed
+MODULES		+= drivers/ets_airspeed
+MODULES		+= drivers/meas_airspeed
+MODULES		+= drivers/frsky_telemetry
+MODULES		+= modules/sensors
+MODULES		+= drivers/mkblctrl
+MODULES		+= drivers/px4flow
+
+#
+# System commands
+#
+MODULES		+= systemcmds/bl_update
+MODULES		+= systemcmds/boardinfo
+MODULES		+= systemcmds/mixer
+MODULES		+= systemcmds/param
+MODULES		+= systemcmds/perf
+MODULES		+= systemcmds/preflight_check
+MODULES		+= systemcmds/pwm
+MODULES		+= systemcmds/esc_calib
+MODULES		+= systemcmds/reboot
+MODULES		+= systemcmds/top
+MODULES		+= systemcmds/config
+MODULES		+= systemcmds/nshterm
+MODULES		+= systemcmds/mtd
+MODULES		+= systemcmds/dumpfile
+MODULES		+= systemcmds/ver
+
+#
+# General system control
+#
+MODULES		+= modules/commander
+MODULES		+= modules/navigator
+MODULES		+= modules/mavlink
+MODULES		+= modules/gpio_led
+# MODULES		+= modules/uavcan
+
+#
+# Estimation modules (EKF/ SO3 / other filters)
+#
+MODULES		+= modules/attitude_estimator_ekf
+MODULES		+= modules/ekf_att_pos_estimator
+MODULES		+= modules/position_estimator_inav
+
+#
+# Vehicle Control
+#
+#MODULES		+= modules/segway # XXX Needs GCC 4.7 fix
+#MODULES		+= modules/fw_pos_control_l1
+#MODULES		+= modules/fw_att_control
+# MODULES		+= modules/mc_att_control
+MODULES		+= modules/mc_att_control_multiplatform
+MODULES		+= examples/subscriber
+MODULES		+= examples/publisher
+# MODULES		+= modules/mc_pos_control
+MODULES		+= modules/mc_pos_control_multiplatform
+MODULES 	+= modules/vtol_att_control
+
+#
+# Logging
+#
+MODULES		+= modules/sdlog2
+
+#
+# Library modules
+#
+MODULES		+= modules/systemlib
+MODULES		+= modules/systemlib/mixer
+MODULES		+= modules/controllib
+MODULES		+= modules/uORB
+MODULES		+= modules/dataman
+
+#
+# Libraries
+#
+LIBRARIES	+= lib/mathlib/CMSIS
+MODULES		+= lib/mathlib
+MODULES		+= lib/mathlib/math/filter
+MODULES		+= lib/ecl
+MODULES		+= lib/external_lgpl
+MODULES		+= lib/geo
+MODULES		+= lib/geo_lookup
+MODULES		+= lib/conversion
+MODULES		+= lib/launchdetection
+MODULES		+= platforms/nuttx
+
+#
+# OBC challenge
+#
+MODULES		+= modules/bottle_drop
+
+#
+# Demo apps
+#
+#MODULES		+= examples/math_demo
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/hello_sky
+#MODULES		+= examples/px4_simple_app
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/daemon
+#MODULES		+= examples/px4_daemon_app
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/debug_values
+#MODULES		+= examples/px4_mavlink_debug
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control
+#MODULES			+= examples/fixedwing_control
+
+# Hardware test
+#MODULES			+= examples/hwtest
+
+# Generate parameter XML file
+GEN_PARAM_XML = 1
+
+#
+# Transitional support - add commands from the NuttX export archive.
+#
+# In general, these should move to modules over time.
+#
+# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro
+# to make the table a bit more readable.
+#
+define _B
+	$(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4)
+endef
+
+#                  command                 priority                   stack  entrypoint
+BUILTIN_COMMANDS := \
+	$(call _B, sercon,                 ,                          2048,  sercon_main                ) \
+	$(call _B, serdis,                 ,                          2048,  serdis_main                )
diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk
index 7c63839ad..22563838b 100644
--- a/makefiles/config_px4fmu-v2_test.mk
+++ b/makefiles/config_px4fmu-v2_test.mk
@@ -39,9 +39,30 @@ MODULES		+= systemcmds/top
 MODULES		+= modules/sensors
 
 #
-# Testing modules
+# System commands
+#
+MODULES		+= systemcmds/bl_update
+MODULES		+= systemcmds/boardinfo
+MODULES		+= systemcmds/mixer
+MODULES		+= systemcmds/param
+MODULES		+= systemcmds/perf
+MODULES		+= systemcmds/preflight_check
+MODULES		+= systemcmds/pwm
+MODULES		+= systemcmds/esc_calib
+MODULES		+= systemcmds/reboot
+MODULES		+= systemcmds/top
+MODULES		+= systemcmds/config
+MODULES		+= systemcmds/nshterm
+MODULES		+= systemcmds/mtd
+MODULES		+= systemcmds/dumpfile
+MODULES		+= systemcmds/ver
+
+#
+# Example modules
 #
 MODULES		+= examples/matlab_csv_serial
+MODULES		+= examples/subscriber
+MODULES		+= examples/publisher
 
 #
 # Library modules
@@ -53,6 +74,7 @@ LIBRARIES	+= lib/mathlib/CMSIS
 MODULES		+= lib/mathlib
 MODULES		+= lib/mathlib/math/filter
 MODULES		+= lib/conversion
+MODULES		+= platforms/nuttx
 
 #
 # Example modules to test-build
diff --git a/makefiles/setup.mk b/makefiles/setup.mk
index c932a6758..351b9f1b7 100644
--- a/makefiles/setup.mk
+++ b/makefiles/setup.mk
@@ -37,13 +37,14 @@
 # Some useful paths.
 #
 # Note that in general we always keep directory paths with the separator
-# at the end, and join paths without explicit separators. This reduces 
+# at the end, and join paths without explicit separators. This reduces
 # the number of duplicate slashes we have lying around in paths,
 # and is consistent with joining the results of $(dir) and $(notdir).
 #
 export PX4_INCLUDE_DIR	 = $(abspath $(PX4_BASE)/src/include)/
 export PX4_MODULE_SRC	 = $(abspath $(PX4_BASE)/src)/
 export PX4_LIB_DIR	 = $(abspath $(PX4_BASE)/src/lib)/
+export PX4_PLATFORMS_DIR = $(abspath $(PX4_BASE)/src/platforms)/
 export PX4_MK_DIR	 = $(abspath $(PX4_BASE)/makefiles)/
 export NUTTX_SRC	 = $(abspath $(PX4_BASE)/NuttX/nuttx)/
 export MAVLINK_SRC	 = $(abspath $(PX4_BASE)/mavlink/include/mavlink/v1.0)/
@@ -61,7 +62,8 @@ export ARCHIVE_DIR	 = $(abspath $(PX4_BASE)/Archives)/
 export INCLUDE_DIRS	:= $(PX4_MODULE_SRC) \
 			   $(PX4_MODULE_SRC)/modules/ \
 			   $(PX4_INCLUDE_DIR) \
-			   $(PX4_LIB_DIR)
+			   $(PX4_LIB_DIR) \
+			   $(PX4_PLATFORMS_DIR)
 
 #
 # Tools
diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk
index d4d73fb84..dc8d3f712 100644
--- a/makefiles/toolchain_gnu-arm-eabi.mk
+++ b/makefiles/toolchain_gnu-arm-eabi.mk
@@ -128,7 +128,7 @@ INSTRUMENTATIONDEFINES	 = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH))
 # Language-specific flags
 #
 ARCHCFLAGS		 = -std=gnu99
-ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics
+ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics -D__CUSTOM_FILE_IO__
 
 # Generic warnings
 #
diff --git a/msg/actuator_armed.msg b/msg/actuator_armed.msg
new file mode 100644
index 000000000..b83adb8f2
--- /dev/null
+++ b/msg/actuator_armed.msg
@@ -0,0 +1,6 @@
+
+uint64 timestamp	# Microseconds since system boot
+bool armed		# Set to true if system is armed
+bool ready_to_arm	# Set to true if system is ready to be armed
+bool lockdown		# Set to true if actuators are forced to being disabled (due to emergency or HIL)
+bool force_failsafe	# Set to true if the actuators are forced to the failsafe position
diff --git a/msg/actuator_controls.msg b/msg/actuator_controls.msg
new file mode 100644
index 000000000..414eb06dd
--- /dev/null
+++ b/msg/actuator_controls.msg
@@ -0,0 +1,5 @@
+uint8 NUM_ACTUATOR_CONTROLS = 8
+uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
+uint64 timestamp
+uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
+float32[8] control
diff --git a/msg/actuator_controls_0.msg b/msg/actuator_controls_0.msg
new file mode 100644
index 000000000..414eb06dd
--- /dev/null
+++ b/msg/actuator_controls_0.msg
@@ -0,0 +1,5 @@
+uint8 NUM_ACTUATOR_CONTROLS = 8
+uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
+uint64 timestamp
+uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
+float32[8] control
diff --git a/msg/actuator_controls_1.msg b/msg/actuator_controls_1.msg
new file mode 100644
index 000000000..414eb06dd
--- /dev/null
+++ b/msg/actuator_controls_1.msg
@@ -0,0 +1,5 @@
+uint8 NUM_ACTUATOR_CONTROLS = 8
+uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
+uint64 timestamp
+uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
+float32[8] control
diff --git a/msg/actuator_controls_2.msg b/msg/actuator_controls_2.msg
new file mode 100644
index 000000000..414eb06dd
--- /dev/null
+++ b/msg/actuator_controls_2.msg
@@ -0,0 +1,5 @@
+uint8 NUM_ACTUATOR_CONTROLS = 8
+uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
+uint64 timestamp
+uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
+float32[8] control
diff --git a/msg/actuator_controls_3.msg b/msg/actuator_controls_3.msg
new file mode 100644
index 000000000..414eb06dd
--- /dev/null
+++ b/msg/actuator_controls_3.msg
@@ -0,0 +1,5 @@
+uint8 NUM_ACTUATOR_CONTROLS = 8
+uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
+uint64 timestamp
+uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
+float32[8] control
diff --git a/msg/actuator_controls_virtual_fw.msg b/msg/actuator_controls_virtual_fw.msg
new file mode 100644
index 000000000..414eb06dd
--- /dev/null
+++ b/msg/actuator_controls_virtual_fw.msg
@@ -0,0 +1,5 @@
+uint8 NUM_ACTUATOR_CONTROLS = 8
+uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
+uint64 timestamp
+uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
+float32[8] control
diff --git a/msg/actuator_controls_virtual_mc.msg b/msg/actuator_controls_virtual_mc.msg
new file mode 100644
index 000000000..414eb06dd
--- /dev/null
+++ b/msg/actuator_controls_virtual_mc.msg
@@ -0,0 +1,5 @@
+uint8 NUM_ACTUATOR_CONTROLS = 8
+uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
+uint64 timestamp
+uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
+float32[8] control
diff --git a/msg/fw_virtual_rates_setpoint.msg b/msg/fw_virtual_rates_setpoint.msg
new file mode 100644
index 000000000..834113c79
--- /dev/null
+++ b/msg/fw_virtual_rates_setpoint.msg
@@ -0,0 +1,6 @@
+uint64 timestamp    # in microseconds since system start
+
+float32 roll	    # body angular rates in NED frame
+float32 pitch	    # body angular rates in NED frame
+float32 yaw	    # body angular rates in NED frame
+float32 thrust	    # thrust normalized to 0..1
diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg
new file mode 100644
index 000000000..d3cfb078b
--- /dev/null
+++ b/msg/manual_control_setpoint.msg
@@ -0,0 +1,44 @@
+
+uint8 SWITCH_POS_NONE = 0		# switch is not mapped
+uint8 SWITCH_POS_ON = 1			# switch activated (value = 1)
+uint8 SWITCH_POS_MIDDLE = 2		# middle position (value = 0)
+uint8 SWITCH_POS_OFF = 3		# switch not activated (value = -1)
+uint64 timestamp
+
+# Any of the channels may not be available and be set to NaN
+# to indicate that it does not contain valid data.
+# The variable names follow the definition of the
+# MANUAL_CONTROL mavlink message.
+# The default range is from -1 to 1 (mavlink message -1000 to 1000)
+# The range for the z variable is defined from 0 to 1. (The z field of
+# the MANUAL_CONTROL mavlink message is defined from -1000 to 1000)
+
+float32 x			 # stick position in x direction -1..1
+				 # in general corresponds to forward/back motion or pitch of vehicle,
+				 # in general a positive value means forward or negative pitch and
+				 # a negative value means backward or positive pitch
+float32 y			 # stick position in y direction -1..1
+				 # in general corresponds to right/left motion or roll of vehicle,
+				 # in general a positive value means right or positive roll and
+				 # a negative value means left or negative roll
+float32 z			 # throttle stick position 0..1
+				 # in general corresponds to up/down motion or thrust of vehicle,
+				 # in general the value corresponds to the demanded throttle by the user,
+				 # if the input is used for setting the setpoint of a vertical position
+				 # controller any value > 0.5 means up and any value < 0.5 means down
+float32 r			 # yaw stick/twist positon, -1..1
+				 # in general corresponds to the righthand rotation around the vertical
+				 # (downwards) axis of the vehicle
+float32 flaps			 # flap position
+float32 aux1			 # default function: camera yaw / azimuth
+float32 aux2			 # default function: camera pitch / tilt
+float32 aux3			 # default function: camera trigger
+float32 aux4			 # default function: camera roll
+float32 aux5			 # default function: payload drop
+
+uint8 mode_switch		 # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO
+uint8 return_switch		 # return to launch 2 position switch (mandatory): _NORMAL_, RTL
+uint8 posctl_switch		 # position control 2 position switch (optional): _ALTCTL_, POSCTL
+uint8 loiter_switch		 # loiter 2 position switch (optional): _MISSION_, LOITER
+uint8 acro_switch		 # acro 2 position switch (optional): _MANUAL_, ACRO
+uint8 offboard_switch		 # offboard 2 position switch (optional): _NORMAL_, OFFBOARD
diff --git a/msg/mc_virtual_rates_setpoint.msg b/msg/mc_virtual_rates_setpoint.msg
new file mode 100644
index 000000000..834113c79
--- /dev/null
+++ b/msg/mc_virtual_rates_setpoint.msg
@@ -0,0 +1,6 @@
+uint64 timestamp    # in microseconds since system start
+
+float32 roll	    # body angular rates in NED frame
+float32 pitch	    # body angular rates in NED frame
+float32 yaw	    # body angular rates in NED frame
+float32 thrust	    # thrust normalized to 0..1
diff --git a/msg/parameter_update.msg b/msg/parameter_update.msg
new file mode 100644
index 000000000..39dc336e0
--- /dev/null
+++ b/msg/parameter_update.msg
@@ -0,0 +1 @@
+uint64 timestamp	# time at which the latest parameter was updated
diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg
new file mode 100644
index 000000000..ae6756aa8
--- /dev/null
+++ b/msg/position_setpoint.msg
@@ -0,0 +1,35 @@
+# this file is only used in the position_setpoint triple as a dependency
+
+uint8 SETPOINT_TYPE_POSITION=0	# position setpoint
+uint8 SETPOINT_TYPE_VELOCITY=1	# velocity setpoint
+uint8 SETPOINT_TYPE_LOITER=2	# loiter setpoint
+uint8 SETPOINT_TYPE_TAKEOFF=3	# takeoff setpoint
+uint8 SETPOINT_TYPE_LAND=4	# land setpoint, altitude must be ignored, descend until landing
+uint8 SETPOINT_TYPE_IDLE=5	# do nothing, switch off motors or keep at idle speed (MC)
+uint8 SETPOINT_TYPE_OFFBOARD=6 	# setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard
+
+bool valid			# true if setpoint is valid
+uint8 type			# setpoint type to adjust behavior of position controller
+float32 x			# local position setpoint in m in NED
+float32 y			# local position setpoint in m in NED
+float32 z			# local position setpoint in m in NED
+bool position_valid		# true if local position setpoint valid
+float32 vx			# local velocity setpoint in m/s in NED
+float32 vy			# local velocity setpoint in m/s in NED
+float32 vz			# local velocity setpoint in m/s in NED
+bool velocity_valid		# true if local velocity setpoint valid
+float64 lat			# latitude, in deg
+float64 lon			# longitude, in deg
+float32 alt			# altitude AMSL, in m
+float32 yaw			# yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw
+bool yaw_valid			# true if yaw setpoint valid
+float32 yawspeed		# yawspeed (only for multirotors, in rad/s)
+bool yawspeed_valid		# true if yawspeed setpoint valid
+float32 loiter_radius		# loiter radius (only for fixed wing), in m
+int8 loiter_direction		# loiter direction: 1 = CW, -1 = CCW
+float32 pitch_min		# minimal pitch angle for fixed wing takeoff waypoints
+float32 a_x			# acceleration x setpoint
+float32 a_y			# acceleration y setpoint
+float32 a_z			# acceleration z setpoint
+bool acceleration_valid		# true if acceleration setpoint is valid/should be used
+bool acceleration_is_force	# interprete acceleration as force
diff --git a/msg/position_setpoint_triplet.msg b/msg/position_setpoint_triplet.msg
new file mode 100644
index 000000000..8717f65d0
--- /dev/null
+++ b/msg/position_setpoint_triplet.msg
@@ -0,0 +1,8 @@
+# Global position setpoint triplet in WGS84 coordinates.
+# This are the three next waypoints (or just the next two or one).
+
+px4/position_setpoint previous
+px4/position_setpoint current
+px4/position_setpoint next
+
+uint8 nav_state	# report the navigation state
diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg
new file mode 100644
index 000000000..0fa5ed2fc
--- /dev/null
+++ b/msg/rc_channels.msg
@@ -0,0 +1,27 @@
+int32 RC_CHANNELS_FUNCTION_MAX=19
+uint8 RC_CHANNELS_FUNCTION_THROTTLE=0
+uint8 RC_CHANNELS_FUNCTION_ROLL=1
+uint8 RC_CHANNELS_FUNCTION_PITCH=2
+uint8 RC_CHANNELS_FUNCTION_YAW=3
+uint8 RC_CHANNELS_FUNCTION_MODE=4
+uint8 RC_CHANNELS_FUNCTION_RETURN=5
+uint8 RC_CHANNELS_FUNCTION_POSCTL=6
+uint8 RC_CHANNELS_FUNCTION_LOITER=7
+uint8 RC_CHANNELS_FUNCTION_OFFBOARD=8
+uint8 RC_CHANNELS_FUNCTION_ACRO=9
+uint8 RC_CHANNELS_FUNCTION_FLAPS=10
+uint8 RC_CHANNELS_FUNCTION_AUX_1=11
+uint8 RC_CHANNELS_FUNCTION_AUX_2=12
+uint8 RC_CHANNELS_FUNCTION_AUX_3=13
+uint8 RC_CHANNELS_FUNCTION_AUX_4=14
+uint8 RC_CHANNELS_FUNCTION_AUX_5=15
+uint8 RC_CHANNELS_FUNCTION_PARAM_1=16
+uint8 RC_CHANNELS_FUNCTION_PARAM_2=17
+uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18
+uint64 timestamp						# Timestamp in microseconds since boot time
+uint64 timestamp_last_valid					# Timestamp of last valid RC signal
+float32[19] channels						# Scaled to -1..1 (throttle: 0..1)
+uint8 channel_count						# Number of valid channels
+int8[19] function						# Functions mapping
+uint8 rssi							# Receive signal strength index
+bool signal_lost						# Control signal lost, should be checked together with topic timeout
diff --git a/msg/templates/px4/ros/msg.h.template b/msg/templates/px4/ros/msg.h.template
new file mode 100644
index 000000000..76abab163
--- /dev/null
+++ b/msg/templates/px4/ros/msg.h.template
@@ -0,0 +1,95 @@
+@###############################################
+@#
+@# PX4 ROS compatible message source code
+@# generation for C++
+@#
+@# This file generates the multiplatform wrapper
+@#
+@# EmPy template for generating <msg>.h files
+@# Based on the original template for ROS
+@#
+@###############################################
+@# Start of Template
+@#
+@# Context:
+@#  - file_name_in (String) Source file
+@#  - spec (msggen.MsgSpec) Parsed specification of the .msg file
+@#  - md5sum (String) MD5Sum of the .msg specification
+@###############################################
+/****************************************************************************
+ *
+ *   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
+ * 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.
+ *
+ ****************************************************************************/
+
+/* Auto-generated by genmsg_cpp from file @file_name_in */
+
+@{
+import genmsg.msgs
+import gencpp
+
+cpp_class = 'px4_%s'%spec.short_name
+native_type = spec.short_name
+topic_name = spec.short_name
+}@
+
+#pragma once
+
+@##############################
+@# Generic Includes
+@##############################
+#include "px4/@(topic_name).h"
+#include "platforms/px4_message.h"
+
+@##############################
+@# Class
+@##############################
+
+namespace px4
+{
+
+class @(cpp_class) :
+	public PX4Message<@(native_type)>
+{
+public:
+	@(cpp_class)() :
+		PX4Message<@(native_type)>()
+	{}
+
+	@(cpp_class)(@(native_type) msg) :
+		PX4Message<@(native_type)>(msg)
+	{}
+
+	~@(cpp_class)() {}
+
+	static PX4TopicHandle handle() {return "@(topic_name)";}
+};
+
+};
diff --git a/msg/templates/px4/uorb/msg.h.template b/msg/templates/px4/uorb/msg.h.template
new file mode 100644
index 000000000..2d4251107
--- /dev/null
+++ b/msg/templates/px4/uorb/msg.h.template
@@ -0,0 +1,95 @@
+@###############################################
+@#
+@# PX4 ROS compatible message source code
+@# generation for C++
+@#
+@# This file generates the multiplatform wrapper
+@#
+@# EmPy template for generating <msg>.h files
+@# Based on the original template for ROS
+@#
+@###############################################
+@# Start of Template
+@#
+@# Context:
+@#  - file_name_in (String) Source file
+@#  - spec (msggen.MsgSpec) Parsed specification of the .msg file
+@#  - md5sum (String) MD5Sum of the .msg specification
+@###############################################
+/****************************************************************************
+ *
+ *   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
+ * 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.
+ *
+ ****************************************************************************/
+
+/* Auto-generated by genmsg_cpp from file @file_name_in */
+
+@{
+import genmsg.msgs
+import gencpp
+
+cpp_class = 'px4_%s'%spec.short_name
+native_type = '%s_s'%spec.short_name
+topic_name = spec.short_name
+}@
+
+#pragma once
+
+@##############################
+@# Generic Includes
+@##############################
+#include <uORB/topics/@(topic_name).h>
+#include "platforms/px4_message.h"
+
+@##############################
+@# Class
+@##############################
+
+namespace px4
+{
+
+class __EXPORT @(cpp_class) :
+	public PX4Message<@(native_type)>
+{
+public:
+	@(cpp_class)() :
+		PX4Message<@(native_type)>()
+	{}
+
+	@(cpp_class)(@(native_type) msg) :
+		PX4Message<@(native_type)>(msg)
+	{}
+
+	~@(cpp_class)() {}
+
+	static PX4TopicHandle handle() {return ORB_ID(@(topic_name));}
+};
+
+};
diff --git a/msg/templates/uorb/msg.h.template b/msg/templates/uorb/msg.h.template
new file mode 100644
index 000000000..f16095c97
--- /dev/null
+++ b/msg/templates/uorb/msg.h.template
@@ -0,0 +1,175 @@
+@###############################################
+@#
+@# PX4 ROS compatible message source code
+@# generation for C++
+@#
+@# EmPy template for generating <msg>.h files
+@# Based on the original template for ROS
+@#
+@###############################################
+@# Start of Template
+@#
+@# Context:
+@#  - file_name_in (String) Source file
+@#  - spec (msggen.MsgSpec) Parsed specification of the .msg file
+@#  - md5sum (String) MD5Sum of the .msg specification
+@###############################################
+/****************************************************************************
+ *
+ *   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
+ * 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.
+ *
+ ****************************************************************************/
+
+/* Auto-generated by genmsg_cpp from file @file_name_in */
+
+@{
+import genmsg.msgs
+import gencpp
+
+uorb_struct = '%s_s'%spec.short_name
+topic_name = spec.short_name
+}@
+
+#pragma once
+
+@##############################
+@# Generic Includes
+@##############################
+#include <stdint.h>
+#include <uORB/uORB.h>
+
+@##############################
+@# Includes for dependencies
+@##############################
+@{
+for field in spec.parsed_fields():
+  if (not field.is_builtin):
+    if (not field.is_header):
+      (package, name) = genmsg.names.package_resource_name(field.base_type)
+      package = package or spec.package # convert '' to package
+      print('#include <uORB/topics/%s.h>'%(name))
+
+}@
+
+@# Constants c style
+#ifndef __cplusplus
+@[for constant in spec.constants]@
+#define @(constant.name) @(int(constant.val))
+@[end for]
+#endif
+
+/**
+ * @@addtogroup topics
+ * @@{
+ */
+
+@##############################
+@# Main struct of message
+@##############################
+@{
+
+type_map = {'int8': 'int8_t',
+  'int16': 'int16_t',
+  'int32': 'int32_t',
+  'int64': 'int64_t',
+  'uint8': 'uint8_t',
+  'uint16': 'uint16_t',
+  'uint32': 'uint32_t',
+  'uint64': 'uint64_t',
+  'float32': 'float',
+  'float64': 'double',
+  'bool': 'bool',
+  'fence_vertex': 'fence_vertex',
+  'position_setpoint': 'position_setpoint'}
+
+# Function to print a standard ros type
+def print_field_def(field):
+  type = field.type
+  # detect embedded types
+  sl_pos = type.find('/')
+  type_appendix = ''
+  type_prefix = ''
+  if (sl_pos >= 0):
+    type = type[sl_pos + 1:]
+    type_prefix = 'struct '
+    type_appendix = '_s'
+
+  # detect arrays
+  a_pos = type.find('[')
+  array_size = ''
+  if (a_pos >= 0):
+    # field is array
+    array_size = type[a_pos:]
+    type = type[:a_pos]
+
+  if type in type_map:
+    # need to add _t: int8 --> int8_t
+    type_px4 = type_map[type]
+  else:
+    raise Exception("Type {0} not supported, add to to template file!".format(type))
+
+  print('\t%s%s%s %s%s;'%(type_prefix, type_px4, type_appendix, field.name, array_size))
+
+}
+#ifdef __cplusplus
+@#class @(uorb_struct) {
+struct __EXPORT @(uorb_struct) {
+@#public:
+#else
+struct @(uorb_struct) {
+#endif
+@{
+# loop over all fields and print the type and name
+for field in spec.parsed_fields():
+  if (not field.is_header):
+    print_field_def(field)
+}@
+#ifdef __cplusplus
+@# Constants again c++-ified
+@{
+for constant in spec.constants:
+  type = constant.type
+  if type in type_map:
+    # need to add _t: int8 --> int8_t
+    type_px4 = type_map[type]
+  else:
+    raise Exception("Type {0} not supported, add to to template file!".format(type))
+
+  print('\tstatic const %s %s = %s;'%(type_px4, constant.name, int(constant.val)))
+}
+#endif
+};
+
+/**
+ * @@}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(@(topic_name));
diff --git a/msg/vehicle_attitude.msg b/msg/vehicle_attitude.msg
new file mode 100644
index 000000000..98018a1df
--- /dev/null
+++ b/msg/vehicle_attitude.msg
@@ -0,0 +1,18 @@
+# This is similar to the mavlink message ATTITUDE, but for onboard use */
+uint64 timestamp	# in microseconds since system start
+# @warning roll, pitch and yaw have always to be valid, the rotation matrix and quaternion are optional
+float32 roll		# Roll angle (rad, Tait-Bryan, NED)
+float32 pitch		# Pitch angle (rad, Tait-Bryan, NED)
+float32 yaw		# Yaw angle (rad, Tait-Bryan, NED)
+float32 rollspeed	# Roll angular speed (rad/s, Tait-Bryan, NED)
+float32 pitchspeed	# Pitch angular speed (rad/s, Tait-Bryan, NED)
+float32 yawspeed	# Yaw angular speed (rad/s, Tait-Bryan, NED)
+float32 rollacc		# Roll angular accelration (rad/s, Tait-Bryan, NED)
+float32 pitchacc	# Pitch angular acceleration (rad/s, Tait-Bryan, NED)
+float32 yawacc		# Yaw angular acceleration (rad/s, Tait-Bryan, NED)
+float32[3] rate_offsets	# Offsets of the body angular rates from zero
+float32[9] R		# Rotation matrix, body to world, (Tait-Bryan, NED)
+float32[4] q		# Quaternion (NED)
+float32[3] g_comp		# Compensated gravity vector
+bool R_valid		# Rotation matrix valid
+bool q_valid		# Quaternion valid
diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg
new file mode 100644
index 000000000..1a8e6e3d5
--- /dev/null
+++ b/msg/vehicle_attitude_setpoint.msg
@@ -0,0 +1,21 @@
+
+uint64 timestamp				# in microseconds since system start, is set whenever the writing thread stores new data
+
+float32 roll_body				# body angle in NED frame
+float32 pitch_body				# body angle in NED frame
+float32 yaw_body					# body angle in NED frame
+
+float32[9] R_body				# Rotation matrix describing the setpoint as rotation from the current body frame
+bool R_valid					# Set to true if rotation matrix is valid
+
+# For quaternion-based attitude control
+float32[4] q_d					# Desired quaternion for quaternion control
+bool q_d_valid					# Set to true if quaternion vector is valid
+float32[4] q_e					# Attitude error in quaternion
+bool q_e_valid					# Set to true if quaternion error vector is valid
+
+float32 thrust					# Thrust in Newton the power system should generate
+
+bool roll_reset_integral			# Reset roll integral part (navigation logic change)
+bool pitch_reset_integral			# Reset pitch integral part (navigation logic change)
+bool yaw_reset_integral				# Reset yaw integral part (navigation logic change)
diff --git a/msg/vehicle_control_mode.msg b/msg/vehicle_control_mode.msg
new file mode 100644
index 000000000..153a642bb
--- /dev/null
+++ b/msg/vehicle_control_mode.msg
@@ -0,0 +1,22 @@
+
+uint64 timestamp				# in microseconds since system start
+						# is set whenever the writing thread stores new data
+
+bool flag_armed
+
+bool flag_external_manual_override_ok		# external override non-fatal for system. Only true for fixed wing
+
+# XXX needs yet to be set by state machine helper
+bool flag_system_hil_enabled
+
+bool flag_control_manual_enabled		# true if manual input is mixed in
+bool flag_control_auto_enabled			# true if onboard autopilot should act
+bool flag_control_offboard_enabled		# true if offboard control should be used
+bool flag_control_rates_enabled			# true if rates are stabilized
+bool flag_control_attitude_enabled		# true if attitude stabilization is mixed in
+bool flag_control_force_enabled			# true if force control is mixed in
+bool flag_control_velocity_enabled		# true if horizontal velocity (implies direction) is controlled
+bool flag_control_position_enabled		# true if position is controlled
+bool flag_control_altitude_enabled		# true if altitude is controlled
+bool flag_control_climb_rate_enabled		# true if climb rate is controlled
+bool flag_control_termination_enabled		# true if flighttermination is enabled
diff --git a/msg/vehicle_global_velocity_setpoint.msg b/msg/vehicle_global_velocity_setpoint.msg
new file mode 100644
index 000000000..ca7dcc826
--- /dev/null
+++ b/msg/vehicle_global_velocity_setpoint.msg
@@ -0,0 +1,4 @@
+# Velocity setpoint in NED frame
+float32 vx		# in m/s NED
+float32 vy		# in m/s NED
+float32 vz		# in m/s NED
diff --git a/msg/vehicle_local_position.msg b/msg/vehicle_local_position.msg
new file mode 100644
index 000000000..4da027ae7
--- /dev/null
+++ b/msg/vehicle_local_position.msg
@@ -0,0 +1,36 @@
+# Fused local position in NED.
+
+uint64 timestamp		# Time of this estimate, in microseconds since system start
+bool xy_valid			# true if x and y are valid
+bool z_valid			# true if z is valid
+bool v_xy_valid		# true if vy and vy are valid
+bool v_z_valid			# true if vz is valid
+
+# Position in local NED frame
+float32 x				# X position in meters in NED earth-fixed frame
+float32 y				# X position in meters in NED earth-fixed frame
+float32 z				# Z position in meters in NED earth-fixed frame (negative altitude)
+
+# Velocity in NED frame
+float32 vx 				# Ground X Speed (Latitude), m/s in NED
+float32 vy				# Ground Y Speed (Longitude), m/s in NED
+float32 vz				# Ground Z Speed (Altitude), m/s	in NED
+
+# Heading
+float32 yaw
+
+# Reference position in GPS / WGS84 frame
+bool xy_global			# true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
+bool z_global			# true if z is valid and has valid global reference (ref_alt)
+uint64 ref_timestamp	# Time when reference position was set
+float64 ref_lat		# Reference point latitude in degrees
+float64 ref_lon		# Reference point longitude in degrees
+float32 ref_alt			# Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level
+
+# Distance to surface
+float32 dist_bottom		# Distance to bottom surface (ground)
+float32 dist_bottom_rate		# Distance to bottom surface (ground) change rate
+uint64 surface_bottom_timestamp		# Time when new bottom surface found
+bool dist_bottom_valid	# true if distance to bottom surface is valid
+float32 eph
+float32 epv
diff --git a/msg/vehicle_local_position_setpoint.msg b/msg/vehicle_local_position_setpoint.msg
new file mode 100644
index 000000000..a2ff8a4ae
--- /dev/null
+++ b/msg/vehicle_local_position_setpoint.msg
@@ -0,0 +1,7 @@
+# Local position in NED frame
+
+uint64 timestamp	# timestamp of the setpoint
+float32 x		# in meters NED
+float32 y		# in meters NED
+float32 z		# in meters NED
+float32 yaw		# in radians NED -PI..+PI
diff --git a/msg/vehicle_rates_setpoint.msg b/msg/vehicle_rates_setpoint.msg
new file mode 100644
index 000000000..834113c79
--- /dev/null
+++ b/msg/vehicle_rates_setpoint.msg
@@ -0,0 +1,6 @@
+uint64 timestamp    # in microseconds since system start
+
+float32 roll	    # body angular rates in NED frame
+float32 pitch	    # body angular rates in NED frame
+float32 yaw	    # body angular rates in NED frame
+float32 thrust	    # thrust normalized to 0..1
diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg
new file mode 100644
index 000000000..18df3252f
--- /dev/null
+++ b/msg/vehicle_status.msg
@@ -0,0 +1,159 @@
+# Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link.
+uint8 MAIN_STATE_MANUAL = 0
+uint8 MAIN_STATE_ALTCTL = 1
+uint8 MAIN_STATE_POSCTL = 2
+uint8 MAIN_STATE_AUTO_MISSION = 3
+uint8 MAIN_STATE_AUTO_LOITER = 4
+uint8 MAIN_STATE_AUTO_RTL = 5
+uint8 MAIN_STATE_ACRO = 6
+uint8 MAIN_STATE_OFFBOARD = 7
+uint8 MAIN_STATE_MAX = 8
+
+# If you change the order, add or remove arming_state_t states make sure to update the arrays
+# in state_machine_helper.cpp as well.
+uint8 ARMING_STATE_INIT = 0
+uint8 ARMING_STATE_STANDBY = 1
+uint8 ARMING_STATE_ARMED = 2
+uint8 ARMING_STATE_ARMED_ERROR = 3
+uint8 ARMING_STATE_STANDBY_ERROR = 4
+uint8 ARMING_STATE_REBOOT = 5
+uint8 ARMING_STATE_IN_AIR_RESTORE = 6
+uint8 ARMING_STATE_MAX = 7
+
+uint8 HIL_STATE_OFF = 0
+uint8 HIL_STATE_ON = 1
+
+# Navigation state, i.e. "what should vehicle do".
+uint8 NAVIGATION_STATE_MANUAL = 0		# Manual mode
+uint8 NAVIGATION_STATE_ALTCTL = 1		# Altitude control mode
+uint8 NAVIGATION_STATE_POSCTL = 2		# Position control mode
+uint8 NAVIGATION_STATE_AUTO_MISSION = 3		# Auto mission mode
+uint8 NAVIGATION_STATE_AUTO_LOITER = 4		# Auto loiter mode
+uint8 NAVIGATION_STATE_AUTO_RTL = 5		# Auto return to launch mode
+uint8 NAVIGATION_STATE_AUTO_RCRECOVER = 6	# RC recover mode
+uint8 NAVIGATION_STATE_AUTO_RTGS = 7		# Auto return to groundstation on data link loss
+uint8 NAVIGATION_STATE_AUTO_LANDENGFAIL = 8 	# Auto land on engine failure
+uint8 NAVIGATION_STATE_AUTO_LANDGPSFAIL = 9	# Auto land on gps failure (e.g. open loop loiter down)
+uint8 NAVIGATION_STATE_ACRO = 10		# Acro mode
+uint8 NAVIGATION_STATE_LAND = 11		# Land mode
+uint8 NAVIGATION_STATE_DESCEND = 12		# Descend mode (no position control)
+uint8 NAVIGATION_STATE_TERMINATION = 13		# Termination mode
+uint8 NAVIGATION_STATE_OFFBOARD = 14
+uint8 NAVIGATION_STATE_MAX = 15
+
+# VEHICLE_MODE_FLAG, same as MAV_MODE_FLAG of MAVLink 1.0 protocol
+uint8 VEHICLE_MODE_FLAG_SAFETY_ARMED = 128
+uint8 VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64
+uint8 VEHICLE_MODE_FLAG_HIL_ENABLED = 32
+uint8 VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16
+uint8 VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8
+uint8 VEHICLE_MODE_FLAG_AUTO_ENABLED = 4
+uint8 VEHICLE_MODE_FLAG_TEST_ENABLED = 2
+uint8 VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1
+
+# VEHICLE_TYPE, should match 1:1 MAVLink's MAV_TYPE ENUM
+uint8 VEHICLE_TYPE_GENERIC = 0		    # Generic micro air vehicle.
+uint8 VEHICLE_TYPE_FIXED_WING = 1	    # Fixed wing aircraft.
+uint8 VEHICLE_TYPE_QUADROTOR = 2	    # Quadrotor
+uint8 VEHICLE_TYPE_COAXIAL = 3		    # Coaxial helicopter
+uint8 VEHICLE_TYPE_HELICOPTER = 4	    # Normal helicopter with tail rotor.
+uint8 VEHICLE_TYPE_ANTENNA_TRACKER = 5	    # Ground installation
+uint8 VEHICLE_TYPE_GCS = 6		    # Operator control unit / ground control station
+uint8 VEHICLE_TYPE_AIRSHIP = 7		    # Airship, controlled
+uint8 VEHICLE_TYPE_FREE_BALLOON = 8	    # Free balloon, uncontrolled
+uint8 VEHICLE_TYPE_ROCKET = 9		    # Rocket
+uint8 VEHICLE_TYPE_GROUND_ROVER = 10	    # Ground rover
+uint8 VEHICLE_TYPE_SURFACE_BOAT = 11	    # Surface vessel, boat, ship
+uint8 VEHICLE_TYPE_SUBMARINE = 12	    # Submarine
+uint8 VEHICLE_TYPE_HEXAROTOR = 13	    # Hexarotor
+uint8 VEHICLE_TYPE_OCTOROTOR = 14	    # Octorotor
+uint8 VEHICLE_TYPE_TRICOPTER = 15	    # Octorotor
+uint8 VEHICLE_TYPE_FLAPPING_WING = 16	    # Flapping wing
+uint8 VEHICLE_TYPE_KITE = 17		    # Kite
+uint8 VEHICLE_TYPE_ONBOARD_CONTROLLER=18    # Onboard companion controller
+uint8 VEHICLE_TYPE_VTOL_DUOROTOR = 19	    # Vtol with two engines
+uint8 VEHICLE_TYPE_VTOL_QUADROTOR = 20	    # Vtol with four engines*/
+uint8 VEHICLE_TYPE_ENUM_END = 21
+
+uint8 VEHICLE_BATTERY_WARNING_NONE = 0	    # no battery low voltage warning active
+uint8 VEHICLE_BATTERY_WARNING_LOW = 1	    # warning of low voltage
+uint8 VEHICLE_BATTERY_WARNING_CRITICAL = 2  # alerting of critical voltage
+
+# state machine / state of vehicle.
+# Encodes the complete system state and is set by the commander app.
+uint16 counter   # incremented by the writing thread everytime new data is stored
+uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data
+
+uint8 main_state		    	# main state machine
+uint8 nav_state				# set navigation state machine to specified value
+uint8 arming_state			# current arming state
+uint8 hil_state				# current hil state
+bool failsafe				# true if system is in failsafe state
+
+int32 system_type			# system type, inspired by MAVLink's VEHICLE_TYPE enum
+int32 system_id				# system id, inspired by MAVLink's system ID field
+int32 component_id			# subsystem / component id, inspired by MAVLink's component ID field
+
+bool is_rotary_wing			# True if system is in rotary wing configuration, so for a VTOL this is only true while flying as a multicopter
+bool is_vtol				# True if the system is VTOL capable
+bool vtol_fw_permanent_stab		# True if vtol should stabilize attitude for fw in manual mode
+
+bool condition_battery_voltage_valid
+bool condition_system_in_air_restore		# true if we can restore in mid air
+bool condition_system_sensors_initialized
+bool condition_system_returned_to_home
+bool condition_auto_mission_available
+bool condition_global_position_valid		# set to true by the commander app if the quality of the position estimate is good enough to use it for navigation
+bool condition_launch_position_valid		# indicates a valid launch position
+bool condition_home_position_valid		# indicates a valid home position (a valid home position is not always a valid launch)
+bool condition_local_position_valid
+bool condition_local_altitude_valid
+bool condition_airspeed_valid			# set to true by the commander app if there is a valid airspeed measurement available
+bool condition_landed				# true if vehicle is landed, always true if disarmed
+bool condition_power_input_valid		# set if input power is valid
+float32 avionics_power_rail_voltage		# voltage of the avionics power rail
+
+bool rc_signal_found_once
+bool rc_signal_lost				# true if RC reception lost
+uint64 rc_signal_lost_timestamp			# Time at which the RC reception was lost
+bool rc_signal_lost_cmd				# true if RC lost mode is commanded
+bool rc_input_blocked				# set if RC input should be ignored
+
+bool data_link_lost				# datalink to GCS lost
+bool data_link_lost_cmd				# datalink to GCS lost mode commanded
+uint8 data_link_lost_counter			# counts unique data link lost events
+bool engine_failure				# Set to true if an engine failure is detected
+bool engine_failure_cmd				# Set to true if an engine failure mode is commanded
+bool gps_failure				# Set to true if a gps failure is detected
+bool gps_failure_cmd				# Set to true if a gps failure mode is commanded
+
+bool barometer_failure				# Set to true if a barometer failure is detected
+
+bool offboard_control_signal_found_once
+bool offboard_control_signal_lost
+bool offboard_control_signal_weak
+uint64 offboard_control_signal_lost_interval	# interval in microseconds without an offboard control message
+bool offboard_control_set_by_command		# true if the offboard mode was set by a mavlink command and should not be overridden by RC
+
+# see SYS_STATUS mavlink message for the following
+uint32 onboard_control_sensors_present
+uint32 onboard_control_sensors_enabled
+uint32 onboard_control_sensors_health
+
+float32 load					# processor load from 0 to 1
+float32 battery_voltage
+float32 battery_current
+float32 battery_remaining
+
+uint8 battery_warning    # current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum
+uint16 drop_rate_comm
+uint16 errors_comm
+uint16 errors_count1
+uint16 errors_count2
+uint16 errors_count3
+uint16 errors_count4
+
+bool circuit_breaker_engaged_power_check
+bool circuit_breaker_engaged_airspd_check
+bool circuit_breaker_engaged_enginefailure_check
+bool circuit_breaker_engaged_gpsfailure_check
diff --git a/package.xml b/package.xml
new file mode 100644
index 000000000..666200390
--- /dev/null
+++ b/package.xml
@@ -0,0 +1,61 @@
+<?xml version="1.0"?>
+<package>
+  <name>px4</name>
+  <version>1.0.0</version>
+  <description>The PX4 Flight Stack package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="lorenz@px4.io">Lorenz Meier</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>BSD</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <url type="website">http://px4.io/ros</url>
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <build_depend>message_generation</build_depend>
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <run_depend>message_runtime</run_depend>
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>eigen</build_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>eigen</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- You can specify that this package is a metapackage here: -->
+    <!-- <metapackage/> -->
+
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c
index 7f1b21a95..7d4b7d880 100644
--- a/src/drivers/ardrone_interface/ardrone_interface.c
+++ b/src/drivers/ardrone_interface/ardrone_interface.c
@@ -55,6 +55,10 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/safety.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/vehicle_control_mode.h>
 
@@ -98,7 +102,7 @@ usage(const char *reason)
  * The deamon app only briefly exists to start
  * the background job. The stack size assigned in the
  * Makefile does only apply to this management task.
- * 
+ *
  * The actual stack size should be set in the call
  * to task_create().
  */
@@ -319,7 +323,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 			/* get a local copy of the actuator controls */
 			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
 			orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
-			
+
 			/* for now only spin if armed and immediately shut down
 			 * if in failsafe
 			 */
diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp
index 6b14f5945..d0253a8d1 100644
--- a/src/drivers/blinkm/blinkm.cpp
+++ b/src/drivers/blinkm/blinkm.cpp
@@ -571,7 +571,7 @@ BlinkM::led()
 				printf("<blinkm> cells found:%d\n", num_of_cells);
 
 			} else {
-				if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
+				if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
 					/* LED Pattern for battery critical alerting */
 					led_color_1 = LED_RED;
 					led_color_2 = LED_RED;
@@ -595,7 +595,7 @@ BlinkM::led()
 					led_color_8 = LED_BLUE;
 					led_blink = LED_BLINK;
 
-				} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
+				} else if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW) {
 					/* LED Pattern for battery low warning */
 					led_color_1 = LED_YELLOW;
 					led_color_2 = LED_YELLOW;
@@ -647,14 +647,14 @@ BlinkM::led()
 
 						if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
 							/* indicate main control state */
-							if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL)
+							if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_POSCTL)
 								led_color_4 = LED_GREEN;
 							/* TODO: add other Auto modes */
-							else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO_MISSION)
+							else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION)
 								led_color_4 = LED_BLUE;
-							else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL)
+							else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_ALTCTL)
 								led_color_4 = LED_YELLOW;
-							else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL)
+							else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_MANUAL)
 								led_color_4 = LED_WHITE;
 							else
 								led_color_4 = LED_OFF;
diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp
index 058425676..767ff5803 100644
--- a/src/drivers/hil/hil.cpp
+++ b/src/drivers/hil/hil.cpp
@@ -75,6 +75,8 @@
 #include <systemlib/mixer/mixer.h>
 
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_outputs.h>
 
@@ -250,7 +252,7 @@ HIL::task_main_trampoline(int argc, char *argv[])
 int
 HIL::set_mode(Mode mode)
 {
-	/* 
+	/*
 	 * Configure for PWM output.
 	 *
 	 * Note that regardless of the configured mode, the task is always
@@ -269,19 +271,19 @@ HIL::set_mode(Mode mode)
 		/* multi-port as 4 PWM outs */
 		_update_rate = 50;	/* default output rate */
 		break;
-            
+
     	case MODE_8PWM:
             debug("MODE_8PWM");
             /* multi-port as 8 PWM outs */
             _update_rate = 50;	/* default output rate */
             break;
-            
+
         case MODE_12PWM:
             debug("MODE_12PWM");
             /* multi-port as 12 PWM outs */
             _update_rate = 50;	/* default output rate */
             break;
-            
+
         case MODE_16PWM:
             debug("MODE_16PWM");
             /* multi-port as 16 PWM outs */
@@ -514,12 +516,12 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		break;
 
 	case PWM_SERVO_SET_UPDATE_RATE:
-		// HIL always outputs at the alternate (usually faster) rate 
+		// HIL always outputs at the alternate (usually faster) rate
 		g_hil->set_pwm_rate(arg);
 		break;
 
 	case PWM_SERVO_SET_SELECT_UPDATE_RATE:
-		// HIL always outputs at the alternate (usually faster) rate 
+		// HIL always outputs at the alternate (usually faster) rate
 		break;
 
 	case PWM_SERVO_SET(2):
@@ -660,7 +662,7 @@ int
 hil_new_mode(PortMode new_mode)
 {
 	// uint32_t gpio_bits;
-	
+
 
 //	/* reset to all-inputs */
 //	g_hil->ioctl(0, GPIO_RESET, 0);
@@ -692,17 +694,17 @@ hil_new_mode(PortMode new_mode)
 		/* select 2-pin PWM mode */
 		servo_mode = HIL::MODE_2PWM;
 		break;
-            
+
         case PORT2_8PWM:
             /* select 8-pin PWM mode */
             servo_mode = HIL::MODE_8PWM;
             break;
-            
+
         case PORT2_12PWM:
             /* select 12-pin PWM mode */
             servo_mode = HIL::MODE_12PWM;
             break;
-            
+
         case PORT2_16PWM:
             /* select 16-pin PWM mode */
             servo_mode = HIL::MODE_16PWM;
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index 15412984b..1331744ae 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -74,6 +74,7 @@
 #include <drivers/drv_mixer.h>
 
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/esc_status.h>
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 363d2cdcf..8fcdc8023 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -70,6 +70,10 @@
 #include <drivers/drv_rc_input.h>
 
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 
@@ -138,11 +142,11 @@ private:
 
 	uint32_t	_groups_required;
 	uint32_t	_groups_subscribed;
-	int		_control_subs[NUM_ACTUATOR_CONTROL_GROUPS];
-	actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS];
-	orb_id_t	_control_topics[NUM_ACTUATOR_CONTROL_GROUPS];
+	int		_control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
+	actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
+	orb_id_t	_control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
 	int		_actuator_output_topic_instance;
-	pollfd	_poll_fds[NUM_ACTUATOR_CONTROL_GROUPS];
+	pollfd	_poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
 	unsigned	_poll_fds_num;
 
 	pwm_limit_t	_pwm_limit;
@@ -508,7 +512,7 @@ PX4FMU::subscribe()
 	uint32_t sub_groups = _groups_required & ~_groups_subscribed;
 	uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
 	_poll_fds_num = 0;
-	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 		if (sub_groups & (1 << i)) {
 			warnx("subscribe to actuator_controls_%d", i);
 			_control_subs[i] = orb_subscribe(_control_topics[i]);
@@ -585,7 +589,7 @@ PX4FMU::task_main()
 			}
 
 			debug("adjusted actuator update interval to %ums", update_rate_in_ms);
-			for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+			for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 				if (_control_subs[i] > 0) {
 					orb_set_interval(_control_subs[i], update_rate_in_ms);
 				}
@@ -612,7 +616,7 @@ PX4FMU::task_main()
 
 			/* get controls for required topics */
 			unsigned poll_id = 0;
-			for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+			for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 				if (_control_subs[i] > 0) {
 					if (_poll_fds[poll_id].revents & POLLIN) {
 						orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
@@ -745,7 +749,7 @@ PX4FMU::task_main()
 
 	}
 
-	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 		if (_control_subs[i] > 0) {
 			::close(_control_subs[i]);
 			_control_subs[i] = -1;
@@ -1142,7 +1146,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		 * and PWM under control of the flight config
 		 * parameters. Note that this does not allow for
 		 * changing a set of pins to be used for serial on
-		 * FMUv1 
+		 * FMUv1
 		 */
 		switch (arg) {
 		case 0:
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 24ece4298..3a9e8ef24 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -75,6 +75,10 @@
 #include <systemlib/circuit_breaker.h>
 
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/safety.h>
@@ -1199,7 +1203,7 @@ PX4IO::io_set_arming_state()
 
 		if (armed.ready_to_arm) {
 			set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
-			
+
 		} else {
 			clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
 		}
@@ -2585,7 +2589,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 
 	case PWM_SERVO_SET_RC_CONFIG: {
 		/* enable setting of RC configuration without relying
-		   on param_get() 
+		   on param_get()
 		*/
 		struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg;
 		if (config->channel >= RC_INPUT_MAX_CHANNELS) {
diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp
index fdaa7f27b..5b945e452 100644
--- a/src/drivers/roboclaw/RoboClaw.cpp
+++ b/src/drivers/roboclaw/RoboClaw.cpp
@@ -54,6 +54,7 @@
 #include <mavlink/mavlink_log.h>
 
 #include <uORB/Publication.hpp>
+#include <uORB/topics/actuator_controls_0.h>
 #include <uORB/topics/debug_key_value.h>
 #include <drivers/drv_hrt.h>
 
@@ -65,7 +66,7 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address,
 	_pulsesPerRev(pulsesPerRev),
 	_uart(0),
 	_controlPoll(),
-	_actuators(NULL, ORB_ID(actuator_controls_0), 20),
+	_actuators(ORB_ID(actuator_controls_0), 20),
 	_motor1Position(0),
 	_motor1Speed(0),
 	_motor1Overflow(0),
@@ -114,7 +115,7 @@ int RoboClaw::readEncoder(e_motor motor)
 	uint8_t rbuf[50];
 	usleep(5000);
 	int nread = read(_uart, rbuf, 50);
-	if (nread < 6) { 
+	if (nread < 6) {
 		printf("failed to read\n");
 		return -1;
 	}
@@ -131,7 +132,7 @@ int RoboClaw::readEncoder(e_motor motor)
 	countBytes[0] = rbuf[3];
 	uint8_t status = rbuf[4];
 	uint8_t checksum = rbuf[5];
-	uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) & 
+	uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) &
 		checksum_mask;
 	// check if checksum is valid
 	if (checksum != checksum_computed) {
@@ -156,11 +157,11 @@ int RoboClaw::readEncoder(e_motor motor)
 	static int64_t overflowAmount = 0x100000000LL;
 	if (motor == MOTOR_1) {
 		_motor1Overflow += overFlow;
-		_motor1Position = float(int64_t(count) + 
+		_motor1Position = float(int64_t(count) +
 			_motor1Overflow*overflowAmount)/_pulsesPerRev;
 	} else if (motor == MOTOR_2) {
 		_motor2Overflow += overFlow;
-		_motor2Position = float(int64_t(count) + 
+		_motor2Position = float(int64_t(count) +
 			_motor2Overflow*overflowAmount)/_pulsesPerRev;
 	}
 	return 0;
@@ -242,7 +243,7 @@ int RoboClaw::setMotorDutyCycle(e_motor motor, float value)
 	return -1;
 }
 
-int RoboClaw::resetEncoders() 
+int RoboClaw::resetEncoders()
 {
 	uint16_t sum = 0;
 	return _sendCommand(CMD_RESET_ENCODERS,
@@ -278,7 +279,7 @@ uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n)
 	return sum;
 }
 
-int RoboClaw::_sendCommand(e_command cmd, uint8_t * data, 
+int RoboClaw::_sendCommand(e_command cmd, uint8_t * data,
 		size_t n_data, uint16_t & prev_sum)
 {
 	tcflush(_uart, TCIOFLUSH); // flush  buffers
@@ -299,7 +300,7 @@ int RoboClaw::_sendCommand(e_command cmd, uint8_t * data,
 	return write(_uart, buf, n_data + 3);
 }
 
-int roboclawTest(const char *deviceName, uint8_t address, 
+int roboclawTest(const char *deviceName, uint8_t address,
 		uint16_t pulsesPerRev)
 {
 	printf("roboclaw test: starting\n");
diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c
index fcbb54c8e..d8b777b91 100644
--- a/src/examples/fixedwing_control/main.c
+++ b/src/examples/fixedwing_control/main.c
@@ -61,6 +61,10 @@
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/parameter_update.h>
@@ -108,7 +112,7 @@ static void usage(const char *reason);
  * @param att The current attitude. The controller should make the attitude match the setpoint
  * @param rates_sp The angular rate setpoint. This is the output of the controller.
  */
-void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, 
+void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp,
 	struct actuator_controls_s *actuators);
 
 /**
@@ -133,18 +137,18 @@ static int deamon_task;				/**< Handle of deamon task / thread */
 static struct params p;
 static struct param_handles ph;
 
-void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, 
+void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp,
 	struct actuator_controls_s *actuators)
 {
 
-	/* 
+	/*
 	 * The PX4 architecture provides a mixer outside of the controller.
 	 * The mixer is fed with a default vector of actuator controls, representing
 	 * moments applied to the vehicle frame. This vector
 	 * is structured as:
 	 *
 	 * Control Group 0 (attitude):
-	 * 
+	 *
 	 *    0  -  roll   (-1..+1)
 	 *    1  -  pitch  (-1..+1)
 	 *    2  -  yaw    (-1..+1)
@@ -226,7 +230,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 	 *
 	 * Wikipedia description:
 	 * http://en.wikipedia.org/wiki/Publish–subscribe_pattern
-	 * 
+	 *
 	 */
 
 
@@ -234,7 +238,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 
 	/*
 	 * Declare and safely initialize all structs to zero.
-	 * 
+	 *
 	 * These structs contain the system state and things
 	 * like attitude, position, the current waypoint, etc.
 	 */
@@ -300,7 +304,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 		if (ret < 0) {
 			/*
 			 * Poll error, this will not really happen in practice,
-			 * but its good design practice to make output an error message. 
+			 * but its good design practice to make output an error message.
 			 */
 			warnx("poll error");
 
@@ -340,7 +344,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 				}
 
 				if (manual_sp_updated)
-					/* get the RC (or otherwise user based) input */ 
+					/* get the RC (or otherwise user based) input */
 					orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
 
 				/* check if the throttle was ever more than 50% - go later only to failsafe if yes */
diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c
index a89ccf933..1ac0c2d04 100644
--- a/src/examples/flow_position_estimator/flow_position_estimator_main.c
+++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c
@@ -67,6 +67,7 @@
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
 #include <poll.h>
+#include <platforms/px4_defines.h>
 
 #include "flow_position_estimator_params.h"
 
@@ -337,7 +338,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
 						{
 							float sum = 0.0f;
 							for(uint8_t j = 0; j < 3; j++)
-								sum = sum + speed[j] * att.R[i][j];
+								sum = sum + speed[j] * PX4_R(att.R, i, j);
 
 							global_speed[i] = sum;
 						}
diff --git a/src/examples/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c
index 95ff346bb..8fd8870da 100644
--- a/src/examples/hwtest/hwtest.c
+++ b/src/examples/hwtest/hwtest.c
@@ -47,6 +47,10 @@
 #include <systemlib/err.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/uORB.h>
 
 __EXPORT int ex_hwtest_main(int argc, char *argv[]);
diff --git a/src/examples/publisher/module.mk b/src/examples/publisher/module.mk
new file mode 100644
index 000000000..62a5f8dd1
--- /dev/null
+++ b/src/examples/publisher/module.mk
@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Publisher Example Application
+#
+
+MODULE_COMMAND	= publisher
+
+SRCS		= publisher_main.cpp \
+		  publisher_start_nuttx.cpp \
+		  publisher_example.cpp
+
+MODULE_STACKSIZE = 1200
diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp
new file mode 100644
index 000000000..e7ab9eb5a
--- /dev/null
+++ b/src/examples/publisher/publisher_example.cpp
@@ -0,0 +1,83 @@
+
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file publisher_example.cpp
+ * Example subscriber for ros and px4
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include "publisher_example.h"
+
+using namespace px4;
+
+PublisherExample::PublisherExample() :
+	_n(),
+	_rc_channels_pub(_n.advertise<px4_rc_channels>()),
+	_v_att_pub(_n.advertise<px4_vehicle_attitude>()),
+	_parameter_update_pub(_n.advertise<px4_parameter_update>())
+{
+}
+
+int PublisherExample::main()
+{
+	px4::Rate loop_rate(10);
+
+	while (px4::ok()) {
+		loop_rate.sleep();
+		_n.spinOnce();
+
+		/* Publish example message */
+		px4_rc_channels rc_channels_msg;
+		rc_channels_msg.data().timestamp_last_valid = px4::get_time_micros();
+		PX4_INFO("rc: %llu", rc_channels_msg.data().timestamp_last_valid);
+		_rc_channels_pub->publish(rc_channels_msg);
+
+		/* Publish example message */
+		px4_vehicle_attitude v_att_msg;
+		v_att_msg.data().timestamp = px4::get_time_micros();
+		PX4_INFO("att: %llu", v_att_msg.data().timestamp);
+		_v_att_pub->publish(v_att_msg);
+
+		/* Publish example message */
+		px4_parameter_update parameter_update_msg;
+		parameter_update_msg.data().timestamp = px4::get_time_micros();
+		PX4_INFO("param update: %llu", parameter_update_msg.data().timestamp);
+		_parameter_update_pub->publish(parameter_update_msg);
+
+	}
+
+	return 0;
+}
diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h
new file mode 100644
index 000000000..8165b0ffc
--- /dev/null
+++ b/src/examples/publisher/publisher_example.h
@@ -0,0 +1,55 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file publisher.h
+ * Example publisher for ros and px4
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#pragma once
+#include <px4.h>
+
+class PublisherExample {
+public:
+	PublisherExample();
+
+	~PublisherExample() {};
+
+	int main();
+protected:
+	px4::NodeHandle _n;
+	px4::Publisher<px4::px4_rc_channels> * _rc_channels_pub;
+	px4::Publisher<px4::px4_vehicle_attitude> * _v_att_pub;
+	px4::Publisher<px4::px4_parameter_update> * _parameter_update_pub;
+};
diff --git a/src/examples/publisher/publisher_main.cpp b/src/examples/publisher/publisher_main.cpp
new file mode 100644
index 000000000..11439acff
--- /dev/null
+++ b/src/examples/publisher/publisher_main.cpp
@@ -0,0 +1,56 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file publisher_main.cpp
+ * Example publisher for ros and px4
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include "publisher_example.h"
+
+bool thread_running = false;     /**< Deamon status flag */
+
+int main(int argc, char **argv)
+{
+	px4::init(argc, argv, "publisher");
+
+	PX4_INFO("starting");
+	PublisherExample p;
+	thread_running = true;
+	p.main();
+
+	PX4_INFO("exiting.");
+	thread_running = false;
+	return 0;
+}
diff --git a/src/examples/publisher/publisher_start_nuttx.cpp b/src/examples/publisher/publisher_start_nuttx.cpp
new file mode 100644
index 000000000..db9d85269
--- /dev/null
+++ b/src/examples/publisher/publisher_start_nuttx.cpp
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file publisher_start_nuttx.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include <string.h>
+#include <cstdlib>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+extern bool thread_running;
+int daemon_task;             /**< Handle of deamon task / thread */
+namespace px4
+{
+bool task_should_exit = false;
+}
+using namespace px4;
+
+extern int main(int argc, char **argv);
+
+extern "C" __EXPORT int publisher_main(int argc, char *argv[]);
+int publisher_main(int argc, char *argv[])
+{
+	if (argc < 1) {
+		errx(1, "usage: publisher {start|stop|status}");
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("already running");
+			/* this is not an error */
+			exit(0);
+		}
+
+		task_should_exit = false;
+
+		daemon_task = task_spawn_cmd("publisher",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2000,
+				       main,
+					(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		task_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("is running");
+
+		} else {
+			warnx("not started");
+		}
+
+		exit(0);
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}
diff --git a/src/examples/subscriber/module.mk b/src/examples/subscriber/module.mk
new file mode 100644
index 000000000..0e02c65b4
--- /dev/null
+++ b/src/examples/subscriber/module.mk
@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Subscriber Example Application
+#
+
+MODULE_COMMAND	= subscriber
+
+SRCS		= subscriber_main.cpp \
+		  subscriber_start_nuttx.cpp \
+		  subscriber_example.cpp \
+		  subscriber_params.c
+
+MODULE_STACKSIZE = 2400
diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp
new file mode 100644
index 000000000..e1292f788
--- /dev/null
+++ b/src/examples/subscriber/subscriber_example.cpp
@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file subscriber_example.cpp
+ * Example subscriber for ros and px4
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include "subscriber_params.h"
+#include "subscriber_example.h"
+
+using namespace px4;
+
+void rc_channels_callback_function(const px4_rc_channels &msg) {
+	PX4_INFO("I heard: [%llu]", msg.data().timestamp_last_valid);
+}
+
+SubscriberExample::SubscriberExample() :
+	_n(),
+	_p_sub_interv(PX4_PARAM_INIT(SUB_INTERV)),
+	_interval(0),
+	_p_test_float(PX4_PARAM_INIT(SUB_TESTF)),
+	_test_float(0.0f)
+{
+	/* Read the parameter back as example */
+	PX4_PARAM_GET(_p_sub_interv, &_interval);
+	PX4_INFO("Param SUB_INTERV = %d", _interval);
+	PX4_PARAM_GET(_p_test_float, &_test_float);
+	PX4_INFO("Param SUB_TESTF = %.3f", (double)_test_float);
+
+	/* Do some subscriptions */
+	/* Function */
+	_n.subscribe<px4_rc_channels>(rc_channels_callback_function, _interval);
+
+	/* No callback */
+	_sub_rc_chan = _n.subscribe<px4_rc_channels>(500);
+
+	/* Class method */
+	_n.subscribe<px4_rc_channels>(&SubscriberExample::rc_channels_callback, this, 1000);
+
+	/* Another class method */
+	_n.subscribe<px4_vehicle_attitude>(&SubscriberExample::vehicle_attitude_callback, this, 1000);
+
+	/* Yet antoher class method */
+	_n.subscribe<px4_parameter_update>(&SubscriberExample::parameter_update_callback, this, 1000);
+
+	PX4_INFO("subscribed");
+}
+
+/**
+ * This tutorial demonstrates simple receipt of messages over the PX4 middleware system.
+ * Also the current value of the _sub_rc_chan subscription is printed
+ */
+void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg) {
+	PX4_INFO("rc_channels_callback (method): [%llu]",
+			msg.data().timestamp_last_valid);
+	PX4_INFO("rc_channels_callback (method): value of _sub_rc_chan: [%llu]",
+			_sub_rc_chan->data().timestamp_last_valid);
+}
+
+void SubscriberExample::vehicle_attitude_callback(const px4_vehicle_attitude &msg) {
+	PX4_INFO("vehicle_attitude_callback (method): [%llu]",
+			msg.data().timestamp);
+}
+
+void SubscriberExample::parameter_update_callback(const px4_parameter_update &msg) {
+	PX4_INFO("parameter_update_callback (method): [%llu]",
+			msg.data().timestamp);
+	PX4_PARAM_GET(_p_sub_interv, &_interval);
+	PX4_INFO("Param SUB_INTERV = %d", _interval);
+	PX4_PARAM_GET(_p_test_float, &_test_float);
+	PX4_INFO("Param SUB_TESTF = %.3f", (double)_test_float);
+}
diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h
new file mode 100644
index 000000000..9b6d890e3
--- /dev/null
+++ b/src/examples/subscriber/subscriber_example.h
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file subscriber_example.h
+ * Example subscriber for ros and px4
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include <px4.h>
+
+using namespace px4;
+
+void rc_channels_callback_function(const px4_rc_channels &msg);
+
+class SubscriberExample {
+public:
+	SubscriberExample();
+
+	~SubscriberExample() {};
+
+	void spin() {_n.spin();}
+protected:
+	px4::NodeHandle _n;
+	px4_param_t _p_sub_interv;
+	int32_t _interval;
+	px4_param_t _p_test_float;
+	float _test_float;
+	px4::Subscriber<px4_rc_channels> * _sub_rc_chan;
+
+	void rc_channels_callback(const px4_rc_channels &msg);
+	void vehicle_attitude_callback(const px4_vehicle_attitude &msg);
+	void parameter_update_callback(const px4_parameter_update &msg);
+
+};
diff --git a/src/examples/subscriber/subscriber_main.cpp b/src/examples/subscriber/subscriber_main.cpp
new file mode 100644
index 000000000..798c74163
--- /dev/null
+++ b/src/examples/subscriber/subscriber_main.cpp
@@ -0,0 +1,55 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file subscriber_main.cpp
+ * Example subscriber for ros and px4
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include "subscriber_example.h"
+bool thread_running = false;     /**< Deamon status flag */
+
+int main(int argc, char **argv)
+{
+	px4::init(argc, argv, "subscriber");
+
+	PX4_INFO("starting");
+	SubscriberExample s;
+	thread_running = true;
+	s.spin();
+
+	PX4_INFO("exiting.");
+	thread_running = false;
+	return 0;
+}
diff --git a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h b/src/examples/subscriber/subscriber_params.c
index 5dac877d0..a43817b5b 100644
--- a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h
+++ b/src/examples/subscriber/subscriber_params.c
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: @author Anton Babushkin <anton.babushkin@me.com>
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,31 +32,25 @@
  ****************************************************************************/
 
 /**
- * @file vehicle_global_velocity_setpoint.h
- * Definition of the global velocity setpoint uORB topic.
+ * @file subscriber_params.c
+ * Parameters for the subscriber example
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#ifndef TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_
-#define TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_
-
-#include "../uORB.h"
+#include <px4_defines.h>
+#include "subscriber_params.h"
 
 /**
- * @addtogroup topics
- * @{
+ * Interval of one subscriber in the example in ms
+ *
+ * @group Subscriber Example
  */
-
-struct vehicle_global_velocity_setpoint_s {
-	float vx;		/**< in m/s NED			  		*/
-	float vy;		/**< in m/s NED			  		*/
-	float vz;		/**< in m/s NED			  		*/
-}; /**< Velocity setpoint in NED frame */
+PX4_PARAM_DEFINE_INT32(SUB_INTERV);
 
 /**
- * @}
+ * Float Demonstration Parameter in the Example
+ *
+ * @group Subscriber Example
  */
-
-/* register this as object request broker structure */
-ORB_DECLARE(vehicle_global_velocity_setpoint);
-
-#endif
+PX4_PARAM_DEFINE_FLOAT(SUB_TESTF);
diff --git a/src/examples/subscriber/subscriber_params.h b/src/examples/subscriber/subscriber_params.h
new file mode 100644
index 000000000..f6f1d6ba0
--- /dev/null
+++ b/src/examples/subscriber/subscriber_params.h
@@ -0,0 +1,43 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file subscriber_params.h
+ * Default parameters for the subscriber example
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#pragma once
+
+#define PARAM_SUB_INTERV_DEFAULT 100
+#define PARAM_SUB_TESTF_DEFAULT 3.14f
diff --git a/src/examples/subscriber/subscriber_start_nuttx.cpp b/src/examples/subscriber/subscriber_start_nuttx.cpp
new file mode 100644
index 000000000..6129b19ac
--- /dev/null
+++ b/src/examples/subscriber/subscriber_start_nuttx.cpp
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file subscriber_start_nuttx.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include <string.h>
+#include <cstdlib>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+extern bool thread_running;
+int daemon_task;             /**< Handle of deamon task / thread */
+namespace px4
+{
+bool task_should_exit = false;
+}
+using namespace px4;
+
+extern int main(int argc, char **argv);
+
+extern "C" __EXPORT int subscriber_main(int argc, char *argv[]);
+int subscriber_main(int argc, char *argv[])
+{
+	if (argc < 1) {
+		errx(1, "usage: subscriber {start|stop|status}");
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("already running");
+			/* this is not an error */
+			exit(0);
+		}
+
+		task_should_exit = false;
+
+		daemon_task = task_spawn_cmd("subscriber",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2000,
+				       main,
+					(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		task_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("is running");
+
+		} else {
+			warnx("not started");
+		}
+
+		exit(0);
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}
diff --git a/src/include/px4.h b/src/include/px4.h
new file mode 100644
index 000000000..34137ee6f
--- /dev/null
+++ b/src/include/px4.h
@@ -0,0 +1,48 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4.h
+ *
+ * Main system header with common convenience functions
+ */
+
+#pragma once
+
+#include "../platforms/px4_includes.h"
+#include "../platforms/px4_defines.h"
+#ifdef __cplusplus
+#include "../platforms/px4_middleware.h"
+#include "../platforms/px4_nodehandle.h"
+#include "../platforms/px4_subscriber.h"
+#endif
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index 2311e0a7c..ca3587b93 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -44,10 +44,7 @@
  */
 
 #pragma once
-
-#include "uORB/topics/fence.h"
-#include "uORB/topics/vehicle_global_position.h"
-
+#include <platforms/px4_defines.h>
 __BEGIN_DECLS
 
 #include "geo_lookup/geo_mag_declination.h"
diff --git a/src/lib/mathlib/math/Limits.cpp b/src/lib/mathlib/math/Limits.cpp
index d4c892d8a..e16f33bd6 100644
--- a/src/lib/mathlib/math/Limits.cpp
+++ b/src/lib/mathlib/math/Limits.cpp
@@ -46,6 +46,9 @@
 
 namespace math {
 
+#ifndef CONFIG_ARCH_ARM	
+#define M_PI_F 3.14159265358979323846f
+#endif
 
 float __EXPORT min(float val1, float val2)
 {
@@ -143,4 +146,4 @@ double __EXPORT degrees(double radians)
 	return (radians / M_PI) * 180.0;
 }
 
-}
\ No newline at end of file
+}
diff --git a/src/lib/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp
index fb778dd66..fca4197b8 100644
--- a/src/lib/mathlib/math/Limits.hpp
+++ b/src/lib/mathlib/math/Limits.hpp
@@ -39,7 +39,7 @@
 
 #pragma once
 
-#include <nuttx/config.h>
+#include <platforms/px4_defines.h>
 #include <stdint.h>
 
 namespace math {
@@ -84,4 +84,4 @@ float __EXPORT degrees(float radians);
 
 double __EXPORT degrees(double radians);
 
-}
\ No newline at end of file
+}
diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index ac1f1538f..f6f4fc5ea 100644
--- a/src/lib/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
@@ -44,12 +44,20 @@
 #define MATRIX_HPP
 
 #include <stdio.h>
+#include <math.h>
+
+#ifdef CONFIG_ARCH_ARM
 #include "../CMSIS/Include/arm_math.h"
+#else
+#include <platforms/ros/eigen_math.h>
+#include <Eigen/Eigen>
+#endif
+#include <platforms/px4_defines.h>
 
 namespace math
 {
 
-template <unsigned int M, unsigned int N>
+template<unsigned int M, unsigned int N>
 class __EXPORT Matrix;
 
 // MxN matrix with float elements
@@ -65,7 +73,11 @@ public:
 	/**
 	 * struct for using arm_math functions
 	 */
+#ifdef CONFIG_ARCH_ARM
 	arm_matrix_instance_f32 arm_mat;
+#else
+	eigen_matrix_instance arm_mat;
+#endif
 
 	/**
 	 * trivial ctor
@@ -114,6 +126,15 @@ public:
 		memcpy(data, d, sizeof(data));
 	}
 
+#if defined(__PX4_ROS)
+	/**
+	 * set data from boost::array
+	 */
+	void set(const boost::array<float, 9ul> d) {
+	set(static_cast<const float*>(d.data()));
+	}
+#endif
+
 	/**
 	 * access by index
 	 */
@@ -273,27 +294,53 @@ public:
 	 */
 	template <unsigned int P>
 	Matrix<M, P> operator *(const Matrix<N, P> &m) const {
+#ifdef CONFIG_ARCH_ARM
 		Matrix<M, P> res;
 		arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat);
 		return res;
+#else
+		Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> >
+				(this->arm_mat.pData);
+		Eigen::Matrix<float, N, P, Eigen::RowMajor> Him = Eigen::Map<Eigen::Matrix<float, N, P, Eigen::RowMajor> >
+				(m.arm_mat.pData);
+		Eigen::Matrix<float, M, P, Eigen::RowMajor> Product = Me * Him;
+		Matrix<M, P> res(Product.data());
+		return res;
+#endif
 	}
 
 	/**
 	 * transpose the matrix
 	 */
 	Matrix<N, M> transposed(void) const {
+#ifdef CONFIG_ARCH_ARM
 		Matrix<N, M> res;
 		arm_mat_trans_f32(&this->arm_mat, &res.arm_mat);
 		return res;
+#else
+		Eigen::Matrix<float, N, M, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, N, M, Eigen::RowMajor> >
+				(this->arm_mat.pData);
+		Me.transposeInPlace();
+		Matrix<N, M> res(Me.data());
+		return res;
+#endif
 	}
 
 	/**
 	 * invert the matrix
 	 */
 	Matrix<M, N> inversed(void) const {
+#ifdef CONFIG_ARCH_ARM
 		Matrix<M, N> res;
 		arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat);
 		return res;
+#else
+		Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> >
+				(this->arm_mat.pData);
+		Eigen::Matrix<float, M, N, Eigen::RowMajor> MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea
+		Matrix<M, N> res(MyInverse.data());
+		return res;
+#endif
 	}
 
 	/**
@@ -352,8 +399,17 @@ public:
 	 * multiplication by a vector
 	 */
 	Vector<M> operator *(const Vector<N> &v) const {
+#ifdef CONFIG_ARCH_ARM
 		Vector<M> res;
 		arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col);
+#else
+		//probably nicer if this could go into a function like "eigen_mat_mult" or so
+		Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> >
+				(this->arm_mat.pData);
+		Eigen::VectorXf Vec = Eigen::Map<Eigen::VectorXf>(v.arm_col.pData, N);
+		Eigen::VectorXf Product = Me * Vec;
+		Vector<M> res(Product.data());
+#endif
 		return res;
 	}
 };
diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp
index 21d05c7ef..b3cca30c6 100644
--- a/src/lib/mathlib/math/Quaternion.hpp
+++ b/src/lib/mathlib/math/Quaternion.hpp
@@ -44,7 +44,7 @@
 #define QUATERNION_HPP
 
 #include <math.h>
-#include "../CMSIS/Include/arm_math.h"
+
 #include "Vector.hpp"
 #include "Matrix.hpp"
 
diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp
index 0ddf77615..781c14d53 100644
--- a/src/lib/mathlib/math/Vector.hpp
+++ b/src/lib/mathlib/math/Vector.hpp
@@ -45,7 +45,14 @@
 
 #include <stdio.h>
 #include <math.h>
+
+#ifdef CONFIG_ARCH_ARM
 #include "../CMSIS/Include/arm_math.h"
+#else
+#include <platforms/ros/eigen_math.h>
+#endif
+
+#include <platforms/px4_defines.h>
 
 namespace math
 {
@@ -65,7 +72,12 @@ public:
 	/**
 	 * struct for using arm_math functions, represents column vector
 	 */
+	#ifdef CONFIG_ARCH_ARM
 	arm_matrix_instance_f32 arm_col;
+	#else
+	eigen_matrix_instance arm_col;
+	#endif
+
 
 	/**
 	 * trivial ctor
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index b0086676a..86f59f0e6 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -592,7 +592,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					R = R_decl * R_body;
 
 					/* copy rotation matrix */
-					memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R));
+					memcpy(&att.R[0], &R.data[0][0], sizeof(att.R));
 					att.R_valid = true;
 
 					if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp
index 4580b338d..b267209fe 100644
--- a/src/modules/bottle_drop/bottle_drop.cpp
+++ b/src/modules/bottle_drop/bottle_drop.cpp
@@ -57,8 +57,13 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/wind_estimate.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_global_position.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 247a2c5b8..8f491acae 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -74,6 +74,10 @@
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/differential_pressure.h>
@@ -364,31 +368,31 @@ void print_status()
 	const char *armed_str;
 
 	switch (state.arming_state) {
-	case ARMING_STATE_INIT:
+	case vehicle_status_s::ARMING_STATE_INIT:
 		armed_str = "INIT";
 		break;
 
-	case ARMING_STATE_STANDBY:
+	case vehicle_status_s::ARMING_STATE_STANDBY:
 		armed_str = "STANDBY";
 		break;
 
-	case ARMING_STATE_ARMED:
+	case vehicle_status_s::ARMING_STATE_ARMED:
 		armed_str = "ARMED";
 		break;
 
-	case ARMING_STATE_ARMED_ERROR:
+	case vehicle_status_s::ARMING_STATE_ARMED_ERROR:
 		armed_str = "ARMED_ERROR";
 		break;
 
-	case ARMING_STATE_STANDBY_ERROR:
+	case vehicle_status_s::ARMING_STATE_STANDBY_ERROR:
 		armed_str = "STANDBY_ERROR";
 		break;
 
-	case ARMING_STATE_REBOOT:
+	case vehicle_status_s::ARMING_STATE_REBOOT:
 		armed_str = "REBOOT";
 		break;
 
-	case ARMING_STATE_IN_AIR_RESTORE:
+	case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE:
 		armed_str = "IN_AIR_RESTORE";
 		break;
 
@@ -411,7 +415,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
 
 	// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
 	// output appropriate error messages if the state cannot transition.
-	arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed,
+	arming_res = arming_state_transition(&status, &safety, arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, &armed,
 					     true /* fRunPreArmChecks */, mavlink_fd_local);
 
 	if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
@@ -448,7 +452,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 			transition_result_t main_ret = TRANSITION_NOT_CHANGED;
 
 			/* set HIL state */
-			hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
+			hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? vehicle_status_s::HIL_STATE_ON : vehicle_status_s::HIL_STATE_OFF;
 			transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd);
 
 			// Transition the arming state
@@ -458,43 +462,43 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 				/* use autopilot-specific mode */
 				if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
 					/* MANUAL */
-					main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL);
+					main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
 					/* ALTCTL */
-					main_ret = main_state_transition(status_local, MAIN_STATE_ALTCTL);
+					main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ALTCTL);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
 					/* POSCTL */
-					main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL);
+					main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
 					/* AUTO */
-					main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
+					main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
 					/* ACRO */
-					main_ret = main_state_transition(status_local, MAIN_STATE_ACRO);
+					main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ACRO);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
 					/* OFFBOARD */
-					main_ret = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
+					main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD);
 				}
 
 			} else {
 				/* use base mode */
 				if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
 					/* AUTO */
-					main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
+					main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION);
 
 				} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
 					if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
 						/* POSCTL */
-						main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL);
+						main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL);
 
 					} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
 						/* MANUAL */
-						main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL);
+						main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL);
 					}
 				}
 			}
@@ -521,7 +525,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 
 				// Flick to inair restore first if this comes from an onboard system
 				if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) {
-					status_local->arming_state = ARMING_STATE_IN_AIR_RESTORE;
+					status_local->arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE;
 				}
 
 				transition_result_t arming_res = arm_disarm(cmd_arms, mavlink_fd, "arm/disarm component command");
@@ -546,12 +550,12 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 			unsigned int mav_goto = (cmd->param1 + 0.5f);
 
 			if (mav_goto == 0) {	// MAV_GOTO_DO_HOLD
-				status_local->nav_state = NAVIGATION_STATE_AUTO_LOITER;
+				status_local->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
 				mavlink_log_critical(mavlink_fd, "Pause mission cmd");
 				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else if (mav_goto == 1) {	// MAV_GOTO_DO_CONTINUE
-				status_local->nav_state = NAVIGATION_STATE_AUTO_MISSION;
+				status_local->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
 				mavlink_log_critical(mavlink_fd, "Continue mission cmd");
 				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
@@ -664,14 +668,14 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 
 	case VEHICLE_CMD_NAV_GUIDED_ENABLE: {
 			transition_result_t res = TRANSITION_DENIED;
-			static main_state_t main_state_pre_offboard = MAIN_STATE_MANUAL;
+			static main_state_t main_state_pre_offboard =vehicle_status_s::MAIN_STATE_MANUAL;
 
-			if (status_local->main_state != MAIN_STATE_OFFBOARD) {
+			if (status_local->main_state !=vehicle_status_s::MAIN_STATE_OFFBOARD) {
 				main_state_pre_offboard = status_local->main_state;
 			}
 
 			if (cmd->param1 > 0.5f) {
-				res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
+				res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD);
 
 				if (res == TRANSITION_DENIED) {
 					print_reject_mode(status_local, "OFFBOARD");
@@ -794,41 +798,41 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_ef_time_thres = param_find("COM_EF_TIME");
 	param_t _param_autostart_id = param_find("SYS_AUTOSTART");
 
-	const char *main_states_str[MAIN_STATE_MAX];
-	main_states_str[MAIN_STATE_MANUAL]			= "MANUAL";
-	main_states_str[MAIN_STATE_ALTCTL]			= "ALTCTL";
-	main_states_str[MAIN_STATE_POSCTL]			= "POSCTL";
-	main_states_str[MAIN_STATE_AUTO_MISSION]		= "AUTO_MISSION";
-	main_states_str[MAIN_STATE_AUTO_LOITER]			= "AUTO_LOITER";
-	main_states_str[MAIN_STATE_AUTO_RTL]			= "AUTO_RTL";
-	main_states_str[MAIN_STATE_ACRO]			= "ACRO";
-	main_states_str[MAIN_STATE_OFFBOARD]			= "OFFBOARD";
-
-	const char *arming_states_str[ARMING_STATE_MAX];
-	arming_states_str[ARMING_STATE_INIT]			= "INIT";
-	arming_states_str[ARMING_STATE_STANDBY]			= "STANDBY";
-	arming_states_str[ARMING_STATE_ARMED]			= "ARMED";
-	arming_states_str[ARMING_STATE_ARMED_ERROR]		= "ARMED_ERROR";
-	arming_states_str[ARMING_STATE_STANDBY_ERROR]		= "STANDBY_ERROR";
-	arming_states_str[ARMING_STATE_REBOOT]			= "REBOOT";
-	arming_states_str[ARMING_STATE_IN_AIR_RESTORE]		= "IN_AIR_RESTORE";
-
-	const char *nav_states_str[NAVIGATION_STATE_MAX];
-	nav_states_str[NAVIGATION_STATE_MANUAL]			= "MANUAL";
-	nav_states_str[NAVIGATION_STATE_ALTCTL]			= "ALTCTL";
-	nav_states_str[NAVIGATION_STATE_POSCTL]			= "POSCTL";
-	nav_states_str[NAVIGATION_STATE_AUTO_MISSION]		= "AUTO_MISSION";
-	nav_states_str[NAVIGATION_STATE_AUTO_LOITER]		= "AUTO_LOITER";
-	nav_states_str[NAVIGATION_STATE_AUTO_RTL]		= "AUTO_RTL";
-	nav_states_str[NAVIGATION_STATE_AUTO_RCRECOVER]		= "AUTO_RCRECOVER";
-	nav_states_str[NAVIGATION_STATE_AUTO_RTGS]		= "AUTO_RTGS";
-	nav_states_str[NAVIGATION_STATE_AUTO_LANDENGFAIL]	= "AUTO_LANDENGFAIL";
-	nav_states_str[NAVIGATION_STATE_AUTO_LANDGPSFAIL]	= "AUTO_LANDGPSFAIL";
-	nav_states_str[NAVIGATION_STATE_ACRO]			= "ACRO";
-	nav_states_str[NAVIGATION_STATE_LAND]			= "LAND";
-	nav_states_str[NAVIGATION_STATE_DESCEND]		= "DESCEND";
-	nav_states_str[NAVIGATION_STATE_TERMINATION]		= "TERMINATION";
-	nav_states_str[NAVIGATION_STATE_OFFBOARD]		= "OFFBOARD";
+	const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX];
+	main_states_str[vehicle_status_s::MAIN_STATE_MANUAL]			= "MANUAL";
+	main_states_str[vehicle_status_s::MAIN_STATE_ALTCTL]			= "ALTCTL";
+	main_states_str[vehicle_status_s::MAIN_STATE_POSCTL]			= "POSCTL";
+	main_states_str[vehicle_status_s::MAIN_STATE_AUTO_MISSION]		= "AUTO_MISSION";
+	main_states_str[vehicle_status_s::MAIN_STATE_AUTO_LOITER]			= "AUTO_LOITER";
+	main_states_str[vehicle_status_s::MAIN_STATE_AUTO_RTL]			= "AUTO_RTL";
+	main_states_str[vehicle_status_s::MAIN_STATE_ACRO]			= "ACRO";
+	main_states_str[vehicle_status_s::MAIN_STATE_OFFBOARD]			= "OFFBOARD";
+
+	const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX];
+	arming_states_str[vehicle_status_s::ARMING_STATE_INIT]			= "INIT";
+	arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY]			= "STANDBY";
+	arming_states_str[vehicle_status_s::ARMING_STATE_ARMED]			= "ARMED";
+	arming_states_str[vehicle_status_s::ARMING_STATE_ARMED_ERROR]		= "ARMED_ERROR";
+	arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY_ERROR]		= "STANDBY_ERROR";
+	arming_states_str[vehicle_status_s::ARMING_STATE_REBOOT]			= "REBOOT";
+	arming_states_str[vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE]		= "IN_AIR_RESTORE";
+
+	const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX];
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL]			= "MANUAL";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL]			= "ALTCTL";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL]			= "POSCTL";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION]		= "AUTO_MISSION";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER]		= "AUTO_LOITER";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTL]		= "AUTO_RTL";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER]		= "AUTO_RCRECOVER";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS]		= "AUTO_RTGS";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL]	= "AUTO_LANDENGFAIL";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL]	= "AUTO_LANDGPSFAIL";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_ACRO]			= "ACRO";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_LAND]			= "LAND";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_DESCEND]		= "DESCEND";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_TERMINATION]		= "TERMINATION";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_OFFBOARD]		= "OFFBOARD";
 
 	/* pthread for slow low prio thread */
 	pthread_t commander_low_prio_thread;
@@ -849,10 +853,10 @@ int commander_thread_main(int argc, char *argv[])
 	status.condition_landed = true;	// initialize to safe value
 	// We want to accept RC inputs as default
 	status.rc_input_blocked = false;
-	status.main_state = MAIN_STATE_MANUAL;
-	status.nav_state = NAVIGATION_STATE_MANUAL;
-	status.arming_state = ARMING_STATE_INIT;
-	status.hil_state = HIL_STATE_OFF;
+	status.main_state =vehicle_status_s::MAIN_STATE_MANUAL;
+	status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
+	status.arming_state = vehicle_status_s::ARMING_STATE_INIT;
+	status.hil_state = vehicle_status_s::HIL_STATE_OFF;
 	status.failsafe = false;
 
 	/* neither manual nor offboard control commands have been received */
@@ -865,7 +869,7 @@ int commander_thread_main(int argc, char *argv[])
 	status.data_link_lost = true;
 
 	/* set battery warning flag */
-	status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
+	status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_NONE;
 	status.condition_battery_voltage_valid = false;
 
 	// XXX for now just set sensors as initialized
@@ -1139,14 +1143,14 @@ int commander_thread_main(int argc, char *argv[])
 				}
 
 				/* disable manual override for all systems that rely on electronic stabilization */
-				if (status.system_type == VEHICLE_TYPE_COAXIAL ||
-				    status.system_type == VEHICLE_TYPE_HELICOPTER ||
-				    status.system_type == VEHICLE_TYPE_TRICOPTER ||
-				    status.system_type == VEHICLE_TYPE_QUADROTOR ||
-				    status.system_type == VEHICLE_TYPE_HEXAROTOR ||
-				    status.system_type == VEHICLE_TYPE_OCTOROTOR ||
-				    (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) ||
-				    (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) {
+				if (status.system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL ||
+				    status.system_type == vehicle_status_s::VEHICLE_TYPE_HELICOPTER ||
+				    status.system_type == vehicle_status_s::VEHICLE_TYPE_TRICOPTER ||
+				    status.system_type == vehicle_status_s::VEHICLE_TYPE_QUADROTOR ||
+				    status.system_type == vehicle_status_s::VEHICLE_TYPE_HEXAROTOR ||
+				    status.system_type == vehicle_status_s::VEHICLE_TYPE_OCTOROTOR ||
+				    (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) ||
+				    (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) {
 
 					status.is_rotary_wing = true;
 
@@ -1155,8 +1159,8 @@ int commander_thread_main(int argc, char *argv[])
 				}
 
 				/* set vehicle_status.is_vtol flag */
-				status.is_vtol = (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) ||
-						 (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR);
+				status.is_vtol = (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR) ||
+						 (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR);
 
 				/* check and update system / component ID */
 				param_get(_param_system_id, &(status.system_id));
@@ -1306,9 +1310,9 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(safety), safety_sub, &safety);
 
 			/* disarm if safety is now on and still armed */
-			if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
-				arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY :
-								   ARMING_STATE_STANDBY_ERROR);
+			if (status.hil_state == vehicle_status_s::HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
+				arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY :
+								   vehicle_status_s::ARMING_STATE_STANDBY_ERROR);
 
 				if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed,
 						true /* fRunPreArmChecks */, mavlink_fd)) {
@@ -1340,7 +1344,7 @@ int commander_thread_main(int argc, char *argv[])
 			status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab;
 
 			/* Make sure that this is only adjusted if vehicle realy is of type vtol*/
-			if ((status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR)) {
+			if ((status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR)) {
 				status.is_rotary_wing = vtol_status.vtol_in_rw_mode;
 			}
 		}
@@ -1512,7 +1516,7 @@ int commander_thread_main(int argc, char *argv[])
 		if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) {
 			low_battery_voltage_actions_done = true;
 			mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
-			status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
+			status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW;
 			status_changed = true;
 
 		} else if (!on_usb_power && status.condition_battery_voltage_valid && status.battery_remaining < 0.09f
@@ -1520,10 +1524,10 @@ int commander_thread_main(int argc, char *argv[])
 			/* critical battery voltage, this is rather an emergency, change state machine */
 			critical_battery_voltage_actions_done = true;
 			mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
-			status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
+			status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL;
 
 			if (armed.armed) {
-				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */,
+				arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */,
 								     mavlink_fd);
 
 				if (arming_ret == TRANSITION_CHANGED) {
@@ -1531,7 +1535,7 @@ int commander_thread_main(int argc, char *argv[])
 				}
 
 			} else {
-				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */,
+				arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */,
 								     mavlink_fd);
 
 				if (arming_ret == TRANSITION_CHANGED) {
@@ -1545,9 +1549,9 @@ int commander_thread_main(int argc, char *argv[])
 		/* End battery voltage check */
 
 		/* If in INIT state, try to proceed to STANDBY state */
-		if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
+		if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
 			/* TODO: check for sensors */
-			arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */,
+			arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */,
 							     mavlink_fd);
 
 			if (arming_ret == TRANSITION_CHANGED) {
@@ -1658,14 +1662,14 @@ int commander_thread_main(int argc, char *argv[])
 			/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm
 			 * do it only for rotary wings */
 			if (status.is_rotary_wing &&
-			    (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
-			    (status.main_state == MAIN_STATE_MANUAL || status.main_state == MAIN_STATE_ACRO || status.condition_landed) &&
+			    (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) &&
+			    (status.main_state == vehicle_status_s::MAIN_STATE_MANUAL || status.main_state == vehicle_status_s::MAIN_STATE_ACRO || status.condition_landed) &&
 			    sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
 
 				if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 					/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
-					arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY :
-									   ARMING_STATE_STANDBY_ERROR);
+					arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY :
+									   vehicle_status_s::ARMING_STATE_STANDBY_ERROR);
 					arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */,
 									     mavlink_fd);
 
@@ -1684,7 +1688,7 @@ int commander_thread_main(int argc, char *argv[])
 			}
 
 			/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
-			if (status.arming_state == ARMING_STATE_STANDBY &&
+			if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY &&
 			    sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
 				if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 
@@ -1692,11 +1696,11 @@ int commander_thread_main(int argc, char *argv[])
 					 * for being in manual mode only applies to manual arming actions.
 					 * the system can be armed in auto if armed via the GCS.
 					 */
-					if (status.main_state != MAIN_STATE_MANUAL) {
+					if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL) {
 						print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
 
 					} else {
-						arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */,
+						arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */,
 										     mavlink_fd);
 
 						if (arming_ret == TRANSITION_CHANGED) {
@@ -1715,7 +1719,7 @@ int commander_thread_main(int argc, char *argv[])
 			}
 
 			if (arming_ret == TRANSITION_CHANGED) {
-				if (status.arming_state == ARMING_STATE_ARMED) {
+				if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
 					mavlink_log_info(mavlink_fd, "ARMED by RC");
 
 				} else {
@@ -1853,10 +1857,10 @@ int commander_thread_main(int argc, char *argv[])
 			/* At this point the data link and the gps system have been checked
 			 * If  we are not in a manual (RC stick controlled mode)
 			 * and both failed we want to terminate the flight */
-			if (status.main_state != MAIN_STATE_MANUAL &&
-			    status.main_state != MAIN_STATE_ACRO &&
-			    status.main_state != MAIN_STATE_ALTCTL &&
-			    status.main_state != MAIN_STATE_POSCTL &&
+			if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL &&
+			    status.main_state !=vehicle_status_s::MAIN_STATE_ACRO &&
+			    status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL &&
+			    status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL &&
 			    ((status.data_link_lost && status.gps_failure) ||
 			     (status.data_link_lost_cmd && status.gps_failure_cmd))) {
 				armed.force_failsafe = true;
@@ -1877,10 +1881,10 @@ int commander_thread_main(int argc, char *argv[])
 			/* At this point the rc signal and the gps system have been checked
 			 * If we are in manual (controlled with RC):
 			 * if both failed we want to terminate the flight */
-			if ((status.main_state == MAIN_STATE_ACRO ||
-			     status.main_state == MAIN_STATE_MANUAL ||
-			     status.main_state == MAIN_STATE_ALTCTL ||
-			     status.main_state == MAIN_STATE_POSCTL) &&
+			if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO ||
+			     status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL ||
+			     status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL ||
+			     status.main_state ==vehicle_status_s::MAIN_STATE_POSCTL) &&
 			    ((status.rc_signal_lost && status.gps_failure) ||
 			     (status.rc_signal_lost_cmd && status.gps_failure_cmd))) {
 				armed.force_failsafe = true;
@@ -1972,11 +1976,11 @@ int commander_thread_main(int argc, char *argv[])
 			set_tune(TONE_ARMING_WARNING_TUNE);
 			arm_tune_played = true;
 
-		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
+		} else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
 			/* play tune on battery critical */
 			set_tune(TONE_BATTERY_WARNING_FAST_TUNE);
 
-		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe) {
+		} else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status.failsafe) {
 			/* play tune on battery warning or failsafe */
 			set_tune(TONE_BATTERY_WARNING_SLOW_TUNE);
 
@@ -2068,15 +2072,15 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
 		bool set_normal_color = false;
 
 		/* set mode */
-		if (status_local->arming_state == ARMING_STATE_ARMED) {
+		if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
 			rgbled_set_mode(RGBLED_MODE_ON);
 			set_normal_color = true;
 
-		} else if (status_local->arming_state == ARMING_STATE_ARMED_ERROR) {
+		} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) {
 			rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
 			rgbled_set_color(RGBLED_COLOR_RED);
 
-		} else if (status_local->arming_state == ARMING_STATE_STANDBY) {
+		} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
 			rgbled_set_mode(RGBLED_MODE_BREATHE);
 			set_normal_color = true;
 
@@ -2087,9 +2091,9 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
 
 		if (set_normal_color) {
 			/* set color */
-			if (status_local->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) {
+			if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) {
 				rgbled_set_color(RGBLED_COLOR_AMBER);
-				/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
+				/* vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL handled as vehicle_status_s::ARMING_STATE_ARMED_ERROR / vehicle_status_s::ARMING_STATE_STANDBY_ERROR */
 
 			} else {
 				if (status_local->condition_local_position_valid) {
@@ -2145,12 +2149,12 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 
 	/* if offboard is set allready by a mavlink command, abort */
 	if (status.offboard_control_set_by_command) {
-		return main_state_transition(status_local, MAIN_STATE_OFFBOARD);
+		return main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD);
 	}
 
 	/* offboard switch overrides main switch */
-	if (sp_man->offboard_switch == SWITCH_POS_ON) {
-		res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
+	if (sp_man->offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
+		res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD);
 
 		if (res == TRANSITION_DENIED) {
 			print_reject_mode(status_local, "OFFBOARD");
@@ -2162,24 +2166,24 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 
 	/* offboard switched off or denied, check main mode switch */
 	switch (sp_man->mode_switch) {
-	case SWITCH_POS_NONE:
+	case manual_control_setpoint_s::SWITCH_POS_NONE:
 		res = TRANSITION_NOT_CHANGED;
 		break;
 
-	case SWITCH_POS_OFF:		// MANUAL
-		if (sp_man->acro_switch == SWITCH_POS_ON) {
-			res = main_state_transition(status_local, MAIN_STATE_ACRO);
+	case manual_control_setpoint_s::SWITCH_POS_OFF:		// MANUAL
+		if (sp_man->acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
+			res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO);
 
 		} else {
-			res = main_state_transition(status_local, MAIN_STATE_MANUAL);
+			res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL);
 		}
 
 		// TRANSITION_DENIED is not possible here
 		break;
 
-	case SWITCH_POS_MIDDLE:		// ASSIST
-		if (sp_man->posctl_switch == SWITCH_POS_ON) {
-			res = main_state_transition(status_local, MAIN_STATE_POSCTL);
+	case manual_control_setpoint_s::SWITCH_POS_MIDDLE:		// ASSIST
+		if (sp_man->posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
+			res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL);
 
 			if (res != TRANSITION_DENIED) {
 				break;	// changed successfully or already in this state
@@ -2189,24 +2193,24 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 		}
 
 		// fallback to ALTCTL
-		res = main_state_transition(status_local, MAIN_STATE_ALTCTL);
+		res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL);
 
 		if (res != TRANSITION_DENIED) {
 			break;	// changed successfully or already in this mode
 		}
 
-		if (sp_man->posctl_switch != SWITCH_POS_ON) {
+		if (sp_man->posctl_switch != manual_control_setpoint_s::SWITCH_POS_ON) {
 			print_reject_mode(status_local, "ALTCTL");
 		}
 
 		// fallback to MANUAL
-		res = main_state_transition(status_local, MAIN_STATE_MANUAL);
+		res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL);
 		// TRANSITION_DENIED is not possible here
 		break;
 
-	case SWITCH_POS_ON:			// AUTO
-		if (sp_man->return_switch == SWITCH_POS_ON) {
-			res = main_state_transition(status_local, MAIN_STATE_AUTO_RTL);
+	case manual_control_setpoint_s::SWITCH_POS_ON:			// AUTO
+		if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
+			res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_RTL);
 
 			if (res != TRANSITION_DENIED) {
 				break;	// changed successfully or already in this state
@@ -2215,14 +2219,14 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 			print_reject_mode(status_local, "AUTO_RTL");
 
 			// fallback to LOITER if home position not set
-			res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
+			res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER);
 
 			if (res != TRANSITION_DENIED) {
 				break;  // changed successfully or already in this state
 			}
 
-		} else if (sp_man->loiter_switch == SWITCH_POS_ON) {
-			res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
+		} else if (sp_man->loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
+			res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER);
 
 			if (res != TRANSITION_DENIED) {
 				break;	// changed successfully or already in this state
@@ -2231,7 +2235,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 			print_reject_mode(status_local, "AUTO_LOITER");
 
 		} else {
-			res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
+			res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_MISSION);
 
 			if (res != TRANSITION_DENIED) {
 				break;	// changed successfully or already in this state
@@ -2240,7 +2244,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 			print_reject_mode(status_local, "AUTO_MISSION");
 
 			// fallback to LOITER if home position not set
-			res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
+			res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER);
 
 			if (res != TRANSITION_DENIED) {
 				break;  // changed successfully or already in this state
@@ -2248,21 +2252,21 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 		}
 
 		// fallback to POSCTL
-		res = main_state_transition(status_local, MAIN_STATE_POSCTL);
+		res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL);
 
 		if (res != TRANSITION_DENIED) {
 			break;  // changed successfully or already in this state
 		}
 
 		// fallback to ALTCTL
-		res = main_state_transition(status_local, MAIN_STATE_ALTCTL);
+		res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL);
 
 		if (res != TRANSITION_DENIED) {
 			break;	// changed successfully or already in this state
 		}
 
 		// fallback to MANUAL
-		res = main_state_transition(status_local, MAIN_STATE_MANUAL);
+		res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL);
 		// TRANSITION_DENIED is not possible here
 		break;
 
@@ -2279,11 +2283,11 @@ set_control_mode()
 	/* set vehicle_control_mode according to set_navigation_state */
 	control_mode.flag_armed = armed.armed;
 	control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol);
-	control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON;
+	control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON;
 	control_mode.flag_control_offboard_enabled = false;
 
 	switch (status.nav_state) {
-	case NAVIGATION_STATE_MANUAL:
+	case vehicle_status_s::NAVIGATION_STATE_MANUAL:
 		control_mode.flag_control_manual_enabled = true;
 		control_mode.flag_control_auto_enabled = false;
 		control_mode.flag_control_rates_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab);
@@ -2295,7 +2299,7 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case NAVIGATION_STATE_ALTCTL:
+	case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
 		control_mode.flag_control_manual_enabled = true;
 		control_mode.flag_control_auto_enabled = false;
 		control_mode.flag_control_rates_enabled = true;
@@ -2307,7 +2311,7 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case NAVIGATION_STATE_POSCTL:
+	case vehicle_status_s::NAVIGATION_STATE_POSCTL:
 		control_mode.flag_control_manual_enabled = true;
 		control_mode.flag_control_auto_enabled = false;
 		control_mode.flag_control_rates_enabled = true;
@@ -2319,12 +2323,12 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case NAVIGATION_STATE_AUTO_MISSION:
-	case NAVIGATION_STATE_AUTO_LOITER:
-	case NAVIGATION_STATE_AUTO_RTL:
-	case NAVIGATION_STATE_AUTO_RCRECOVER:
-	case NAVIGATION_STATE_AUTO_RTGS:
-	case NAVIGATION_STATE_AUTO_LANDENGFAIL:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = true;
 		control_mode.flag_control_rates_enabled = true;
@@ -2336,7 +2340,7 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = false;
 		control_mode.flag_control_rates_enabled = true;
@@ -2348,7 +2352,7 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case NAVIGATION_STATE_ACRO:
+	case vehicle_status_s::NAVIGATION_STATE_ACRO:
 		control_mode.flag_control_manual_enabled = true;
 		control_mode.flag_control_auto_enabled = false;
 		control_mode.flag_control_rates_enabled = true;
@@ -2361,7 +2365,7 @@ set_control_mode()
 		break;
 
 
-	case NAVIGATION_STATE_LAND:
+	case vehicle_status_s::NAVIGATION_STATE_LAND:
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = true;
 		control_mode.flag_control_rates_enabled = true;
@@ -2374,7 +2378,7 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case NAVIGATION_STATE_DESCEND:
+	case vehicle_status_s::NAVIGATION_STATE_DESCEND:
 		/* TODO: check if this makes sense */
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = true;
@@ -2387,7 +2391,7 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case NAVIGATION_STATE_TERMINATION:
+	case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
 		/* disable all controllers on termination */
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = false;
@@ -2400,7 +2404,7 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = true;
 		break;
 
-	case NAVIGATION_STATE_OFFBOARD:
+	case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = false;
 		control_mode.flag_control_offboard_enabled = true;
@@ -2599,7 +2603,7 @@ void *commander_low_prio_loop(void *arg)
 				int calib_ret = ERROR;
 
 				/* try to go to INIT/PREFLIGHT arming state */
-				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed,
+				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed,
 						true /* fRunPreArmChecks */, mavlink_fd)) {
 					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
 					break;
@@ -2663,7 +2667,7 @@ void *commander_low_prio_loop(void *arg)
 					tune_negative(true);
 				}
 
-				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
+				arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 
 				break;
 			}
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index 2022e99fb..8a4451100 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -72,16 +72,16 @@ static const int ERROR = -1;
 
 bool is_multirotor(const struct vehicle_status_s *current_status)
 {
-	return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
-		(current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
-		(current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
-		(current_status->system_type == VEHICLE_TYPE_TRICOPTER));
+	return ((current_status->system_type == vehicle_status_s::VEHICLE_TYPE_QUADROTOR) ||
+		(current_status->system_type == vehicle_status_s::VEHICLE_TYPE_HEXAROTOR) ||
+		(current_status->system_type == vehicle_status_s::VEHICLE_TYPE_OCTOROTOR) ||
+		(current_status->system_type == vehicle_status_s::VEHICLE_TYPE_TRICOPTER));
 }
 
 bool is_rotary_wing(const struct vehicle_status_s *current_status)
 {
-	return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER)
-	       || (current_status->system_type == VEHICLE_TYPE_COAXIAL);
+	return is_multirotor(current_status) || (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_HELICOPTER)
+	       || (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL);
 }
 
 static int buzzer = -1;
diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
index 3cfa8b4c6..4ddb4e0fb 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
@@ -73,7 +73,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
         bool            armed;          // actuator_armed_s.armed
         bool            ready_to_arm;   // actuator_armed_s.ready_to_arm
     } ArmingTransitionVolatileState_t;
-    
+
     // This structure represents a test case for arming_state_transition. It contains the machine
     // state prior to transtion, the requested state to transition to and finally the expected
     // machine state after transition.
@@ -88,7 +88,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
         ArmingTransitionVolatileState_t expected_state;                         // Expected machine state after transition
         transition_result_t             expected_transition_result;             // Expected result from arming_state_transition
     } ArmingTransitionTest_t;
-    
+
     // We use these defines so that our test cases are more readable
     #define ATT_ARMED true
     #define ATT_DISARMED false
@@ -100,182 +100,182 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
     #define ATT_SAFETY_NOT_AVAILABLE true
     #define ATT_SAFETY_OFF true
     #define ATT_SAFETY_ON false
-    
+
     // These are test cases for arming_state_transition
     static const ArmingTransitionTest_t rgArmingTransitionTests[] = {
         // TRANSITION_NOT_CHANGED tests
-        
+
         { "no transition: identical states",
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_INIT,
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_INIT,
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED },
+
         // TRANSITION_CHANGED tests
-        
+
         // Check all basic valid transitions, these don't require special state in vehicle_status_t or safety_s
-        
+
         { "transition: init to standby",
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY,
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY,
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: init to standby error",
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY_ERROR,
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: init to reboot",
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_REBOOT,
-            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_REBOOT,
+            { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: standby to init",
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_INIT,
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_INIT,
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: standby to standby error",
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY_ERROR,
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: standby to reboot",
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_REBOOT,
-            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_REBOOT,
+            { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: armed to standby",
-            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY,
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY,
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: armed to armed error",
-            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_ARMED_ERROR,
-            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_ARMED_ERROR,
+            { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: armed error to standby error",
-            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY_ERROR,
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: standby error to reboot",
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_REBOOT,
-            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_REBOOT,
+            { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: in air restore to armed",
-            { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_ARMED,
-            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_ARMED,
+            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: in air restore to reboot",
-            { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_REBOOT,
-            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_REBOOT,
+            { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         // hil on tests, standby error to standby not normally allowed
-        
+
         { "transition: standby error to standby, hil on",
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY,
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY,
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         // Safety switch arming tests
-        
+
         { "transition: standby to armed, no safety switch",
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
-            ARMING_STATE_ARMED,
-            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
+            vehicle_status_s::ARMING_STATE_ARMED,
+            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: standby to armed, safety switch off",
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
-            ARMING_STATE_ARMED,
-            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
+            vehicle_status_s::ARMING_STATE_ARMED,
+            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         // standby error
         { "transition: armed error to standby error requested standby",
-            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY,
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY,
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         // TRANSITION_DENIED tests
-        
+
         // Check some important basic invalid transitions, these don't require special state in vehicle_status_t or safety_s
-        
+
         { "no transition: init to armed",
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_ARMED,
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_ARMED,
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+
         { "no transition: standby to armed error",
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_ARMED_ERROR,
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_ARMED_ERROR,
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
+
         { "no transition: armed to init",
-            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_INIT,
-            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_INIT,
+            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
+
         { "no transition: armed to reboot",
-            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_REBOOT,
-            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_REBOOT,
+            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
+
         { "no transition: armed error to armed",
-            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_ARMED,
-            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_ARMED,
+            { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+
         { "no transition: armed error to reboot",
-            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_REBOOT,
-            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_REBOOT,
+            { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+
         { "no transition: standby error to armed",
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_ARMED,
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_ARMED,
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+
         { "no transition: standby error to standby",
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY,
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY,
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+
         { "no transition: reboot to armed",
-            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_ARMED,
-            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_ARMED,
+            { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+
         { "no transition: in air restore to standby",
-            { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY,
-            { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY,
+            { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+
         // Sensor tests
-        
+
         { "no transition: init to standby - sensors not initialized",
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY,
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY,
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+
         // Safety switch arming tests
-        
+
         { "no transition: init to standby, safety switch on",
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_ARMED,
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_ARMED,
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
     };
-    
+
 	struct vehicle_status_s status;
 	struct safety_s         safety;
 	struct actuator_armed_s armed;
-    
+
     size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
     for (size_t i=0; i<cArmingTransitionTests; i++) {
         const ArmingTransitionTest_t* test = &rgArmingTransitionTests[i];
-        
+
         // Setup initial machine state
         status.arming_state = test->current_state.arming_state;
         status.condition_system_sensors_initialized = test->condition_system_sensors_initialized;
@@ -286,10 +286,10 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
         safety.safety_off = test->safety_off;
         armed.armed = test->current_state.armed;
         armed.ready_to_arm = test->current_state.ready_to_arm;
-        
+
         // Attempt transition
         transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, 0 /* no mavlink_fd */);
-        
+
         // Validate result of transition
         ut_assert(test->assertMsg, test->expected_transition_result == result);
         ut_assert(test->assertMsg, status.arming_state == test->expected_state.arming_state);
@@ -310,7 +310,7 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
 		main_state_t	to_state;				// State to transition to
 		transition_result_t	expected_transition_result;	// Expected result from main_state_transition call
 	} MainTransitionTest_t;
-	
+
 	// Bits for condition_bits
 	#define MTT_ALL_NOT_VALID		0
 	#define MTT_ROTARY_WING			1 << 0
@@ -318,108 +318,108 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
 	#define MTT_LOC_POS_VALID		1 << 2
 	#define MTT_HOME_POS_VALID		1 << 3
 	#define MTT_GLOBAL_POS_VALID		1 << 4
-	
+
 	static const MainTransitionTest_t rgMainTransitionTests[] = {
-		
+
 		// TRANSITION_NOT_CHANGED tests
-		
+
 		{ "no transition: identical states",
 			MTT_ALL_NOT_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED },
-				
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED },
+
 		// TRANSITION_CHANGED tests
-		
+
 		{ "transition: MANUAL to ACRO",
 			MTT_ALL_NOT_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_ACRO, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ACRO, TRANSITION_CHANGED },
 
 		{ "transition: ACRO to MANUAL",
 			MTT_ALL_NOT_VALID,
-			MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_ACRO, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to AUTO_MISSION - global position valid, home position valid",
 			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
 
 		{ "transition: AUTO_MISSION to MANUAL - global position valid, home position valid",
 			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
-			MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_AUTO_MISSION, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to AUTO_LOITER - global position valid",
 			MTT_GLOBAL_POS_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED },
 
 		{ "transition: AUTO_LOITER to MANUAL - global position valid",
 			MTT_GLOBAL_POS_VALID,
-			MAIN_STATE_AUTO_LOITER, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_AUTO_LOITER, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to AUTO_RTL - global position valid, home position valid",
 			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED },
 
 		{ "transition: AUTO_RTL to MANUAL - global position valid, home position valid",
 			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
-			MAIN_STATE_AUTO_RTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_AUTO_RTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to ALTCTL - not rotary",
 			MTT_ALL_NOT_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid",
 			MTT_ROTARY_WING | MTT_LOC_ALT_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
-		
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
+
 		{ "transition: MANUAL to ALTCTL - rotary, global position valid, local altitude not valid",
 			MTT_ROTARY_WING | MTT_GLOBAL_POS_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
-		
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
+
 		{ "transition: ALTCTL to MANUAL - local altitude valid",
 			MTT_LOC_ALT_VALID,
-			MAIN_STATE_ALTCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_ALTCTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to POSCTL - local position not valid, global position valid",
 			MTT_GLOBAL_POS_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to POSCTL - local position valid, global position not valid",
 			MTT_LOC_POS_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED },
 
 		{ "transition: POSCTL to MANUAL - local position valid, global position valid",
 			MTT_LOC_POS_VALID,
-			MAIN_STATE_POSCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_POSCTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		// TRANSITION_DENIED tests
 
 		{ "no transition: MANUAL to AUTO_MISSION - global position not valid",
 			MTT_ALL_NOT_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED },
-		
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED },
+
 		{ "no transition: MANUAL to AUTO_LOITER - global position not valid",
 			MTT_ALL_NOT_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED },
-		
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED },
+
 		{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position not valid",
 			MTT_ALL_NOT_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
-		
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
+
 		{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position valid",
 			MTT_HOME_POS_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
 
 		{ "no transition: MANUAL to AUTO_RTL - global position valid, home position not valid",
 			MTT_GLOBAL_POS_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
-		
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
+
 		{ "no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid",
 			MTT_ROTARY_WING,
-			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_DENIED },
-		
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED },
+
 		{ "no transition: MANUAL to POSCTL - local position not valid, global position not valid",
 			MTT_ALL_NOT_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_DENIED },
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_DENIED },
 	};
-	
+
 	size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]);
 	for (size_t i=0; i<cMainTransitionTests; i++) {
 		const MainTransitionTest_t* test = &rgMainTransitionTests[i];
@@ -432,10 +432,10 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
 		current_state.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID;
 		current_state.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID;
 		current_state.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID;
-		
+
 		// Attempt transition
 		transition_result_t result = main_state_transition(&current_state, test->to_state);
-		
+
 		// Validate result of transition
 		ut_assert(test->assertMsg, test->expected_transition_result == result);
 		if (test->expected_transition_result == result) {
@@ -495,8 +495,8 @@ bool StateMachineHelperTest::run_tests(void)
 	ut_run_test(armingStateTransitionTest);
 	ut_run_test(mainStateTransitionTest);
 	ut_run_test(isSafeTest);
-	
+
 	return (_tests_failed == 0);
 }
 
-ut_declare_test(stateMachineHelperTest, StateMachineHelperTest)
\ No newline at end of file
+ut_declare_test(stateMachineHelperTest, StateMachineHelperTest)
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 465f9cdc5..40da9c77b 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -76,19 +76,19 @@ static const int ERROR = -1;
 // will be true for a valid transition or false for a invalid transition. In some cases even
 // though the transition is marked as true additional checks must be made. See arming_state_transition
 // code for those checks.
-static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = {
+static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = {
 	//                                  INIT,  STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
-	{ /* ARMING_STATE_INIT */           true,  true,    false, false,       false,         false,  false },
-	{ /* ARMING_STATE_STANDBY */        true,  true,    true,  true,        false,         false,  false },
-	{ /* ARMING_STATE_ARMED */          false, true,    true,  false,       false,         false,  true },
-	{ /* ARMING_STATE_ARMED_ERROR */    false, false,   true,  true,        false,         false,  false },
-	{ /* ARMING_STATE_STANDBY_ERROR */  true,  true,    false, true,        true,          false,  false },
-	{ /* ARMING_STATE_REBOOT */         true,  true,    false, false,       true,          true,   true },
-	{ /* ARMING_STATE_IN_AIR_RESTORE */ false, false,   false, false,       false,         false,  false }, // NYI
+	{ /* vehicle_status_s::ARMING_STATE_INIT */           true,  true,    false, false,       false,         false,  false },
+	{ /* vehicle_status_s::ARMING_STATE_STANDBY */        true,  true,    true,  true,        false,         false,  false },
+	{ /* vehicle_status_s::ARMING_STATE_ARMED */          false, true,    true,  false,       false,         false,  true },
+	{ /* vehicle_status_s::ARMING_STATE_ARMED_ERROR */    false, false,   true,  true,        false,         false,  false },
+	{ /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */  true,  true,    false, true,        true,          false,  false },
+	{ /* vehicle_status_s::ARMING_STATE_REBOOT */         true,  true,    false, false,       true,          true,   true },
+	{ /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false,   false, false,       false,         false,  false }, // NYI
 };
 
 // You can index into the array with an arming_state_t in order to get it's textual representation
-static const char * const state_names[ARMING_STATE_MAX] = {
+static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = {
 	"ARMING_STATE_INIT",
 	"ARMING_STATE_STANDBY",
 	"ARMING_STATE_ARMED",
@@ -107,8 +107,8 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 			const int               mavlink_fd)		///< mavlink fd for error reporting, 0 for none
 {
 	// Double check that our static arrays are still valid
-	ASSERT(ARMING_STATE_INIT == 0);
-	ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
+	ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0);
+	ASSERT(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1);
 
 	transition_result_t ret = TRANSITION_DENIED;
 	arming_state_t current_arming_state = status->arming_state;
@@ -126,7 +126,7 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 		int prearm_ret = OK;
 
 		/* only perform the check if we have to */
-		if (fRunPreArmChecks && new_arming_state == ARMING_STATE_ARMED) {
+		if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
 			prearm_ret = prearm_check(status, mavlink_fd);
 		}
 
@@ -136,7 +136,7 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 		irqstate_t flags = irqsave();
 
 		/* enforce lockdown in HIL */
-		if (status->hil_state == HIL_STATE_ON) {
+		if (status->hil_state == vehicle_status_s::HIL_STATE_ON) {
 			armed->lockdown = true;
 
 		} else {
@@ -148,12 +148,12 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 
 		if (valid_transition) {
 			// We have a good transition. Now perform any secondary validation.
-			if (new_arming_state == ARMING_STATE_ARMED) {
+			if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
 
 				//      Do not perform pre-arm checks if coming from in air restore
-				//      Allow if HIL_STATE_ON
-				if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE &&
-					status->hil_state == HIL_STATE_OFF) {
+				//      Allow if vehicle_status_s::HIL_STATE_ON
+				if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE &&
+					status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
 
 					// Fail transition if pre-arm check fails
 					if (prearm_ret) {
@@ -200,18 +200,18 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 
 				}
 
-			} else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) {
-				new_arming_state = ARMING_STATE_STANDBY_ERROR;
+			} else if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) {
+				new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
 			}
 		}
 
 		// HIL can always go to standby
-		if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) {
+		if (status->hil_state == vehicle_status_s::HIL_STATE_ON && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
 			valid_transition = true;
 		}
 
 		/* Sensors need to be initialized for STANDBY state */
-		if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
+		if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
 			mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational.");
 			feedback_provided = true;
 			valid_transition = false;
@@ -219,8 +219,8 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 
 		// Finish up the state transition
 		if (valid_transition) {
-			armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR;
-			armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY;
+			armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR;
+			armed->ready_to_arm = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY;
 			ret = TRANSITION_CHANGED;
 			status->arming_state = new_arming_state;
 		}
@@ -264,12 +264,12 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 
 	/* transition may be denied even if the same state is requested because conditions may have changed */
 	switch (new_main_state) {
-	case MAIN_STATE_MANUAL:
-	case MAIN_STATE_ACRO:
+	case vehicle_status_s::MAIN_STATE_MANUAL:
+	case vehicle_status_s::MAIN_STATE_ACRO:
 		ret = TRANSITION_CHANGED;
 		break;
 
-	case MAIN_STATE_ALTCTL:
+	case vehicle_status_s::MAIN_STATE_ALTCTL:
 		/* need at minimum altitude estimate */
 		/* TODO: add this for fixedwing as well */
 		if (!status->is_rotary_wing ||
@@ -279,7 +279,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 		}
 		break;
 
-	case MAIN_STATE_POSCTL:
+	case vehicle_status_s::MAIN_STATE_POSCTL:
 		/* need at minimum local position estimate */
 		if (status->condition_local_position_valid ||
 		    status->condition_global_position_valid) {
@@ -287,22 +287,22 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 		}
 		break;
 
-	case MAIN_STATE_AUTO_LOITER:
+	case vehicle_status_s::MAIN_STATE_AUTO_LOITER:
 		/* need global position estimate */
 		if (status->condition_global_position_valid) {
 			ret = TRANSITION_CHANGED;
 		}
 		break;
 
-	case MAIN_STATE_AUTO_MISSION:
-	case MAIN_STATE_AUTO_RTL:
+	case vehicle_status_s::MAIN_STATE_AUTO_MISSION:
+	case vehicle_status_s::MAIN_STATE_AUTO_RTL:
 		/* need global position and home position */
 		if (status->condition_global_position_valid && status->condition_home_position_valid) {
 			ret = TRANSITION_CHANGED;
 		}
 		break;
 
-	case MAIN_STATE_OFFBOARD:
+	case vehicle_status_s::MAIN_STATE_OFFBOARD:
 
 		/* need offboard signal */
 		if (!status->offboard_control_signal_lost) {
@@ -311,7 +311,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 
 		break;
 
-	case MAIN_STATE_MAX:
+	case vehicle_status_s::MAIN_STATE_MAX:
 	default:
 		break;
 	}
@@ -338,16 +338,16 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
 
 	} else {
 		switch (new_state) {
-		case HIL_STATE_OFF:
+		case vehicle_status_s::HIL_STATE_OFF:
 			/* we're in HIL and unexpected things can happen if we disable HIL now */
 			mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)");
 			ret = TRANSITION_DENIED;
 			break;
 
-		case HIL_STATE_ON:
-			if (current_status->arming_state == ARMING_STATE_INIT
-			    || current_status->arming_state == ARMING_STATE_STANDBY
-			    || current_status->arming_state == ARMING_STATE_STANDBY_ERROR) {
+		case vehicle_status_s::HIL_STATE_ON:
+			if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT
+			    || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY
+			    || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
 
 				/* Disable publication of all attached sensors */
 				/* list directory */
@@ -448,55 +448,55 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 {
 	navigation_state_t nav_state_old = status->nav_state;
 
-	bool armed = (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR);
+	bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
 	status->failsafe = false;
 
 	/* evaluate main state to decide in normal (non-failsafe) mode */
 	switch (status->main_state) {
-	case MAIN_STATE_ACRO:
-	case MAIN_STATE_MANUAL:
-	case MAIN_STATE_ALTCTL:
-	case MAIN_STATE_POSCTL:
+	case vehicle_status_s::MAIN_STATE_ACRO:
+	case vehicle_status_s::MAIN_STATE_MANUAL:
+	case vehicle_status_s::MAIN_STATE_ALTCTL:
+	case vehicle_status_s::MAIN_STATE_POSCTL:
 		/* require RC for all manual modes */
 		if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) {
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
 			} else if (status->condition_local_position_valid) {
-				status->nav_state = NAVIGATION_STATE_LAND;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
 			} else if (status->condition_local_altitude_valid) {
-				status->nav_state = NAVIGATION_STATE_DESCEND;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
 			} else {
-				status->nav_state = NAVIGATION_STATE_TERMINATION;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
 			}
 
 		} else {
 			switch (status->main_state) {
-			case MAIN_STATE_ACRO:
-				status->nav_state = NAVIGATION_STATE_ACRO;
+			case vehicle_status_s::MAIN_STATE_ACRO:
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO;
 				break;
 
-			case MAIN_STATE_MANUAL:
-				status->nav_state = NAVIGATION_STATE_MANUAL;
+			case vehicle_status_s::MAIN_STATE_MANUAL:
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
 				break;
 
-			case MAIN_STATE_ALTCTL:
-				status->nav_state = NAVIGATION_STATE_ALTCTL;
+			case vehicle_status_s::MAIN_STATE_ALTCTL:
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
 				break;
 
-			case MAIN_STATE_POSCTL:
-				status->nav_state = NAVIGATION_STATE_POSCTL;
+			case vehicle_status_s::MAIN_STATE_POSCTL:
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
 				break;
 
 			default:
-				status->nav_state = NAVIGATION_STATE_MANUAL;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
 				break;
 			}
 		}
 		break;
 
-	case MAIN_STATE_AUTO_MISSION:
+	case vehicle_status_s::MAIN_STATE_AUTO_MISSION:
 
 		/* go into failsafe
 		 * - if commanded to do so
@@ -505,19 +505,19 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 
 		/* first look at the commands */
 		if (status->engine_failure_cmd) {
-			status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
 		} else if (status->data_link_lost_cmd) {
-			status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
 		} else if (status->gps_failure_cmd) {
-			status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
 		} else if (status->rc_signal_lost_cmd) {
-			status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
 
 		/* finished handling commands which have priority, now handle failures */
 		} else if (status->gps_failure) {
-			status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
 		} else if (status->engine_failure) {
-			status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
 
 		/* datalink loss enabled:
 		 * check for datalink lost: this should always trigger RTGS */
@@ -525,13 +525,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
 			} else if (status->condition_local_position_valid) {
-				status->nav_state = NAVIGATION_STATE_LAND;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
 			} else if (status->condition_local_altitude_valid) {
-				status->nav_state = NAVIGATION_STATE_DESCEND;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
 			} else {
-				status->nav_state = NAVIGATION_STATE_TERMINATION;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
 			}
 
 		/* datalink loss disabled:
@@ -542,37 +542,37 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
 			} else if (status->condition_local_position_valid) {
-				status->nav_state = NAVIGATION_STATE_LAND;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
 			} else if (status->condition_local_altitude_valid) {
-				status->nav_state = NAVIGATION_STATE_DESCEND;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
 			} else {
-				status->nav_state = NAVIGATION_STATE_TERMINATION;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
 			}
 
 		/* stay where you are if you should stay in failsafe, otherwise everything is perfect */
 		} else if (!stay_in_failsafe){
-			status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
 		}
 		break;
 
-	case MAIN_STATE_AUTO_LOITER:
+	case vehicle_status_s::MAIN_STATE_AUTO_LOITER:
 		/* go into failsafe on a engine failure */
 		if (status->engine_failure) {
-			status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
 		/* also go into failsafe if just datalink is lost */
 		} else if (status->data_link_lost && data_link_loss_enabled) {
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
 			} else if (status->condition_local_position_valid) {
-				status->nav_state = NAVIGATION_STATE_LAND;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
 			} else if (status->condition_local_altitude_valid) {
-				status->nav_state = NAVIGATION_STATE_DESCEND;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
 			} else {
-				status->nav_state = NAVIGATION_STATE_TERMINATION;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
 			}
 
 		/* go into failsafe if RC is lost and datalink loss is not set up */
@@ -580,65 +580,65 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
 			} else if (status->condition_local_position_valid) {
-				status->nav_state = NAVIGATION_STATE_LAND;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
 			} else if (status->condition_local_altitude_valid) {
-				status->nav_state = NAVIGATION_STATE_DESCEND;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
 			} else {
-				status->nav_state = NAVIGATION_STATE_TERMINATION;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
 			}
 
 		/* don't bother if RC is lost if datalink is connected */
 		} else if (status->rc_signal_lost) {
 
 			/* this mode is ok, we don't need RC for loitering */
-			status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
 		} else {
 			/* everything is perfect */
-			status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
 		}
 		break;
 
-	case MAIN_STATE_AUTO_RTL:
+	case vehicle_status_s::MAIN_STATE_AUTO_RTL:
 		/* require global position and home, also go into failsafe on an engine failure */
 
 		if (status->engine_failure) {
-			status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
 		} else if ((!status->condition_global_position_valid ||
 					!status->condition_home_position_valid)) {
 			status->failsafe = true;
 
 			if (status->condition_local_position_valid) {
-				status->nav_state = NAVIGATION_STATE_LAND;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
 			} else if (status->condition_local_altitude_valid) {
-				status->nav_state = NAVIGATION_STATE_DESCEND;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
 			} else {
-				status->nav_state = NAVIGATION_STATE_TERMINATION;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
 			}
 		} else {
-			status->nav_state = NAVIGATION_STATE_AUTO_RTL;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
 		}
 		break;
 
-	case MAIN_STATE_OFFBOARD:
+	case vehicle_status_s::MAIN_STATE_OFFBOARD:
 		/* require offboard control, otherwise stay where you are */
 		if (status->offboard_control_signal_lost && !status->rc_signal_lost) {
 			status->failsafe = true;
 
-			status->nav_state = NAVIGATION_STATE_POSCTL;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
 		} else if (status->offboard_control_signal_lost && status->rc_signal_lost) {
 			status->failsafe = true;
 
 			if (status->condition_local_position_valid) {
-				status->nav_state = NAVIGATION_STATE_LAND;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
 			} else if (status->condition_local_altitude_valid) {
-				status->nav_state = NAVIGATION_STATE_DESCEND;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
 			} else {
-				status->nav_state = NAVIGATION_STATE_TERMINATION;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
 			}
 		} else {
-			status->nav_state = NAVIGATION_STATE_OFFBOARD;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
 		}
 	default:
 		break;
diff --git a/src/modules/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp
index 6768bfa7e..f3cd87728 100644
--- a/src/modules/controllib/block/Block.cpp
+++ b/src/modules/controllib/block/Block.cpp
@@ -101,7 +101,7 @@ void Block::updateParams()
 
 void Block::updateSubscriptions()
 {
-	uORB::SubscriptionBase *sub = getSubscriptions().getHead();
+	uORB::SubscriptionNode *sub = getSubscriptions().getHead();
 	int count = 0;
 
 	while (sub != NULL) {
@@ -119,7 +119,7 @@ void Block::updateSubscriptions()
 
 void Block::updatePublications()
 {
-	uORB::PublicationBase *pub = getPublications().getHead();
+	uORB::PublicationNode *pub = getPublications().getHead();
 	int count = 0;
 
 	while (pub != NULL) {
diff --git a/src/modules/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp
index 9bd80b15b..d2f9cdf07 100644
--- a/src/modules/controllib/block/Block.hpp
+++ b/src/modules/controllib/block/Block.hpp
@@ -46,8 +46,8 @@
 
 // forward declaration
 namespace uORB {
-	class SubscriptionBase;
-	class PublicationBase;
+	class SubscriptionNode;
+	class PublicationNode;
 }
 
 namespace control
@@ -83,15 +83,15 @@ public:
 protected:
 // accessors
 	SuperBlock *getParent() { return _parent; }
-	List<uORB::SubscriptionBase *> & getSubscriptions() { return _subscriptions; }
-	List<uORB::PublicationBase *> & getPublications() { return _publications; }
+	List<uORB::SubscriptionNode *> & getSubscriptions() { return _subscriptions; }
+	List<uORB::PublicationNode *> & getPublications() { return _publications; }
 	List<BlockParamBase *> & getParams() { return _params; }
 // attributes
 	const char *_name;
 	SuperBlock *_parent;
 	float _dt;
-	List<uORB::SubscriptionBase *> _subscriptions;
-	List<uORB::PublicationBase *> _publications;
+	List<uORB::SubscriptionNode *> _subscriptions;
+	List<uORB::PublicationNode *> _publications;
 	List<BlockParamBase *> _params;
 
 private:
diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp
index e8fecef0d..454d0db19 100644
--- a/src/modules/controllib/uorb/blocks.cpp
+++ b/src/modules/controllib/uorb/blocks.cpp
@@ -82,16 +82,16 @@ void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
 BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
 	SuperBlock(parent, name),
 	// subscriptions
-	_att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20),
-	_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
-	_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
-	_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
-	_missionCmd(&getSubscriptions(), ORB_ID(position_setpoint_triplet), 20),
-	_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
-	_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
-	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
+	_att(ORB_ID(vehicle_attitude), 20, &getSubscriptions()),
+	_attCmd(ORB_ID(vehicle_attitude_setpoint), 20, &getSubscriptions()),
+	_ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, &getSubscriptions()),
+	_pos(ORB_ID(vehicle_global_position), 20, &getSubscriptions()),
+	_missionCmd(ORB_ID(position_setpoint_triplet), 20, &getSubscriptions()),
+	_manual(ORB_ID(manual_control_setpoint), 20, &getSubscriptions()),
+	_status(ORB_ID(vehicle_status), 20, &getSubscriptions()),
+	_param_update(ORB_ID(parameter_update), 1000, &getSubscriptions()), // limit to 1 Hz
 	// publications
-	_actuators(&getPublications(), ORB_ID(actuator_controls_0))
+	_actuators(ORB_ID(actuator_controls_0), &getPublications())
 {
 }
 
diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp
index a8a70507e..491d4681d 100644
--- a/src/modules/controllib/uorb/blocks.hpp
+++ b/src/modules/controllib/uorb/blocks.hpp
@@ -47,6 +47,10 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/parameter_update.h>
 
 #include <stdio.h>
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 d0f5fb6f8..38660b433 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
@@ -84,6 +84,7 @@
 #include <mathlib/mathlib.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
 #include <mavlink/mavlink_log.h>
+#include <platforms/px4_defines.h>
 
 #include "estimator_22states.h"
 
@@ -816,7 +817,7 @@ FixedwingEstimator::task_main()
 		if (fds[1].revents & POLLIN) {
 
 			/* check vehicle status for changes to publication state */
-			bool prev_hil = (_vstatus.hil_state == HIL_STATE_ON);
+			bool prev_hil = (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON);
 			vehicle_status_poll();
 
 			bool accel_updated;
@@ -825,7 +826,7 @@ FixedwingEstimator::task_main()
 			perf_count(_perf_gyro);
 
 			/* Reset baro reference if switching to HIL, reset sensor states */
-			if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) {
+			if (!prev_hil && (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON)) {
 				/* system is in HIL now, wait for measurements to come in one last round */
 				usleep(60000);
 
@@ -1416,7 +1417,7 @@ FixedwingEstimator::task_main()
 					math::Vector<3> euler = R.to_euler();
 
 					for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
-							_att.R[i][j] = R(i, j);
+							PX4_R(_att.R, i, j) = R(i, j);
 
 					_att.timestamp = _last_sensor_timestamp;
 					_att.q[0] = _ekf->states[0];
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 4cb6c1aef..00c2080a0 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -59,7 +59,11 @@
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_virtual_fw.h>
+#include <uORB/topics/actuator_controls_virtual_mc.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/fw_virtual_rates_setpoint.h>
+#include <uORB/topics/mc_virtual_rates_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/parameter_update.h>
@@ -76,6 +80,7 @@
 #include <ecl/attitude_fw/ecl_pitch_controller.h>
 #include <ecl/attitude_fw/ecl_roll_controller.h>
 #include <ecl/attitude_fw/ecl_yaw_controller.h>
+#include <platforms/px4_defines.h>
 
 /**
  * Fixedwing attitude control app start / stop handling function
@@ -742,15 +747,15 @@ FixedwingAttitudeControl::task_main()
 				_att.roll    = euler_angles(0);
 				_att.pitch   = euler_angles(1);
 				_att.yaw     = euler_angles(2);
-				_att.R[0][0] = R_adapted(0, 0);
-				_att.R[0][1] = R_adapted(0, 1);
-				_att.R[0][2] = R_adapted(0, 2);
-				_att.R[1][0] = R_adapted(1, 0);
-				_att.R[1][1] = R_adapted(1, 1);
-				_att.R[1][2] = R_adapted(1, 2);
-				_att.R[2][0] = R_adapted(2, 0);
-				_att.R[2][1] = R_adapted(2, 1);
-				_att.R[2][2] = R_adapted(2, 2);
+				PX4_R(_att.R, 0, 0) = R_adapted(0, 0);
+				PX4_R(_att.R, 0, 1) = R_adapted(0, 1);
+				PX4_R(_att.R, 0, 2) = R_adapted(0, 2);
+				PX4_R(_att.R, 1, 0) = R_adapted(1, 0);
+				PX4_R(_att.R, 1, 1) = R_adapted(1, 1);
+				PX4_R(_att.R, 1, 2) = R_adapted(1, 2);
+				PX4_R(_att.R, 2, 0) = R_adapted(2, 0);
+				PX4_R(_att.R, 2, 1) = R_adapted(2, 1);
+				PX4_R(_att.R, 2, 2) = R_adapted(2, 2);
 
 				/* lastly, roll- and yawspeed have to be swaped */
 				float helper = _att.rollspeed;
@@ -924,9 +929,9 @@ FixedwingAttitudeControl::task_main()
 				float speed_body_v = 0.0f;
 				float speed_body_w = 0.0f;
 				if(_att.R_valid) 	{
-					speed_body_u = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d;
-					speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d;
-					speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d;
+					speed_body_u = PX4_R(_att.R, 0, 0) * _global_pos.vel_n + PX4_R(_att.R, 1, 0) * _global_pos.vel_e + PX4_R(_att.R, 2, 0) * _global_pos.vel_d;
+					speed_body_v = PX4_R(_att.R, 0, 1) * _global_pos.vel_n + PX4_R(_att.R, 1, 1) * _global_pos.vel_e + PX4_R(_att.R, 2, 1) * _global_pos.vel_d;
+					speed_body_w = PX4_R(_att.R, 0, 2) * _global_pos.vel_n + PX4_R(_att.R, 1, 2) * _global_pos.vel_e + PX4_R(_att.R, 2, 2) * _global_pos.vel_d;
 				} else	{
 					if (_debug && loop_counter % 10 == 0) {
 						warnx("Did not get a valid R\n");
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index dbf15d98a..0bdc285e7 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -77,6 +77,7 @@
 #include <uORB/topics/navigation_capabilities.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_status.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/pid/pid.h>
@@ -90,6 +91,7 @@
 #include <external_lgpl/tecs/tecs.h>
 #include "landingslope.h"
 #include "mtecs/mTecs.h"
+#include <platforms/px4_defines.h>
 
 static int	_control_task = -1;			/**< task handle for sensor task */
 
@@ -735,7 +737,7 @@ FixedwingPositionControl::vehicle_attitude_poll()
 
 		/* set rotation matrix */
 		for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
-				_R_nb(i, j) = _att.R[i][j];
+				_R_nb(i, j) = PX4_R(_att.R, i, j);
 	}
 }
 
@@ -814,7 +816,7 @@ void
 FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet)
 {
 
-	if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) {
+	if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) {
 
 		/* rotate ground speed vector with current attitude */
 		math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
@@ -961,7 +963,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		}
 
-		if (pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION) {
+		if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
 			/* waypoint is a plain navigation waypoint */
 			_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
 			_att_sp.roll_body = _l1_control.nav_roll();
@@ -972,7 +974,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 						_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
 						false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
 
-		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) {
+		} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
 
 			/* waypoint is a loiter waypoint */
 			_l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius,
@@ -985,7 +987,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 						_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
 						false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
 
-		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
+		} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
 
 			float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
 			float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
@@ -1138,7 +1140,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 						ground_speed);
 			}
 
-		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) {
+		} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
 
 			/* Perform launch detection */
 			if (launchDetector.launchDetectionEnabled() &&
@@ -1233,12 +1235,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		}
 
 		/* reset landing state */
-		if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) {
+		if (pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_LAND) {
 			reset_landing_state();
 		}
 
 		/* reset takeoff/launch state */
-		if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) {
+		if (pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
 			reset_takeoff_state();
 		}
 
@@ -1325,7 +1327,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		/* Set thrust to 0 to minimize damage */
 		_att_sp.thrust = 0.0f;
 	} else if (_control_mode_current ==  FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto
-			pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
+			pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
 			launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
 		 /* making sure again that the correct thrust is used,
 		 * without depending on library calls for safety reasons */
@@ -1339,7 +1341,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	/* During a takeoff waypoint while waiting for launch the pitch sp is set
 	 * already (not by tecs) */
 	if (!(_control_mode_current ==  FW_POSCTRL_MODE_AUTO &&
-				pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
+				pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
 				launch_detection_state == LAUNCHDETECTION_RES_NONE)) {
 		_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
 	}
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index bffeefc1f..ecdab2936 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -52,7 +52,7 @@ mTecs::mTecs() :
 	_mTecsEnabled(this, "ENABLED"),
 	_airspeedMin(this, "FW_AIRSPD_MIN", false),
 	/* Publications */
-	_status(&getPublications(), ORB_ID(tecs_status)),
+	_status(ORB_ID(tecs_status), &getPublications()),
 	/* control blocks */
 	_controlTotalEnergy(this, "THR"),
 	_controlEnergyDistribution(this, "PIT", true),
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index d6e9982de..6bd0c7bce 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1431,7 +1431,7 @@ Mavlink::task_main(int argc, char *argv[])
 
 		if (status_sub->update(&status_time, &status)) {
 			/* switch HIL mode if required */
-			set_hil_enabled(status.hil_state == HIL_STATE_ON);
+			set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON);
 		}
 
 		/* check for requested subscriptions */
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 4a095a765..be7d91c65 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -103,13 +103,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 	*mavlink_custom_mode = 0;
 
 	/* HIL */
-	if (status->hil_state == HIL_STATE_ON) {
+	if (status->hil_state == vehicle_status_s::HIL_STATE_ON) {
 		*mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
 	}
 
 	/* arming state */
-	if (status->arming_state == ARMING_STATE_ARMED
-	    || status->arming_state == ARMING_STATE_ARMED_ERROR) {
+	if (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED
+	    || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) {
 		*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
 	}
 
@@ -121,31 +121,31 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 
 	switch (status->nav_state) {
 
-		case NAVIGATION_STATE_MANUAL:
+		case vehicle_status_s::NAVIGATION_STATE_MANUAL:
 			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
 			                      | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
 			break;
 
-		case NAVIGATION_STATE_ACRO:
+		case vehicle_status_s::NAVIGATION_STATE_ACRO:
 			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
 			break;
 
-		case NAVIGATION_STATE_ALTCTL:
+		case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
 			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
 			                      | MAV_MODE_FLAG_STABILIZE_ENABLED;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;
 			break;
 
-		case NAVIGATION_STATE_POSCTL:
+		case vehicle_status_s::NAVIGATION_STATE_POSCTL:
 			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
 			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
 					      | MAV_MODE_FLAG_GUIDED_ENABLED;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
 			break;
 
-		case NAVIGATION_STATE_AUTO_MISSION:
+		case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
 			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
 			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
 					      | MAV_MODE_FLAG_GUIDED_ENABLED;
@@ -153,7 +153,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
 			break;
 
-		case NAVIGATION_STATE_AUTO_LOITER:
+		case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
 			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
 			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
 					      | MAV_MODE_FLAG_GUIDED_ENABLED;
@@ -161,9 +161,9 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
 			break;
 
-		case NAVIGATION_STATE_AUTO_RTL:
+		case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
 			/* fallthrough */
-		case NAVIGATION_STATE_AUTO_RCRECOVER:
+		case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
 			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
 			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
 					      | MAV_MODE_FLAG_GUIDED_ENABLED;
@@ -171,11 +171,11 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
 			break;
 
-		case NAVIGATION_STATE_LAND:
-		case NAVIGATION_STATE_AUTO_LANDENGFAIL:
-		case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
+		case vehicle_status_s::NAVIGATION_STATE_LAND:
+		case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
+		case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
 			/* fallthrough */
-		case NAVIGATION_STATE_DESCEND:
+		case vehicle_status_s::NAVIGATION_STATE_DESCEND:
 			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
 			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
 					      | MAV_MODE_FLAG_GUIDED_ENABLED;
@@ -183,7 +183,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
 			break;
 
-		case NAVIGATION_STATE_AUTO_RTGS:
+		case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
 			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
 			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
 					      | MAV_MODE_FLAG_GUIDED_ENABLED;
@@ -191,19 +191,19 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS;
 			break;
 
-		case NAVIGATION_STATE_TERMINATION:
+		case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
 			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
 			break;
 
-		case NAVIGATION_STATE_OFFBOARD:
+		case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
 			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
 			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
 					      | MAV_MODE_FLAG_GUIDED_ENABLED;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD;
 			break;
 
-		case NAVIGATION_STATE_MAX:
+		case vehicle_status_s::NAVIGATION_STATE_MAX:
 			/* this is an unused case, ignore */
 			break;
 
@@ -212,21 +212,21 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 	*mavlink_custom_mode = custom_mode.data;
 
 	/* set system state */
-	if (status->arming_state == ARMING_STATE_INIT
-	    || status->arming_state == ARMING_STATE_IN_AIR_RESTORE
-	    || status->arming_state == ARMING_STATE_STANDBY_ERROR) {	// TODO review
+	if (status->arming_state == vehicle_status_s::ARMING_STATE_INIT
+	    || status->arming_state == vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE
+	    || status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {	// TODO review
 		*mavlink_state = MAV_STATE_UNINIT;
 
-	} else if (status->arming_state == ARMING_STATE_ARMED) {
+	} else if (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
 		*mavlink_state = MAV_STATE_ACTIVE;
 
-	} else if (status->arming_state == ARMING_STATE_ARMED_ERROR) {
+	} else if (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) {
 		*mavlink_state = MAV_STATE_CRITICAL;
 
-	} else if (status->arming_state == ARMING_STATE_STANDBY) {
+	} else if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
 		*mavlink_state = MAV_STATE_STANDBY;
 
-	} else if (status->arming_state == ARMING_STATE_REBOOT) {
+	} else if (status->arming_state == vehicle_status_s::ARMING_STATE_REBOOT) {
 		*mavlink_state = MAV_STATE_POWEROFF;
 
 	} else {
@@ -1431,7 +1431,7 @@ protected:
 		updated |= _pos_sp_triplet_sub->update(&_pos_sp_triplet_time, &pos_sp_triplet);
 		updated |= _status_sub->update(&_status_time, &status);
 
-		if (updated && (status.arming_state == ARMING_STATE_ARMED)) {
+		if (updated && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
 			/* translate the current syste state to mavlink state and mode */
 			uint8_t mavlink_state;
 			uint8_t mavlink_base_mode;
@@ -2198,7 +2198,7 @@ protected:
 		msg.param2 = 0;
 		msg.param3 = 0;
 		/* set camera capture ON/OFF depending on arming state */
-		msg.param4 = (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) ? 1 : 0;
+		msg.param4 = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) ? 1 : 0;
 		msg.param5 = 0;
 		msg.param6 = 0;
 		msg.param7 = 0;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index cbd24f0d4..4d7b35f03 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -617,7 +617,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 					pos_sp_triplet.previous.valid = false;
 					pos_sp_triplet.next.valid = false;
 					pos_sp_triplet.current.valid = true;
-					pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others
+					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' */
@@ -799,7 +799,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 					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, att_sp.R_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;
 					memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d));
@@ -986,10 +986,10 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg)
 
 	} else if (tsync.tc1 > 0) {
 
-		int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1*2)/2 ; 
+		int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1*2)/2 ;
 		int64_t dt = _time_offset - offset_ns;
 
-		if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew 
+		if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew
 			_time_offset = offset_ns;
 			warnx("[timesync] Hard setting offset.");
 		} else {
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 c828f1f94..f20bba52e 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -67,7 +67,11 @@
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_virtual_fw.h>
+#include <uORB/topics/actuator_controls_virtual_mc.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/fw_virtual_rates_setpoint.h>
+#include <uORB/topics/mc_virtual_rates_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_status.h>
@@ -619,14 +623,14 @@ MulticopterAttitudeControl::control_attitude(float dt)
 
 	if (_v_att_sp.R_valid) {
 		/* rotation matrix in _att_sp is valid, use it */
-		R_sp.set(&_v_att_sp.R_body[0][0]);
+		R_sp.set(&_v_att_sp.R_body[0]);
 
 	} else {
 		/* rotation matrix in _att_sp is not valid, use euler angles instead */
 		R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body);
 
 		/* copy rotation matrix back to setpoint struct */
-		memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_v_att_sp.R_body));
+		memcpy(&_v_att_sp.R_body[0], &R_sp.data[0], sizeof(_v_att_sp.R_body));
 		_v_att_sp.R_valid = true;
 	}
 
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp
new file mode 100644
index 000000000..1e40c2b05
--- /dev/null
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp
@@ -0,0 +1,301 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_att_control.cpp
+ * Multicopter attitude controller.
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Roman Bapst <bapstr@ethz.ch>
+ */
+
+#include "mc_att_control.h"
+#include "mc_att_control_params.h"
+#include "math.h"
+
+#define YAW_DEADZONE	0.05f
+#define MIN_TAKEOFF_THRUST    0.2f
+#define RATES_I_LIMIT	0.3f
+
+namespace mc_att_control
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+}
+
+MulticopterAttitudeControl::MulticopterAttitudeControl() :
+	MulticopterAttitudeControlBase(),
+	_task_should_exit(false),
+	_actuators_0_circuit_breaker_enabled(false),
+
+	/* publications */
+	_att_sp_pub(nullptr),
+	_v_rates_sp_pub(nullptr),
+	_actuators_0_pub(nullptr),
+	_n(),
+
+	/* performance counters */
+	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
+
+{
+	_params_handles.roll_p			= 	PX4_PARAM_INIT(MC_ROLL_P);
+	_params_handles.roll_rate_p		= 	PX4_PARAM_INIT(MC_ROLLRATE_P);
+	_params_handles.roll_rate_i		= 	PX4_PARAM_INIT(MC_ROLLRATE_I);
+	_params_handles.roll_rate_d		= 	PX4_PARAM_INIT(MC_ROLLRATE_D);
+	_params_handles.pitch_p			= 	PX4_PARAM_INIT(MC_PITCH_P);
+	_params_handles.pitch_rate_p		= 	PX4_PARAM_INIT(MC_PITCHRATE_P);
+	_params_handles.pitch_rate_i		= 	PX4_PARAM_INIT(MC_PITCHRATE_I);
+	_params_handles.pitch_rate_d		= 	PX4_PARAM_INIT(MC_PITCHRATE_D);
+	_params_handles.yaw_p			=	PX4_PARAM_INIT(MC_YAW_P);
+	_params_handles.yaw_rate_p		= 	PX4_PARAM_INIT(MC_YAWRATE_P);
+	_params_handles.yaw_rate_i		= 	PX4_PARAM_INIT(MC_YAWRATE_I);
+	_params_handles.yaw_rate_d		= 	PX4_PARAM_INIT(MC_YAWRATE_D);
+	_params_handles.yaw_ff			= 	PX4_PARAM_INIT(MC_YAW_FF);
+	_params_handles.yaw_rate_max		= 	PX4_PARAM_INIT(MC_YAWRATE_MAX);
+	_params_handles.man_roll_max		= 	PX4_PARAM_INIT(MC_MAN_R_MAX);
+	_params_handles.man_pitch_max		= 	PX4_PARAM_INIT(MC_MAN_P_MAX);
+	_params_handles.man_yaw_max		= 	PX4_PARAM_INIT(MC_MAN_Y_MAX);
+	_params_handles.acro_roll_max		= 	PX4_PARAM_INIT(MC_ACRO_R_MAX);
+	_params_handles.acro_pitch_max		= 	PX4_PARAM_INIT(MC_ACRO_P_MAX);
+	_params_handles.acro_yaw_max		= 	PX4_PARAM_INIT(MC_ACRO_Y_MAX);
+
+	/* fetch initial parameter values */
+	parameters_update();
+
+	/*
+	 * do subscriptions
+	 */
+	_v_att = _n.subscribe<px4_vehicle_attitude>(&MulticopterAttitudeControl::handle_vehicle_attitude, this, 0);
+	_v_att_sp = _n.subscribe<px4_vehicle_attitude_setpoint>(0);
+	_v_rates_sp = _n.subscribe<px4_vehicle_rates_setpoint>(0);
+	_v_control_mode = _n.subscribe<px4_vehicle_control_mode>(0);
+	_parameter_update = _n.subscribe<px4_parameter_update>(
+			&MulticopterAttitudeControl::handle_parameter_update, this, 1000);
+	_manual_control_sp = _n.subscribe<px4_manual_control_setpoint>(0);
+	_armed = _n.subscribe<px4_actuator_armed>(0);
+	_v_status = _n.subscribe<px4_vehicle_status>(0);
+
+}
+
+MulticopterAttitudeControl::~MulticopterAttitudeControl()
+{
+}
+
+int
+MulticopterAttitudeControl::parameters_update()
+{
+	float v;
+
+	/* roll gains */
+	PX4_PARAM_GET(_params_handles.roll_p, &v);
+	_params.att_p(0) = v;
+	PX4_PARAM_GET(_params_handles.roll_rate_p, &v);
+	_params.rate_p(0) = v;
+	PX4_PARAM_GET(_params_handles.roll_rate_i, &v);
+	_params.rate_i(0) = v;
+	PX4_PARAM_GET(_params_handles.roll_rate_d, &v);
+	_params.rate_d(0) = v;
+
+	/* pitch gains */
+	PX4_PARAM_GET(_params_handles.pitch_p, &v);
+	_params.att_p(1) = v;
+	PX4_PARAM_GET(_params_handles.pitch_rate_p, &v);
+	_params.rate_p(1) = v;
+	PX4_PARAM_GET(_params_handles.pitch_rate_i, &v);
+	_params.rate_i(1) = v;
+	PX4_PARAM_GET(_params_handles.pitch_rate_d, &v);
+	_params.rate_d(1) = v;
+
+	/* yaw gains */
+	PX4_PARAM_GET(_params_handles.yaw_p, &v);
+	_params.att_p(2) = v;
+	PX4_PARAM_GET(_params_handles.yaw_rate_p, &v);
+	_params.rate_p(2) = v;
+	PX4_PARAM_GET(_params_handles.yaw_rate_i, &v);
+	_params.rate_i(2) = v;
+	PX4_PARAM_GET(_params_handles.yaw_rate_d, &v);
+	_params.rate_d(2) = v;
+
+	PX4_PARAM_GET(_params_handles.yaw_ff, &_params.yaw_ff);
+	PX4_PARAM_GET(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
+	_params.yaw_rate_max = math::radians(_params.yaw_rate_max);
+
+	/* manual control scale */
+	PX4_PARAM_GET(_params_handles.man_roll_max, &_params.man_roll_max);
+	PX4_PARAM_GET(_params_handles.man_pitch_max, &_params.man_pitch_max);
+	PX4_PARAM_GET(_params_handles.man_yaw_max, &_params.man_yaw_max);
+	_params.man_roll_max = math::radians(_params.man_roll_max);
+	_params.man_pitch_max = math::radians(_params.man_pitch_max);
+	_params.man_yaw_max = math::radians(_params.man_yaw_max);
+
+	/* acro control scale */
+	PX4_PARAM_GET(_params_handles.acro_roll_max, &v);
+	_params.acro_rate_max(0) = math::radians(v);
+	PX4_PARAM_GET(_params_handles.acro_pitch_max, &v);
+	_params.acro_rate_max(1) = math::radians(v);
+	PX4_PARAM_GET(_params_handles.acro_yaw_max, &v);
+	_params.acro_rate_max(2) = math::radians(v);
+
+	_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
+
+	return OK;
+}
+
+void MulticopterAttitudeControl::handle_parameter_update(const px4_parameter_update &msg)
+{
+	parameters_update();
+}
+
+void  MulticopterAttitudeControl::handle_vehicle_attitude(const px4_vehicle_attitude &msg) {
+
+	perf_begin(_loop_perf);
+
+	/* run controller on attitude changes */
+	static uint64_t last_run = 0;
+	float dt = (px4::get_time_micros() - last_run) / 1000000.0f;
+	last_run = px4::get_time_micros();
+
+	/* guard against too small (< 2ms) and too large (> 20ms) dt's */
+	if (dt < 0.002f) {
+		dt = 0.002f;
+
+	} else if (dt > 0.02f) {
+		dt = 0.02f;
+	}
+
+	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);
+		_v_rates_sp_mod.data().yaw = _rates_sp(2);
+		_v_rates_sp_mod.data().thrust = _thrust_sp;
+		_v_rates_sp_mod.data().timestamp = px4::get_time_micros();
+
+		if (_v_rates_sp_pub != nullptr) {
+			_v_rates_sp_pub->publish(_v_rates_sp_mod);
+		} else {
+			if (_v_status->data().is_vtol) {
+				//XXX add a second publisher?
+				// _v_rates_sp_pub = _n.advertise<px4_mc_virtual_rates_setpoint>();
+			} else {
+				_v_rates_sp_pub = _n.advertise<px4_vehicle_rates_setpoint>();
+			}
+		}
+
+	} else {
+		/* attitude controller disabled, poll rates setpoint topic */
+		if (_v_control_mode->data().flag_control_manual_enabled) {
+			/* manual rates control - ACRO mode */
+			_rates_sp = math::Vector<3>(_manual_control_sp->data().y, -_manual_control_sp->data().x,
+						    _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);
+			_v_rates_sp_mod.data().yaw = _rates_sp(2);
+			_v_rates_sp_mod.data().thrust = _thrust_sp;
+			_v_rates_sp_mod.data().timestamp = px4::get_time_micros();
+
+			if (_v_rates_sp_pub != nullptr) {
+				_v_rates_sp_pub->publish(_v_rates_sp_mod);
+
+			} else {
+				if (_v_status->data().is_vtol) {
+					//XXX add a second publisher?
+					// _v_rates_sp_pub = _n.advertise<px4_mc_virtual_rates_setpoint>();
+				} else {
+					_v_rates_sp_pub = _n.advertise<px4_vehicle_rates_setpoint>();
+				}
+			}
+
+		} else {
+			/* attitude controller disabled, poll rates setpoint topic */
+			_rates_sp(0) = _v_rates_sp->data().roll;
+			_rates_sp(1) = _v_rates_sp->data().pitch;
+			_rates_sp(2) = _v_rates_sp->data().yaw;
+			_thrust_sp = _v_rates_sp->data().thrust;
+		}
+	}
+
+	if (_v_control_mode->data().flag_control_rates_enabled) {
+		control_attitude_rates(dt);
+
+		/* publish actuator controls */
+		_actuators.data().control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
+		_actuators.data().control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
+		_actuators.data().control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
+		_actuators.data().control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
+		_actuators.data().timestamp = px4::get_time_micros();
+
+		if (!_actuators_0_circuit_breaker_enabled) {
+			if (_actuators_0_pub != nullptr) {
+				_actuators_0_pub->publish(_actuators);
+
+			} else {
+				if (_v_status->data().is_vtol) {
+					//XXX add a second publisher?
+					// _actuators_0_pub = _n.advertise<px4_actuator_controls_virtual_mc>();
+				} else {
+					_actuators_0_pub = _n.advertise<px4_actuator_controls_0>();
+				}
+			}
+		}
+	}
+}
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.h b/src/modules/mc_att_control_multiplatform/mc_att_control.h
new file mode 100644
index 000000000..95d608684
--- /dev/null
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control.h
@@ -0,0 +1,130 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_att_control.h
+ * Multicopter attitude controller.
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ * The controller has two loops: P loop for angular error and PD loop for angular rate error.
+ * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch.
+ * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw,
+ * so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis.
+ * These two approaches fused seamlessly with weight depending on angular error.
+ * When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity.
+ * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
+ * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
+ */
+
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/circuit_breaker.h>
+#include <lib/mathlib/mathlib.h>
+
+#include "mc_att_control_base.h"
+
+class MulticopterAttitudeControl :
+	public MulticopterAttitudeControlBase
+{
+public:
+	/**
+	 * Constructor
+	 */
+	MulticopterAttitudeControl();
+
+	/**
+	 * Destructor, also kills the sensors task.
+	 */
+	~MulticopterAttitudeControl();
+
+	/* Callbacks for topics */
+	void handle_vehicle_attitude(const px4_vehicle_attitude &msg);
+	void handle_parameter_update(const px4_parameter_update &msg);
+
+	void spin() { _n.spin(); }
+
+private:
+	bool		_task_should_exit;			/**< if true, sensor task should exit */
+	bool		_actuators_0_circuit_breaker_enabled;	/**< circuit breaker to suppress output */
+
+	px4::Publisher<px4_vehicle_attitude_setpoint> *	_att_sp_pub;			/**< attitude setpoint publication */
+	px4::Publisher<px4_vehicle_rates_setpoint> *	_v_rates_sp_pub;		/**< rate setpoint publication */
+	px4::Publisher<px4_actuator_controls_0> *	_actuators_0_pub;		/**< attitude actuator controls publication */
+
+	px4::NodeHandle _n;
+
+	struct {
+		px4_param_t roll_p;
+		px4_param_t roll_rate_p;
+		px4_param_t roll_rate_i;
+		px4_param_t roll_rate_d;
+		px4_param_t pitch_p;
+		px4_param_t pitch_rate_p;
+		px4_param_t pitch_rate_i;
+		px4_param_t pitch_rate_d;
+		px4_param_t yaw_p;
+		px4_param_t yaw_rate_p;
+		px4_param_t yaw_rate_i;
+		px4_param_t yaw_rate_d;
+		px4_param_t yaw_ff;
+		px4_param_t yaw_rate_max;
+
+		px4_param_t man_roll_max;
+		px4_param_t man_pitch_max;
+		px4_param_t man_yaw_max;
+		px4_param_t acro_roll_max;
+		px4_param_t acro_pitch_max;
+		px4_param_t acro_yaw_max;
+
+		px4_param_t autostart_id;
+	}		_params_handles;		/**< handles for interesting parameters */
+
+	perf_counter_t _loop_perf; /**< loop performance counter */
+
+	/**
+	 * Update our local parameter cache.
+	 */
+	int			parameters_update();
+};
+
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
new file mode 100644
index 000000000..1f0d88bcd
--- /dev/null
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
@@ -0,0 +1,310 @@
+/* Copyright (c) 2014 PX4 Development Team. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+* notice, this list of conditions and the following disclaimer in
+* the documentation and/or other materials provided with the
+* distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+* used to endorse or promote products derived from this software
+* without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+/**
+ * @file mc_att_control_base.cpp
+ *
+ * MC Attitude Controller : Control and math code
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ */
+
+#include "mc_att_control_base.h"
+#include <geo/geo.h>
+#include <math.h>
+#include <lib/mathlib/mathlib.h>
+
+#ifdef CONFIG_ARCH_ARM
+#else
+#include <cmath>
+using namespace std;
+#endif
+
+MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
+	_v_att_sp_mod(),
+	_v_rates_sp_mod(),
+	_actuators(),
+	_publish_att_sp(false)
+
+{
+	_params.att_p.zero();
+	_params.rate_p.zero();
+	_params.rate_i.zero();
+	_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();
+	_rates_sp.zero();
+	_rates_int.zero();
+	_thrust_sp = 0.0f;
+	_att_control.zero();
+
+	_I.identity();
+}
+
+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;
+		}
+
+		/* move yaw setpoint in all modes */
+		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;
+		}
+
+		/* 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) {
+			/* 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;
+
+	/* 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;
+	}
+
+	/* rotation matrix for current state */
+	math::Matrix<3, 3> R;
+	R.set(_v_att->data().R);
+
+	/* 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));
+
+	/* axis and sin(angle) of desired rotation */
+	math::Vector < 3 > e_R = R.transposed() * (R_z % R_sp_z);
+
+	/* calculate angle error */
+	float e_R_z_sin = e_R.length();
+	float e_R_z_cos = R_z * R_sp_z;
+
+	/* calculate weight for yaw control */
+	float yaw_w = R_sp(2, 2) * R_sp(2, 2);
+
+	/* calculate rotation matrix after roll/pitch only rotation */
+	math::Matrix<3, 3> R_rp;
+
+	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;
+
+		e_R = e_R_z_axis * e_R_z_angle;
+
+		/* cross product matrix for e_R_axis */
+		math::Matrix<3, 3> e_R_cp;
+		e_R_cp.zero();
+		e_R_cp(0, 1) = -e_R_z_axis(2);
+		e_R_cp(0, 2) = e_R_z_axis(1);
+		e_R_cp(1, 0) = e_R_z_axis(2);
+		e_R_cp(1, 2) = -e_R_z_axis(0);
+		e_R_cp(2, 0) = -e_R_z_axis(1);
+		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));
+
+	} else {
+		/* zero roll/pitch rotation */
+		R_rp = R;
+	}
+
+	/* 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));
+	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) {
+		/* for large thrust vector rotations use another rotation method:
+		 * 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();
+		e_R_d.normalize();
+		e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));
+
+		/* use fusion of Z axis based rotation and direct rotation */
+		float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
+		e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
+	}
+
+	/* calculate angular rates setpoint */
+	_rates_sp = _params.att_p.emult(e_R);
+
+	/* limit yaw rate */
+	_rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max,
+				       _params.yaw_rate_max);
+
+	/* feed forward yaw setpoint rate */
+	_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;
+}
+
+void MulticopterAttitudeControlBase::control_attitude_rates(float dt)
+{
+	/* reset integral if disarmed */
+	if (!_armed->data().armed || !_v_status->data().is_rotary_wing) {
+		_rates_int.zero();
+	}
+
+	/* current body angular rates */
+	math::Vector < 3 > rates;
+	rates(0) = _v_att->data().rollspeed;
+	rates(1) = _v_att->data().pitchspeed;
+	rates(2) = _v_att->data().yawspeed;
+
+	/* angular rates error */
+	math::Vector < 3 > rates_err = _rates_sp - rates;
+	_att_control = _params.rate_p.emult(rates_err)
+		       + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int;
+	_rates_prev = rates;
+
+	/* update integral only if not saturated on low limit */
+	if (_thrust_sp > MIN_TAKEOFF_THRUST) {
+		for (int i = 0; i < 3; i++) {
+			if (fabsf(_att_control(i)) < _thrust_sp) {
+				float rate_i = _rates_int(i)
+					       + _params.rate_i(i) * rates_err(i) * dt;
+
+				if (isfinite(
+					    rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
+				    _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
+					_rates_int(i) = rate_i;
+				}
+			}
+		}
+	}
+
+}
+
+void MulticopterAttitudeControlBase::set_actuator_controls()
+{
+	_actuators.data().control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
+	_actuators.data().control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
+	_actuators.data().control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
+	_actuators.data().control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
+}
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
new file mode 100644
index 000000000..124147079
--- /dev/null
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.h
@@ -0,0 +1,136 @@
+#ifndef MC_ATT_CONTROL_BASE_H_
+#define MC_ATT_CONTROL_BASE_H_
+
+/* Copyright (c) 2014 PX4 Development Team. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+* notice, this list of conditions and the following disclaimer in
+* the documentation and/or other materials provided with the
+* distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+* used to endorse or promote products derived from this software
+* without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+/**
+ * @file mc_att_control_base.h
+ *
+ * MC Attitude Controller : Control and math code
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ */
+#include <px4.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <errno.h>
+#include <math.h>
+
+#include <systemlib/perf_counter.h>
+#include <lib/mathlib/mathlib.h>
+
+
+
+#define YAW_DEADZONE	0.05f
+#define MIN_TAKEOFF_THRUST    0.2f
+#define RATES_I_LIMIT	0.3f
+
+using namespace px4;
+
+class MulticopterAttitudeControlBase
+{
+public:
+	/**
+	 * Constructor
+	 */
+	MulticopterAttitudeControlBase();
+
+	/**
+	 * Destructor
+	 */
+	~MulticopterAttitudeControlBase();
+
+	/**
+	 * Start the sensors task.
+	 *
+	 * @return		OK on success.
+	 */
+	void control_attitude(float dt);
+	void control_attitude_rates(float dt);
+
+	void set_actuator_controls();
+
+protected:
+	px4::Subscriber<px4_vehicle_attitude> *_v_att;			    /**< vehicle attitude */
+	px4::Subscriber<px4_vehicle_attitude_setpoint> *_v_att_sp;	    /**< vehicle attitude setpoint */
+	px4::Subscriber<px4_vehicle_rates_setpoint> *_v_rates_sp;	    /**< vehicle rates setpoint */
+	px4::Subscriber<px4_vehicle_control_mode> *_v_control_mode;	    /**< vehicle control mode */
+	px4::Subscriber<px4_parameter_update> *_parameter_update;	    /**< parameter update */
+	px4::Subscriber<px4_manual_control_setpoint> *_manual_control_sp;   /**< manual control setpoint */
+	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 */
+
+	math::Vector<3> _rates_prev; /**< angular rates on previous step */
+	math::Vector<3> _rates_sp; /**< angular rates setpoint */
+	math::Vector<3> _rates_int; /**< angular rates integral error */
+	float _thrust_sp; /**< thrust setpoint */
+	math::Vector<3> _att_control; /**< attitude control vector */
+
+	math::Matrix<3, 3> _I; /**< identity matrix */
+
+	bool _reset_yaw_sp; /**< reset yaw setpoint flag */
+
+	struct {
+		math::Vector<3> att_p; /**< P gain for angular error */
+		math::Vector<3> rate_p; /**< P gain for angular rate error */
+		math::Vector<3> rate_i; /**< I gain for angular rate error */
+		math::Vector<3> rate_d; /**< D gain for angular rate error */
+		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;
+	} _params;
+
+	bool _publish_att_sp;
+
+};
+
+#endif /* MC_ATT_CONTROL_BASE_H_ */
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
new file mode 100644
index 000000000..5a79a8c6b
--- /dev/null
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_att_control_main.cpp
+ * Multicopter attitude controller.
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ * The controller has two loops: P loop for angular error and PD loop for angular rate error.
+ * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch.
+ * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw,
+ * so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis.
+ * These two approaches fused seamlessly with weight depending on angular error.
+ * When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity.
+ * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
+ * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
+ */
+
+#include "mc_att_control.h"
+
+bool thread_running = false;     /**< Deamon status flag */
+
+int main(int argc, char **argv)
+{
+	px4::init(argc, argv, "mc_att_control_m");
+
+	PX4_INFO("starting");
+	MulticopterAttitudeControl attctl;
+	thread_running = true;
+	attctl.spin();
+
+	PX4_INFO("exiting.");
+	thread_running = false;
+	return 0;
+}
+
+
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
new file mode 100644
index 000000000..a0b1706f2
--- /dev/null
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_params.c
@@ -0,0 +1,248 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_att_control_params.c
+ * Parameters for multicopter attitude controller.
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <px4_defines.h>
+#include "mc_att_control_params.h"
+#include <systemlib/param/param.h>
+
+/**
+ * Roll P gain
+ *
+ * Roll proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_ROLL_P);
+
+/**
+ * Roll rate P gain
+ *
+ * Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_P);
+
+/**
+ * Roll rate I gain
+ *
+ * Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_I);
+
+/**
+ * Roll rate D gain
+ *
+ * Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_D);
+
+/**
+ * Pitch P gain
+ *
+ * Pitch proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
+ *
+ * @unit 1/s
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_PITCH_P);
+
+/**
+ * Pitch rate P gain
+ *
+ * Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_P);
+
+/**
+ * Pitch rate I gain
+ *
+ * Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_I);
+
+/**
+ * Pitch rate D gain
+ *
+ * Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_D);
+
+/**
+ * Yaw P gain
+ *
+ * Yaw proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
+ *
+ * @unit 1/s
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_YAW_P);
+
+/**
+ * Yaw rate P gain
+ *
+ * Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_P);
+
+/**
+ * Yaw rate I gain
+ *
+ * Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_I);
+
+/**
+ * Yaw rate D gain
+ *
+ * Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_D);
+
+/**
+ * Yaw feed forward
+ *
+ * Feed forward weight for manual yaw control. 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_YAW_FF);
+
+/**
+ * Max yaw rate
+ *
+ * Limit for yaw rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation.
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 360.0
+ * @group Multicopter Attitude Control
+ */
+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
+ * @min 0.0
+ * @max 360.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX);
+
+/**
+ * Max acro pitch rate
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 360.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX);
+
+/**
+ * Max acro yaw rate
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX);
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
new file mode 100644
index 000000000..59dd5240f
--- /dev/null
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_params.h
@@ -0,0 +1,65 @@
+
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_att_control_params.h
+ * Parameters for multicopter attitude controller.
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#pragma once
+
+#define PARAM_MC_ROLL_P_DEFAULT 6.0f
+#define PARAM_MC_ROLLRATE_P_DEFAULT 0.1f
+#define PARAM_MC_ROLLRATE_I_DEFAULT 0.0f
+#define PARAM_MC_ROLLRATE_D_DEFAULT 0.002f
+#define PARAM_MC_PITCH_P_DEFAULT 6.0f
+#define PARAM_MC_PITCHRATE_P_DEFAULT 0.1f
+#define PARAM_MC_PITCHRATE_I_DEFAULT 0.0f
+#define PARAM_MC_PITCHRATE_D_DEFAULT 0.002f
+#define PARAM_MC_YAW_P_DEFAULT 2.0f
+#define PARAM_MC_YAWRATE_P_DEFAULT 0.3f
+#define PARAM_MC_YAWRATE_I_DEFAULT 0.0f
+#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_sim.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_sim.cpp
new file mode 100644
index 000000000..d516d7e52
--- /dev/null
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_sim.cpp
@@ -0,0 +1,142 @@
+/* Copyright (c) 2014 PX4 Development Team. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+* notice, this list of conditions and the following disclaimer in
+* the documentation and/or other materials provided with the
+* distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+* used to endorse or promote products derived from this software
+* without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+/**
+ * @file mc_att_control_sim.cpp
+ *
+ * MC Attitude Controller Interface for usage in a simulator
+ *
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ */
+
+#include "mc_att_control_sim.h"
+#include <geo/geo.h>
+#include <math.h>
+
+#ifdef CONFIG_ARCH_ARM
+#else
+#include <cmath>
+using namespace std;
+#endif
+
+MulticopterAttitudeControlSim::MulticopterAttitudeControlSim()
+{
+	/* setup standard gains */
+	//XXX: make these configurable
+	_params.att_p(0)      = 5.0;
+	_params.rate_p(0)     = 0.05;
+	_params.rate_i(0)     = 0.0;
+	_params.rate_d(0)     = 0.003;
+	/* pitch gains */
+	_params.att_p(1)      = 5.0;
+	_params.rate_p(1)     = 0.05;
+	_params.rate_i(1)     = 0.0;
+	_params.rate_d(1)     = 0.003;
+	/* yaw gains */
+	_params.att_p(2)      = 2.8;
+	_params.rate_p(2)     = 0.2;
+	_params.rate_i(2)     = 0.1;
+	_params.rate_d(2)     = 0.0;
+	_params.yaw_rate_max  = 0.5;
+	_params.yaw_ff        = 0.5;
+	_params.man_roll_max  = 0.6;
+	_params.man_pitch_max = 0.6;
+	_params.man_yaw_max   = 0.6;
+}
+
+MulticopterAttitudeControlSim::~MulticopterAttitudeControlSim()
+{
+}
+
+void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion<double> attitude)
+{
+	math::Quaternion quat;
+	quat(0) = (float)attitude.w();
+	quat(1) = (float)attitude.x();
+	quat(2) = (float)attitude.y();
+	quat(3) = (float)attitude.z();
+
+	_v_att.q[0] = quat(0);
+	_v_att.q[1] = quat(1);
+	_v_att.q[2] = quat(2);
+	_v_att.q[3] = quat(3);
+
+	math::Matrix<3, 3> Rot = quat.to_dcm();
+	_v_att.R[0][0] = Rot(0, 0);
+	_v_att.R[1][0] = Rot(1, 0);
+	_v_att.R[2][0] = Rot(2, 0);
+	_v_att.R[0][1] = Rot(0, 1);
+	_v_att.R[1][1] = Rot(1, 1);
+	_v_att.R[2][1] = Rot(2, 1);
+	_v_att.R[0][2] = Rot(0, 2);
+	_v_att.R[1][2] = Rot(1, 2);
+	_v_att.R[2][2] = Rot(2, 2);
+
+	_v_att.R_valid = true;
+}
+
+void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d &angular_rate)
+{
+	// check if this is consistent !!!
+	_v_att.rollspeed  = angular_rate(0);
+	_v_att.pitchspeed = angular_rate(1);
+	_v_att.yawspeed   = angular_rate(2);
+}
+
+void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference)
+{
+	_v_att_sp.roll_body  = control_attitude_thrust_reference(0);
+	_v_att_sp.pitch_body = control_attitude_thrust_reference(1);
+	_v_att_sp.yaw_body   = control_attitude_thrust_reference(2);
+	_v_att_sp.thrust     = (control_attitude_thrust_reference(3) - 30) * (-1) / 30;
+
+	// setup rotation matrix
+	math::Matrix<3, 3> Rot_sp;
+	Rot_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body);
+	_v_att_sp.R_body[0][0] = Rot_sp(0, 0);
+	_v_att_sp.R_body[1][0] = Rot_sp(1, 0);
+	_v_att_sp.R_body[2][0] = Rot_sp(2, 0);
+	_v_att_sp.R_body[0][1] = Rot_sp(0, 1);
+	_v_att_sp.R_body[1][1] = Rot_sp(1, 1);
+	_v_att_sp.R_body[2][1] = Rot_sp(2, 1);
+	_v_att_sp.R_body[0][2] = Rot_sp(0, 2);
+	_v_att_sp.R_body[1][2] = Rot_sp(1, 2);
+	_v_att_sp.R_body[2][2] = Rot_sp(2, 2);
+}
+
+void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d &motor_inputs)
+{
+	motor_inputs(0) = _actuators.control[0];
+	motor_inputs(1) = _actuators.control[1];
+	motor_inputs(2) = _actuators.control[2];
+	motor_inputs(3) = _actuators.control[3];
+}
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_sim.h b/src/modules/mc_att_control_multiplatform/mc_att_control_sim.h
new file mode 100644
index 000000000..a1bf44fc9
--- /dev/null
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_sim.h
@@ -0,0 +1,97 @@
+#ifndef MC_ATT_CONTROL_BASE_H_
+#define MC_ATT_CONTROL_BASE_H_
+
+/* Copyright (c) 2014 PX4 Development Team. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+* notice, this list of conditions and the following disclaimer in
+* the documentation and/or other materials provided with the
+* distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+* used to endorse or promote products derived from this software
+* without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+/**
+ * @file mc_att_control_sim.h
+ *
+ * MC Attitude Controller Interface for usage in a simulator
+ *
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <errno.h>
+#include <math.h>
+#include <drivers/drv_hrt.h>
+
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/actuator_armed.h>
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+#include <lib/mathlib/mathlib.h>
+#inlcude "mc_att_control_base.h"
+
+
+
+#define YAW_DEADZONE	0.05f
+#define MIN_TAKEOFF_THRUST    0.2f
+#define RATES_I_LIMIT	0.3f
+
+class MulticopterAttitudeControlSim :
+	public MulticopterAttitudeControlBase
+
+{
+public:
+	/**
+	 * Constructor
+	 */
+	MulticopterAttitudeControlSim();
+
+	/**
+	 * Destructor
+	 */
+	~MulticopterAttitudeControlSim();
+
+	/* setters and getters for interface with euroc-gazebo simulator */
+	void set_attitude(const Eigen::Quaternion<double> attitude);
+	void set_attitude_rates(const Eigen::Vector3d &angular_rate);
+	void set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference);
+	void get_mixer_input(Eigen::Vector4d &motor_inputs);
+
+protected:
+	void vehicle_attitude_setpoint_poll() {};
+
+
+};
+
+#endif /* MC_ATT_CONTROL_BASE_H_ */
diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp
index 668f8f164..c2b847075 100644
--- a/src/modules/uORB/topics/actuator_controls.h
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -32,51 +32,68 @@
  ****************************************************************************/
 
 /**
- * @file actuator_controls.h
+ * @file mc_att_control_m_start_nuttx.cpp
  *
- * Actuator control topics - mixer inputs.
- *
- * Values published to these topics are the outputs of the vehicle control
- * system, and are expected to be mixed and used to drive the actuators
- * (servos, speed controls, etc.) that operate the vehicle.
- *
- * Each topic can be published by a single controller
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
+#include <string.h>
+#include <cstdlib>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
 
-#ifndef TOPIC_ACTUATOR_CONTROLS_H
-#define TOPIC_ACTUATOR_CONTROLS_H
+extern bool thread_running;
+int daemon_task;             /**< Handle of deamon task / thread */
+namespace px4
+{
+bool task_should_exit = false;
+}
+using namespace px4;
 
-#include <stdint.h>
-#include "../uORB.h"
+extern int main(int argc, char **argv);
 
-#define NUM_ACTUATOR_CONTROLS		8
-#define NUM_ACTUATOR_CONTROL_GROUPS	4	/**< for sanity checking */
+extern "C" __EXPORT int mc_att_control_m_main(int argc, char *argv[]);
+int mc_att_control_m_main(int argc, char *argv[])
+{
+	if (argc < 1) {
+		errx(1, "usage: mc_att_control_m {start|stop|status}");
+	}
 
-/* control sets with pre-defined applications */
-#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS	ORB_ID(actuator_controls_0)
+	if (!strcmp(argv[1], "start")) {
 
-/**
- * @addtogroup topics
- * @{
- */
+		if (thread_running) {
+			warnx("already running");
+			/* this is not an error */
+			exit(0);
+		}
 
-struct actuator_controls_s {
-	uint64_t timestamp;
-	uint64_t timestamp_sample;			/**< the timestamp the data this control response is based on was sampled */
-	float	control[NUM_ACTUATOR_CONTROLS];
-};
+		task_should_exit = false;
 
-/**
- * @}
- */
+		daemon_task = task_spawn_cmd("mc_att_control_m",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       3000,
+				       main,
+					(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		task_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("is running");
 
-/* actuator control sets; this list can be expanded as more controllers emerge */
-ORB_DECLARE(actuator_controls_0);
-ORB_DECLARE(actuator_controls_1);
-ORB_DECLARE(actuator_controls_2);
-ORB_DECLARE(actuator_controls_3);
-ORB_DECLARE(actuator_controls_virtual_mc);
-ORB_DECLARE(actuator_controls_virtual_fw);
+		} else {
+			warnx("not started");
+		}
 
+		exit(0);
+	}
 
-#endif
+	warnx("unrecognized command");
+	return 1;
+}
diff --git a/src/modules/mc_att_control_multiplatform/module.mk b/src/modules/mc_att_control_multiplatform/module.mk
new file mode 100644
index 000000000..959f6492b
--- /dev/null
+++ b/src/modules/mc_att_control_multiplatform/module.mk
@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Multirotor attitude controller (vector based, no Euler singularities)
+#
+
+MODULE_COMMAND	= mc_att_control_m
+
+SRCS		= mc_att_control_main.cpp \
+		  mc_att_control_start_nuttx.cpp \
+		  mc_att_control.cpp \
+		  mc_att_control_base.cpp \
+		  mc_att_control_params.c
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 cad6cf3ae..b9692ffbf 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -49,8 +49,9 @@
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-#include <nuttx/config.h>
-#include <stdio.h>
+#include <px4.h>
+#include <functional>
+#include <cstdio>
 #include <stdlib.h>
 #include <string.h>
 #include <unistd.h>
@@ -60,8 +61,7 @@
 #include <poll.h>
 #include <drivers/drv_hrt.h>
 #include <arch/board/board.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
+
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
@@ -73,12 +73,13 @@
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_global_velocity_setpoint.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
-#include <systemlib/param/param.h>
-#include <systemlib/err.h>
+// #include <systemlib/param/param.h>
+// #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
 #include <mathlib/mathlib.h>
 #include <lib/geo/geo.h>
 #include <mavlink/mavlink_log.h>
+#include <platforms/px4_defines.h>
 
 #define TILT_COS_MAX	0.7f
 #define SIGMA			0.000001f
@@ -537,9 +538,9 @@ MulticopterPositionControl::reset_pos_sp()
 	if (_reset_pos_sp) {
 		_reset_pos_sp = false;
 		/* shift position setpoint to make attitude setpoint continuous */
-		_pos_sp(0) = _pos(0) + (_vel(0) - _att_sp.R_body[0][2] * _att_sp.thrust / _params.vel_p(0)
+		_pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust / _params.vel_p(0)
 				- _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
-		_pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1)
+		_pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust / _params.vel_p(1)
 				- _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
 		mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
 	}
@@ -767,7 +768,7 @@ MulticopterPositionControl::control_auto(float dt)
 		/* by default use current setpoint as is */
 		math::Vector<3> pos_sp_s = curr_sp_s;
 
-		if (_pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) {
+		if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) {
 			/* follow "previous - current" line */
 			math::Vector<3> prev_sp;
 			map_projection_project(&_ref_pos,
@@ -997,10 +998,10 @@ MulticopterPositionControl::task_main()
 			}
 
 
-			if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) {
+			if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
 				/* idle state, don't run controller and set zero thrust */
 				R.identity();
-				memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
+				memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
 				_att_sp.R_valid = true;
 
 				_att_sp.roll_body = 0.0f;
@@ -1036,7 +1037,7 @@ MulticopterPositionControl::task_main()
 				}
 
 				/* use constant descend rate when landing, ignore altitude setpoint */
-				if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
+				if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
 					_vel_sp(2) = _params.land_speed;
 				}
 
@@ -1123,7 +1124,7 @@ MulticopterPositionControl::task_main()
 
 					/* adjust limits for landing mode */
 					if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&
-					    _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
+					    _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
 						/* limit max tilt and min lift when landing */
 						tilt_max = _params.tilt_max_land;
 
@@ -1161,11 +1162,11 @@ MulticopterPositionControl::task_main()
 						/* thrust compensation for altitude only control mode */
 						float att_comp;
 
-						if (_att.R[2][2] > TILT_COS_MAX) {
-							att_comp = 1.0f / _att.R[2][2];
+						if (PX4_R(_att.R, 2, 2) > TILT_COS_MAX) {
+							att_comp = 1.0f / PX4_R(_att.R, 2, 2);
 
-						} else if (_att.R[2][2] > 0.0f) {
-							att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f;
+						} else if (PX4_R(_att.R, 2, 2) > 0.0f) {
+							att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att.R, 2, 2) + 1.0f;
 							saturation_z = true;
 
 						} else {
@@ -1273,7 +1274,7 @@ MulticopterPositionControl::task_main()
 						}
 
 						/* copy rotation matrix to attitude setpoint topic */
-						memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
+						memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
 						_att_sp.R_valid = true;
 
 						/* calculate euler angles, for logging only, must not be used for control */
@@ -1288,7 +1289,7 @@ MulticopterPositionControl::task_main()
 						R.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
 
 						/* copy rotation matrix to attitude setpoint topic */
-						memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
+						memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
 						_att_sp.R_valid = true;
 
 						_att_sp.roll_body = 0.0f;
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
new file mode 100644
index 000000000..a46163e6f
--- /dev/null
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
@@ -0,0 +1,949 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_pos_control.cpp
+ * Multicopter position controller.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include "mc_pos_control.h"
+#include "mc_pos_control_params.h"
+
+#define TILT_COS_MAX	0.7f
+#define SIGMA		0.000001f
+#define MIN_DIST	0.01f
+
+MulticopterPositionControl::MulticopterPositionControl() :
+
+	_task_should_exit(false),
+	_control_task(-1),
+	_mavlink_fd(-1),
+
+/* publications */
+	_att_sp_pub(nullptr),
+	_local_pos_sp_pub(nullptr),
+	_global_vel_sp_pub(nullptr),
+
+	_ref_alt(0.0f),
+	_ref_timestamp(0),
+
+	_reset_pos_sp(true),
+	_reset_alt_sp(true),
+	_mode_auto(false),
+	_thrust_int(),
+	_R()
+{
+	memset(&_ref_pos, 0, sizeof(_ref_pos));
+
+	/*
+	 * 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);
+	_local_pos_sp = _n.subscribe<px4_vehicle_local_position_setpoint>(0);
+	_global_vel_sp = _n.subscribe<px4_vehicle_global_velocity_setpoint>(0);
+
+
+
+	_params.pos_p.zero();
+	_params.vel_p.zero();
+	_params.vel_i.zero();
+	_params.vel_d.zero();
+	_params.vel_max.zero();
+	_params.vel_ff.zero();
+	_params.sp_offs_max.zero();
+
+	_pos.zero();
+	_pos_sp.zero();
+	_vel.zero();
+	_vel_sp.zero();
+	_vel_prev.zero();
+	_vel_ff.zero();
+	_sp_move_rate.zero();
+
+	_params_handles.thr_min		= PX4_PARAM_INIT(MPC_THR_MIN);
+	_params_handles.thr_max		= PX4_PARAM_INIT(MPC_THR_MAX);
+	_params_handles.z_p		= PX4_PARAM_INIT(MPC_Z_P);
+	_params_handles.z_vel_p		= PX4_PARAM_INIT(MPC_Z_VEL_P);
+	_params_handles.z_vel_i		= PX4_PARAM_INIT(MPC_Z_VEL_I);
+	_params_handles.z_vel_d		= PX4_PARAM_INIT(MPC_Z_VEL_D);
+	_params_handles.z_vel_max	= PX4_PARAM_INIT(MPC_Z_VEL_MAX);
+	_params_handles.z_ff		= PX4_PARAM_INIT(MPC_Z_FF);
+	_params_handles.xy_p		= PX4_PARAM_INIT(MPC_XY_P);
+	_params_handles.xy_vel_p	= PX4_PARAM_INIT(MPC_XY_VEL_P);
+	_params_handles.xy_vel_i	= PX4_PARAM_INIT(MPC_XY_VEL_I);
+	_params_handles.xy_vel_d	= PX4_PARAM_INIT(MPC_XY_VEL_D);
+	_params_handles.xy_vel_max	= PX4_PARAM_INIT(MPC_XY_VEL_MAX);
+	_params_handles.xy_ff		= PX4_PARAM_INIT(MPC_XY_FF);
+	_params_handles.tilt_max_air	= PX4_PARAM_INIT(MPC_TILTMAX_AIR);
+	_params_handles.land_speed	= PX4_PARAM_INIT(MPC_LAND_SPEED);
+	_params_handles.tilt_max_land	= PX4_PARAM_INIT(MPC_TILTMAX_LND);
+
+	/* fetch initial parameter values */
+	parameters_update();
+
+	_R.identity();
+}
+
+MulticopterPositionControl::~MulticopterPositionControl()
+{
+}
+
+int
+MulticopterPositionControl::parameters_update()
+{
+	PX4_PARAM_GET(_params_handles.thr_min, &_params.thr_min);
+	PX4_PARAM_GET(_params_handles.thr_max, &_params.thr_max);
+	PX4_PARAM_GET(_params_handles.tilt_max_air, &_params.tilt_max_air);
+	_params.tilt_max_air = math::radians(_params.tilt_max_air);
+	PX4_PARAM_GET(_params_handles.land_speed, &_params.land_speed);
+	PX4_PARAM_GET(_params_handles.tilt_max_land, &_params.tilt_max_land);
+	_params.tilt_max_land = math::radians(_params.tilt_max_land);
+
+	float v;
+	PX4_PARAM_GET(_params_handles.xy_p, &v);
+	_params.pos_p(0) = v;
+	_params.pos_p(1) = v;
+	PX4_PARAM_GET(_params_handles.z_p, &v);
+	_params.pos_p(2) = v;
+	PX4_PARAM_GET(_params_handles.xy_vel_p, &v);
+	_params.vel_p(0) = v;
+	_params.vel_p(1) = v;
+	PX4_PARAM_GET(_params_handles.z_vel_p, &v);
+	_params.vel_p(2) = v;
+	PX4_PARAM_GET(_params_handles.xy_vel_i, &v);
+	_params.vel_i(0) = v;
+	_params.vel_i(1) = v;
+	PX4_PARAM_GET(_params_handles.z_vel_i, &v);
+	_params.vel_i(2) = v;
+	PX4_PARAM_GET(_params_handles.xy_vel_d, &v);
+	_params.vel_d(0) = v;
+	_params.vel_d(1) = v;
+	PX4_PARAM_GET(_params_handles.z_vel_d, &v);
+	_params.vel_d(2) = v;
+	PX4_PARAM_GET(_params_handles.xy_vel_max, &v);
+	_params.vel_max(0) = v;
+	_params.vel_max(1) = v;
+	PX4_PARAM_GET(_params_handles.z_vel_max, &v);
+	_params.vel_max(2) = v;
+	PX4_PARAM_GET(_params_handles.xy_ff, &v);
+	v = math::constrain(v, 0.0f, 1.0f);
+	_params.vel_ff(0) = v;
+	_params.vel_ff(1) = v;
+	PX4_PARAM_GET(_params_handles.z_ff, &v);
+	v = math::constrain(v, 0.0f, 1.0f);
+	_params.vel_ff(2) = v;
+
+	_params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f;
+
+	return OK;
+}
+
+
+float
+MulticopterPositionControl::scale_control(float ctl, float end, float dz)
+{
+	if (ctl > dz) {
+		return (ctl - dz) / (end - dz);
+
+	} else if (ctl < -dz) {
+		return (ctl + dz) / (end - dz);
+
+	} else {
+		return 0.0f;
+	}
+}
+
+void
+MulticopterPositionControl::update_ref()
+{
+	if (_local_pos->data().ref_timestamp != _ref_timestamp) {
+		double lat_sp, lon_sp;
+		float alt_sp = 0.0f;
+
+		if (_ref_timestamp != 0) {
+			/* calculate current position setpoint in global frame */
+			map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp);
+			alt_sp = _ref_alt - _pos_sp(2);
+		}
+
+		/* update local projection reference */
+		map_projection_init(&_ref_pos, _local_pos->data().ref_lat, _local_pos->data().ref_lon);
+		_ref_alt = _local_pos->data().ref_alt;
+
+		if (_ref_timestamp != 0) {
+			/* reproject position setpoint to new reference */
+			map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]);
+			_pos_sp(2) = -(alt_sp - _ref_alt);
+		}
+
+		_ref_timestamp = _local_pos->data().ref_timestamp;
+	}
+}
+
+void
+MulticopterPositionControl::reset_pos_sp()
+{
+	if (_reset_pos_sp) {
+		_reset_pos_sp = false;
+		/* shift position setpoint to make attitude setpoint continuous */
+		_pos_sp(0) = _pos(0); //+ (_vel(0) - PX4_R(_att_sp_msg.data().R_body, 0, 2) * _att_sp_msg.data().thrust / _params.vel_p(0)
+				// - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
+		_pos_sp(1) = _pos(1); //+ (_vel(1) - PX4_R(_att_sp_msg.data().R_body, 1, 2) * _att_sp_msg.data().thrust / _params.vel_p(1)
+				// - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
+
+		//XXX: port this once a mavlink like interface is available
+		// mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
+		PX4_INFO("[mpc] reset pos sp: %2.3f, %2.3f", (double)_pos_sp(0), (double)_pos_sp(1));
+	}
+}
+
+void
+MulticopterPositionControl::reset_alt_sp()
+{
+	if (_reset_alt_sp) {
+		_reset_alt_sp = false;
+		_pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2);
+
+		//XXX: port this once a mavlink like interface is available
+		// mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2));
+		PX4_INFO("[mpc] reset alt sp: %2.3f", -(double)_pos_sp(2));
+	}
+}
+
+void
+MulticopterPositionControl::limit_pos_sp_offset()
+{
+	math::Vector<3> pos_sp_offs;
+	pos_sp_offs.zero();
+
+	if (_control_mode->data().flag_control_position_enabled) {
+		pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
+		pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
+	}
+
+	if (_control_mode->data().flag_control_altitude_enabled) {
+		pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
+	}
+
+	float pos_sp_offs_norm = pos_sp_offs.length();
+
+	if (pos_sp_offs_norm > 1.0f) {
+		pos_sp_offs /= pos_sp_offs_norm;
+		_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
+	}
+}
+
+void
+MulticopterPositionControl::control_manual(float dt)
+{
+	_sp_move_rate.zero();
+
+	if (_control_mode->data().flag_control_altitude_enabled) {
+		/* move altitude setpoint with throttle stick */
+		_sp_move_rate(2) = -scale_control(_manual_control_sp->data().z - 0.5f, 0.5f, alt_ctl_dz);
+	}
+
+	if (_control_mode->data().flag_control_position_enabled) {
+		/* move position setpoint with roll/pitch stick */
+		_sp_move_rate(0) = _manual_control_sp->data().x;
+		_sp_move_rate(1) = _manual_control_sp->data().y;
+	}
+
+	/* limit setpoint move rate */
+	float sp_move_norm = _sp_move_rate.length();
+
+	if (sp_move_norm > 1.0f) {
+		_sp_move_rate /= sp_move_norm;
+	}
+
+	/* _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);
+	_sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max);
+
+	if (_control_mode->data().flag_control_altitude_enabled) {
+		/* reset alt setpoint to current altitude if needed */
+		reset_alt_sp();
+	}
+
+	if (_control_mode->data().flag_control_position_enabled) {
+		/* reset position setpoint to current position if needed */
+		reset_pos_sp();
+	}
+
+	/* feed forward setpoint move rate with weight vel_ff */
+	_vel_ff = _sp_move_rate.emult(_params.vel_ff);
+
+	/* move position setpoint */
+	_pos_sp += _sp_move_rate * dt;
+
+	/* check if position setpoint is too far from actual position */
+	math::Vector<3> pos_sp_offs;
+	pos_sp_offs.zero();
+
+	if (_control_mode->data().flag_control_position_enabled) {
+		pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
+		pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
+	}
+
+	if (_control_mode->data().flag_control_altitude_enabled) {
+		pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
+	}
+
+	float pos_sp_offs_norm = pos_sp_offs.length();
+
+	if (pos_sp_offs_norm > 1.0f) {
+		pos_sp_offs /= pos_sp_offs_norm;
+		_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
+	}
+}
+
+void
+MulticopterPositionControl::control_offboard(float dt)
+{
+	if (_pos_sp_triplet->data().current.valid) {
+		if (_control_mode->data().flag_control_position_enabled && _pos_sp_triplet->data().current.position_valid) {
+			/* control position */
+			_pos_sp(0) = _pos_sp_triplet->data().current.x;
+			_pos_sp(1) = _pos_sp_triplet->data().current.y;
+		} else if (_control_mode->data().flag_control_velocity_enabled && _pos_sp_triplet->data().current.velocity_valid) {
+			/* control velocity */
+			/* reset position setpoint to current position if needed */
+			reset_pos_sp();
+
+			/* set position setpoint move rate */
+			_sp_move_rate(0) = _pos_sp_triplet->data().current.vx;
+			_sp_move_rate(1) = _pos_sp_triplet->data().current.vy;
+		}
+
+		if (_pos_sp_triplet->data().current.yaw_valid) {
+			_att_sp_msg.data().yaw_body = _pos_sp_triplet->data().current.yaw;
+		} else if (_pos_sp_triplet->data().current.yawspeed_valid) {
+			_att_sp_msg.data().yaw_body = _att_sp_msg.data().yaw_body + _pos_sp_triplet->data().current.yawspeed * dt;
+		}
+
+		if (_control_mode->data().flag_control_altitude_enabled && _pos_sp_triplet->data().current.position_valid) {
+			/* Control altitude */
+			_pos_sp(2) = _pos_sp_triplet->data().current.z;
+		} else if (_control_mode->data().flag_control_climb_rate_enabled && _pos_sp_triplet->data().current.velocity_valid) {
+			/* reset alt setpoint to current altitude if needed */
+			reset_alt_sp();
+
+			/* set altitude setpoint move rate */
+			_sp_move_rate(2) = _pos_sp_triplet->data().current.vz;
+		}
+
+		/* feed forward setpoint move rate with weight vel_ff */
+		_vel_ff = _sp_move_rate.emult(_params.vel_ff);
+
+		/* move position setpoint */
+		_pos_sp += _sp_move_rate * dt;
+
+	} else {
+		reset_pos_sp();
+		reset_alt_sp();
+	}
+}
+
+bool
+MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r,
+		const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res)
+{
+	/* project center of sphere on line */
+	/* normalized AB */
+	math::Vector<3> ab_norm = line_b - line_a;
+	ab_norm.normalize();
+	math::Vector<3> d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm);
+	float cd_len = (sphere_c - d).length();
+
+	/* we have triangle CDX with known CD and CX = R, find DX */
+	if (sphere_r > cd_len) {
+		/* have two roots, select one in A->B direction from D */
+		float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len);
+		res = d + ab_norm * dx_len;
+		return true;
+
+	} else {
+		/* have no roots, return D */
+		res = d;
+		return false;
+	}
+}
+
+void
+MulticopterPositionControl::control_auto(float dt)
+{
+	if (!_mode_auto) {
+		_mode_auto = true;
+		/* reset position setpoint on AUTO mode activation */
+		reset_pos_sp();
+		reset_alt_sp();
+	}
+
+	if (_pos_sp_triplet->data().current.valid) {
+		/* in case of interrupted mission don't go to waypoint but stay at current position */
+		_reset_pos_sp = true;
+		_reset_alt_sp = true;
+
+		/* project setpoint to local frame */
+		math::Vector<3> curr_sp;
+		map_projection_project(&_ref_pos,
+				       _pos_sp_triplet->data().current.lat, _pos_sp_triplet->data().current.lon,
+				       &curr_sp.data[0], &curr_sp.data[1]);
+		curr_sp(2) = -(_pos_sp_triplet->data().current.alt - _ref_alt);
+
+		/* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */
+		math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max);	// TODO add mult param here
+
+		/* convert current setpoint to scaled space */
+		math::Vector<3> curr_sp_s = curr_sp.emult(scale);
+
+		/* by default use current setpoint as is */
+		math::Vector<3> pos_sp_s = curr_sp_s;
+
+		if (_pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_POSITION && _pos_sp_triplet->data().previous.valid) {
+			/* follow "previous - current" line */
+			math::Vector<3> prev_sp;
+			map_projection_project(&_ref_pos,
+						   _pos_sp_triplet->data().previous.lat, _pos_sp_triplet->data().previous.lon,
+						   &prev_sp.data[0], &prev_sp.data[1]);
+			prev_sp(2) = -(_pos_sp_triplet->data().previous.alt - _ref_alt);
+
+			if ((curr_sp - prev_sp).length() > MIN_DIST) {
+
+				/* find X - cross point of L1 sphere and trajectory */
+				math::Vector<3> pos_s = _pos.emult(scale);
+				math::Vector<3> prev_sp_s = prev_sp.emult(scale);
+				math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s;
+				math::Vector<3> curr_pos_s = pos_s - curr_sp_s;
+				float curr_pos_s_len = curr_pos_s.length();
+				if (curr_pos_s_len < 1.0f) {
+					/* copter is closer to waypoint than L1 radius */
+					/* check next waypoint and use it to avoid slowing down when passing via waypoint */
+					if (_pos_sp_triplet->data().next.valid) {
+						math::Vector<3> next_sp;
+						map_projection_project(&_ref_pos,
+									   _pos_sp_triplet->data().next.lat, _pos_sp_triplet->data().next.lon,
+									   &next_sp.data[0], &next_sp.data[1]);
+						next_sp(2) = -(_pos_sp_triplet->data().next.alt - _ref_alt);
+
+						if ((next_sp - curr_sp).length() > MIN_DIST) {
+							math::Vector<3> next_sp_s = next_sp.emult(scale);
+
+							/* calculate angle prev - curr - next */
+							math::Vector<3> curr_next_s = next_sp_s - curr_sp_s;
+							math::Vector<3> prev_curr_s_norm = prev_curr_s.normalized();
+
+							/* cos(a) * curr_next, a = angle between current and next trajectory segments */
+							float cos_a_curr_next = prev_curr_s_norm * curr_next_s;
+
+							/* cos(b), b = angle pos - curr_sp - prev_sp */
+							float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len;
+
+							if (cos_a_curr_next > 0.0f && cos_b > 0.0f) {
+								float curr_next_s_len = curr_next_s.length();
+								/* if curr - next distance is larger than L1 radius, limit it */
+								if (curr_next_s_len > 1.0f) {
+									cos_a_curr_next /= curr_next_s_len;
+								}
+
+								/* feed forward position setpoint offset */
+								math::Vector<3> pos_ff = prev_curr_s_norm *
+										cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) *
+										(1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f));
+								pos_sp_s += pos_ff;
+							}
+						}
+					}
+
+				} else {
+					bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s);
+					if (near) {
+						/* L1 sphere crosses trajectory */
+
+					} else {
+						/* copter is too far from trajectory */
+						/* if copter is behind prev waypoint, go directly to prev waypoint */
+						if ((pos_sp_s - prev_sp_s) * prev_curr_s < 0.0f) {
+							pos_sp_s = prev_sp_s;
+						}
+
+						/* if copter is in front of curr waypoint, go directly to curr waypoint */
+						if ((pos_sp_s - curr_sp_s) * prev_curr_s > 0.0f) {
+							pos_sp_s = curr_sp_s;
+						}
+
+						pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized();
+					}
+				}
+			}
+		}
+
+		/* move setpoint not faster than max allowed speed */
+		math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale);
+
+		/* difference between current and desired position setpoints, 1 = max speed */
+		math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p);
+		float d_pos_m_len = d_pos_m.length();
+		if (d_pos_m_len > dt) {
+			pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p);
+		}
+
+		/* scale result back to normal space */
+		_pos_sp = pos_sp_s.edivide(scale);
+
+		/* update yaw setpoint if needed */
+		if (isfinite(_pos_sp_triplet->data().current.yaw)) {
+			_att_sp_msg.data().yaw_body = _pos_sp_triplet->data().current.yaw;
+		}
+
+	} else {
+		/* no waypoint, do nothing, setpoint was already reset */
+	}
+}
+
+void MulticopterPositionControl::handle_parameter_update(const px4_parameter_update &msg)
+{
+	parameters_update();
+}
+
+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 was_armed = false;
+	static uint64_t t_prev = 0;
+
+	uint64_t t = get_time_micros();
+	float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.005f;
+	t_prev = t;
+
+	if (_control_mode->data().flag_armed && !was_armed) {
+		/* reset setpoints and integrals on arming */
+		_reset_pos_sp = true;
+		_reset_alt_sp = true;
+		reset_int_z = true;
+		reset_int_xy = true;
+	}
+
+	was_armed = _control_mode->data().flag_armed;
+
+	update_ref();
+
+	if (_control_mode->data().flag_control_altitude_enabled ||
+		_control_mode->data().flag_control_position_enabled ||
+		_control_mode->data().flag_control_climb_rate_enabled ||
+		_control_mode->data().flag_control_velocity_enabled) {
+
+		_pos(0) = _local_pos->data().x;
+		_pos(1) = _local_pos->data().y;
+		_pos(2) = _local_pos->data().z;
+
+		_vel(0) = _local_pos->data().vx;
+		_vel(1) = _local_pos->data().vy;
+		_vel(2) = _local_pos->data().vz;
+
+		_vel_ff.zero();
+		_sp_move_rate.zero();
+
+		/* select control source */
+		if (_control_mode->data().flag_control_manual_enabled) {
+			/* manual control */
+			control_manual(dt);
+			_mode_auto = false;
+
+		} else if (_control_mode->data().flag_control_offboard_enabled) {
+			/* offboard control */
+			control_offboard(dt);
+			_mode_auto = false;
+
+		} else {
+			/* AUTO */
+			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();
+			memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body));
+			_att_sp_msg.data().R_valid = true;
+
+			_att_sp_msg.data().roll_body = 0.0f;
+			_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();
+
+			/* publish attitude setpoint */
+			if (_att_sp_pub != nullptr) {
+				_att_sp_pub->publish(_att_sp_msg);
+
+			} else {
+				_att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>();
+			}
+
+		} else {
+			/* run position & altitude controllers, calculate velocity setpoint */
+			math::Vector<3> pos_err = _pos_sp - _pos;
+
+			_vel_sp = pos_err.emult(_params.pos_p) + _vel_ff;
+
+			if (!_control_mode->data().flag_control_altitude_enabled) {
+				_reset_alt_sp = true;
+				_vel_sp(2) = 0.0f;
+			}
+
+			if (!_control_mode->data().flag_control_position_enabled) {
+				_reset_pos_sp = true;
+				_vel_sp(0) = 0.0f;
+				_vel_sp(1) = 0.0f;
+			}
+
+			/* use constant descend rate when landing, ignore altitude 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_LAND) {
+				_vel_sp(2) = _params.land_speed;
+			}
+
+			_global_vel_sp_msg.data().vx = _vel_sp(0);
+			_global_vel_sp_msg.data().vy = _vel_sp(1);
+			_global_vel_sp_msg.data().vz = _vel_sp(2);
+
+			/* publish velocity setpoint */
+			if (_global_vel_sp_pub != nullptr) {
+				_global_vel_sp_pub->publish(_global_vel_sp_msg);
+
+			} else {
+				_global_vel_sp_pub = _n.advertise<px4_vehicle_global_velocity_setpoint>();
+			}
+
+			if (_control_mode->data().flag_control_climb_rate_enabled || _control_mode->data().flag_control_velocity_enabled) {
+				/* reset integrals if needed */
+				if (_control_mode->data().flag_control_climb_rate_enabled) {
+					if (reset_int_z) {
+						reset_int_z = false;
+						float i = _params.thr_min;
+
+						if (reset_int_z_manual) {
+							i = _manual_control_sp->data().z;
+
+							if (i < _params.thr_min) {
+								i = _params.thr_min;
+
+							} else if (i > _params.thr_max) {
+								i = _params.thr_max;
+							}
+						}
+
+						_thrust_int(2) = -i;
+					}
+
+				} else {
+					reset_int_z = true;
+				}
+
+				if (_control_mode->data().flag_control_velocity_enabled) {
+					if (reset_int_xy) {
+						reset_int_xy = false;
+						_thrust_int(0) = 0.0f;
+						_thrust_int(1) = 0.0f;
+					}
+
+				} else {
+					reset_int_xy = true;
+				}
+
+				/* velocity error */
+				math::Vector<3> vel_err = _vel_sp - _vel;
+
+				/* derivative of velocity error, not includes setpoint acceleration */
+				math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
+				_vel_prev = _vel;
+
+				/* thrust vector in NED frame */
+				math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + _thrust_int;
+
+				if (!_control_mode->data().flag_control_velocity_enabled) {
+					thrust_sp(0) = 0.0f;
+					thrust_sp(1) = 0.0f;
+				}
+
+				if (!_control_mode->data().flag_control_climb_rate_enabled) {
+					thrust_sp(2) = 0.0f;
+				}
+
+				/* limit thrust vector and check for saturation */
+				bool saturation_xy = false;
+				bool saturation_z = false;
+
+				/* limit min lift */
+				float thr_min = _params.thr_min;
+
+				if (!_control_mode->data().flag_control_velocity_enabled && thr_min < 0.0f) {
+					/* don't allow downside thrust direction in manual attitude mode */
+					thr_min = 0.0f;
+				}
+
+				float tilt_max = _params.tilt_max_air;
+
+				/* adjust limits for landing mode */
+				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_LAND) {
+					/* limit max tilt and min lift when landing */
+					tilt_max = _params.tilt_max_land;
+
+					if (thr_min < 0.0f) {
+						thr_min = 0.0f;
+					}
+				}
+
+				/* limit min lift */
+				if (-thrust_sp(2) < thr_min) {
+					thrust_sp(2) = -thr_min;
+					saturation_z = true;
+				}
+
+				if (_control_mode->data().flag_control_velocity_enabled) {
+					/* limit max tilt */
+					if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) {
+						/* absolute horizontal thrust */
+						float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
+
+						if (thrust_sp_xy_len > 0.01f) {
+							/* max horizontal thrust for given vertical thrust*/
+							float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max);
+
+							if (thrust_sp_xy_len > thrust_xy_max) {
+								float k = thrust_xy_max / thrust_sp_xy_len;
+								thrust_sp(0) *= k;
+								thrust_sp(1) *= k;
+								saturation_xy = true;
+							}
+						}
+					}
+
+				} else {
+					/* thrust compensation for altitude only control mode */
+					float att_comp;
+
+					if (PX4_R(_att->data().R, 2, 2) > TILT_COS_MAX) {
+						att_comp = 1.0f / PX4_R(_att->data().R, 2, 2);
+
+					} else if (PX4_R(_att->data().R, 2, 2) > 0.0f) {
+						att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att->data().R, 2, 2) + 1.0f;
+						saturation_z = true;
+
+					} else {
+						att_comp = 1.0f;
+						saturation_z = true;
+					}
+
+					thrust_sp(2) *= att_comp;
+				}
+
+				/* limit max thrust */
+				float thrust_abs = thrust_sp.length();
+
+				if (thrust_abs > _params.thr_max) {
+					if (thrust_sp(2) < 0.0f) {
+						if (-thrust_sp(2) > _params.thr_max) {
+							/* thrust Z component is too large, limit it */
+							thrust_sp(0) = 0.0f;
+							thrust_sp(1) = 0.0f;
+							thrust_sp(2) = -_params.thr_max;
+							saturation_xy = true;
+							saturation_z = true;
+
+						} else {
+							/* preserve thrust Z component and lower XY, keeping altitude is more important than position */
+							float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2));
+							float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
+							float k = thrust_xy_max / thrust_xy_abs;
+							thrust_sp(0) *= k;
+							thrust_sp(1) *= k;
+							saturation_xy = true;
+						}
+
+					} else {
+						/* Z component is negative, going down, simply limit thrust vector */
+						float k = _params.thr_max / thrust_abs;
+						thrust_sp *= k;
+						saturation_xy = true;
+						saturation_z = true;
+					}
+
+					thrust_abs = _params.thr_max;
+				}
+
+				/* update integrals */
+				if (_control_mode->data().flag_control_velocity_enabled && !saturation_xy) {
+					_thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt;
+					_thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt;
+				}
+
+				if (_control_mode->data().flag_control_climb_rate_enabled && !saturation_z) {
+					_thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt;
+
+					/* protection against flipping on ground when landing */
+					if (_thrust_int(2) > 0.0f) {
+						_thrust_int(2) = 0.0f;
+					}
+				}
+
+				/* calculate attitude setpoint from thrust vector */
+				if (_control_mode->data().flag_control_velocity_enabled) {
+					/* desired body_z axis = -normalize(thrust_vector) */
+					math::Vector<3> body_x;
+					math::Vector<3> body_y;
+					math::Vector<3> body_z;
+
+					if (thrust_abs > SIGMA) {
+						body_z = -thrust_sp / thrust_abs;
+
+					} else {
+						/* no thrust, set Z axis to safe value */
+						body_z.zero();
+						body_z(2) = 1.0f;
+					}
+
+					/* vector of desired yaw direction in XY plane, rotated by PI/2 */
+					math::Vector<3> y_C(-sinf(_att_sp_msg.data().yaw_body), cosf(_att_sp_msg.data().yaw_body), 0.0f);
+
+					if (fabsf(body_z(2)) > SIGMA) {
+						/* desired body_x axis, orthogonal to body_z */
+						body_x = y_C % body_z;
+
+						/* keep nose to front while inverted upside down */
+						if (body_z(2) < 0.0f) {
+							body_x = -body_x;
+						}
+
+						body_x.normalize();
+
+					} else {
+						/* desired thrust is in XY plane, set X downside to construct correct matrix,
+							* but yaw component will not be used actually */
+						body_x.zero();
+						body_x(2) = 1.0f;
+					}
+
+					/* desired body_y axis */
+					body_y = body_z % body_x;
+
+					/* fill rotation matrix */
+					for (int i = 0; i < 3; i++) {
+						_R(i, 0) = body_x(i);
+						_R(i, 1) = body_y(i);
+						_R(i, 2) = body_z(i);
+					}
+
+					/* copy rotation matrix to attitude setpoint topic */
+					memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body));
+					_att_sp_msg.data().R_valid = true;
+
+					/* calculate euler angles, for logging only, must not be used for control */
+					math::Vector<3> euler = _R.to_euler();
+					_att_sp_msg.data().roll_body = euler(0);
+					_att_sp_msg.data().pitch_body = euler(1);
+					/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
+
+				} else if (!_control_mode->data().flag_control_manual_enabled) {
+					/* autonomous altitude control without position control (failsafe landing),
+						* force level attitude, don't change yaw */
+					_R.from_euler(0.0f, 0.0f, _att_sp_msg.data().yaw_body);
+
+					/* copy rotation matrix to attitude setpoint topic */
+					memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body));
+					_att_sp_msg.data().R_valid = true;
+
+					_att_sp_msg.data().roll_body = 0.0f;
+					_att_sp_msg.data().pitch_body = 0.0f;
+				}
+
+				_att_sp_msg.data().thrust = thrust_abs;
+
+				_att_sp_msg.data().timestamp = get_time_micros();
+
+				/* publish attitude setpoint */
+				if (_att_sp_pub != nullptr) {
+					_att_sp_pub->publish(_att_sp_msg);
+
+				} else {
+					_att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>();
+				}
+
+			} else {
+				reset_int_z = true;
+			}
+		}
+
+	} else {
+		/* position controller disabled, reset setpoints */
+		_reset_alt_sp = true;
+		_reset_pos_sp = true;
+		_mode_auto = false;
+		reset_int_z = true;
+		reset_int_xy = true;
+	}
+
+	/* 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
new file mode 100644
index 000000000..05bd1387b
--- /dev/null
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h
@@ -0,0 +1,219 @@
+/****************************************************************************
+ *
+ *   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
+ * 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 mc_pos_control.h
+ * Multicopter position controller.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#pragma once
+
+#include <px4.h>
+#include <cstdio>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+// #include <poll.h>
+// #include <drivers/drv_hrt.h>
+// #include <arch/board/board.h>
+// #include <systemlib/systemlib.h>
+#include <mathlib/mathlib.h>
+#include <lib/geo/geo.h>
+// #include <mavlink/mavlink_log.h>
+
+using namespace px4;
+
+class MulticopterPositionControl
+{
+public:
+	/**
+	 * Constructor
+	 */
+	MulticopterPositionControl();
+
+	/**
+	 * Destructor, also kills task.
+	 */
+	~MulticopterPositionControl();
+
+	/* Callbacks for topics */
+	void handle_vehicle_attitude(const px4_vehicle_attitude &msg);
+	void handle_parameter_update(const px4_parameter_update &msg);
+
+	void spin() { _n.spin(); }
+
+protected:
+	const float alt_ctl_dz = 0.2f;
+
+	bool		_task_should_exit;		/**< if true, task should exit */
+	int		_control_task;			/**< task handle for task */
+	int		_mavlink_fd;			/**< mavlink fd */
+
+	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 */
+
+
+	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 */
+
+	px4_vehicle_attitude_setpoint _att_sp_msg;
+	px4_vehicle_local_position_setpoint _local_pos_sp_msg;
+	px4_vehicle_global_velocity_setpoint _global_vel_sp_msg;
+
+	px4::NodeHandle _n;
+
+
+	struct {
+		px4_param_t thr_min;
+		px4_param_t thr_max;
+		px4_param_t z_p;
+		px4_param_t z_vel_p;
+		px4_param_t z_vel_i;
+		px4_param_t z_vel_d;
+		px4_param_t z_vel_max;
+		px4_param_t z_ff;
+		px4_param_t xy_p;
+		px4_param_t xy_vel_p;
+		px4_param_t xy_vel_i;
+		px4_param_t xy_vel_d;
+		px4_param_t xy_vel_max;
+		px4_param_t xy_ff;
+		px4_param_t tilt_max_air;
+		px4_param_t land_speed;
+		px4_param_t tilt_max_land;
+	}		_params_handles;		/**< handles for interesting parameters */
+
+	struct {
+		float thr_min;
+		float thr_max;
+		float tilt_max_air;
+		float land_speed;
+		float tilt_max_land;
+
+		math::Vector<3> pos_p;
+		math::Vector<3> vel_p;
+		math::Vector<3> vel_i;
+		math::Vector<3> vel_d;
+		math::Vector<3> vel_ff;
+		math::Vector<3> vel_max;
+		math::Vector<3> sp_offs_max;
+	}		_params;
+
+	struct map_projection_reference_s _ref_pos;
+	float _ref_alt;
+	uint64_t _ref_timestamp;
+
+	bool _reset_pos_sp;
+	bool _reset_alt_sp;
+	bool _mode_auto;
+
+	math::Vector<3> _pos;
+	math::Vector<3> _pos_sp;
+	math::Vector<3> _vel;
+	math::Vector<3> _vel_sp;
+	math::Vector<3> _vel_prev;			/**< velocity on previous step */
+	math::Vector<3> _vel_ff;
+	math::Vector<3> _sp_move_rate;
+
+	math::Vector<3> _thrust_int;
+	math::Matrix<3, 3> _R;
+
+	/**
+	 * Update our local parameter cache.
+	 */
+	int			parameters_update();
+
+	/**
+	 * Update control outputs
+	 */
+	void		control_update();
+
+	static float	scale_control(float ctl, float end, float dz);
+
+	/**
+	 * Update reference for local position projection
+	 */
+	void		update_ref();
+	/**
+	 * Reset position setpoint to current position
+	 */
+	void		reset_pos_sp();
+
+	/**
+	 * Reset altitude setpoint to current altitude
+	 */
+	void		reset_alt_sp();
+
+	/**
+	 * Check if position setpoint is too far from current position and adjust it if needed.
+	 */
+	void		limit_pos_sp_offset();
+
+	/**
+	 * Set position setpoint using manual control
+	 */
+	void		control_manual(float dt);
+
+	/**
+	 * Set position setpoint using offboard control
+	 */
+	void		control_offboard(float dt);
+
+	bool		cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r,
+					const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res);
+
+	/**
+	 * Set position setpoint for AUTO
+	 */
+	void		control_auto(float dt);
+
+	/**
+	 * Select between barometric and global (AMSL) altitudes
+	 */
+	void		select_alt(bool global);
+};
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp
new file mode 100644
index 000000000..0b5775736
--- /dev/null
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp
@@ -0,0 +1,63 @@
+/****************************************************************************
+ *
+ *   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
+ * 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 mc_pos_control_main.cpp
+ * Multicopter position controller.
+ *
+ * The controller has two loops: P loop for position error and PID loop for velocity error.
+ * Output of velocity controller is thrust vector that splitted to thrust direction
+ * (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself).
+ * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include "mc_pos_control.h"
+
+bool thread_running = false;     /**< Deamon status flag */
+
+int main(int argc, char **argv)
+{
+	px4::init(argc, argv, "mc_pos_control_m");
+
+	PX4_INFO("starting");
+	MulticopterPositionControl posctl;
+	thread_running = true;
+	posctl.spin();
+
+	PX4_INFO("exiting.");
+	thread_running = false;
+	return 0;
+}
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
new file mode 100644
index 000000000..c741a7f0a
--- /dev/null
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c
@@ -0,0 +1,212 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: @author Anton Babushkin <anton.babushkin@me.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) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_pos_control_params.c
+ * Multicopter position controller parameters.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <px4_defines.h>
+#include "mc_pos_control_params.h"
+#include <systemlib/param/param.h>
+
+/**
+ * Minimum thrust
+ *
+ * Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_THR_MIN);
+
+/**
+ * Maximum thrust
+ *
+ * Limit max allowed thrust.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_THR_MAX);
+
+/**
+ * Proportional gain for vertical position error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_Z_P);
+
+/**
+ * Proportional gain for vertical velocity error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_P);
+
+/**
+ * Integral gain for vertical velocity error
+ *
+ * Non zero value allows hovering thrust estimation on stabilized or autonomous takeoff.
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_I);
+
+/**
+ * Differential gain for vertical velocity error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_D);
+
+/**
+ * Maximum vertical velocity
+ *
+ * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL).
+ *
+ * @unit m/s
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX);
+
+/**
+ * Vertical velocity feed forward
+ *
+ * Feed forward weight for altitude control in stabilized modes (ALTCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_Z_FF);
+
+/**
+ * Proportional gain for horizontal position error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_XY_P);
+
+/**
+ * Proportional gain for horizontal velocity error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_P);
+
+/**
+ * Integral gain for horizontal velocity error
+ *
+ * Non-zero value allows to resist wind.
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_I);
+
+/**
+ * Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_D);
+
+/**
+ * Maximum horizontal velocity
+ *
+ * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSCTRL).
+ *
+ * @unit m/s
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX);
+
+/**
+ * Horizontal velocity feed forward
+ *
+ * Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_XY_FF);
+
+/**
+ * Maximum tilt angle in air
+ *
+ * Limits maximum tilt in AUTO and POSCTRL modes during flight.
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR);
+
+/**
+ * Maximum tilt during landing
+ *
+ * Limits maximum tilt angle on landing.
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND);
+
+/**
+ * Landing descend rate
+ *
+ * @unit m/s
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_LAND_SPEED);
+
diff --git a/src/modules/uORB/topics/vehicle_local_position_setpoint.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h
index 6766bb58a..fec3bcb7c 100644
--- a/src/modules/uORB/topics/vehicle_local_position_setpoint.h
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h
@@ -1,9 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: @author Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,33 +33,29 @@
  ****************************************************************************/
 
 /**
- * @file vehicle_local_position_setpoint.h
- * Definition of the local NED position setpoint uORB topic.
- */
-
-#ifndef TOPIC_VEHICLE_LOCAL_POSITION_SETPOINT_H_
-#define TOPIC_VEHICLE_LOCAL_POSITION_SETPOINT_H_
-
-#include "../uORB.h"
-
-/**
- * @addtogroup topics
- * @{
- */
-
-struct vehicle_local_position_setpoint_s {
-	uint64_t timestamp;	/**< timestamp of the setpoint */
-	float x;		/**< in meters NED			  		*/
-	float y;		/**< in meters NED			  		*/
-	float z;		/**< in meters NED			  		*/
-	float yaw;		/**< in radians NED -PI..+PI  		*/
-}; /**< Local position in NED frame */
-
-/**
- * @}
+ * @file mc_pos_control_params.h
+ * Multicopter position controller parameters.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-/* register this as object request broker structure */
-ORB_DECLARE(vehicle_local_position_setpoint);
+#pragma once
+
+#define PARAM_MPC_THR_MIN_DEFAULT 0.1f
+#define PARAM_MPC_THR_MAX_DEFAULT 1.0f
+#define PARAM_MPC_Z_P_DEFAULT 1.0f
+#define PARAM_MPC_Z_VEL_P_DEFAULT 0.1f
+#define PARAM_MPC_Z_VEL_I_DEFAULT 0.02f
+#define PARAM_MPC_Z_VEL_D_DEFAULT 0.0f
+#define PARAM_MPC_Z_VEL_MAX_DEFAULT 5.0f
+#define PARAM_MPC_Z_FF_DEFAULT 0.5f
+#define PARAM_MPC_XY_P_DEFAULT 1.0f
+#define PARAM_MPC_XY_VEL_P_DEFAULT 0.1f
+#define PARAM_MPC_XY_VEL_I_DEFAULT 0.02f
+#define PARAM_MPC_XY_VEL_D_DEFAULT 0.01f
+#define PARAM_MPC_XY_VEL_MAX_DEFAULT 5.0f
+#define PARAM_MPC_XY_FF_DEFAULT 0.5f
+#define PARAM_MPC_TILTMAX_AIR_DEFAULT 45.0f
+#define PARAM_MPC_TILTMAX_LND_DEFAULT 15.0f
+#define PARAM_MPC_LAND_SPEED_DEFAULT 1.0f
 
-#endif
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
new file mode 100644
index 000000000..87996d93b
--- /dev/null
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   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
+ * 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 mc_pos_control_m_start_nuttx.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include <string.h>
+#include <cstdlib>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+extern bool thread_running;
+int daemon_task;             /**< Handle of deamon task / thread */
+namespace px4
+{
+bool task_should_exit = false;
+}
+using namespace px4;
+
+extern int main(int argc, char **argv);
+
+extern "C" __EXPORT int mc_pos_control_m_main(int argc, char *argv[]);
+int mc_pos_control_m_main(int argc, char *argv[])
+{
+	if (argc < 1) {
+		errx(1, "usage: mc_pos_control_m {start|stop|status}");
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("already running");
+			/* this is not an error */
+			exit(0);
+		}
+
+		task_should_exit = false;
+
+		daemon_task = task_spawn_cmd("mc_pos_control_m",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       3000,
+				       main,
+					(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		task_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("is running");
+
+		} else {
+			warnx("not started");
+		}
+
+		exit(0);
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}
diff --git a/src/modules/mc_pos_control_multiplatform/module.mk b/src/modules/mc_pos_control_multiplatform/module.mk
new file mode 100644
index 000000000..2852ebbec
--- /dev/null
+++ b/src/modules/mc_pos_control_multiplatform/module.mk
@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Build multicopter position controller
+#
+
+MODULE_COMMAND	= mc_pos_control_m
+
+SRCS		= mc_pos_control_main.cpp \
+		  mc_pos_control_start_nuttx.cpp \
+		  mc_pos_control.cpp \
+		  mc_pos_control_params.c
diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp
index f827e70c9..a744d58cf 100644
--- a/src/modules/navigator/loiter.cpp
+++ b/src/modules/navigator/loiter.cpp
@@ -82,7 +82,7 @@ Loiter::on_activation()
 	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->next.valid = false;
 
-	_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
+	_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
 
 	_navigator->set_position_setpoint_triplet_updated();
 }
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index b52b12ce7..a94082d6f 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -397,7 +397,7 @@ Mission::set_mission_items()
 		pos_sp_triplet->next.valid = false;
 
 		/* reuse setpoint for LOITER only if it's not IDLE */
-		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
+		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
 
 		set_mission_finished();
 
@@ -476,7 +476,7 @@ Mission::set_mission_items()
 	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
 
 	/* require takeoff after landing or idle */
-	if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) {
+	if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
 		_need_takeoff = true;
 	}
 
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index e39fb3216..52ccebac9 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -194,25 +194,25 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 
 	switch (item->nav_cmd) {
 	case NAV_CMD_IDLE:
-		sp->type = SETPOINT_TYPE_IDLE;
+		sp->type = position_setpoint_s::SETPOINT_TYPE_IDLE;
 		break;
 
 	case NAV_CMD_TAKEOFF:
-		sp->type = SETPOINT_TYPE_TAKEOFF;
+		sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
 		break;
 
 	case NAV_CMD_LAND:
-		sp->type = SETPOINT_TYPE_LAND;
+		sp->type = position_setpoint_s::SETPOINT_TYPE_LAND;
 		break;
 
 	case NAV_CMD_LOITER_TIME_LIMIT:
 	case NAV_CMD_LOITER_TURN_COUNT:
 	case NAV_CMD_LOITER_UNLIMITED:
-		sp->type = SETPOINT_TYPE_LOITER;
+		sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
 		break;
 
 	default:
-		sp->type = SETPOINT_TYPE_POSITION;
+		sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
 		break;
 	}
 }
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index edc61dcc6..ad2349c94 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -217,7 +217,7 @@ Navigator::vehicle_status_update()
 {
 	if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) {
 		/* in case the commander is not be running */
-		_vstatus.arming_state = ARMING_STATE_STANDBY;
+		_vstatus.arming_state = vehicle_status_s::ARMING_STATE_STANDBY;
 	}
 }
 
@@ -416,25 +416,25 @@ Navigator::task_main()
 
 		/* Do stuff according to navigation state set by commander */
 		switch (_vstatus.nav_state) {
-			case NAVIGATION_STATE_MANUAL:
-			case NAVIGATION_STATE_ACRO:
-			case NAVIGATION_STATE_ALTCTL:
-			case NAVIGATION_STATE_POSCTL:
-			case NAVIGATION_STATE_LAND:
-			case NAVIGATION_STATE_TERMINATION:
-			case NAVIGATION_STATE_OFFBOARD:
+			case vehicle_status_s::NAVIGATION_STATE_MANUAL:
+			case vehicle_status_s::NAVIGATION_STATE_ACRO:
+			case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
+			case vehicle_status_s::NAVIGATION_STATE_POSCTL:
+			case vehicle_status_s::NAVIGATION_STATE_LAND:
+			case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
+			case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
 				_navigation_mode = nullptr;
 				_can_loiter_at_sp = false;
 				break;
-			case NAVIGATION_STATE_AUTO_MISSION:
+			case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
 				_pos_sp_triplet_published_invalid_once = false;
 				_navigation_mode = &_mission;
 				break;
-			case NAVIGATION_STATE_AUTO_LOITER:
+			case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
 				_pos_sp_triplet_published_invalid_once = false;
 				_navigation_mode = &_loiter;
 				break;
-			case NAVIGATION_STATE_AUTO_RCRECOVER:
+			case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
 				_pos_sp_triplet_published_invalid_once = false;
 				if (_param_rcloss_obc.get() != 0) {
 					_navigation_mode = &_rcLoss;
@@ -442,11 +442,11 @@ Navigator::task_main()
 					_navigation_mode = &_rtl;
 				}
 				break;
-			case NAVIGATION_STATE_AUTO_RTL:
+			case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
 				_pos_sp_triplet_published_invalid_once = false;
 				_navigation_mode = &_rtl;
 				break;
-			case NAVIGATION_STATE_AUTO_RTGS:
+			case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
 				/* Use complex data link loss mode only when enabled via param
 				* otherwise use rtl */
 				_pos_sp_triplet_published_invalid_once = false;
@@ -456,11 +456,11 @@ Navigator::task_main()
 					_navigation_mode = &_rtl;
 				}
 				break;
-			case NAVIGATION_STATE_AUTO_LANDENGFAIL:
+			case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
 				_pos_sp_triplet_published_invalid_once = false;
 				_navigation_mode = &_engineFailure;
 				break;
-			case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
+			case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
 				_pos_sp_triplet_published_invalid_once = false;
 				_navigation_mode = &_gpsFailure;
 				break;
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 86764d620..ec297e7f0 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -53,6 +53,10 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -68,6 +72,7 @@
 #include <geo/geo.h>
 #include <systemlib/systemlib.h>
 #include <drivers/drv_hrt.h>
+#include <platforms/px4_defines.h>
 
 #include "position_estimator_inav_params.h"
 #include "inertial_filter.h"
@@ -458,7 +463,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 							acc[i] = 0.0f;
 
 							for (int j = 0; j < 3; j++) {
-								acc[i] += att.R[i][j] * sensor.accelerometer_m_s2[j];
+								acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j];
 							}
 						}
 
@@ -491,7 +496,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 				if ((flow.ground_distance_m > 0.31f) &&
 					(flow.ground_distance_m < 4.0f) &&
-					(att.R[2][2] > 0.7f) &&
+					(PX4_R(att.R, 2, 2) > 0.7f) &&
 					(fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) {
 
 					sonar_time = t;
@@ -528,15 +533,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				float flow_q = flow.quality / 255.0f;
 				float dist_bottom = - z_est[0] - surface_offset;
 
-				if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7f) {
+				if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && PX4_R(att.R, 2, 2) > 0.7f) {
 					/* distance to surface */
-					float flow_dist = dist_bottom / att.R[2][2];
+					float flow_dist = dist_bottom / PX4_R(att.R, 2, 2);
 					/* check if flow if too large for accurate measurements */
 					/* calculate estimated velocity in body frame */
 					float body_v_est[2] = { 0.0f, 0.0f };
 
 					for (int i = 0; i < 2; i++) {
-						body_v_est[i] = att.R[0][i] * x_est[1] + att.R[1][i] * y_est[1] + att.R[2][i] * z_est[1];
+						body_v_est[i] = PX4_R(att.R, 0, i) * x_est[1] + PX4_R(att.R, 1, i) * y_est[1] + PX4_R(att.R, 2, i) * z_est[1];
 					}
 
 					/* set this flag if flow should be accurate according to current velocity and attitude rate estimate */
@@ -559,7 +564,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					/* project measurements vector to NED basis, skip Z component */
 					for (int i = 0; i < 2; i++) {
 						for (int j = 0; j < 3; j++) {
-							flow_v[i] += att.R[i][j] * flow_m[j];
+							flow_v[i] += PX4_R(att.R, i, j) * flow_m[j];
 						}
 					}
 
@@ -568,7 +573,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					corr_flow[1] = flow_v[1] - y_est[1];
 					/* adjust correction weight */
 					float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min);
-					w_flow = att.R[2][2] * flow_q_weight / fmaxf(1.0f, flow_dist);
+					w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist);
 
 					/* if flow is not accurate, reduce weight for it */
 					// TODO make this more fuzzy
@@ -940,7 +945,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			float c = 0.0f;
 
 			for (int j = 0; j < 3; j++) {
-				c += att.R[j][i] * accel_bias_corr[j];
+				c += PX4_R(att.R, j, i) * accel_bias_corr[j];
 			}
 
 			if (isfinite(c)) {
@@ -965,7 +970,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			float c = 0.0f;
 
 			for (int j = 0; j < 3; j++) {
-				c += att.R[j][i] * accel_bias_corr[j];
+				c += PX4_R(att.R, j, i) * accel_bias_corr[j];
 			}
 
 			if (isfinite(c)) {
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 99632ef0b..8ac87b238 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -70,6 +70,10 @@
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position.h>
@@ -1117,7 +1121,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
 	subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));
 	subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate));
-	
+
 	/* we need to rate-limit wind, as we do not need the full update rate */
 	orb_set_interval(subs.wind_sub, 90);
 	subs.encoders_sub = orb_subscribe(ORB_ID(encoders));
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 1bd3bc58c..87d7da537 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -184,14 +184,13 @@ private:
 	/**
 	 * Get and limit value for specified RC function. Returns NAN if not mapped.
 	 */
-	float		get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value);
+	float		get_rc_value(uint8_t func, float min_value, float max_value);
 
 	/**
 	 * Get switch position for specified function.
 	 */
-	switch_pos_t	get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th,
-					       bool mid_inv);
-	switch_pos_t	get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv);
+	switch_pos_t	get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv);
+	switch_pos_t	get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv);
 
 	/**
 	 * Update paramters from RC channels if the functionality is activated and the
@@ -816,28 +815,28 @@ Sensors::parameters_update()
 	_parameters.rc_offboard_th = fabs(_parameters.rc_offboard_th);
 
 	/* update RC function mappings */
-	_rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
-	_rc.function[ROLL] = _parameters.rc_map_roll - 1;
-	_rc.function[PITCH] = _parameters.rc_map_pitch - 1;
-	_rc.function[YAW] = _parameters.rc_map_yaw - 1;
-
-	_rc.function[MODE] = _parameters.rc_map_mode_sw - 1;
-	_rc.function[RETURN] = _parameters.rc_map_return_sw - 1;
-	_rc.function[POSCTL] = _parameters.rc_map_posctl_sw - 1;
-	_rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1;
-	_rc.function[ACRO] = _parameters.rc_map_acro_sw - 1;
-	_rc.function[OFFBOARD] = _parameters.rc_map_offboard_sw - 1;
-
-	_rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
-
-	_rc.function[AUX_1] = _parameters.rc_map_aux1 - 1;
-	_rc.function[AUX_2] = _parameters.rc_map_aux2 - 1;
-	_rc.function[AUX_3] = _parameters.rc_map_aux3 - 1;
-	_rc.function[AUX_4] = _parameters.rc_map_aux4 - 1;
-	_rc.function[AUX_5] = _parameters.rc_map_aux5 - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ROLL] = _parameters.rc_map_roll - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PITCH] = _parameters.rc_map_pitch - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_YAW] = _parameters.rc_map_yaw - 1;
+
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1;
+
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1;
+
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1] = _parameters.rc_map_aux1 - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2] = _parameters.rc_map_aux2 - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3] = _parameters.rc_map_aux3 - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1;
 
 	for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
-		_rc.function[PARAM_1 + i] = _parameters.rc_map_param[i] - 1;
+		_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1;
 	}
 
 	/* gyro offsets */
@@ -1477,7 +1476,7 @@ Sensors::rc_parameter_map_poll(bool forced)
 
 		/* update paramter handles to which the RC channels are mapped */
 		for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
-			if (_rc.function[PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
+			if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
 				/* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
 				 * or no request to map this channel to a param has been sent via mavlink
 				 */
@@ -1629,7 +1628,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 }
 
 float
-Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value)
+Sensors::get_rc_value(uint8_t func, float min_value, float max_value)
 {
 	if (_rc.function[func] >= 0) {
 		float value = _rc.channels[_rc.function[func]];
@@ -1650,41 +1649,41 @@ Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max
 }
 
 switch_pos_t
-Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv)
+Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv)
 {
 	if (_rc.function[func] >= 0) {
 		float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
 
 		if (on_inv ? value < on_th : value > on_th) {
-			return SWITCH_POS_ON;
+			return manual_control_setpoint_s::SWITCH_POS_ON;
 
 		} else if (mid_inv ? value < mid_th : value > mid_th) {
-			return SWITCH_POS_MIDDLE;
+			return manual_control_setpoint_s::SWITCH_POS_MIDDLE;
 
 		} else {
-			return SWITCH_POS_OFF;
+			return manual_control_setpoint_s::SWITCH_POS_OFF;
 		}
 
 	} else {
-		return SWITCH_POS_NONE;
+		return manual_control_setpoint_s::SWITCH_POS_NONE;
 	}
 }
 
 switch_pos_t
-Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv)
+Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv)
 {
 	if (_rc.function[func] >= 0) {
 		float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
 
 		if (on_inv ? value < on_th : value > on_th) {
-			return SWITCH_POS_ON;
+			return manual_control_setpoint_s::SWITCH_POS_ON;
 
 		} else {
-			return SWITCH_POS_OFF;
+			return manual_control_setpoint_s::SWITCH_POS_OFF;
 		}
 
 	} else {
-		return SWITCH_POS_NONE;
+		return manual_control_setpoint_s::SWITCH_POS_NONE;
 	}
 }
 
@@ -1694,15 +1693,14 @@ Sensors::set_params_from_rc()
 	static float param_rc_values[RC_PARAM_MAP_NCHAN] = {};
 
 	for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
-		if (_rc.function[PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
+		if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
 			/* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
 			 * or no request to map this channel to a param has been sent via mavlink
 			 */
 			continue;
 		}
 
-		float rc_val = get_rc_value((enum RC_CHANNELS_FUNCTION)(PARAM_1 + i), -1.0, 1.0);
-
+		float rc_val = get_rc_value((rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0);
 		/* Check if the value has changed,
 		 * maybe we need to introduce a more aggressive limit here */
 		if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) {
@@ -1834,25 +1832,24 @@ Sensors::rc_poll()
 			manual.timestamp = rc_input.timestamp_last_signal;
 
 			/* limit controls */
-			manual.y = get_rc_value(ROLL, -1.0, 1.0);
-			manual.x = get_rc_value(PITCH, -1.0, 1.0);
-			manual.r = get_rc_value(YAW, -1.0, 1.0);
-			manual.z = get_rc_value(THROTTLE, 0.0, 1.0);
-			manual.flaps = get_rc_value(FLAPS, -1.0, 1.0);
-			manual.aux1 = get_rc_value(AUX_1, -1.0, 1.0);
-			manual.aux2 = get_rc_value(AUX_2, -1.0, 1.0);
-			manual.aux3 = get_rc_value(AUX_3, -1.0, 1.0);
-			manual.aux4 = get_rc_value(AUX_4, -1.0, 1.0);
-			manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0);
+			manual.y = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0);
+			manual.x = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0);
+			manual.r = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0);
+			manual.z = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0);
+			manual.flaps = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0);
+			manual.aux1 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0);
+			manual.aux2 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0);
+			manual.aux3 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0);
+			manual.aux4 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0);
+			manual.aux5 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0);
 
 			/* mode switches */
-			manual.mode_switch = get_rc_sw3pos_position(MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv,
-					     _parameters.rc_assist_th, _parameters.rc_assist_inv);
-			manual.posctl_switch = get_rc_sw2pos_position(POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv);
-			manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv);
-			manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv);
-			manual.acro_switch = get_rc_sw2pos_position(ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv);
-			manual.offboard_switch = get_rc_sw2pos_position(OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv);
+			manual.mode_switch = get_rc_sw3pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv);
+			manual.posctl_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv);
+			manual.return_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, _parameters.rc_return_inv);
+			manual.loiter_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv);
+			manual.acro_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv);
+			manual.offboard_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv);
 
 			/* publish manual_control_setpoint topic */
 			if (_manual_control_pub > 0) {
diff --git a/src/modules/systemlib/circuit_breaker.cpp b/src/modules/systemlib/circuit_breaker.cpp
new file mode 100644
index 000000000..ea478a60f
--- /dev/null
+++ b/src/modules/systemlib/circuit_breaker.cpp
@@ -0,0 +1,55 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file circuit_breaker.c
+ *
+ * Circuit breaker parameters.
+ * Analog to real aviation circuit breakers these parameters
+ * allow to disable subsystems. They are not supported as standard
+ * operation procedure and are only provided for development purposes.
+ * To ensure they are not activated accidentally, the associated
+ * parameter needs to set to the key (magic).
+ */
+
+#include <px4.h>
+#include <systemlib/circuit_breaker.h>
+
+bool circuit_breaker_enabled(const char* breaker, int32_t magic)
+{
+	int32_t val;
+	(void)PX4_PARAM_GET_BYNAME(breaker, &val);
+
+	return (val == magic);
+}
+
diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h
index b3431722f..c97dbc26f 100644
--- a/src/modules/systemlib/circuit_breaker.h
+++ b/src/modules/systemlib/circuit_breaker.h
@@ -61,7 +61,7 @@
 
 __BEGIN_DECLS
 
-__EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic);
+extern "C" __EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic);
 
 __END_DECLS
 
diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker_params.c
index 12187d70e..e499ae27a 100644
--- a/src/modules/systemlib/circuit_breaker.c
+++ b/src/modules/systemlib/circuit_breaker_params.c
@@ -42,8 +42,8 @@
  * parameter needs to set to the key (magic).
  */
 
-#include <systemlib/param/param.h>
-#include <systemlib/circuit_breaker.h>
+#include <px4.h>
+#include <systemlib/circuit_breaker_params.h>
 
 /**
  * Circuit breaker for power supply check
@@ -56,7 +56,7 @@
  * @max 894281
  * @group Circuit Breaker
  */
-PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0);
+PX4_PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK);
 
 /**
  * Circuit breaker for rate controller output
@@ -69,7 +69,7 @@ PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0);
  * @max 140253
  * @group Circuit Breaker
  */
-PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0);
+PX4_PARAM_DEFINE_INT32(CBRK_RATE_CTRL);
 
 /**
  * Circuit breaker for IO safety
@@ -81,7 +81,7 @@ PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0);
  * @max 22027
  * @group Circuit Breaker
  */
-PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0);
+PX4_PARAM_DEFINE_INT32(CBRK_IO_SAFETY);
 
 /**
  * Circuit breaker for airspeed sensor
@@ -93,7 +93,7 @@ PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0);
  * @max 162128
  * @group Circuit Breaker
  */
-PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
+PX4_PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK);
 
 /**
  * Circuit breaker for flight termination
@@ -106,7 +106,7 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
  * @max 121212
  * @group Circuit Breaker
  */
-PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212);
+PX4_PARAM_DEFINE_INT32(CBRK_FLIGHTTERM);
 
 /**
  * Circuit breaker for engine failure detection
@@ -120,27 +120,4 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212);
  * @max 284953
  * @group Circuit Breaker
  */
-PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953);
-
-/**
- * Circuit breaker for gps failure detection
- *
- * Setting this parameter to 240024 will disable the gps failure detection.
- * If the aircraft is in gps failure mode the gps failure flag will be
- * set to healthy
- * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
- *
- * @min 0
- * @max 240024
- * @group Circuit Breaker
- */
-PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024);
-
-bool circuit_breaker_enabled(const char* breaker, int32_t magic)
-{
-	int32_t val;
-	(void)param_get(param_find(breaker), &val);
-
-	return (val == magic);
-}
-
+PX4_PARAM_DEFINE_INT32(CBRK_ENGINEFAIL);
diff --git a/src/modules/systemlib/circuit_breaker_params.h b/src/modules/systemlib/circuit_breaker_params.h
new file mode 100644
index 000000000..768bf7f53
--- /dev/null
+++ b/src/modules/systemlib/circuit_breaker_params.h
@@ -0,0 +1,7 @@
+#define PARAM_CBRK_SUPPLY_CHK_DEFAULT 0
+#define PARAM_CBRK_RATE_CTRL_DEFAULT 0
+#define PARAM_CBRK_IO_SAFETY_DEFAULT 0
+#define PARAM_CBRK_AIRSPD_CHK_DEFAULT 0
+#define PARAM_CBRK_FLIGHTTERM_DEFAULT 121212
+#define PARAM_CBRK_ENGINEFAIL_DEFAULT 284953
+#define PARAM_CBRK_GPSFAIL_DEFAULT 240024
diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk
index f4dff2838..f2499bbb1 100644
--- a/src/modules/systemlib/module.mk
+++ b/src/modules/systemlib/module.mk
@@ -53,7 +53,8 @@ SRCS		 = err.c \
 		   otp.c \
 		   board_serial.c \
 		   pwm_limit/pwm_limit.c \
-		   circuit_breaker.c \
+		   circuit_breaker.cpp \
+		   circuit_breaker_params.c \
 		   mcu_version.c
 
 MAXOPTIMIZATION	 = -Os
diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h
index 0c1243de3..8543ba7bb 100644
--- a/src/modules/systemlib/perf_counter.h
+++ b/src/modules/systemlib/perf_counter.h
@@ -40,6 +40,7 @@
 #define _SYSTEMLIB_PERF_COUNTER_H value
 
 #include <stdint.h>
+#include <platforms/px4_defines.h>
 
 /**
  * Counter types.
diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp
index d33f93060..eb2d84726 100644
--- a/src/modules/uORB/Publication.cpp
+++ b/src/modules/uORB/Publication.cpp
@@ -49,15 +49,16 @@
 #include "topics/actuator_direct.h"
 #include "topics/encoders.h"
 #include "topics/tecs_status.h"
+#include "topics/rc_channels.h"
 
 namespace uORB {
 
 template<class T>
 Publication<T>::Publication(
-	List<PublicationBase *> * list,
-	const struct orb_metadata *meta) :
+	const struct orb_metadata *meta,
+	List<PublicationNode *> * list) :
 	T(), // initialize data structure to zero
-	PublicationBase(list, meta) {
+	PublicationNode(meta, list) {
 }
 
 template<class T>
@@ -80,5 +81,6 @@ template class __EXPORT Publication<actuator_outputs_s>;
 template class __EXPORT Publication<actuator_direct_s>;
 template class __EXPORT Publication<encoders_s>;
 template class __EXPORT Publication<tecs_status_s>;
+template class __EXPORT Publication<rc_channels_s>;
 
 }
diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp
index b64559734..8cbe51119 100644
--- a/src/modules/uORB/Publication.hpp
+++ b/src/modules/uORB/Publication.hpp
@@ -38,6 +38,8 @@
 
 #pragma once
 
+#include <assert.h>
+
 #include <uORB/uORB.h>
 #include <containers/List.hpp>
 
@@ -49,55 +51,112 @@ namespace uORB
  * Base publication warapper class, used in list traversal
  * of various publications.
  */
-class __EXPORT PublicationBase : public ListNode<uORB::PublicationBase *>
+class __EXPORT PublicationBase
 {
 public:
 
-	PublicationBase(
-		List<PublicationBase *> * list,
-		const struct orb_metadata *meta) :
+	/**
+	 * Constructor
+	 *
+	 *
+	 * @param meta The uORB metadata (usually from the ORB_ID()
+	 * 	macro) for the topic.
+	 */
+	PublicationBase(const struct orb_metadata *meta) :
 		_meta(meta),
 		_handle(-1) {
-		if (list != NULL) list->add(this);
 	}
-	void update() {
+
+	/**
+	 * Update the struct
+	 * @param data The uORB message struct we are updating.
+	 */
+	void update(void * data) {
 		if (_handle > 0) {
-			orb_publish(getMeta(), getHandle(), getDataVoidPtr());
+			orb_publish(getMeta(), getHandle(), data);
 		} else {
-			setHandle(orb_advertise(getMeta(), getDataVoidPtr()));
+			setHandle(orb_advertise(getMeta(), data));
 		}
 	}
-	virtual void *getDataVoidPtr() = 0;
+
+	/**
+	 * Deconstructor
+	 */
 	virtual ~PublicationBase() {
 		orb_unsubscribe(getHandle());
 	}
+// accessors
 	const struct orb_metadata *getMeta() { return _meta; }
 	int getHandle() { return _handle; }
 protected:
+// accessors
 	void setHandle(orb_advert_t handle) { _handle = handle; }
+// attributes
 	const struct orb_metadata *_meta;
 	orb_advert_t _handle;
 };
 
 /**
+ * alias class name so it is clear that the base class
+ * can be used by itself if desired
+ */
+typedef PublicationBase PublicationTiny;
+
+/**
+ * The publication base class as a list node.
+ */
+class __EXPORT PublicationNode :
+	public PublicationBase,
+	public ListNode<PublicationNode *>
+{
+public:
+	/**
+	 * Constructor
+	 *
+	 *
+	 * @param meta The uORB metadata (usually from the ORB_ID()
+	 * 	macro) for the topic.
+	 * @param list 	A pointer to a list of subscriptions
+	 * 	that this should be appended to.
+	 */
+	PublicationNode(const struct orb_metadata *meta,
+		List<PublicationNode *> * list=nullptr) :
+		PublicationBase(meta) {
+		if (list != nullptr) list->add(this);
+	}
+
+	/**
+	 * This function is the callback for list traversal
+	 * updates, a child class must implement it.
+	 */
+	virtual void update() = 0;
+};
+
+/**
  * Publication wrapper class
  */
 template<class T>
 class Publication :
 	public T, // this must be first!
-	public PublicationBase
+	public PublicationNode
 {
 public:
 	/**
 	 * Constructor
 	 *
-	 * @param list      A list interface for adding to list during construction
-	 * @param meta		The uORB metadata (usually from the ORB_ID() macro)
-	 *			for the topic.
+	 * @param meta The uORB metadata (usually from
+	 * 	the ORB_ID() macro) for the topic.
+	 * @param list A list interface for adding to
+	 * 	list during construction
 	 */
-	Publication(List<PublicationBase *> * list,
-		const struct orb_metadata *meta);
+	Publication(const struct orb_metadata *meta,
+		List<PublicationNode *> * list=nullptr);
+
+	/**
+	 * Deconstructor
+	 **/
 	virtual ~Publication();
+
 	/*
 	 * XXX
 	 * This function gets the T struct, assuming
@@ -106,6 +165,13 @@ public:
 	 * seem to be available
 	 */
 	void *getDataVoidPtr();
+
+	/**
+	 * Create an update function that uses the embedded struct.
+	 */
+	void update() {
+		PublicationBase::update(getDataVoidPtr());
+	}
 };
 
 } // namespace uORB
diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp
index 8884e5a3a..db47218d9 100644
--- a/src/modules/uORB/Subscription.cpp
+++ b/src/modules/uORB/Subscription.cpp
@@ -52,25 +52,18 @@
 #include "topics/vehicle_local_position.h"
 #include "topics/vehicle_attitude_setpoint.h"
 #include "topics/vehicle_rates_setpoint.h"
+#include "topics/rc_channels.h"
 
 namespace uORB
 {
 
-bool __EXPORT SubscriptionBase::updated()
-{
-	bool isUpdated = false;
-	orb_check(_handle, &isUpdated);
-	return isUpdated;
-}
-
 template<class T>
 Subscription<T>::Subscription(
-	List<SubscriptionBase *> * list,
-	const struct orb_metadata *meta, unsigned interval) :
+	const struct orb_metadata *meta,
+	unsigned interval,
+	List<SubscriptionNode *> * list) :
 	T(), // initialize data structure to zero
-	SubscriptionBase(list, meta) {
-	setHandle(orb_subscribe(getMeta()));
-	orb_set_interval(getHandle(), interval);
+	SubscriptionNode(meta, interval, list) {
 }
 
 template<class T>
@@ -101,5 +94,8 @@ template class __EXPORT Subscription<vehicle_local_position_setpoint_s>;
 template class __EXPORT Subscription<vehicle_local_position_s>;
 template class __EXPORT Subscription<vehicle_attitude_setpoint_s>;
 template class __EXPORT Subscription<vehicle_rates_setpoint_s>;
+template class __EXPORT Subscription<rc_channels_s>;
+template class __EXPORT Subscription<vehicle_control_mode_s>;
+template class __EXPORT Subscription<actuator_armed_s>;
 
 } // namespace uORB
diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp
index 75cf36254..12301ce96 100644
--- a/src/modules/uORB/Subscription.hpp
+++ b/src/modules/uORB/Subscription.hpp
@@ -38,10 +38,11 @@
 
 #pragma once
 
+#include <assert.h>
+
 #include <uORB/uORB.h>
 #include <containers/List.hpp>
 
-
 namespace uORB
 {
 
@@ -49,8 +50,7 @@ namespace uORB
  * Base subscription warapper class, used in list traversal
  * of various subscriptions.
  */
-class __EXPORT SubscriptionBase :
-	public ListNode<SubscriptionBase *>
+class __EXPORT SubscriptionBase
 {
 public:
 // methods
@@ -58,23 +58,42 @@ public:
 	/**
 	 * Constructor
 	 *
-	 * @param meta		The uORB metadata (usually from the ORB_ID() macro)
-	 *			for the topic.
+	 * @param meta The uORB metadata (usually from the ORB_ID()
+	 * 	macro) for the topic.
+	 *
+	 * @param interval  The minimum interval in milliseconds
+	 * 	between updates
 	 */
-	SubscriptionBase(
-		List<SubscriptionBase *> * list,
-		const struct orb_metadata *meta) :
+	SubscriptionBase(const struct orb_metadata *meta,
+		unsigned interval=0) :
 		_meta(meta),
 		_handle() {
-		if (list != NULL) list->add(this);
+		setHandle(orb_subscribe(getMeta()));
+		orb_set_interval(getHandle(), interval);
 	}
-	bool updated();
-	void update() {
+
+	/**
+	 * Check if there is a new update.
+	 * */
+	bool updated() {
+		bool isUpdated = false;
+		orb_check(_handle, &isUpdated);
+		return isUpdated;
+	}
+
+	/**
+	 * Update the struct
+	 * @param data The uORB message struct we are updating.
+	 */
+	void update(void * data) {
 		if (updated()) {
-			orb_copy(_meta, _handle, getDataVoidPtr());
+			orb_copy(_meta, _handle, data);
 		}
 	}
-	virtual void *getDataVoidPtr() = 0;
+
+	/**
+	 * Deconstructor
+	 */
 	virtual ~SubscriptionBase() {
 		orb_unsubscribe(_handle);
 	}
@@ -90,30 +109,86 @@ protected:
 };
 
 /**
+ * alias class name so it is clear that the base class
+ */
+typedef SubscriptionBase SubscriptionTiny;
+
+/**
+ * The publication base class as a list node.
+ */
+class __EXPORT SubscriptionNode :
+
+	public SubscriptionBase,
+	public ListNode<SubscriptionNode *>
+{
+public:
+	/**
+	 * Constructor
+	 *
+	 *
+	 * @param meta The uORB metadata (usually from the ORB_ID()
+	 * 	macro) for the topic.
+	 * @param interval  The minimum interval in milliseconds
+	 * 	between updates
+	 * @param list 	A pointer to a list of subscriptions
+	 * 	that this should be appended to.
+	 */
+	SubscriptionNode(const struct orb_metadata *meta,
+		unsigned interval=0,
+		List<SubscriptionNode *> * list=nullptr) :
+		SubscriptionBase(meta, interval),
+		_interval(interval) {
+		if (list != nullptr) list->add(this);
+	}
+
+	/**
+	 * This function is the callback for list traversal
+	 * updates, a child class must implement it.
+	 */
+	virtual void update() = 0;
+// accessors
+	unsigned getInterval() { return _interval; }
+protected:
+// attributes
+	unsigned _interval;
+
+};
+
+/**
  * Subscription wrapper class
  */
 template<class T>
 class __EXPORT Subscription :
 	public T, // this must be first!
-	public SubscriptionBase
+	public SubscriptionNode
 {
 public:
 	/**
 	 * Constructor
 	 *
-	 * @param list      A list interface for adding to list during construction
-	 * @param meta		The uORB metadata (usually from the ORB_ID() macro)
-	 *			for the topic.
-	 * @param interval  The minimum interval in milliseconds between updates
+	 * @param meta The uORB metadata (usually from
+	 * 	the ORB_ID() macro) for the topic.
+	 * @param interval  The minimum interval in milliseconds
+	 * 	between updates
+	 * @param list A list interface for adding to
+	 * 	list during construction
 	 */
-	Subscription(
-		List<SubscriptionBase *> * list,
-		const struct orb_metadata *meta, unsigned interval);
+	Subscription(const struct orb_metadata *meta,
+		unsigned interval=0,
+		List<SubscriptionNode *> * list=nullptr);
 	/**
 	 * Deconstructor
 	 */
 	virtual ~Subscription();
 
+
+	/**
+	 * Create an update function that uses the embedded struct.
+	 */
+	void update() {
+		SubscriptionBase::update(getDataVoidPtr());
+	}
+
 	/*
 	 * XXX
 	 * This function gets the T struct, assuming
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index ba1ac0350..f60aa8d86 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -113,8 +113,10 @@ ORB_DEFINE(vehicle_vicon_position, struct vehicle_vicon_position_s);
 
 #include "topics/vehicle_rates_setpoint.h"
 ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s);
-ORB_DEFINE(mc_virtual_rates_setpoint, struct vehicle_rates_setpoint_s);
-ORB_DEFINE(fw_virtual_rates_setpoint, struct vehicle_rates_setpoint_s);
+#include "topics/mc_virtual_rates_setpoint.h"
+ORB_DEFINE(mc_virtual_rates_setpoint, struct mc_virtual_rates_setpoint_s);
+#include "topics/fw_virtual_rates_setpoint.h"
+ORB_DEFINE(fw_virtual_rates_setpoint, struct fw_virtual_rates_setpoint_s);
 
 #include "topics/rc_channels.h"
 ORB_DEFINE(rc_channels, struct rc_channels_s);
@@ -184,13 +186,19 @@ ORB_DEFINE(subsystem_info, struct subsystem_info_s);
 
 /* actuator controls, as requested by controller */
 #include "topics/actuator_controls.h"
-ORB_DEFINE(actuator_controls_0, struct actuator_controls_s);
-ORB_DEFINE(actuator_controls_1, struct actuator_controls_s);
-ORB_DEFINE(actuator_controls_2, struct actuator_controls_s);
-ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
+#include "topics/actuator_controls_0.h"
+ORB_DEFINE(actuator_controls_0, struct actuator_controls_0_s);
+#include "topics/actuator_controls_1.h"
+ORB_DEFINE(actuator_controls_1, struct actuator_controls_1_s);
+#include "topics/actuator_controls_2.h"
+ORB_DEFINE(actuator_controls_2, struct actuator_controls_2_s);
+#include "topics/actuator_controls_3.h"
+ORB_DEFINE(actuator_controls_3, struct actuator_controls_3_s);
 //Virtual control groups, used for VTOL operation
-ORB_DEFINE(actuator_controls_virtual_mc, struct actuator_controls_s);
-ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_s);
+#include "topics/actuator_controls_virtual_mc.h"
+ORB_DEFINE(actuator_controls_virtual_mc, struct actuator_controls_virtual_mc_s);
+#include "topics/actuator_controls_virtual_fw.h"
+ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_virtual_fw_s);
 
 #include "topics/actuator_armed.h"
 ORB_DEFINE(actuator_armed, struct actuator_armed_s);
diff --git a/src/modules/uORB/topics/airspeed.h b/src/modules/uORB/topics/airspeed.h
index d2ee754cd..676c37c77 100644
--- a/src/modules/uORB/topics/airspeed.h
+++ b/src/modules/uORB/topics/airspeed.h
@@ -40,7 +40,7 @@
 #ifndef TOPIC_AIRSPEED_H_
 #define TOPIC_AIRSPEED_H_
 
-#include "../uORB.h"
+#include <platforms/px4_defines.h>
 #include <stdint.h>
 
 /**
diff --git a/src/modules/uORB/topics/fence.h b/src/modules/uORB/topics/fence.h
index 6f16c51cf..a61f078ba 100644
--- a/src/modules/uORB/topics/fence.h
+++ b/src/modules/uORB/topics/fence.h
@@ -42,7 +42,7 @@
 
 #include <stdint.h>
 #include <stdbool.h>
-#include "../uORB.h"
+#include <platforms/px4_defines.h>
 
 /**
  * @addtogroup topics
diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
deleted file mode 100644
index 50b7bd9e5..000000000
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ /dev/null
@@ -1,111 +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 manual_control_setpoint.h
- * Definition of the manual_control_setpoint uORB topic.
- */
-
-#ifndef TOPIC_MANUAL_CONTROL_SETPOINT_H_
-#define TOPIC_MANUAL_CONTROL_SETPOINT_H_
-
-#include <stdint.h>
-#include "../uORB.h"
-
-/**
- * Switch position
- */
-typedef enum {
-	SWITCH_POS_NONE = 0,		/**< switch is not mapped */
-	SWITCH_POS_ON,			/**< switch activated (value = 1) */
-	SWITCH_POS_MIDDLE,		/**< middle position (value = 0) */
-	SWITCH_POS_OFF			/**< switch not activated (value = -1) */
-} switch_pos_t;
-
-/**
- * @addtogroup topics
- * @{
- */
-
-struct manual_control_setpoint_s {
-	uint64_t timestamp;
-
-	/**
-	 * Any of the channels may not be available and be set to NaN
-	 * to indicate that it does not contain valid data.
-	 * The variable names follow the definition of the
-	 * MANUAL_CONTROL mavlink message.
-	 * The default range is from -1 to 1 (mavlink message -1000 to 1000)
-	 * The range for the z variable is defined from 0 to 1. (The z field of
-	 * the MANUAL_CONTROL mavlink message is defined from -1000 to 1000)
-	 */
-	float x;			/**< stick position in x direction -1..1
-					  in general corresponds to forward/back motion or pitch of vehicle,
-					  in general a positive value means forward or negative pitch and
-					  a negative value means backward or positive pitch */
-	float y;			/**< stick position in y direction -1..1
-					  in general corresponds to right/left motion or roll of vehicle,
-					  in general a positive value means right or positive roll and
-					  a negative value means left or negative roll */
-	float z;			/**< throttle stick position 0..1
-					  in general corresponds to up/down motion or thrust of vehicle,
-					  in general the value corresponds to the demanded throttle by the user,
-					  if the input is used for setting the setpoint of a vertical position
-					  controller any value > 0.5 means up and any value < 0.5 means down */
-	float r;			/**< yaw stick/twist positon, -1..1
-					  in general corresponds to the righthand rotation around the vertical
-					  (downwards) axis of the vehicle */
-	float flaps;			/**< flap position */
-	float aux1;			/**< default function: camera yaw / azimuth */
-	float aux2;			/**< default function: camera pitch / tilt */
-	float aux3;			/**< default function: camera trigger */
-	float aux4;			/**< default function: camera roll */
-	float aux5;			/**< default function: payload drop */
-
-	switch_pos_t mode_switch;	/**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */
-	switch_pos_t return_switch;	/**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */
-	switch_pos_t posctl_switch;	/**< position control 2 position switch (optional): _ALTCTL_, POSCTL */
-	switch_pos_t loiter_switch;	/**< loiter 2 position switch (optional): _MISSION_, LOITER */
-	switch_pos_t acro_switch;	/**< acro 2 position switch (optional): _MANUAL_, ACRO */
-	switch_pos_t offboard_switch;	/**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */
-};
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(manual_control_setpoint);
-
-#endif
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
deleted file mode 100644
index cb2262534..000000000
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ /dev/null
@@ -1,116 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file mission_item_triplet.h
- * Definition of the global WGS84 position setpoint uORB topic.
- *
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-#ifndef TOPIC_MISSION_ITEM_TRIPLET_H_
-#define TOPIC_MISSION_ITEM_TRIPLET_H_
-
-#include <stdint.h>
-#include <stdbool.h>
-#include "../uORB.h"
-
-/**
- * @addtogroup topics
- * @{
- */
-
-enum SETPOINT_TYPE
-{
-	SETPOINT_TYPE_POSITION = 0,	/**< position setpoint */
-	SETPOINT_TYPE_VELOCITY,		/**< velocity setpoint */
-	SETPOINT_TYPE_LOITER,		/**< loiter setpoint */
-	SETPOINT_TYPE_TAKEOFF,		/**< takeoff setpoint */
-	SETPOINT_TYPE_LAND,		/**< land setpoint, altitude must be ignored, descend until landing */
-	SETPOINT_TYPE_IDLE,		/**< do nothing, switch off motors or keep at idle speed (MC) */
-	SETPOINT_TYPE_OFFBOARD, 	/**< setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard */
-};
-
-struct position_setpoint_s
-{
-	bool valid;			/**< true if setpoint is valid */
-	enum SETPOINT_TYPE type;	/**< setpoint type to adjust behavior of position controller */
-	float x;			/**< local position setpoint in m in NED */
-	float y;			/**< local position setpoint in m in NED */
-	float z;			/**< local position setpoint in m in NED */
-	bool position_valid;	/**< true if local position setpoint valid */
-	float vx;			/**< local velocity setpoint in m/s in NED */
-	float vy;			/**< local velocity setpoint in m/s in NED */
-	float vz;			/**< local velocity setpoint in m/s in NED */
-	bool velocity_valid;		/**< true if local velocity setpoint valid */
-	double lat;			/**< latitude, in deg */
-	double lon;			/**< longitude, in deg */
-	float alt;			/**< altitude AMSL, in m */
-	float yaw;			/**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */
-	bool yaw_valid;			/**< true if yaw setpoint valid */
-	float yawspeed;			/**< yawspeed (only for multirotors, in rad/s) */
-	bool yawspeed_valid;		/**< true if yawspeed setpoint valid */
-	float loiter_radius;		/**< loiter radius (only for fixed wing), in m */
-	int8_t loiter_direction;	/**< loiter direction: 1 = CW, -1 = CCW */
-	float pitch_min;		/**< minimal pitch angle for fixed wing takeoff waypoints */
-	float a_x;			//**< acceleration x setpoint */
-	float a_y;			//**< acceleration y setpoint */
-	float a_z;			//**< acceleration z setpoint */
-	bool acceleration_valid;	//*< true if acceleration setpoint is valid/should be used */
-	bool acceleration_is_force;	//*< interprete acceleration as force */
-};
-
-/**
- * Global position setpoint triplet in WGS84 coordinates.
- *
- * This are the three next waypoints (or just the next two or one).
- */
-struct position_setpoint_triplet_s
-{
-	struct position_setpoint_s previous;
-	struct position_setpoint_s current;
-	struct position_setpoint_s next;
-
-	unsigned nav_state;				/**< report the navigation state */
-};
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(position_setpoint_triplet);
-
-#endif
diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h
deleted file mode 100755
index 019944dc0..000000000
--- a/src/modules/uORB/topics/vehicle_attitude.h
+++ /dev/null
@@ -1,91 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
- *           @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 vehicle_attitude.h
- * Definition of the attitude uORB topic.
- */
-
-#ifndef VEHICLE_ATTITUDE_H_
-#define VEHICLE_ATTITUDE_H_
-
-#include <stdint.h>
-#include <stdbool.h>
-#include "../uORB.h"
-
-/**
- * @addtogroup topics
- * @{
- */
-
-/**
- * Attitude in NED body frame in SI units.
- *
- * @see http://en.wikipedia.org/wiki/International_System_of_Units
- */
-struct vehicle_attitude_s {
-
-	uint64_t timestamp;	/**< in microseconds since system start          */
-
-	/* This is similar to the mavlink message ATTITUDE, but for onboard use */
-
-	/** @warning roll, pitch and yaw have always to be valid, the rotation matrix and quaternion are optional */
-
-	float roll;		/**< Roll angle (rad, Tait-Bryan, NED)				*/
-	float pitch;		/**< Pitch angle (rad, Tait-Bryan, NED)				*/
-	float yaw;		/**< Yaw angle (rad, Tait-Bryan, NED)				*/
-	float rollspeed;	/**< Roll angular speed (rad/s, Tait-Bryan, NED)		*/
-	float pitchspeed;	/**< Pitch angular speed (rad/s, Tait-Bryan, NED)		*/
-	float yawspeed;		/**< Yaw angular speed (rad/s, Tait-Bryan, NED)			*/
-	float rollacc;		/**< Roll angular accelration (rad/s, Tait-Bryan, NED)		*/
-	float pitchacc;		/**< Pitch angular acceleration (rad/s, Tait-Bryan, NED)	*/
-	float yawacc;		/**< Yaw angular acceleration (rad/s, Tait-Bryan, NED)		*/
-	float rate_offsets[3];	/**< Offsets of the body angular rates from zero		*/
-	float R[3][3];		/**< Rotation matrix body to world, (Tait-Bryan, NED)		*/
-	float q[4];		/**< Quaternion (NED)						*/
-	float g_comp[3];	/**< Compensated gravity vector					*/
-	bool R_valid;		/**< Rotation matrix valid					*/
-	bool q_valid;		/**< Quaternion valid						*/
-};
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(vehicle_attitude);
-
-#endif
diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
deleted file mode 100644
index 1cfc37cc6..000000000
--- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h
+++ /dev/null
@@ -1,89 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 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 vehicle_attitude_setpoint.h
- * Definition of the vehicle attitude setpoint uORB topic.
- */
-
-#ifndef TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_
-#define TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_
-
-#include <stdint.h>
-#include <stdbool.h>
-#include "../uORB.h"
-
-/**
- * @addtogroup topics
- * @{
- */
-
-/**
- * vehicle attitude setpoint.
- */
-struct vehicle_attitude_setpoint_s {
-	uint64_t timestamp;		/**< in microseconds since system start, is set whenever the writing thread stores new data */
-
-	float roll_body;				/**< body angle in NED frame		*/
-	float pitch_body;				/**< body angle in NED frame		*/
-	float yaw_body;					/**< body angle in NED frame		*/
-	//float body_valid;				/**< Set to true if body angles are valid */
-
-	float R_body[3][3];				/**< Rotation matrix describing the setpoint as rotation from the current body frame */
-	bool R_valid;					/**< Set to true if rotation matrix is valid */
-
-	//! For quaternion-based attitude control
-	float q_d[4];				/** Desired quaternion for quaternion control */
-	bool q_d_valid;					/**< Set to true if quaternion vector is valid */
-	float q_e[4];				/** Attitude error in quaternion */
-	bool q_e_valid;					/**< Set to true if quaternion error vector is valid */
-
-	float thrust;					/**< Thrust in Newton the power system should generate */
-
-	bool	roll_reset_integral;			/**< Reset roll integral part (navigation logic change) */
-	bool	pitch_reset_integral;			/**< Reset pitch integral part (navigation logic change) */
-	bool	yaw_reset_integral;			/**< Reset yaw integral part (navigation logic change) */
-
-};
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(vehicle_attitude_setpoint);
-ORB_DECLARE(mc_virtual_attitude_setpoint);
-ORB_DECLARE(fw_virtual_attitude_setpoint);
-
-#endif /* TOPIC_ARDRONE_CONTROL_H_ */
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
deleted file mode 100644
index ca7705456..000000000
--- a/src/modules/uORB/topics/vehicle_control_mode.h
+++ /dev/null
@@ -1,95 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file vehicle_control_mode.h
- * Definition of the vehicle_control_mode uORB topic.
- *
- * All control apps should depend their actions based on the flags set here.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- */
-
-#ifndef VEHICLE_CONTROL_MODE
-#define VEHICLE_CONTROL_MODE
-
-#include <stdint.h>
-#include <stdbool.h>
-#include "../uORB.h"
-#include "vehicle_status.h"
-
-/**
- * @addtogroup topics @{
- */
-
-
-/**
- * state machine / state of vehicle.
- *
- * Encodes the complete system state and is set by the commander app.
- */
-
-struct vehicle_control_mode_s {
-	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
-
-	bool flag_armed;
-
-	bool flag_external_manual_override_ok;	/**< external override non-fatal for system. Only true for fixed wing */
-
-	// XXX needs yet to be set by state machine helper
-	bool flag_system_hil_enabled;
-
-	bool flag_control_manual_enabled;		/**< true if manual input is mixed in */
-	bool flag_control_auto_enabled;			/**< true if onboard autopilot should act */
-	bool flag_control_offboard_enabled;		/**< true if offboard control should be used */
-	bool flag_control_rates_enabled;		/**< true if rates are stabilized */
-	bool flag_control_attitude_enabled;		/**< true if attitude stabilization is mixed in */
-	bool flag_control_force_enabled;		/**< true if force control is mixed in */
-	bool flag_control_velocity_enabled;		/**< true if horizontal velocity (implies direction) is controlled */
-	bool flag_control_position_enabled;		/**< true if position is controlled */
-	bool flag_control_altitude_enabled;		/**< true if altitude is controlled */
-	bool flag_control_climb_rate_enabled;	/**< true if climb rate is controlled */
-	bool flag_control_termination_enabled;	/**< true if flighttermination is enabled */
-};
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(vehicle_control_mode);
-
-#endif
diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h
index bc7046690..137c86dd5 100644
--- a/src/modules/uORB/topics/vehicle_global_position.h
+++ b/src/modules/uORB/topics/vehicle_global_position.h
@@ -45,7 +45,7 @@
 
 #include <stdint.h>
 #include <stdbool.h>
-#include "../uORB.h"
+#include <platforms/px4_defines.h>
 
 /**
  * @addtogroup topics
diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h
deleted file mode 100644
index 8b46c5a3f..000000000
--- a/src/modules/uORB/topics/vehicle_local_position.h
+++ /dev/null
@@ -1,96 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file vehicle_local_position.h
- * Definition of the local fused NED position uORB topic.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Anton Babushkin <anton.babushkin@me.com>
- */
-
-#ifndef TOPIC_VEHICLE_LOCAL_POSITION_H_
-#define TOPIC_VEHICLE_LOCAL_POSITION_H_
-
-#include <stdint.h>
-#include <stdbool.h>
-#include "../uORB.h"
-
-/**
- * @addtogroup topics
- * @{
- */
-
-/**
- * Fused local position in NED.
- */
-struct vehicle_local_position_s {
-	uint64_t timestamp;		/**< Time of this estimate, in microseconds since system start */
-	bool xy_valid;			/**< true if x and y are valid */
-	bool z_valid;			/**< true if z is valid */
-	bool v_xy_valid;		/**< true if vy and vy are valid */
-	bool v_z_valid;			/**< true if vz is valid */
-	/* Position in local NED frame */
-	float x;				/**< X position in meters in NED earth-fixed frame */
-	float y;				/**< X position in meters in NED earth-fixed frame */
-	float z;				/**< Z position in meters in NED earth-fixed frame (negative altitude) */
-	/* Velocity in NED frame */
-	float vx; 				/**< Ground X Speed (Latitude), m/s in NED */
-	float vy;				/**< Ground Y Speed (Longitude), m/s in NED */
-	float vz;				/**< Ground Z Speed (Altitude), m/s	in NED */
-	/* Heading */
-	float yaw;
-	/* Reference position in GPS / WGS84 frame */
-	bool xy_global;			/**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */
-	bool z_global;			/**< true if z is valid and has valid global reference (ref_alt) */
-	uint64_t ref_timestamp;	/**< Time when reference position was set */
-	double ref_lat;		/**< Reference point latitude in degrees */
-	double ref_lon;		/**< Reference point longitude in degrees */
-	float ref_alt;			/**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */
-	/* Distance to surface */
-	float dist_bottom;		/**< Distance to bottom surface (ground) */
-	float dist_bottom_rate;		/**< Distance to bottom surface (ground) change rate */
-	uint64_t surface_bottom_timestamp;		/**< Time when new bottom surface found */
-	bool dist_bottom_valid;	/**< true if distance to bottom surface is valid */
-	float eph;
-	float epv;
-};
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(vehicle_local_position);
-
-#endif
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
deleted file mode 100644
index b56e81e04..000000000
--- a/src/modules/uORB/topics/vehicle_status.h
+++ /dev/null
@@ -1,263 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 - 2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file vehicle_status.h
- * Definition of the vehicle_status uORB topic.
- *
- * Published the state machine and the system status bitfields
- * (see SYS_STATUS mavlink message), published only by commander app.
- *
- * All apps should write to subsystem_info:
- *
- *  (any app) --> subsystem_info (published) --> (commander app state machine)  --> vehicle_status --> (mavlink app)
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <julian@oes.ch>
- */
-
-#ifndef VEHICLE_STATUS_H_
-#define VEHICLE_STATUS_H_
-
-#include <stdint.h>
-#include <stdbool.h>
-#include "../uORB.h"
-
-/**
- * @addtogroup topics @{
- */
-
-/**
- * Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link.
- */
-typedef enum {
-	MAIN_STATE_MANUAL = 0,
-	MAIN_STATE_ALTCTL,
-	MAIN_STATE_POSCTL,
-	MAIN_STATE_AUTO_MISSION,
-	MAIN_STATE_AUTO_LOITER,
-	MAIN_STATE_AUTO_RTL,
-	MAIN_STATE_ACRO,
-	MAIN_STATE_OFFBOARD,
-	MAIN_STATE_MAX
-} main_state_t;
-
-// If you change the order, add or remove arming_state_t states make sure to update the arrays
-// in state_machine_helper.cpp as well.
-typedef enum {
-	ARMING_STATE_INIT = 0,
-	ARMING_STATE_STANDBY,
-	ARMING_STATE_ARMED,
-	ARMING_STATE_ARMED_ERROR,
-	ARMING_STATE_STANDBY_ERROR,
-	ARMING_STATE_REBOOT,
-	ARMING_STATE_IN_AIR_RESTORE,
-	ARMING_STATE_MAX,
-} arming_state_t;
-
-typedef enum {
-	HIL_STATE_OFF = 0,
-	HIL_STATE_ON
-} hil_state_t;
-
-/**
- * Navigation state, i.e. "what should vehicle do".
- */
-typedef enum {
-	NAVIGATION_STATE_MANUAL = 0,		/**< Manual mode */
-	NAVIGATION_STATE_ALTCTL,		/**< Altitude control mode */
-	NAVIGATION_STATE_POSCTL,		/**< Position control mode */
-	NAVIGATION_STATE_AUTO_MISSION,		/**< Auto mission mode */
-	NAVIGATION_STATE_AUTO_LOITER,		/**< Auto loiter mode */
-	NAVIGATION_STATE_AUTO_RTL,		/**< Auto return to launch mode */
-	NAVIGATION_STATE_AUTO_RCRECOVER,	/**< RC recover mode */
-	NAVIGATION_STATE_AUTO_RTGS,		/**< Auto return to groundstation on data link loss */
-	NAVIGATION_STATE_AUTO_LANDENGFAIL,	/**< Auto land on engine failure */
-	NAVIGATION_STATE_AUTO_LANDGPSFAIL,	/**< Auto land on gps failure (e.g. open loop loiter down) */
-	NAVIGATION_STATE_ACRO,			/**< Acro mode */
-	NAVIGATION_STATE_LAND,			/**< Land mode */
-	NAVIGATION_STATE_DESCEND,		/**< Descend mode (no position control) */
-	NAVIGATION_STATE_TERMINATION,		/**< Termination mode */
-	NAVIGATION_STATE_OFFBOARD,
-	NAVIGATION_STATE_MAX,
-} navigation_state_t;
-
-enum VEHICLE_MODE_FLAG {
-	VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
-	VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64,
-	VEHICLE_MODE_FLAG_HIL_ENABLED = 32,
-	VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16,
-	VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8,
-	VEHICLE_MODE_FLAG_AUTO_ENABLED = 4,
-	VEHICLE_MODE_FLAG_TEST_ENABLED = 2,
-	VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1
-}; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */
-
-/**
- * Should match 1:1 MAVLink's MAV_TYPE ENUM
- */
-enum VEHICLE_TYPE {
-	VEHICLE_TYPE_GENERIC = 0, /* Generic micro air vehicle. | */
-	VEHICLE_TYPE_FIXED_WING = 1, /* Fixed wing aircraft. | */
-	VEHICLE_TYPE_QUADROTOR = 2, /* Quadrotor | */
-	VEHICLE_TYPE_COAXIAL = 3, /* Coaxial helicopter | */
-	VEHICLE_TYPE_HELICOPTER = 4, /* Normal helicopter with tail rotor. | */
-	VEHICLE_TYPE_ANTENNA_TRACKER = 5, /* Ground installation | */
-	VEHICLE_TYPE_GCS = 6, /* Operator control unit / ground control station | */
-	VEHICLE_TYPE_AIRSHIP = 7, /* Airship, controlled | */
-	VEHICLE_TYPE_FREE_BALLOON = 8, /* Free balloon, uncontrolled | */
-	VEHICLE_TYPE_ROCKET = 9, /* Rocket | */
-	VEHICLE_TYPE_GROUND_ROVER = 10, /* Ground rover | */
-	VEHICLE_TYPE_SURFACE_BOAT = 11, /* Surface vessel, boat, ship | */
-	VEHICLE_TYPE_SUBMARINE = 12, /* Submarine | */
-	VEHICLE_TYPE_HEXAROTOR = 13, /* Hexarotor | */
-	VEHICLE_TYPE_OCTOROTOR = 14, /* Octorotor | */
-	VEHICLE_TYPE_TRICOPTER = 15, /* Octorotor | */
-	VEHICLE_TYPE_FLAPPING_WING = 16, /* Flapping wing | */
-	VEHICLE_TYPE_KITE = 17, /* Kite | */
-	VEHICLE_TYPE_ONBOARD_CONTROLLER=18, /* Onboard companion controller | */
-	VEHICLE_TYPE_VTOL_DUOROTOR = 19, /* Vtol with two engines */
-	VEHICLE_TYPE_VTOL_QUADROTOR = 20, /* Vtol with four engines*/
-	VEHICLE_TYPE_ENUM_END = 21 /*  | */
-};
-
-enum VEHICLE_BATTERY_WARNING {
-	VEHICLE_BATTERY_WARNING_NONE = 0,	/**< no battery low voltage warning active */
-	VEHICLE_BATTERY_WARNING_LOW,	/**< warning of low voltage */
-	VEHICLE_BATTERY_WARNING_CRITICAL	/**< alerting of critical voltage */
-};
-
-/**
- * @addtogroup topics
- * @{
- */
-
-/**
- * state machine / state of vehicle.
- *
- * Encodes the complete system state and is set by the commander app.
- */
-struct vehicle_status_s {
-	/* use of a counter and timestamp recommended (but not necessary) */
-
-	uint16_t counter;   /**< incremented by the writing thread everytime new data is stored */
-	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
-
-	main_state_t main_state;		    	/**< main state machine */
-	navigation_state_t nav_state;		/**< set navigation state machine to specified value */
-	arming_state_t arming_state;			/**< current arming state */
-	hil_state_t hil_state;				/**< current hil state */
-	bool failsafe;					/**< true if system is in failsafe state */
-
-	int32_t system_type;				/**< system type, inspired by MAVLink's VEHICLE_TYPE enum */
-	int32_t	system_id;				/**< system id, inspired by MAVLink's system ID field */
-	int32_t component_id;				/**< subsystem / component id, inspired by MAVLink's component ID field */
-
-	bool is_rotary_wing;				/**< True if system is in rotary wing configuration, so for a VTOL
-							  this is only true while flying as a multicopter */
-	bool is_vtol;					/**< True if the system is VTOL capable */
-
-	bool vtol_fw_permanent_stab;	/**< True if vtol should stabilize attitude for fw in manual mode */
-
-	bool condition_battery_voltage_valid;
-	bool condition_system_in_air_restore;	/**< true if we can restore in mid air */
-	bool condition_system_sensors_initialized;
-	bool condition_system_returned_to_home;
-	bool condition_auto_mission_available;
-	bool condition_global_position_valid;		/**< set to true by the commander app if the quality of the position estimate is good enough to use it for navigation */
-	bool condition_launch_position_valid;		/**< indicates a valid launch position */
-	bool condition_home_position_valid;		/**< indicates a valid home position (a valid home position is not always a valid launch) */
-	bool condition_local_position_valid;
-	bool condition_local_altitude_valid;
-	bool condition_airspeed_valid;			/**< set to true by the commander app if there is a valid airspeed measurement available */
-	bool condition_landed;					/**< true if vehicle is landed, always true if disarmed */
-	bool condition_power_input_valid;		/**< set if input power is valid */
-	float avionics_power_rail_voltage;		/**< voltage of the avionics power rail */
-
-	bool rc_signal_found_once;
-	bool rc_signal_lost;				/**< true if RC reception lost */
-	uint64_t rc_signal_lost_timestamp;		/**< Time at which the RC reception was lost */
-	bool rc_signal_lost_cmd;				/**< true if RC lost mode is commanded */
-	bool rc_input_blocked;				/**< set if RC input should be ignored */
-
-	bool data_link_lost;				/**< datalink to GCS lost */
-	bool data_link_lost_cmd;			/**< datalink to GCS lost mode commanded */
-	uint8_t data_link_lost_counter;			/**< counts unique data link lost events */
-	bool engine_failure;				/** Set to true if an engine failure is detected */
-	bool engine_failure_cmd;			/** Set to true if an engine failure mode is commanded */
-	bool gps_failure;				/** Set to true if a gps failure is detected */
-	bool gps_failure_cmd;				/** Set to true if a gps failure mode is commanded */
-
-	bool barometer_failure;				/** Set to true if a barometer failure is detected */
-
-	bool offboard_control_signal_found_once;
-	bool offboard_control_signal_lost;
-	bool offboard_control_signal_weak;
-	uint64_t offboard_control_signal_lost_interval;	/**< interval in microseconds without an offboard control message */
-	bool offboard_control_set_by_command; /**< true if the offboard mode was set by a mavlink command
-						    and should not be overridden by RC */
-
-	/* see SYS_STATUS mavlink message for the following */
-	uint32_t onboard_control_sensors_present;
-	uint32_t onboard_control_sensors_enabled;
-	uint32_t onboard_control_sensors_health;
-
-	float load;					/**< processor load from 0 to 1 */
-	float battery_voltage;
-	float battery_current;
-	float battery_remaining;
-
-	enum VEHICLE_BATTERY_WARNING battery_warning;    /**< current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum */
-	uint16_t drop_rate_comm;
-	uint16_t errors_comm;
-	uint16_t errors_count1;
-	uint16_t errors_count2;
-	uint16_t errors_count3;
-	uint16_t errors_count4;
-
-	bool circuit_breaker_engaged_power_check;
-	bool circuit_breaker_engaged_airspd_check;
-	bool circuit_breaker_engaged_enginefailure_check;
-	bool circuit_breaker_engaged_gpsfailure_check;
-};
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(vehicle_status);
-
-#endif
diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h
index 9c33c8a3e..b54f0322b 100644
--- a/src/modules/uORB/uORB.h
+++ b/src/modules/uORB/uORB.h
@@ -144,7 +144,7 @@ typedef intptr_t	orb_advert_t;
  * node in /obj if required and publishes the initial data.
  *
  * Any number of advertisers may publish to a topic; publications are atomic
- * but co-ordination between publishers is not provided by the ORB. 
+ * but co-ordination between publishers is not provided by the ORB.
  *
  * @param meta		The uORB metadata (usually from the ORB_ID() macro)
  *			for the topic.
@@ -351,4 +351,13 @@ extern int	orb_set_interval(int handle, unsigned interval) __EXPORT;
 
 __END_DECLS
 
+/* Diverse uORB header defines */ //XXX: move to better location
+#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS    ORB_ID(actuator_controls_0)
+typedef struct vehicle_attitude_setpoint_s fw_virtual_attitude_setpoint_s;
+typedef uint8_t arming_state_t;
+typedef uint8_t main_state_t;
+typedef uint8_t hil_state_t;
+typedef uint8_t navigation_state_t;
+typedef uint8_t switch_pos_t;
+
 #endif /* _UORB_UORB_H */
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 4dc03b61b..b93a95f96 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -381,7 +381,7 @@ int UavcanNode::run()
 		} else {
 			// get controls for required topics
 			bool controls_updated = false;
-			for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+			for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 				if (_control_subs[i] > 0) {
 					if (_poll_fds[_poll_ids[i]].revents & POLLIN) {
 						controls_updated = true;
@@ -393,14 +393,14 @@ int UavcanNode::run()
 			/*
 			  see if we have any direct actuator updates
 			 */
-			if (_actuator_direct_sub != -1 && 
+			if (_actuator_direct_sub != -1 &&
 			    (_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) &&
 			    orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK &&
 			    !_test_in_progress) {
 				if (_actuator_direct.nvalues > NUM_ACTUATOR_OUTPUTS) {
 					_actuator_direct.nvalues = NUM_ACTUATOR_OUTPUTS;
 				}
-				memcpy(&_outputs.output[0], &_actuator_direct.values[0], 
+				memcpy(&_outputs.output[0], &_actuator_direct.values[0],
 				       _actuator_direct.nvalues*sizeof(float));
 				_outputs.noutputs = _actuator_direct.nvalues;
 				new_output = true;
@@ -476,7 +476,7 @@ int UavcanNode::run()
 		if (updated) {
 			orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
 
-			// Update the armed status and check that we're not locked down and motor 
+			// Update the armed status and check that we're not locked down and motor
 			// test is not running
 			bool set_armed = _armed.armed && !_armed.lockdown && !_test_in_progress;
 
@@ -502,7 +502,7 @@ UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t co
 int
 UavcanNode::teardown()
 {
-	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 		if (_control_subs[i] > 0) {
 			::close(_control_subs[i]);
 			_control_subs[i] = -1;
@@ -526,7 +526,7 @@ UavcanNode::subscribe()
 	uint32_t sub_groups = _groups_required & ~_groups_subscribed;
 	uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
 	// the first fd used by CAN
-	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 		if (sub_groups & (1 << i)) {
 			warnx("subscribe to actuator_controls_%d", i);
 			_control_subs[i] = orb_subscribe(_control_topics[i]);
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 8e68730b8..0a333eade 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -58,7 +58,11 @@
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_virtual_mc.h>
+#include <uORB/topics/actuator_controls_virtual_fw.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/mc_virtual_rates_setpoint.h>
+#include <uORB/topics/fw_virtual_rates_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vtol_vehicle_status.h>
diff --git a/platforms/empty.c b/src/platforms/empty.c
index 139531354..139531354 100644
--- a/platforms/empty.c
+++ b/src/platforms/empty.c
diff --git a/src/platforms/nuttx/module.mk b/src/platforms/nuttx/module.mk
new file mode 100644
index 000000000..4a2aff824
--- /dev/null
+++ b/src/platforms/nuttx/module.mk
@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# NuttX / uORB adapter library
+#
+
+SRCS		 = 	px4_nuttx_impl.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/modules/uORB/topics/parameter_update.h b/src/platforms/nuttx/px4_nuttx_impl.cpp
index 68964deb0..70e292320 100644
--- a/src/modules/uORB/topics/parameter_update.h
+++ b/src/platforms/nuttx/px4_nuttx_impl.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -32,30 +32,26 @@
  ****************************************************************************/
 
 /**
- * @file parameter_update.h
- * Notification about a parameter update.
+ * @file px4_nuttx_impl.cpp
+ *
+ * PX4 Middleware Wrapper NuttX Implementation
  */
 
-#ifndef TOPIC_PARAMETER_UPDATE_H
-#define TOPIC_PARAMETER_UPDATE_H
-
-#include <stdint.h>
-#include "../uORB.h"
+#include <px4.h>
+#include <drivers/drv_hrt.h>
 
-/**
- * @addtogroup topics
- * @{
- */
 
-struct parameter_update_s {
-	/** time at which the latest parameter was updated */
-	uint64_t	timestamp;
-};
+namespace px4
+{
 
-/**
- * @}
- */
+void init(int argc, char *argv[], const char *process_name)
+{
+	PX4_WARN("process: %s", process_name);
+}
 
-ORB_DECLARE(parameter_update);
+uint64_t get_time_micros()
+{
+	return hrt_absolute_time();
+}
 
-#endif
\ No newline at end of file
+}
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
new file mode 100644
index 000000000..fa4e1398e
--- /dev/null
+++ b/src/platforms/px4_defines.h
@@ -0,0 +1,206 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_defines.h
+ *
+ * Generally used magic defines
+ */
+
+#pragma once
+/* Get the name of the default value fiven the param name */
+#define PX4_PARAM_DEFAULT_VALUE_NAME(_name) PARAM_##_name##_DEFAULT
+
+/* Shortcuts to define parameters when the default value is defined according to PX4_PARAM_DEFAULT_VALUE_NAME */
+#define PX4_PARAM_DEFINE_INT32(_name) PARAM_DEFINE_INT32(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name))
+#define PX4_PARAM_DEFINE_FLOAT(_name) PARAM_DEFINE_FLOAT(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name))
+
+
+#if defined(__PX4_ROS)
+/*
+ * Building for running within the ROS environment
+ */
+#define noreturn_function
+#ifdef __cplusplus
+#include "ros/ros.h"
+#endif
+/* Main entry point */
+#define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv)
+
+/* Print/output wrappers */
+#define PX4_WARN ROS_WARN
+#define PX4_INFO ROS_INFO
+
+/* Topic Handle */
+#define PX4_TOPIC(_name) #_name
+
+/* Topic type */
+#define PX4_TOPIC_T(_name) px4::_name
+
+/* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */
+#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _objptr, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, _objptr);
+/* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */
+#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf);
+/* Subscribe without a callback (do not use directly, use PX4_SUBSCRIBE instead) */
+#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name));
+
+/* Parameter handle datatype */
+typedef const char *px4_param_t;
+
+/* Helper functions to set ROS params, only int and float supported */
+static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value)
+{
+	if (!ros::param::has(name)) {
+		ros::param::set(name, value);
+	}
+
+	return (px4_param_t)name;
+};
+static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value)
+{
+	if (!ros::param::has(name)) {
+		ros::param::set(name, value);
+	}
+
+	return (px4_param_t)name;
+};
+
+/* Initialize a param, in case of ROS the parameter is sent to the parameter server here */
+#define PX4_PARAM_INIT(_name) PX4_ROS_PARAM_SET(#_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name))
+
+/* Get value of parameter by handle */
+#define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt)
+
+/* Get value of parameter by name, which is equal to the handle for ros */
+#define PX4_PARAM_GET_BYNAME(_name, _destpt) PX4_PARAM_GET(_name, _destpt)
+
+#define OK 0
+#define ERROR -1
+
+//XXX hack to be able to use isfinte from math.h, -D_GLIBCXX_USE_C99_MATH seems not to work
+#define isfinite(_value) std::isfinite(_value)
+
+/* Useful constants.  */
+#define M_E_F			2.7182818284590452354f
+#define M_LOG2E_F		1.4426950408889634074f
+#define M_LOG10E_F		0.43429448190325182765f
+#define M_LN2_F			_M_LN2_F
+#define M_LN10_F		2.30258509299404568402f
+#define M_PI_F			3.14159265358979323846f
+#define M_TWOPI_F       (M_PI_F * 2.0f)
+#define M_PI_2_F		1.57079632679489661923f
+#define M_PI_4_F		0.78539816339744830962f
+#define M_3PI_4_F		2.3561944901923448370E0f
+#define M_SQRTPI_F      1.77245385090551602792981f
+#define M_1_PI_F		0.31830988618379067154f
+#define M_2_PI_F		0.63661977236758134308f
+#define M_2_SQRTPI_F	1.12837916709551257390f
+#define M_DEG_TO_RAD_F 	0.01745329251994f
+#define M_RAD_TO_DEG_F 	57.2957795130823f
+#define M_SQRT2_F		1.41421356237309504880f
+#define M_SQRT1_2_F		0.70710678118654752440f
+#define M_LN2LO_F       1.9082149292705877000E-10f
+#define M_LN2HI_F       6.9314718036912381649E-1f
+#define M_SQRT3_F		1.73205080756887719000f
+#define M_IVLN10_F      0.43429448190325182765f /* 1 / log(10) */
+#define M_LOG2_E_F      _M_LN2_F
+#define M_INVLN2_F      1.4426950408889633870E0f  /* 1 / log(2) */
+#define M_DEG_TO_RAD 	0.01745329251994
+#define M_RAD_TO_DEG 	57.2957795130823
+
+#else
+/*
+ * Building for NuttX
+ */
+#include <platforms/px4_includes.h>
+/* Main entry point */
+#define PX4_MAIN_FUNCTION(_prefix) int _prefix##_task_main(int argc, char *argv[])
+
+/* Print/output wrappers */
+#define PX4_WARN warnx
+#define PX4_INFO warnx
+
+/* Topic Handle */
+#define PX4_TOPIC(_name) ORB_ID(_name)
+
+/* Topic type */
+#define PX4_TOPIC_T(_name) _name##_s
+
+/* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */
+#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _objptr, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, _objptr, std::placeholders::_1), _interval)
+/* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */
+#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval)
+/* Subscribe without a callback (do not use directly, use PX4_SUBSCRIBE instead) */
+#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), _interval)
+
+/* Parameter handle datatype */
+#include <systemlib/param/param.h>
+typedef param_t px4_param_t;
+
+/* Initialize a param, get param handle */
+#define PX4_PARAM_INIT(_name) param_find(#_name)
+
+/* Get value of parameter by handle */
+#define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt)
+
+/* Get value of parameter by name */
+#define PX4_PARAM_GET_BYNAME(_name, _destpt) param_get(param_find(_name), _destpt)
+
+/* XXX this is a hack to resolve conflicts with NuttX headers */
+#if !defined(__PX4_TESTS)
+#define isspace(c) \
+	((c) == ' '  || (c) == '\t' || (c) == '\n' || \
+	 (c) == '\r' || (c) == '\f' || c== '\v')
+#endif
+
+#endif
+
+/* Defines for all platforms */
+
+/* Shortcut for subscribing to topics
+ * Overload the PX4_SUBSCRIBE macro to suppport methods, pure functions as callback and no callback at all
+ */
+#define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME
+#define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC, PX4_SUBSCRIBE_NOCB)(__VA_ARGS__)
+
+/* Get a subscriber class type based on the topic name */
+#define PX4_SUBSCRIBER(_name) Subscriber<PX4_TOPIC_T(_name)>
+
+/* shortcut for advertising topics */
+#define PX4_ADVERTISE(_nodehandle, _name) _nodehandle.advertise<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name))
+
+/* wrapper for 2d matrices */
+#define PX4_ARRAY2D(_array, _ncols, _x, _y) (_array[_x * _ncols + _y])
+
+/* wrapper for rotation matrices stored in arrays */
+#define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y)
diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
new file mode 100644
index 000000000..f8561fa3b
--- /dev/null
+++ b/src/platforms/px4_includes.h
@@ -0,0 +1,101 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_includes.h
+ *
+ * Includes headers depending on the build target
+ */
+
+#pragma once
+
+#include <stdbool.h>
+
+#if defined(__PX4_ROS)
+/*
+ * Building for running within the ROS environment
+ */
+
+#ifdef __cplusplus
+#include "ros/ros.h"
+#include <px4_rc_channels.h>
+#include <px4_vehicle_attitude.h>
+#include <px4_vehicle_attitude_setpoint.h>
+#include <px4_manual_control_setpoint.h>
+#include <px4_actuator_controls.h>
+#include <px4_actuator_controls_0.h>
+#include <px4_actuator_controls_virtual_mc.h>
+#include <px4_vehicle_rates_setpoint.h>
+#include <px4_mc_virtual_rates_setpoint.h>
+#include <px4_vehicle_attitude.h>
+#include <px4_vehicle_control_mode.h>
+#include <px4_actuator_armed.h>
+#include <px4_parameter_update.h>
+#include <px4_vehicle_status.h>
+#include <px4_vehicle_local_position_setpoint.h>
+#include <px4_vehicle_global_velocity_setpoint.h>
+#include <px4_vehicle_local_position.h>
+#include <px4_position_setpoint_triplet.h>
+#endif
+
+#else
+/*
+ * Building for NuttX
+ */
+#include <nuttx/config.h>
+#include <uORB/uORB.h>
+#ifdef __cplusplus
+#include <platforms/nuttx/px4_messages/px4_rc_channels.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_attitude_setpoint.h>
+#include <platforms/nuttx/px4_messages/px4_manual_control_setpoint.h>
+#include <platforms/nuttx/px4_messages/px4_actuator_controls.h>
+#include <platforms/nuttx/px4_messages/px4_actuator_controls_0.h>
+#include <platforms/nuttx/px4_messages/px4_actuator_controls_1.h>
+#include <platforms/nuttx/px4_messages/px4_actuator_controls_2.h>
+#include <platforms/nuttx/px4_messages/px4_actuator_controls_3.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_rates_setpoint.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_attitude.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_control_mode.h>
+#include <platforms/nuttx/px4_messages/px4_actuator_armed.h>
+#include <platforms/nuttx/px4_messages/px4_parameter_update.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_status.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_local_position_setpoint.h>
+#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>
+#endif
+#include <systemlib/err.h>
+#include <systemlib/param/param.h>
+#include <systemlib/systemlib.h>
+
+#endif
diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/platforms/px4_message.h
index 0f6c9aca1..bff7aa313 100644
--- a/src/modules/uORB/topics/actuator_armed.h
+++ b/src/platforms/px4_message.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -32,38 +32,46 @@
  ****************************************************************************/
 
 /**
- * @file actuator_armed.h
- *
- * Actuator armed topic
+ * @file px4_message.h
  *
+ * Defines the message base types
  */
+#pragma once
 
-#ifndef TOPIC_ACTUATOR_ARMED_H
-#define TOPIC_ACTUATOR_ARMED_H
+#if defined(__PX4_ROS)
+typedef const char* PX4TopicHandle;
+#else
+#include <uORB/uORB.h>
+typedef orb_id_t PX4TopicHandle;
+#endif
 
-#include <stdint.h>
-#include "../uORB.h"
+namespace px4
+{
 
-/**
- * @addtogroup topics
- * @{
- */
+template <typename M>
+class __EXPORT PX4Message
+{
+	// friend class NodeHandle;
+// #if defined(__PX4_ROS)
+	// template<typename T>
+	// friend class SubscriberROS;
+// #endif
 
-/** global 'actuator output is live' control. */
-struct actuator_armed_s {
+public:
+	PX4Message() :
+		_data()
+	{}
 
-	uint64_t	timestamp;	/**< Microseconds since system boot */
-	bool		armed;		/**< Set to true if system is armed */
-	bool		ready_to_arm;	/**< Set to true if system is ready to be armed */
-	bool		lockdown;	/**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
-	bool		force_failsafe; /**< Set to true if the actuators are forced to the failsafe position */
-};
+	PX4Message(M msg) :
+		_data(msg)
+	{}
 
-/**
- * @}
- */
+	virtual ~PX4Message() {};
 
-/* register this as object request broker structure */
-ORB_DECLARE(actuator_armed);
+	virtual M& data() {return _data;}
+	virtual const M& data() const {return _data;}
+private:
+	M _data;
+};
 
-#endif
+}
diff --git a/src/modules/uORB/topics/vehicle_rates_setpoint.h b/src/platforms/px4_middleware.h
index 47d51f199..735d34234 100644
--- a/src/modules/uORB/topics/vehicle_rates_setpoint.h
+++ b/src/platforms/px4_middleware.h
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,36 +32,53 @@
  ****************************************************************************/
 
 /**
- * @file vehicle_rates_setpoint.h
- * Definition of the vehicle rates setpoint topic
+ * @file px4_middleware.h
+ *
+ * PX4 generic middleware wrapper
  */
 
-#ifndef TOPIC_VEHICLE_RATES_SETPOINT_H_
-#define TOPIC_VEHICLE_RATES_SETPOINT_H_
+#pragma once
 
 #include <stdint.h>
-#include "../uORB.h"
+#include <unistd.h>
+
+namespace px4
+{
+
+__EXPORT void init(int argc, char *argv[], const char *process_name);
 
+__EXPORT uint64_t get_time_micros();
+
+#if defined(__PX4_ROS)
+/**
+ * Returns true if the app/task should continue to run
+ */
+inline bool ok() { return ros::ok(); }
+#else
+extern bool task_should_exit;
 /**
- * @addtogroup topics
- * @{
+ * Returns true if the app/task should continue to run
  */
-struct vehicle_rates_setpoint_s {
-	uint64_t timestamp; /**< in microseconds since system start */
+__EXPORT inline bool ok() { return !task_should_exit; }
+#endif
 
-	float roll;	/**< body angular rates in NED frame		*/
-	float pitch;	/**< body angular rates in NED frame		*/
-	float yaw;	/**< body angular rates in NED frame		*/
-	float thrust;	/**< thrust normalized to 0..1			*/
+class Rate
+{
+public:
+	/**
+	 * Construct the Rate object and set rate
+	 * @param rate_hz rate from which sleep time is calculated in Hz
+	 */
+	explicit Rate(unsigned rate_hz) { sleep_interval = 1e6 / rate_hz; }
 
-}; /**< vehicle_rates_setpoint */
+	/**
+	 * Sleep for 1/rate_hz s
+	 */
+	void sleep() { usleep(sleep_interval); }
 
-/**
-* @}
-*/
+private:
+	uint64_t sleep_interval;
 
-/* register this as object request broker structure */
-ORB_DECLARE(vehicle_rates_setpoint);
-ORB_DECLARE(mc_virtual_rates_setpoint);
-ORB_DECLARE(fw_virtual_rates_setpoint);
-#endif
+};
+
+} // namespace px4
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
new file mode 100644
index 000000000..83a3e422d
--- /dev/null
+++ b/src/platforms/px4_nodehandle.h
@@ -0,0 +1,302 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_nodehandle.h
+ *
+ * PX4 Middleware Wrapper Node Handle
+ */
+#pragma once
+
+/* includes for all platforms */
+#include "px4_subscriber.h"
+#include "px4_publisher.h"
+#include "px4_middleware.h"
+
+#if defined(__PX4_ROS)
+/* includes when building for ros */
+#include "ros/ros.h"
+#include <list>
+#include <inttypes.h>
+#include <type_traits>
+#else
+/* includes when building for NuttX */
+#include <poll.h>
+#endif
+#include <functional>
+
+namespace px4
+{
+#if defined(__PX4_ROS)
+class NodeHandle :
+	private ros::NodeHandle
+{
+public:
+	NodeHandle() :
+		ros::NodeHandle(),
+		_subs(),
+		_pubs()
+	{}
+
+	~NodeHandle()
+	{
+		_subs.clear();
+		_pubs.clear();
+	};
+
+	/**
+	 * Subscribe with callback to function
+	 * @param topic		Name of the topic
+	 * @param fb		Callback, executed on receiving a new message
+	 */
+	template<typename T>
+	Subscriber<T> *subscribe(void(*fp)(const T &), unsigned interval)
+	{
+		SubscriberBase *sub = new SubscriberROS<T>((ros::NodeHandle*)this, std::bind(fp, std::placeholders::_1));
+		_subs.push_back(sub);
+		return (Subscriber<T> *)sub;
+	}
+
+	/**
+	 * Subscribe with callback to class method
+	 * @param fb		Callback, executed on receiving a new message
+	 * @param obj	        pointer class instance
+	 */
+	template<typename T, typename C>
+	Subscriber<T> *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval)
+	{
+		SubscriberBase *sub = new SubscriberROS<T>((ros::NodeHandle*)this, std::bind(fp, obj, std::placeholders::_1));
+		_subs.push_back(sub);
+		return (Subscriber<T> *)sub;
+	}
+
+	/**
+	 * Subscribe with no callback, just the latest value is stored on updates
+	 */
+	template<typename T>
+	Subscriber<T> *subscribe(unsigned interval)
+	{
+		SubscriberBase *sub = new SubscriberROS<T>((ros::NodeHandle*)this);
+		_subs.push_back(sub);
+		return (Subscriber<T> *)sub;
+	}
+
+	/**
+	 * Advertise topic
+	 */
+	template<typename T>
+	Publisher<T>* advertise()
+	{
+		PublisherROS<T> *pub =  new PublisherROS<T>((ros::NodeHandle*)this);
+		_pubs.push_back((PublisherBase*)pub);
+		return (Publisher<T>*)pub;
+	}
+
+	/**
+	 * Calls all callback waiting to be called
+	 */
+	void spinOnce() { ros::spinOnce(); }
+
+	/**
+	 * Keeps calling callbacks for incomming messages, returns when module is terminated
+	 */
+	void spin() { ros::spin(); }
+
+
+protected:
+	std::list<SubscriberBase *> _subs;				/**< Subcriptions of node */
+	std::list<PublisherBase *> _pubs;				/**< Publications of node */
+};
+#else //Building for NuttX
+class __EXPORT NodeHandle
+{
+public:
+	NodeHandle() :
+		_subs(),
+		_pubs(),
+		_sub_min_interval(nullptr)
+	{}
+
+	~NodeHandle()
+	{
+		/* Empty subscriptions list */
+		SubscriberNode *sub = _subs.getHead();
+		int count = 0;
+
+		while (sub != nullptr) {
+			if (count++ > kMaxSubscriptions) {
+				PX4_WARN("exceeded max subscriptions");
+				break;
+			}
+
+			SubscriberNode *sib = sub->getSibling();
+			delete sub;
+			sub = sib;
+		}
+
+		/* Empty publications list */
+		PublisherNode *pub = _pubs.getHead();
+		count = 0;
+
+		while (pub != nullptr) {
+			if (count++ > kMaxPublications) {
+				PX4_WARN("exceeded max publications");
+				break;
+			}
+
+			PublisherNode *sib = pub->getSibling();
+			delete pub;
+			pub = sib;
+		}
+	};
+
+	/**
+	 * Subscribe with callback to function
+	 * @param fp		Callback, executed on receiving a new message
+	 * @param interval	Minimal interval between calls to callback
+	 */
+
+	template<typename T>
+	Subscriber<T> *subscribe(void(*fp)(const T &),  unsigned interval)
+	{
+		(void)interval;
+		SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(interval, std::bind(fp, std::placeholders::_1));
+		update_sub_min_interval(interval, sub_px4);
+		_subs.add((SubscriberNode *)sub_px4);
+		return (Subscriber<T> *)sub_px4;
+	}
+
+	/**
+	 * Subscribe with callback to class method
+	 * @param fb		Callback, executed on receiving a new message
+	 * @param obj	        pointer class instance
+	 */
+	template<typename T, typename C>
+	Subscriber<T> *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval)
+	{
+		(void)interval;
+		SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(interval, std::bind(fp, obj, std::placeholders::_1));
+		update_sub_min_interval(interval, sub_px4);
+		_subs.add((SubscriberNode *)sub_px4);
+		return (Subscriber<T> *)sub_px4;
+	}
+
+	/**
+	 * Subscribe without callback to function
+	 * @param interval	Minimal interval between data fetches from orb
+	 */
+
+	template<typename T>
+	Subscriber<T> *subscribe(unsigned interval)
+	{
+		(void)interval;
+		SubscriberUORB<T> *sub_px4 = new SubscriberUORB<T>(interval);
+		update_sub_min_interval(interval, sub_px4);
+		_subs.add((SubscriberNode *)sub_px4);
+		return (Subscriber<T> *)sub_px4;
+	}
+
+	/**
+	 * Advertise topic
+	 */
+	template<typename T>
+	Publisher<T> *advertise()
+	{
+		PublisherUORB<T> *pub = new PublisherUORB<T>();
+		_pubs.add(pub);
+		return (Publisher<T>*)pub;
+	}
+
+	/**
+	 * Calls all callback waiting to be called
+	 */
+	void spinOnce()
+	{
+		/* Loop through subscriptions, call callback for updated subscriptions */
+		SubscriberNode *sub = _subs.getHead();
+		int count = 0;
+
+		while (sub != nullptr) {
+			if (count++ > kMaxSubscriptions) {
+				PX4_WARN("exceeded max subscriptions");
+				break;
+			}
+
+			sub->update();
+			sub = sub->getSibling();
+		}
+	}
+
+	/**
+	 * Keeps calling callbacks for incomming messages, returns when module is terminated
+	 */
+	void spin()
+	{
+		while (ok()) {
+			const int timeout_ms = 100;
+
+			/* Only continue in the loop if the nodehandle has subscriptions */
+			if (_sub_min_interval == nullptr) {
+				usleep(timeout_ms * 1000);
+				continue;
+			}
+
+			/* Poll fd with smallest interval */
+			struct pollfd pfd;
+			pfd.fd = _sub_min_interval->getUORBHandle();
+			pfd.events = POLLIN;
+			poll(&pfd, 1, timeout_ms);
+			spinOnce();
+		}
+	}
+protected:
+	static const uint16_t kMaxSubscriptions = 100;
+	static const uint16_t kMaxPublications = 100;
+	List<SubscriberNode *> _subs;		/**< Subcriptions of node */
+	List<PublisherNode *> _pubs;		/**< Publications of node */
+	SubscriberNode *_sub_min_interval;	/**< Points to the sub wtih the smallest interval
+							  of all Subscriptions in _subs*/
+
+	/**
+	 * Check if this is the smallest interval so far and update _sub_min_interval
+	 */
+	template<typename T>
+	void update_sub_min_interval(unsigned interval, SubscriberUORB<T> *sub)
+	{
+		if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) {
+			_sub_min_interval = sub;
+		}
+	}
+};
+#endif
+}
diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h
new file mode 100644
index 000000000..d9cd7a3c1
--- /dev/null
+++ b/src/platforms/px4_publisher.h
@@ -0,0 +1,161 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_nodehandle.h
+ *
+ * PX4 Middleware Wrapper Node Handle
+ */
+#pragma once
+#if defined(__PX4_ROS)
+/* includes when building for ros */
+#include "ros/ros.h"
+#else
+/* includes when building for NuttX */
+#include <uORB/Publication.hpp>
+#include <containers/List.hpp>
+#endif
+
+#include <platforms/px4_message.h>
+
+namespace px4
+{
+
+/**
+ * Untemplated publisher base class
+ * */
+class __EXPORT PublisherBase
+{
+public:
+	PublisherBase() {};
+	~PublisherBase() {};
+};
+
+/**
+ * Publisher base class, templated with the message type
+ * */
+template <typename T>
+class __EXPORT Publisher
+{
+public:
+	Publisher() {};
+	~Publisher() {};
+
+	virtual int publish(const T &msg)  = 0;
+};
+
+#if defined(__PX4_ROS)
+template <typename T>
+class PublisherROS :
+	public Publisher<T>
+{
+public:
+	/**
+	 * Construct Publisher by providing a ros::Publisher
+	 * @param ros_pub the ros publisher which will be used to perform the publications
+	 */
+	PublisherROS(ros::NodeHandle *rnh) :
+		Publisher<T>(),
+		_ros_pub(rnh->advertise<typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &>(T::handle(), kQueueSizeDefault))
+	{}
+
+	~PublisherROS() {};
+
+	/** Publishes msg
+	 * @param msg	    the message which is published to the topic
+	 */
+	int publish(const T &msg)
+	{
+		_ros_pub.publish(msg.data());
+		return 0;
+	}
+protected:
+	static const uint32_t kQueueSizeDefault = 1;		/**< Size of queue for ROS */
+	ros::Publisher _ros_pub;	/**< Handle to the ros publisher */
+};
+#else
+/**
+ * Because we maintain a list of publishers we need a node class
+ */
+class __EXPORT PublisherNode :
+	public ListNode<PublisherNode *>
+{
+public:
+	PublisherNode() :
+		ListNode()
+	{}
+
+	virtual ~PublisherNode() {}
+
+	virtual void update() = 0;
+};
+
+template <typename T>
+class __EXPORT PublisherUORB :
+	public Publisher<T>,
+	public PublisherNode
+
+{
+public:
+	/**
+	 * Construct Publisher by providing orb meta data
+	 */
+	PublisherUORB() :
+		Publisher<T>(),
+		PublisherNode(),
+		_uorb_pub(new uORB::PublicationBase(T::handle()))
+	{}
+
+	~PublisherUORB() {
+		delete _uorb_pub;
+	};
+
+	/** Publishes msg
+	 * @param msg	    the message which is published to the topic
+	 */
+	int publish(const T &msg)
+	{
+		_uorb_pub->update((void *)&(msg.data()));
+		return 0;
+	}
+
+	/**
+	 * Empty callback for list traversal
+	 */
+	void update() {} ;
+private:
+	uORB::PublicationBase * _uorb_pub;	/**< Handle to the publisher */
+
+};
+#endif
+}
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
new file mode 100644
index 000000000..6ca35b173
--- /dev/null
+++ b/src/platforms/px4_subscriber.h
@@ -0,0 +1,284 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_subscriber.h
+ *
+ * PX4 Middleware Wrapper Subscriber
+ */
+#pragma once
+
+#include <functional>
+#include <type_traits>
+
+#if defined(__PX4_ROS)
+/* includes when building for ros */
+#include "ros/ros.h"
+#else
+/* includes when building for NuttX */
+#include <uORB/Subscription.hpp>
+#include <containers/List.hpp>
+#include "px4_nodehandle.h"
+#endif
+
+namespace px4
+{
+
+/**
+ * Untemplated subscriber base class
+ * */
+class __EXPORT SubscriberBase
+{
+public:
+	SubscriberBase() {};
+	virtual ~SubscriberBase() {};
+
+};
+
+/**
+ * Subscriber class which is used by nodehandle
+ */
+template<typename T>
+class __EXPORT Subscriber :
+	public SubscriberBase
+{
+public:
+	Subscriber() :
+		SubscriberBase(),
+		_msg_current()
+	{};
+
+	virtual ~Subscriber() {}
+
+	/* Accessors*/
+	/**
+	 * Get the last message value
+	 */
+	virtual T& get() {return _msg_current;}
+
+	/**
+	 * Get the last native message value
+	 */
+	virtual decltype(((T*)nullptr)->data()) data()
+	{
+		return _msg_current.data();
+	}
+
+protected:
+	T _msg_current;				/**< Current Message value */
+};
+
+#if defined(__PX4_ROS)
+/**
+ * Subscriber class that is templated with the ros n message type
+ */
+template<typename T>
+class SubscriberROS :
+	public Subscriber<T>
+{
+public:
+	/**
+	 * Construct Subscriber without a callback function
+	 */
+	SubscriberROS(ros::NodeHandle *rnh) :
+		px4::Subscriber<T>(),
+		_cbf(NULL),
+		_ros_sub(rnh->subscribe(T::handle(), kQueueSizeDefault, &SubscriberROS<T>::callback, this))
+	{}
+
+	/**
+	 * Construct Subscriber by providing a callback function
+	 */
+	SubscriberROS(ros::NodeHandle *rnh, std::function<void(const T &)> cbf) :
+		_cbf(cbf),
+		_ros_sub(rnh->subscribe(T::handle(), kQueueSizeDefault, &SubscriberROS<T>::callback, this))
+	{}
+
+	virtual ~SubscriberROS() {};
+
+protected:
+	static const uint32_t kQueueSizeDefault = 1;		/**< Size of queue for ROS */
+
+	/**
+	 * Called on topic update, saves the current message and then calls the provided callback function
+	 * needs to use the native type as it is called by ROS
+	 */
+	void callback(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &msg)
+	{
+		/* Store data */
+		this->_msg_current.data() = msg;
+
+		/* Call callback */
+		if (_cbf != NULL) {
+			_cbf(this->get());
+		}
+
+	}
+
+	ros::Subscriber _ros_sub;		/**< Handle to ros subscriber */
+	std::function<void(const T &)> _cbf;	/**< Callback that the user provided on the subscription */
+
+};
+
+#else // Building for NuttX
+/**
+ * Because we maintain a list of subscribers we need a node class
+ */
+class __EXPORT SubscriberNode :
+	public ListNode<SubscriberNode *>
+{
+public:
+	SubscriberNode(unsigned interval) :
+		ListNode(),
+		_interval(interval)
+	{}
+
+	virtual ~SubscriberNode() {}
+
+	virtual void update() = 0;
+
+	virtual int getUORBHandle() = 0;
+
+	unsigned get_interval() { return _interval; }
+
+protected:
+	unsigned _interval;
+
+};
+
+/**
+ * Subscriber class that is templated with the uorb subscription message type
+ */
+template<typename T>
+class __EXPORT SubscriberUORB :
+	public Subscriber<T>,
+	public SubscriberNode
+{
+public:
+
+	/**
+	 * Construct SubscriberUORB by providing orb meta data without callback
+	 * @param interval	Minimal interval between calls to callback
+	 */
+	SubscriberUORB(unsigned interval) :
+		SubscriberNode(interval),
+		_uorb_sub(new uORB::SubscriptionBase(T::handle(), interval))
+	{}
+
+	virtual ~SubscriberUORB() {
+		delete _uorb_sub;
+	};
+
+	/**
+	 * Update Subscription
+	 * Invoked by the list traversal in NodeHandle::spinOnce
+	 */
+	virtual void update()
+	{
+		if (!_uorb_sub->updated()) {
+			/* Topic not updated, do not call callback */
+			return;
+		}
+
+		_uorb_sub->update(get_void_ptr());
+	};
+
+	/* Accessors*/
+	int getUORBHandle() { return _uorb_sub->getHandle(); }
+
+protected:
+	uORB::SubscriptionBase * _uorb_sub;	/**< Handle to the subscription */
+
+	typename std::remove_reference<decltype(((T*)nullptr)->data())>::type getUORBData()
+	{
+		return (typename std::remove_reference<decltype(((T*)nullptr)->data())>::type)*_uorb_sub;
+	}
+
+	/**
+	 * Get void pointer to last message value
+	 */
+	void *get_void_ptr() { return (void *)&(this->_msg_current.data()); }
+
+};
+
+//XXX reduce to one class with overloaded constructor?
+template<typename T>
+class __EXPORT SubscriberUORBCallback :
+	public SubscriberUORB<T>
+{
+public:
+	/**
+	 * Construct SubscriberUORBCallback by providing orb meta data
+	 * @param cbf		Callback, executed on receiving a new message
+	 * @param interval	Minimal interval between calls to callback
+	 */
+	SubscriberUORBCallback(unsigned interval,
+			       std::function<void(const T &)> cbf) :
+		SubscriberUORB<T>(interval),
+		_cbf(cbf)
+	{}
+
+	virtual ~SubscriberUORBCallback() {};
+
+	/**
+	 * Update Subscription
+	 * Invoked by the list traversal in NodeHandle::spinOnce
+	 * If new data is available the callback is called
+	 */
+	virtual void update()
+	{
+		if (!this->_uorb_sub->updated()) {
+			/* Topic not updated, do not call callback */
+			return;
+		}
+
+		/* get latest data */
+		this->_uorb_sub->update(this->get_void_ptr());
+
+
+		/* Check if there is a callback */
+		if (_cbf == nullptr) {
+			return;
+		}
+
+		/* Call callback which performs actions based on this data */
+		_cbf(Subscriber<T>::get());
+
+	};
+
+protected:
+	std::function<void(const T &)> _cbf;	/**< Callback that the user provided on the subscription */
+};
+#endif
+
+}
diff --git a/src/platforms/ros/eigen_math.h b/src/platforms/ros/eigen_math.h
new file mode 100755
index 000000000..c7271c157
--- /dev/null
+++ b/src/platforms/ros/eigen_math.h
@@ -0,0 +1,19 @@
+/*
+ * eigen_math.h
+ *
+ *  Created on: Aug 25, 2014
+ *      Author: roman
+ */
+
+#ifndef EIGEN_MATH_H_
+#define EIGEN_MATH_H_
+
+
+struct eigen_matrix_instance {
+	int numRows;
+	int numCols;
+	float *pData;
+};
+
+
+#endif /* EIGEN_MATH_H_ */
diff --git a/src/platforms/ros/geo.cpp b/src/platforms/ros/geo.cpp
new file mode 100644
index 000000000..04094de8b
--- /dev/null
+++ b/src/platforms/ros/geo.cpp
@@ -0,0 +1,266 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file geo.c
+ *
+ * Geo / math functions to perform geodesic calculations
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <geo/geo.h>
+#include <px4.h>
+#include <unistd.h>
+#include <pthread.h>
+#include <stdio.h>
+#include <math.h>
+#include <stdbool.h>
+#include <string.h>
+#include <float.h>
+
+__EXPORT float _wrap_pi(float bearing)
+{
+	/* value is inf or NaN */
+	if (!isfinite(bearing)) {
+		return bearing;
+	}
+
+	int c = 0;
+
+	while (bearing >= M_PI_F) {
+		bearing -= M_TWOPI_F;
+
+		if (c++ > 3) {
+			return NAN;
+		}
+	}
+
+	c = 0;
+
+	while (bearing < -M_PI_F) {
+		bearing += M_TWOPI_F;
+
+		if (c++ > 3) {
+			return NAN;
+		}
+	}
+
+	return bearing;
+}
+
+__EXPORT float _wrap_2pi(float bearing)
+{
+	/* value is inf or NaN */
+	if (!isfinite(bearing)) {
+		return bearing;
+	}
+
+	int c = 0;
+
+	while (bearing >= M_TWOPI_F) {
+		bearing -= M_TWOPI_F;
+
+		if (c++ > 3) {
+			return NAN;
+		}
+	}
+
+	c = 0;
+
+	while (bearing < 0.0f) {
+		bearing += M_TWOPI_F;
+
+		if (c++ > 3) {
+			return NAN;
+		}
+	}
+
+	return bearing;
+}
+
+__EXPORT float _wrap_180(float bearing)
+{
+	/* value is inf or NaN */
+	if (!isfinite(bearing)) {
+		return bearing;
+	}
+
+	int c = 0;
+
+	while (bearing >= 180.0f) {
+		bearing -= 360.0f;
+
+		if (c++ > 3) {
+			return NAN;
+		}
+	}
+
+	c = 0;
+
+	while (bearing < -180.0f) {
+		bearing += 360.0f;
+
+		if (c++ > 3) {
+			return NAN;
+		}
+	}
+
+	return bearing;
+}
+
+__EXPORT float _wrap_360(float bearing)
+{
+	/* value is inf or NaN */
+	if (!isfinite(bearing)) {
+		return bearing;
+	}
+
+	int c = 0;
+
+	while (bearing >= 360.0f) {
+		bearing -= 360.0f;
+
+		if (c++ > 3) {
+			return NAN;
+		}
+	}
+
+	c = 0;
+
+	while (bearing < 0.0f) {
+		bearing += 360.0f;
+
+		if (c++ > 3) {
+			return NAN;
+		}
+	}
+
+	return bearing;
+}
+
+__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref)
+{
+	return ref->init_done;
+}
+
+__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref)
+{
+	return ref->timestamp;
+}
+
+__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
+{
+
+	ref->lat_rad = lat_0 * M_DEG_TO_RAD;
+	ref->lon_rad = lon_0 * M_DEG_TO_RAD;
+	ref->sin_lat = sin(ref->lat_rad);
+	ref->cos_lat = cos(ref->lat_rad);
+
+	ref->timestamp = timestamp;
+	ref->init_done = true;
+
+	return 0;
+}
+
+__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
+{
+	return map_projection_init_timestamped(ref, lat_0, lon_0, px4::get_time_micros());
+}
+
+__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad)
+{
+	if (!map_projection_initialized(ref)) {
+		return -1;
+	}
+
+	*ref_lat_rad = ref->lat_rad;
+	*ref_lon_rad = ref->lon_rad;
+
+	return 0;
+}
+
+__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
+{
+	if (!map_projection_initialized(ref)) {
+		return -1;
+	}
+
+	double lat_rad = lat * M_DEG_TO_RAD;
+	double lon_rad = lon * M_DEG_TO_RAD;
+
+	double sin_lat = sin(lat_rad);
+	double cos_lat = cos(lat_rad);
+	double cos_d_lon = cos(lon_rad - ref->lon_rad);
+
+	double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon);
+	double k = (fabs(c) < DBL_EPSILON) ? 1.0 : (c / sin(c));
+
+	*x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH;
+	*y = k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH;
+
+	return 0;
+}
+
+__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
+{
+	if (!map_projection_initialized(ref)) {
+		return -1;
+	}
+
+	double x_rad = x / CONSTANTS_RADIUS_OF_EARTH;
+	double y_rad = y / CONSTANTS_RADIUS_OF_EARTH;
+	double c = sqrtf(x_rad * x_rad + y_rad * y_rad);
+	double sin_c = sin(c);
+	double cos_c = cos(c);
+
+	double lat_rad;
+	double lon_rad;
+
+	if (fabs(c) > DBL_EPSILON) {
+		lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c);
+		lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
+
+	} else {
+		lat_rad = ref->lat_rad;
+		lon_rad = ref->lon_rad;
+	}
+
+	*lat = lat_rad * 180.0 / M_PI;
+	*lon = lon_rad * 180.0 / M_PI;
+
+	return 0;
+}
diff --git a/src/platforms/ros/nodes/README.md b/src/platforms/ros/nodes/README.md
new file mode 100644
index 000000000..aafc647ff
--- /dev/null
+++ b/src/platforms/ros/nodes/README.md
@@ -0,0 +1,22 @@
+# PX4 Nodes
+
+This directory contains several small ROS nodes which are intended to run alongside the PX4 multi-platform nodes in
+ROS. They act as a bridge between the PX4 specific topics and the ROS topics.
+
+## Joystick Input
+
+You will need to install the ros joystick packages
+See http://wiki.ros.org/joy/Tutorials/ConfiguringALinuxJoystick
+
+### Arch Linux
+```sh
+yaourt -Sy ros-indigo-joystick-drivers --noconfirm
+```
+check joystick
+```sh
+ls /dev/input/
+ls -l /dev/input/js0
+```
+(replace 0 by the number you find with the first command)
+
+make sure the joystick is accessible by the `input` group and that your user is in the `input` group
diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
new file mode 100644
index 000000000..1d36e3d99
--- /dev/null
+++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
@@ -0,0 +1,152 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file att_estimator.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Roman Bapst <romanbapst@yahoo.de>
+*/
+
+#include "attitude_estimator.h"
+
+#include <px4/vehicle_attitude.h>
+#include <mathlib/mathlib.h>
+#include <platforms/px4_defines.h>
+#include <platforms/px4_middleware.h>
+
+AttitudeEstimator::AttitudeEstimator() :
+	_n(),
+	// _sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &AttitudeEstimator::ModelStatesCallback, this)),
+	_sub_imu(_n.subscribe("/px4_multicopter/imu", 1, &AttitudeEstimator::ImuCallback, this)),
+	_vehicle_attitude_pub(_n.advertise<px4::vehicle_attitude>("vehicle_attitude", 1))
+{
+}
+
+void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg)
+{
+	px4::vehicle_attitude msg_v_att;
+
+	/* Fill px4 attitude topic with contents from modelstates topic */
+
+	/* Convert quaternion to rotation matrix */
+	math::Quaternion quat;
+	//XXX: search for ardrone or other (other than 'plane') vehicle here
+	int index = 1;
+	quat(0) = (float)msg->pose[index].orientation.w;
+	quat(1) = (float)msg->pose[index].orientation.x;
+	quat(2) = (float) - msg->pose[index].orientation.y;
+	quat(3) = (float) - msg->pose[index].orientation.z;
+
+	msg_v_att.q[0] = quat(0);
+	msg_v_att.q[1] = quat(1);
+	msg_v_att.q[2] = quat(2);
+	msg_v_att.q[3] = quat(3);
+
+	math::Matrix<3, 3> rot = quat.to_dcm();
+
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++) {
+			PX4_R(msg_v_att.R, i, j) = rot(i, j);
+		}
+	}
+
+	msg_v_att.R_valid = true;
+
+	math::Vector<3> euler = rot.to_euler();
+	msg_v_att.roll = euler(0);
+	msg_v_att.pitch = euler(1);
+	msg_v_att.yaw = euler(2);
+
+	//XXX this is in inertial frame
+	// msg_v_att.rollspeed = (float)msg->twist[index].angular.x;
+	// msg_v_att.pitchspeed = -(float)msg->twist[index].angular.y;
+	// msg_v_att.yawspeed = -(float)msg->twist[index].angular.z;
+
+	msg_v_att.timestamp = px4::get_time_micros();
+	_vehicle_attitude_pub.publish(msg_v_att);
+}
+
+void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr &msg)
+{
+	px4::vehicle_attitude msg_v_att;
+
+	/* Fill px4 attitude topic with contents from modelstates topic */
+
+	/* Convert quaternion to rotation matrix */
+	math::Quaternion quat;
+	//XXX: search for ardrone or other (other than 'plane') vehicle here
+	int index = 1;
+	quat(0) = (float)msg->orientation.w;
+	quat(1) = (float)msg->orientation.x;
+	quat(2) = (float) - msg->orientation.y;
+	quat(3) = (float) - msg->orientation.z;
+
+	msg_v_att.q[0] = quat(0);
+	msg_v_att.q[1] = quat(1);
+	msg_v_att.q[2] = quat(2);
+	msg_v_att.q[3] = quat(3);
+
+	math::Matrix<3, 3> rot = quat.to_dcm();
+
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++) {
+			PX4_R(msg_v_att.R, i, j) = rot(i, j);
+		}
+	}
+
+	msg_v_att.R_valid = true;
+
+	math::Vector<3> euler = rot.to_euler();
+	msg_v_att.roll = euler(0);
+	msg_v_att.pitch = euler(1);
+	msg_v_att.yaw = euler(2);
+
+	msg_v_att.rollspeed = (float)msg->angular_velocity.x;
+	msg_v_att.pitchspeed = -(float)msg->angular_velocity.y;
+	msg_v_att.yawspeed = -(float)msg->angular_velocity.z;
+
+	msg_v_att.timestamp = px4::get_time_micros();
+	_vehicle_attitude_pub.publish(msg_v_att);
+}
+
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "attitude_estimator");
+	AttitudeEstimator m;
+
+	ros::spin();
+
+	return 0;
+}
diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h
new file mode 100644
index 000000000..f760a39d8
--- /dev/null
+++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h
@@ -0,0 +1,62 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file att_estimator.h
+ * Dummy attitude estimator that forwards attitude from gazebo to px4 topic
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "ros/ros.h"
+#include <gazebo_msgs/ModelStates.h>
+#include <sensor_msgs/Imu.h>
+
+class AttitudeEstimator
+{
+public:
+	AttitudeEstimator();
+
+	~AttitudeEstimator() {}
+
+protected:
+	void ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg);
+	void ImuCallback(const sensor_msgs::ImuConstPtr &msg);
+
+	ros::NodeHandle _n;
+	ros::Subscriber _sub_modelstates;
+	ros::Subscriber _sub_imu;
+	ros::Publisher _vehicle_attitude_pub;
+
+
+};
diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp
new file mode 100644
index 000000000..2673122c7
--- /dev/null
+++ b/src/platforms/ros/nodes/commander/commander.cpp
@@ -0,0 +1,161 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file commander.cpp
+ * Dummy commander node that publishes the various status topics
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "commander.h"
+
+#include <platforms/px4_middleware.h>
+
+Commander::Commander() :
+	_n(),
+	_man_ctrl_sp_sub(_n.subscribe("manual_control_setpoint", 10, &Commander::ManualControlInputCallback, 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()
+{
+}
+
+void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg)
+{
+	px4::vehicle_status msg_vehicle_status;
+
+	/* fill vehicle control mode based on (faked) stick positions*/
+	EvalSwitches(msg, msg_vehicle_status, _msg_vehicle_control_mode);
+	_msg_vehicle_control_mode.timestamp = px4::get_time_micros();
+
+	/* fill actuator armed */
+	float arm_th = 0.95;
+	_msg_actuator_armed.timestamp = px4::get_time_micros();
+
+	if (_msg_actuator_armed.armed) {
+		/* Check for disarm */
+		if (msg->r < -arm_th && msg->z < (1 - arm_th)) {
+			_msg_actuator_armed.armed = false;
+			_msg_vehicle_control_mode.flag_armed = false;
+			msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_STANDBY;
+		}
+
+	} else {
+		/* Check for arm */
+		if (msg->r > arm_th && msg->z < (1 - arm_th)) {
+			_msg_actuator_armed.armed = true;
+			_msg_vehicle_control_mode.flag_armed = true;
+			msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED;
+		}
+	}
+
+	/* fill 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;
+
+	_vehicle_control_mode_pub.publish(_msg_vehicle_control_mode);
+	_actuator_armed_pub.publish(_msg_actuator_armed);
+	_vehicle_status_pub.publish(msg_vehicle_status);
+
+	/* Fill parameter update */
+	if (px4::get_time_micros() - _msg_parameter_update.timestamp > 1e6) {
+		_msg_parameter_update.timestamp = px4::get_time_micros();
+		_parameter_update_pub.publish(_msg_parameter_update);
+	}
+}
+
+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
+
+	switch (msg->mode_switch) {
+		case px4::manual_control_setpoint::SWITCH_POS_NONE:
+		ROS_WARN("Joystick button mapping error, main mode not set");
+		break;
+
+	case px4::manual_control_setpoint::SWITCH_POS_OFF:		// MANUAL
+		msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_MANUAL;
+		msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_MANUAL;
+		msg_vehicle_control_mode.flag_control_manual_enabled = true;
+		msg_vehicle_control_mode.flag_control_rates_enabled = true;
+		msg_vehicle_control_mode.flag_control_attitude_enabled = true;
+		msg_vehicle_control_mode.flag_control_altitude_enabled = false;
+		msg_vehicle_control_mode.flag_control_climb_rate_enabled = false;
+		msg_vehicle_control_mode.flag_control_position_enabled = false;
+		msg_vehicle_control_mode.flag_control_velocity_enabled = false;
+
+		break;
+
+	case px4::manual_control_setpoint::SWITCH_POS_MIDDLE:		// ASSIST
+		if (msg->posctl_switch == px4::manual_control_setpoint::SWITCH_POS_ON) {
+			msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_POSCTL;
+			msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_POSCTL;
+			msg_vehicle_control_mode.flag_control_manual_enabled = true;
+			msg_vehicle_control_mode.flag_control_rates_enabled = true;
+			msg_vehicle_control_mode.flag_control_attitude_enabled = true;
+			msg_vehicle_control_mode.flag_control_altitude_enabled = true;
+			msg_vehicle_control_mode.flag_control_climb_rate_enabled = true;
+			msg_vehicle_control_mode.flag_control_position_enabled = true;
+			msg_vehicle_control_mode.flag_control_velocity_enabled = true;
+		} else {
+			msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_ALTCTL;
+			msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_ALTCTL;
+			msg_vehicle_control_mode.flag_control_manual_enabled = true;
+			msg_vehicle_control_mode.flag_control_rates_enabled = true;
+			msg_vehicle_control_mode.flag_control_attitude_enabled = true;
+			msg_vehicle_control_mode.flag_control_altitude_enabled = true;
+			msg_vehicle_control_mode.flag_control_climb_rate_enabled = true;
+			msg_vehicle_control_mode.flag_control_position_enabled = false;
+			msg_vehicle_control_mode.flag_control_velocity_enabled = false;
+		}
+		break;
+	}
+
+}
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "commander");
+	Commander m;
+	ros::spin();
+	return 0;
+}
diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h
new file mode 100644
index 000000000..58b7257b7
--- /dev/null
+++ b/src/platforms/ros/nodes/commander/commander.h
@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file commander.h
+ * Dummy commander node that publishes the various status topics
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "ros/ros.h"
+#include <px4/manual_control_setpoint.h>
+#include <px4/vehicle_control_mode.h>
+#include <px4/vehicle_status.h>
+#include <px4/parameter_update.h>
+#include <px4/actuator_armed.h>
+
+class Commander
+{
+public:
+	Commander();
+
+	~Commander() {}
+
+protected:
+	/**
+	 * Based on manual control input the status will be set
+	 */
+	void ManualControlInputCallback(const px4::manual_control_setpointConstPtr &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);
+
+	ros::NodeHandle _n;
+	ros::Subscriber _man_ctrl_sp_sub;
+	ros::Publisher _vehicle_control_mode_pub;
+	ros::Publisher _actuator_armed_pub;
+	ros::Publisher _vehicle_status_pub;
+	ros::Publisher _parameter_update_pub;
+
+	px4::parameter_update _msg_parameter_update;
+	px4::actuator_armed _msg_actuator_armed;
+	px4::vehicle_control_mode _msg_vehicle_control_mode;
+
+};
diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp
new file mode 100644
index 000000000..72f6e252f
--- /dev/null
+++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp
@@ -0,0 +1,158 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file manual_input.cpp
+ * Reads from the ros joystick topic and publishes to the px4 manual control input topic.
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "manual_input.h"
+
+#include <platforms/px4_middleware.h>
+
+ManualInput::ManualInput() :
+	_n(),
+	_joy_sub(_n.subscribe("joy", 1, &ManualInput::JoyCallback, this)),
+	_man_ctrl_sp_pub(_n.advertise<px4::manual_control_setpoint>("manual_control_setpoint", 1)),
+	_buttons_state(),
+	_msg_mc_sp()
+{
+	double dz_default = 0.2;
+	/* Load parameters, default values work for Microsoft XBox Controller */
+	_n.param("map_x", _param_axes_map[0], 4);
+	_n.param("scale_x", _param_axes_scale[0], 1.0);
+	_n.param("off_x", _param_axes_offset[0], 0.0);
+	_n.param("dz_x", _param_axes_dz[0], dz_default);
+
+	_n.param("map_y", _param_axes_map[1], 3);
+	_n.param("scale_y", _param_axes_scale[1], -1.0);
+	_n.param("off_y", _param_axes_offset[1], 0.0);
+	_n.param("dz_y", _param_axes_dz[1], dz_default);
+
+	_n.param("map_z", _param_axes_map[2], 2);
+	_n.param("scale_z", _param_axes_scale[2], -0.5);
+	_n.param("off_z", _param_axes_offset[2], -1.0);
+	_n.param("dz_z", _param_axes_dz[2], 0.0);
+
+	_n.param("map_r", _param_axes_map[3], 0);
+	_n.param("scale_r", _param_axes_scale[3], -1.0);
+	_n.param("off_r", _param_axes_offset[3], 0.0);
+	_n.param("dz_r", _param_axes_dz[3], dz_default);
+
+	_n.param("map_manual", _param_buttons_map[0], 0);
+	_n.param("map_altctl", _param_buttons_map[1], 1);
+	_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);
+
+	/* Default to manual */
+	_msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+	_msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+	_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;
+
+}
+
+void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr &msg)
+{
+
+	/* Fill px4 manual control setpoint topic with contents from ros joystick */
+	/* Map sticks to x, y, z, r */
+	MapAxis(msg, _param_axes_map[0], _param_axes_scale[0], _param_axes_offset[0], _param_axes_dz[0], _msg_mc_sp.x);
+	MapAxis(msg, _param_axes_map[1], _param_axes_scale[1], _param_axes_offset[1], _param_axes_dz[1], _msg_mc_sp.y);
+	MapAxis(msg, _param_axes_map[2], _param_axes_scale[2], _param_axes_offset[2], _param_axes_dz[2], _msg_mc_sp.z);
+	MapAxis(msg, _param_axes_map[3], _param_axes_scale[3], _param_axes_offset[3], _param_axes_dz[3], _msg_mc_sp.r);
+	//ROS_INFO("x: %1.4f y: %1.4f z: %1.4f r: %1.4f", msg_mc_sp.x, msg_out.y, msg_out.z, msg_out.r);
+
+	/* Map buttons to switches */
+	MapButtons(msg, _msg_mc_sp);
+
+	_msg_mc_sp.timestamp = px4::get_time_micros();
+
+	_man_ctrl_sp_pub.publish(_msg_mc_sp);
+}
+
+void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, double scale, double offset,
+			  double deadzone, float &out)
+{
+	double val = msg->axes[map_index];
+
+	if (val + offset > deadzone || val + offset < -deadzone) {
+		out = (float)((val + offset)) * scale;
+	} else {
+		out = 0.0f;
+	}
+}
+
+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;
+		msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		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;
+		return;
+
+	} else if (_buttons_state[MAIN_STATE_ALTCTL] != msg->buttons[_param_buttons_map[MAIN_STATE_ALTCTL]] == 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_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;
+		return;
+
+	} else if (_buttons_state[MAIN_STATE_POSCTL] != msg->buttons[_param_buttons_map[MAIN_STATE_POSCTL]] == 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;
+		return;
+	}
+
+}
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "manual_input");
+	ManualInput m;
+	ros::spin();
+	return 0;
+}
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/platforms/ros/nodes/manual_input/manual_input.h
index 2fde5d424..bf704f675 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/platforms/ros/nodes/manual_input/manual_input.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -32,68 +32,62 @@
  ****************************************************************************/
 
 /**
- * @file rc_channels.h
- * Definition of the rc_channels uORB topic.
+ * @file manual_input.h
+ * Reads from the ros joystick topic and publishes to the px4 manual control setpoint topic.
  *
- * @deprecated DO NOT USE FOR NEW CODE
- */
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
 
-#ifndef RC_CHANNELS_H_
-#define RC_CHANNELS_H_
+#include "ros/ros.h"
+#include <px4/manual_control_setpoint.h>
+#include <sensor_msgs/Joy.h>
 
-#include <stdint.h>
-#include "../uORB.h"
+class ManualInput
+{
+public:
+	ManualInput();
 
-/**
- * This defines the mapping of the RC functions.
- * The value assigned to the specific function corresponds to the entry of
- * the channel array channels[].
- */
-enum RC_CHANNELS_FUNCTION {
-	THROTTLE = 0,
-	ROLL,
-	PITCH,
-	YAW,
-	MODE,
-	RETURN,
-	POSCTL,
-	LOITER,
-	OFFBOARD,
-	ACRO,
-	FLAPS,
-	AUX_1,
-	AUX_2,
-	AUX_3,
-	AUX_4,
-	AUX_5,
-	PARAM_1,
-	PARAM_2,
-	PARAM_3
-};
+	~ManualInput() {}
 
-// MAXIMUM FUNCTIONS IS != MAXIMUM RC INPUT CHANNELS
+protected:
+	/**
+	 * Takes ROS joystick message and converts/publishes to px4 manual control setpoint topic
+	 */
+	void JoyCallback(const sensor_msgs::JoyConstPtr &msg);
 
-#define RC_CHANNELS_FUNCTION_MAX 19
+	/**
+	 * Helper function to map and scale joystick axis
+	 */
+	void MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, double scale, double offset, double deadzone,
+		     float &out);
+	/**
+	 * Helper function to map joystick buttons
+	 */
+	void MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_control_setpoint &msg_mc_sp);
 
-/**
- * @addtogroup topics
- * @{
- */
-struct rc_channels_s {
-	uint64_t timestamp;									/**< Timestamp in microseconds since boot time */
-	uint64_t timestamp_last_valid;						/**< Timestamp of last valid RC signal */
-	float channels[RC_CHANNELS_FUNCTION_MAX];			/**< Scaled to -1..1 (throttle: 0..1) */
-	uint8_t channel_count;								/**< Number of valid channels */
-	int8_t function[RC_CHANNELS_FUNCTION_MAX];			/**< Functions mapping */
-	uint8_t rssi;										/**< Receive signal strength index */
-	bool signal_lost;									/**< Control signal lost, should be checked together with topic timeout */
-}; /**< radio control channels. */
+	ros::NodeHandle _n;
+	ros::Subscriber _joy_sub;
+	ros::Publisher _man_ctrl_sp_pub;
 
-/**
- * @}
- */
+	/* Parameters */
+	enum MAIN_STATE {
+		MAIN_STATE_MANUAL = 0,
+		MAIN_STATE_ALTCTL,
+		MAIN_STATE_POSCTL,
+		MAIN_STATE_AUTO_MISSION,
+		MAIN_STATE_AUTO_LOITER,
+		MAIN_STATE_AUTO_RTL,
+		MAIN_STATE_MAX
+	};
+
+	int _param_buttons_map[MAIN_STATE_MAX];	    /**< joystick button mapping, order according to MAIN_STATE */
+	bool _buttons_state[MAIN_STATE_MAX];	    /**< joystick button state of last iteration,
+						      order according to MAIN_STATE */
 
-/* register this as object request broker structure */
-ORB_DECLARE(rc_channels);
+	int _param_axes_map[4];
+	double _param_axes_scale[4];
+	double _param_axes_offset[4];
+	double _param_axes_dz[4];
 
-#endif
+	px4::manual_control_setpoint _msg_mc_sp;
+};
diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
new file mode 100644
index 000000000..0749c8e92
--- /dev/null
+++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
@@ -0,0 +1,272 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_mixer.cpp
+ * Dummy multicopter mixer for euroc simulator (gazebo)
+ *
+ * @author Roman Bapst <romanbapst@yahoo.de>
+*/
+#include <ros/ros.h>
+#include <px4.h>
+#include <lib/mathlib/mathlib.h>
+#include <mav_msgs/MotorSpeed.h>
+#include <string>
+
+class MultirotorMixer
+{
+public:
+
+	MultirotorMixer();
+
+	struct Rotor {
+		float roll_scale;
+		float pitch_scale;
+		float yaw_scale;
+	};
+
+	void actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg);
+	void actuatorArmedCallback(const PX4_TOPIC_T(actuator_armed) &msg);
+
+private:
+
+	ros::NodeHandle _n;
+	ros::Subscriber _sub;
+	ros::Publisher _pub;
+
+	const Rotor *_rotors;
+
+	unsigned _rotor_count;
+
+	struct {
+		float control[8];
+	} inputs;
+
+	struct  {
+		float control[8];
+	} outputs;
+
+	bool _armed;
+	ros::Subscriber _sub_actuator_armed;
+
+	void mix();
+};
+
+const MultirotorMixer::Rotor _config_x[] = {
+	{ -0.707107,  0.707107,  1.00 },
+	{  0.707107, -0.707107,  1.00 },
+	{  0.707107,  0.707107, -1.00 },
+	{ -0.707107, -0.707107, -1.00 },
+};
+
+const MultirotorMixer::Rotor _config_quad_plus[] = {
+	{ -1.000000,  0.000000,  1.00 },
+	{  1.000000,  0.000000,  1.00 },
+	{  0.000000,  1.000000, -1.00 },
+	{ -0.000000, -1.000000, -1.00 },
+};
+
+const MultirotorMixer::Rotor _config_quad_plus_euroc[] = {
+	{  0.000000,  1.000000,  1.00 },
+	{ -0.000000, -1.000000,  1.00 },
+	{  1.000000,  0.000000, -1.00 },
+	{ -1.000000,  0.000000, -1.00 },
+};
+const MultirotorMixer::Rotor _config_quad_wide[] = {
+	{ -0.927184,  0.374607,  1.000000 },
+	{  0.777146, -0.629320,  1.000000 },
+	{  0.927184,  0.374607, -1.000000 },
+	{ -0.777146, -0.629320, -1.000000 },
+};
+const MultirotorMixer::Rotor _config_quad_iris[] = {
+	{ -0.876559,  0.481295,  1.000000 },
+	{  0.826590, -0.562805,  1.000000 },
+	{  0.876559,  0.481295, -1.000000 },
+	{ -0.826590, -0.562805, -1.000000 },
+};
+
+const MultirotorMixer::Rotor *_config_index[5] = {
+	&_config_x[0],
+	&_config_quad_plus[0],
+	&_config_quad_plus_euroc[0],
+	&_config_quad_wide[0],
+	&_config_quad_iris[0]
+};
+
+MultirotorMixer::MultirotorMixer():
+	_n(),
+	_rotor_count(4),
+	_rotors(_config_index[0])
+{
+	_sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback, this);
+	_pub = _n.advertise<mav_msgs::MotorSpeed>("/mixed_motor_commands", 10);
+
+	if (!_n.hasParam("motor_scaling_radps")) {
+		_n.setParam("motor_scaling_radps", 150.0);
+	}
+
+	if (!_n.hasParam("motor_offset_radps")) {
+		_n.setParam("motor_offset_radps", 600.0);
+	}
+
+	if (!_n.hasParam("mixer")) {
+		_n.setParam("mixer", "x");
+	}
+
+	_sub_actuator_armed = _n.subscribe("actuator_armed", 1, &MultirotorMixer::actuatorArmedCallback, this);
+}
+
+void MultirotorMixer::mix()
+{
+	float roll = math::constrain(inputs.control[0], -1.0f, 1.0f);
+	float pitch = math::constrain(inputs.control[1], -1.0f, 1.0f);
+	float yaw = math::constrain(inputs.control[2], -1.0f, 1.0f);
+	float thrust = math::constrain(inputs.control[3], 0.0f, 1.0f);
+	float min_out = 0.0f;
+	float max_out = 0.0f;
+
+	/* perform initial mix pass yielding unbounded outputs, ignore yaw */
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		float out = roll * _rotors[i].roll_scale
+			    + pitch * _rotors[i].pitch_scale + thrust;
+
+		/* limit yaw if it causes outputs clipping */
+		if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) {
+			yaw = -out / _rotors[i].yaw_scale;
+		}
+
+		/* calculate min and max output values */
+		if (out < min_out) {
+			min_out = out;
+		}
+
+		if (out > max_out) {
+			max_out = out;
+		}
+
+		outputs.control[i] = out;
+	}
+
+	/* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */
+	if (min_out < 0.0f) {
+		float scale_in = thrust / (thrust - min_out);
+
+		/* mix again with adjusted controls */
+		for (unsigned i = 0; i < _rotor_count; i++) {
+			outputs.control[i] = scale_in
+					     * (roll * _rotors[i].roll_scale
+						+ pitch * _rotors[i].pitch_scale) + thrust;
+		}
+
+	} else {
+		/* roll/pitch mixed without limiting, add yaw control */
+		for (unsigned i = 0; i < _rotor_count; i++) {
+			outputs.control[i] += yaw * _rotors[i].yaw_scale;
+		}
+	}
+
+	/* scale down all outputs if some outputs are too large, reduce total thrust */
+	float scale_out;
+
+	if (max_out > 1.0f) {
+		scale_out = 1.0f / max_out;
+
+	} else {
+		scale_out = 1.0f;
+	}
+
+	/* scale outputs to range _idle_speed..1, and do final limiting */
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		outputs.control[i] = math::constrain(outputs.control[i], 0.0f, 1.0f);
+	}
+}
+
+void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg)
+{
+	// read message
+	for (int i = 0; i < msg.NUM_ACTUATOR_CONTROLS; i++) {
+		inputs.control[i] = msg.control[i];
+	}
+
+	// switch mixer if necessary
+	std::string mixer_name;
+	_n.getParamCached("mixer", mixer_name);
+	if (mixer_name == "x") {
+		_rotors = _config_index[0];
+	} else if (mixer_name == "+") {
+		_rotors = _config_index[1];
+	} else if (mixer_name == "e") {
+		_rotors = _config_index[2];
+	} else if (mixer_name == "w") {
+		_rotors = _config_index[3];
+	} else if (mixer_name == "i") {
+		_rotors = _config_index[4];
+	}
+
+	// mix
+	mix();
+
+	// publish message
+	mav_msgs::MotorSpeed rotor_vel_msg;
+	double scaling;
+	double offset;
+	_n.getParamCached("motor_scaling_radps", scaling);
+	_n.getParamCached("motor_offset_radps", offset);
+
+	if (_armed) {
+		for (int i = 0; i < _rotor_count; i++) {
+			rotor_vel_msg.motor_speed.push_back(outputs.control[i] * scaling + offset);
+		}
+
+	} else {
+		for (int i = 0; i < _rotor_count; i++) {
+			rotor_vel_msg.motor_speed.push_back(0.0);
+		}
+	}
+
+	_pub.publish(rotor_vel_msg);
+}
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "mc_mixer");
+	MultirotorMixer mixer;
+	ros::spin();
+
+	return 0;
+}
+
+void MultirotorMixer::actuatorArmedCallback(const PX4_TOPIC_T(actuator_armed) &msg)
+{
+	_armed = msg.armed;
+}
diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.cpp b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp
new file mode 100644
index 000000000..ed3a4efa5
--- /dev/null
+++ b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp
@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file position_estimator.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Roman Bapst <romanbapst@yahoo.de>
+*/
+
+#include "position_estimator.h"
+
+#include <px4/vehicle_local_position.h>
+#include <mathlib/mathlib.h>
+#include <platforms/px4_defines.h>
+#include <platforms/px4_middleware.h>
+#include <vector>
+#include <string>
+#include <gazebo_msgs/ModelStates.h>
+
+PositionEstimator::PositionEstimator() :
+	_n(),
+	_sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &PositionEstimator::ModelStatesCallback, this)),
+	_vehicle_position_pub(_n.advertise<px4::vehicle_local_position>("vehicle_local_position", 1)),
+	_startup_time(1)
+{
+}
+
+void PositionEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg)
+{
+	//XXX: use a proper sensor topic
+
+	px4::vehicle_local_position msg_v_l_pos;
+
+	/* Fill px4 position topic with contents from modelstates topic */
+	int index = 0;
+	//XXX: maybe a more clever approach would be to do this not on every loop, need to check if and when
+	//gazebo rearranges indexes.
+	for(std::vector<std::string>::const_iterator it = msg->name.begin(); it != msg->name.end(); ++it) {
+		if (*it == "iris" || *it == "ardrone") {
+			index = it -  msg->name.begin();
+			break;
+		}
+	}
+	msg_v_l_pos.xy_valid = true;
+	msg_v_l_pos.z_valid = true;
+	msg_v_l_pos.v_xy_valid = true;
+	msg_v_l_pos.v_z_valid = true;
+
+	msg_v_l_pos.x = msg->pose[index].position.x;
+	msg_v_l_pos.y = -msg->pose[index].position.y;
+	msg_v_l_pos.z = -msg->pose[index].position.z;
+	msg_v_l_pos.vx = msg->twist[index].linear.x;
+	msg_v_l_pos.vy = -msg->twist[index].linear.y;
+	msg_v_l_pos.vz = -msg->twist[index].linear.z;
+
+	msg_v_l_pos.xy_global = true;
+	msg_v_l_pos.z_global = true;
+	msg_v_l_pos.ref_timestamp = _startup_time;
+	msg_v_l_pos.ref_lat = 47.378301;
+	msg_v_l_pos.ref_lon = 8.538777;
+	msg_v_l_pos.ref_alt = 1200.0f;
+
+
+	msg_v_l_pos.timestamp = px4::get_time_micros();
+	_vehicle_position_pub.publish(msg_v_l_pos);
+}
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "position_estimator");
+	PositionEstimator m;
+
+	ros::spin();
+
+	return 0;
+}
diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.h b/src/platforms/ros/nodes/position_estimator/position_estimator.h
new file mode 100644
index 000000000..da1fc2c5a
--- /dev/null
+++ b/src/platforms/ros/nodes/position_estimator/position_estimator.h
@@ -0,0 +1,62 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file position_estimator.h
+ * Dummy position estimator that forwards position from gazebo to px4 topic
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "ros/ros.h"
+#include <gazebo_msgs/ModelStates.h>
+#include <sensor_msgs/Imu.h>
+
+class PositionEstimator
+{
+public:
+	PositionEstimator();
+
+	~PositionEstimator() {}
+
+protected:
+	void ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg);
+
+	ros::NodeHandle _n;
+	ros::Subscriber _sub_modelstates;
+	ros::Publisher _vehicle_position_pub;
+
+	uint64_t _startup_time;
+
+
+};
diff --git a/src/platforms/ros/perf_counter.cpp b/src/platforms/ros/perf_counter.cpp
new file mode 100755
index 000000000..a71801397
--- /dev/null
+++ b/src/platforms/ros/perf_counter.cpp
@@ -0,0 +1,176 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 - 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file perf_counter.cpp
+ *
+ * Empty function calls for ros compatibility
+ *
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ */
+#include <stdlib.h>
+#include <stdio.h>
+#include <systemlib/perf_counter.h>
+
+
+
+perf_counter_t	perf_alloc(enum perf_counter_type type, const char *name)
+{
+	return NULL;
+}
+
+/**
+ * Free a counter.
+ *
+ * @param handle		The performance counter's handle.
+ */
+void perf_free(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Count a performance event.
+ *
+ * This call only affects counters that take single events; PC_COUNT, PC_INTERVAL etc.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+void		perf_count(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Begin a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+void		perf_begin(perf_counter_t handle)
+{
+
+}
+
+/**
+ * End a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * If a call is made without a corresopnding perf_begin call, or if perf_cancel
+ * has been called subsequently, no change is made to the counter.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+void		perf_end(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Cancel a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * It reverts the effect of a previous perf_begin.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+void		perf_cancel(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Reset a performance counter.
+ *
+ * This call resets performance counter to initial state
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+void		perf_reset(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Print one performance counter to stdout
+ *
+ * @param handle		The counter to print.
+ */
+void		perf_print_counter(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Print one performance counter to a fd.
+ *
+ * @param fd			File descriptor to print to - e.g. 0 for stdout
+ * @param handle		The counter to print.
+ */
+void		perf_print_counter_fd(int fd, perf_counter_t handle)
+{
+
+}
+
+/**
+ * Print all of the performance counters.
+ *
+ * @param fd			File descriptor to print to - e.g. 0 for stdout
+ */
+void		perf_print_all(int fd)
+{
+
+}
+
+/**
+ * Reset all of the performance counters.
+ */
+void		perf_reset_all(void)
+{
+
+}
+
+/**
+ * Return current event_count
+ *
+ * @param handle		The counter returned from perf_alloc.
+ * @return			event_count
+ */
+uint64_t	perf_event_count(perf_counter_t handle)
+{
+
+}
+
+
diff --git a/src/platforms/ros/px4_nodehandle.cpp b/src/platforms/ros/px4_nodehandle.cpp
new file mode 100644
index 000000000..6ac3c76d3
--- /dev/null
+++ b/src/platforms/ros/px4_nodehandle.cpp
@@ -0,0 +1,44 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_nodehandle.cpp
+ *
+ * PX4 Middleware Wrapper Nodehandle
+ */
+#include <px4_nodehandle.h>
+
+namespace px4
+{
+}
+
diff --git a/src/platforms/ros/px4_publisher.cpp b/src/platforms/ros/px4_publisher.cpp
new file mode 100644
index 000000000..f02dbe4c9
--- /dev/null
+++ b/src/platforms/ros/px4_publisher.cpp
@@ -0,0 +1,41 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_publisher.cpp
+ *
+ * PX4 Middleware Wrapper for Publisher
+ */
+#include <px4_publisher.h>
+
+
diff --git a/src/platforms/ros/px4_ros_impl.cpp b/src/platforms/ros/px4_ros_impl.cpp
new file mode 100644
index 000000000..854986a7f
--- /dev/null
+++ b/src/platforms/ros/px4_ros_impl.cpp
@@ -0,0 +1,56 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_ros_impl.cpp
+ *
+ * PX4 Middleware Wrapper ROS Implementation
+ */
+
+#include <px4.h>
+
+namespace px4
+{
+
+void init(int argc, char *argv[], const char *process_name)
+{
+	ros::init(argc, argv, process_name);
+}
+
+uint64_t get_time_micros()
+{
+	ros::Time time = ros::Time::now();
+	return time.sec * 1e6 + time.nsec / 1000;
+}
+
+}
diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c
index 7d80af307..32682f890 100644
--- a/src/systemcmds/esc_calib/esc_calib.c
+++ b/src/systemcmds/esc_calib/esc_calib.c
@@ -39,6 +39,7 @@
  */
 
 #include <nuttx/config.h>
+#include <platforms/px4_defines.h>
 
 #include <stdio.h>
 #include <stdlib.h>
@@ -63,6 +64,10 @@
 #include "drivers/drv_pwm_output.h"
 
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 
 static void	usage(const char *reason);
 __EXPORT int	esc_calib_main(int argc, char *argv[]);
diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt
index 3e44f822d..a87c23be2 100644
--- a/unittests/CMakeLists.txt
+++ b/unittests/CMakeLists.txt
@@ -26,6 +26,9 @@ include_directories(${PX_SRC}/lib)
 include_directories(${PX_SRC}/drivers)
 
 add_definitions(-D__EXPORT=)
+add_definitions(-D__PX4_TESTS)
+add_definitions(-Dnoreturn_function=)
+add_definitions(-Dmain_t=int)
 add_definitions(-DERROR=-1)
 add_definitions(-DOK=0)