Merge branch 'master' of github.com:PX4/Firmware into beta

.gitignore

@@ -46,3 +46,5 @@ Firmware.zip
 unittests/build
 *.generated.h
 .vagrant
+*.pretty
+

.travis.yml

@@ -37,6 +37,7 @@ env:
 # Email address: $PX4_EMAIL
     - secure: "ei3hKAw6Pk+vEkQBI5Y2Ak74BRAaXcK2UHVnVadviBHI4EVPwn1YGP6A4Y0wnLe4U7ETTl0UiijRoVxyDW0Mq896Pv0siw02amNpjSZZYu+RfN1+//MChB48OxsLDirUdHVrULhl/bOARM02h2Bg28jDE2g7IqmJwg3em3oMbjU="
     - PX4_REPORT=report.txt
+    - BUILD_URI=https://px4-travis.s3.amazonaws.com/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/Firmware.zip
 
 script:
   - arm-none-eabi-gcc --version
@@ -58,11 +59,14 @@ after_script:
 # upload newest build for this branch with s3 index
   - ./CI-Tools/s3cmd-put Images/px4*.px4 CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/
 # archive newest build by date with s3 index
-  - ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/`date "+%Y-%m-%d"`-$TRAVIS_BUILD_ID/
+  - ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/
   - ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/
 # upload top level index.html and timestamp.html
   - ./CI-Tools/s3cmd-put CI-Tools/index.html index.html
   - ./CI-Tools/s3cmd-put CI-Tools/timestamp.html timestamp.html
+  - echo ""
+  - echo "Binaries have been posted to:"
+  - echo https://px4-travis.s3.amazonaws.com/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/Firmware.zip
 
 deploy:
   provider: releases

CMakeLists.txt

@@ -3,6 +3,7 @@ project(px4)
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
 add_definitions(-D__PX4_ROS)
 add_definitions(-D__EXPORT=)
+add_definitions(-DMAVLINK_DIALECT=common)
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
@@ -11,11 +12,14 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
   std_msgs
+  geometry_msgs
   message_generation
   cmake_modules
   gazebo_msgs
   sensor_msgs
   mav_msgs
+  libmavconn
+  tf
 )
 find_package(Eigen REQUIRED)
 
@@ -74,6 +78,8 @@ add_message_files(
   position_setpoint_triplet.msg
   vehicle_local_position_setpoint.msg
   vehicle_global_velocity_setpoint.msg
+  offboard_control_mode.msg
+  vehicle_force_setpoint.msg
 )
 
 ## Generate services in the 'srv' folder
@@ -109,7 +115,7 @@ generate_messages(
 catkin_package(
   INCLUDE_DIRS src/include
   LIBRARIES px4
-  CATKIN_DEPENDS message_runtime roscpp rospy std_msgs
+  CATKIN_DEPENDS message_runtime roscpp rospy std_msgs libmavconn
   DEPENDS system_lib
 )
 
@@ -128,6 +134,7 @@ include_directories(
   src/
   src/lib
   ${EIGEN_INCLUDE_DIRS}
+  integrationtests
 )
 
 ## generate multiplatform wrapper headers
@@ -231,15 +238,42 @@ target_link_libraries(mc_mixer
   px4
 )
 
-## Commander
+## Commander dummy
 add_executable(commander
 	src/platforms/ros/nodes/commander/commander.cpp)
-add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp)
+add_dependencies(commander ${PROJECT_NAME}_generate_messages_cpp_cpp)
 target_link_libraries(commander
   ${catkin_LIBRARIES}
   px4
 )
 
+## Mavlink dummy
+add_executable(mavlink
+	src/platforms/ros/nodes/mavlink/mavlink.cpp)
+add_dependencies(mavlink ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(mavlink
+  ${catkin_LIBRARIES}
+  px4
+)
+
+## Offboard Position Setpoint Demo
+add_executable(demo_offboard_position_setpoints
+	src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp)
+add_dependencies(demo_offboard_position_setpoints ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(demo_offboard_position_setpoints
+  ${catkin_LIBRARIES}
+  px4
+)
+
+## Offboard Attitude Setpoint Demo
+add_executable(demo_offboard_attitude_setpoints
+	src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp)
+add_dependencies(demo_offboard_attitude_setpoints ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(demo_offboard_attitude_setpoints
+  ${catkin_LIBRARIES}
+  px4
+)
+
 #############
 ## Install ##
 #############
@@ -287,3 +321,11 @@ install(TARGETS ${PROJECT_NAME}
 
 ## Add folders to be run by python nosetests
 # catkin_add_nosetests(test)
+
+if(CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+  add_rostest(integrationtests/demo_tests/direct_tests.launch)
+  add_rostest(integrationtests/demo_tests/mavros_tests.launch)
+endif()
+
+

Firmware.sublime-project

@@ -38,10 +38,18 @@
 	"build_systems":
 	[
 		{
-			"name": "PX4",
+			"name": "PX4: make all",
 			"working_dir": "${project_path}",
 			"file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$",
-			"cmd": ["make"]
+			"cmd": ["make"],
+			"shell": true
+		},
+		{
+			"name": "PX4: make and upload",
+			"working_dir": "${project_path}",
+			"file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$",
+			"cmd": ["make upload px4fmu-v2_default -j8"],
+			"shell": true
 		}
 	]
 }

Makefile

@@ -124,7 +124,7 @@ $(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4
 .PHONY: $(FIRMWARES)
 $(BUILD_DIR)%.build/firmware.px4: config   = $(patsubst $(BUILD_DIR)%.build/firmware.px4,%,$@)
 $(BUILD_DIR)%.build/firmware.px4: work_dir = $(BUILD_DIR)$(config).build/
-$(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4:	checksubmodules generateuorbtopicheaders
+$(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4:	generateuorbtopicheaders checksubmodules
 	@$(ECHO) %%%%
 	@$(ECHO) %%%% Building $(config) in $(work_dir)
 	@$(ECHO) %%%%
@@ -236,7 +236,7 @@ GENMSG_PYTHONPATH = $(PX4_BASE)Tools/genmsg/src
 GENCPP_PYTHONPATH = $(PX4_BASE)Tools/gencpp/src
 
 .PHONY: generateuorbtopicheaders
-generateuorbtopicheaders:
+generateuorbtopicheaders: checksubmodules
 	@$(ECHO) "Generating uORB topic headers"
 	$(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH):$(PYTHONPATH) $(PYTHON) \
 		$(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \
@@ -262,6 +262,10 @@ testbuild:
 tests:	generateuorbtopicheaders
 	$(Q) (mkdir -p $(PX4_BASE)/unittests/build && cd $(PX4_BASE)/unittests/build && cmake .. && $(MAKE) unittests)
 
+.PHONY: format check_format
+check_format:
+	$(Q) (./Tools/check_code_style.sh | sort -n)
+
 #
 # Cleanup targets.  'clean' should remove all built products and force
 # a complete re-compilation, 'distclean' should remove everything

ROMFS/px4fmu_common/init.d/10019_sk450_deadcat

@@ -29,3 +29,10 @@ set MIXER sk450_deadcat
 
 set PWM_OUT 1234
 set PWM_MIN 1050
+
+set PWM_AUX_OUT 1234
+# set PWM_AUX_MIN 900
+# set PWM_AUX_MAX 2100
+set PWM_AUX_RATE 100
+
+gimbal start

ROMFS/px4fmu_common/init.d/50001_rover

@@ -3,8 +3,8 @@
 # Generic rover
 #
 
+#load some defaults e.g. PWM values
 sh /etc/init.d/rc.rover_defaults
 
+#choose a mixer, for rover control we need a plain passthrough to the servos
 set MIXER IO_pass
-
-set PWM_OUT 1234

ROMFS/px4fmu_common/init.d/rc.interface

@@ -11,7 +11,7 @@ then
 	# Load main mixer
 	#
 
-	if [ $MIXER_AUX == none  -a $USE_IO == yes ]
+	if [ $MIXER_AUX == none -a $USE_IO == yes ]
 	then
 		set MIXER_AUX $MIXER.aux
 	fi
@@ -103,6 +103,7 @@ then
 	#
 
 	set MIXER_AUX_FILE none
+	set OUTPUT_AUX_DEV /dev/pwm_output1
 
 	if [ -f $SDCARD_MIXERS_PATH/$MIXER_AUX.mix ]
 	then
@@ -119,10 +120,43 @@ then
 	then
 		if fmu mode_pwm
 		then
-			mixer load /dev/pwm_output1 $MIXER_AUX_FILE
+			mixer load $OUTPUT_AUX_DEV $MIXER_AUX_FILE
 		else
 			tone_alarm $TUNE_ERR
 		fi
+
+		if [ $PWM_AUX_OUT != none ]
+		then
+			#
+			# Set PWM_AUX output frequency
+			#
+			if [ $PWM_AUX_RATE != none ]
+			then
+				pwm rate -c $PWM_AUX_OUT -r $PWM_AUX_RATE -d $OUTPUT_AUX_DEV
+			fi
+
+			#
+			# Set disarmed, min and max PWM_AUX values
+			#
+			if [ $PWM_AUX_DISARMED != none ]
+			then
+				pwm disarmed -c $PWM_AUX_OUT -p $PWM_AUX_DISARMED -d $OUTPUT_AUX_DEV
+			fi
+			if [ $PWM_AUX_MIN != none ]
+			then
+				pwm min -c $PWM_AUX_OUT -p $PWM_AUX_MIN -d $OUTPUT_AUX_DEV
+			fi
+			if [ $PWM_AUX_MAX != none ]
+			then
+				pwm max -c $PWM_AUX_OUT -p $PWM_AUX_MAX -d $OUTPUT_AUX_DEV
+			fi
+		fi
+
+		if [ $FAILSAFE_AUX != none ]
+		then
+			pwm failsafe -d $OUTPUT_AUX_DEV $FAILSAFE
+		fi
+
 	fi
 fi
 unset OUTPUT_DEV

ROMFS/px4fmu_common/init.d/rc.mc_apps

@@ -4,9 +4,15 @@
 # att & pos estimator, att & pos control.
 #
 
-attitude_estimator_ekf start
-#ekf_att_pos_estimator start
-position_estimator_inav start
+# previously (2014) the system was relying on
+# INAV, which defaults to 0 now. 
+if param compare INAV_ENABLED 1
+then
+	attitude_estimator_ekf start
+	position_estimator_inav start
+else
+	ekf_att_pos_estimator start
+fi
 
 if mc_att_control start
 then

ROMFS/px4fmu_common/init.d/rc.rover_defaults

@@ -2,12 +2,27 @@
 
 set VEHICLE_TYPE rover
 
-if [ $AUTOCNF == yes ]
-then
-	# param set MC_ROLL_P 7.0
-fi
+# This section can be enabled once tuning parameters for this particular
+# rover model are known. It allows to configure default gains via the GUI
+#if [ $AUTOCNF == yes ]
+#then
+#	# param set MC_ROLL_P 7.0
+#fi
 
+# PWM Hz - 50 Hz is the normal rate in RC cars, higher rates
+# may damage analog servos.
 set PWM_RATE 50
-set PWM_DISARMED 1100
-set PWM_MIN 1100
-set PWM_MAX 1900
+
+# PWM default value for "disarmed" mode
+# this centers the steering and throttle, which means no motion
+# for a rover
+set PWM_DISARMED 1500
+
+# PWM range
+set PWM_MIN 1200
+set PWM_MAX 1800
+
+# Enable servo output on pins 3 and 4 (steering and thrust)
+# but also include 1+2 as they form together one output group
+# and need to be set together.
+set PWM_OUT 1234

ROMFS/px4fmu_common/init.d/rcS

@@ -109,6 +109,11 @@ then
 	set PWM_DISARMED none
 	set PWM_MIN none
 	set PWM_MAX none
+	set PWM_AUX_OUT none
+	set PWM_AUX_RATE none
+	set PWM_AUX_DISARMED none
+	set PWM_AUX_MIN none
+	set PWM_AUX_MAX none
 	set MK_MODE none
 	set FMU_MODE pwm
 	set MAVLINK_F default
@@ -441,7 +446,7 @@ then
 	then
 		if param compare SYS_COMPANION 921600
 		then
-			mavlink start -d /dev/ttyS2 -b 921600 -m onboard
+			mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 20000
 		fi
 	fi
 

ROMFS/px4fmu_common/mixers/sk450_deadcat.aux.mix

@@ -0,0 +1,38 @@
+Gimbal / payload mixer for PX4FMU
+===========================
+
+Configuration with 2 gimbals:
+ - 2 axes inline GoPro gimbal (pitch, roll)
+ - 2 axes FPV gimbal (pitch, yaw), physically attached GoPro gimbal's roll stabilization
+-----------------------------------------------------
+
+# gimbal roll
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 2 0  -11500  -10000  900 -10000  10000
+
+# gimbal pitch
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 2 1  12000  12000   2000 -10000  10000
+
+# FPV gimbal yaw (not implemented, yet)
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 2 2  10000  10000      0 -10000  10000
+
+# FPV gimbal pitch
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 2 1  -12000  -12000 -3000 -10000  10000
+
+# reserved, not used
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 2 4  10000  10000      0 -10000  10000
+
+# parachute
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 2 7  10000  10000      0 -10000  10000
+

ROMFS/px4fmu_common/mixers/sk450_deadcat.main.mix

@@ -1,4 +1,4 @@
-Multirotor mixer for PX4FMU
+Multirotor mixer for PX4IO
 ===========================
 
 This file defines a single mixer for a quadrotor in SK450 DeadCat configuration.  All controls are mixed 100%.

ROMFS/px4fmu_test/init.d/rcS

@@ -230,3 +230,5 @@ else
 fi
 
 ver all
+
+free

Tools/check_code_style.sh

@@ -0,0 +1,31 @@
+#!/usr/bin/env bash
+set -eu
+failed=0
+for fn in $(find . -path './src/lib/uavcan' -prune -o \
+                   -path './src/lib/mathlib/CMSIS' -prune -o \
+                   -path './src/modules/attitude_estimator_ekf/codegen/' -prune -o \
+                   -path './NuttX' -prune -o \
+                   -path './Build' -prune -o \
+                   -path './mavlink' -prune -o \
+                   -path './unittests/gtest' -prune -o \
+                   -path './unittests/build' -prune -o \
+                   -name '*.c' -o -name '*.cpp' -o -name '*.hpp' -o -name '*.h' -type f); do
+  if [ -f "$fn" ];
+  then
+    ./Tools/fix_code_style.sh --quiet < $fn > $fn.pretty
+    diffsize=$(diff -y --suppress-common-lines $fn $fn.pretty | wc -l)
+    rm -f $fn.pretty
+    if [ $diffsize -ne 0 ]; then
+      failed=1
+      echo $diffsize $fn
+    fi
+  fi
+done
+
+if [ $failed -eq 0 ]; then
+    echo "Format checks passed"
+    exit 0
+else
+    echo "Format checks failed; please run ./Tools/fix_code_style.sh on each file"
+    exit 1
+fi

Tools/fix_code_style.sh

@@ -18,4 +18,5 @@ astyle \
     --exclude=EASTL		\
     --add-brackets		\
     --max-code-length=120	\
+    --preserve-date             \
     $*

Tools/pre-commit

@@ -0,0 +1,26 @@
+#!/bin/sh
+if git rev-parse --verify HEAD >/dev/null 2>&1
+then
+  against=HEAD
+else
+  # Initial commit: diff against an empty tree object
+  against=4b825dc642cb6eb9a060e54bf8d69288fbee4904
+fi
+
+# Redirect output to stderr.
+exec 1>&2
+
+CHANGED_FILES=`git diff --cached --name-only --diff-filter=ACM $against | grep '\.c\|\.cpp\|\.h\|\.hpp'`
+FAILED=0
+if [ ! -z "$CHANGED_FILES" -a "$CHANGED_FILES" != " " ]; then
+  for FILE in $CHANGED_FILES; do
+    ./Tools/fix_code_style.sh --quiet < $FILE > $FILE.pretty
+    diff -u $FILE $FILE.pretty || FAILED=1
+    rm -f $FILE.pretty
+    if [ $FAILED -ne 0 ]; then
+      echo "There are code formatting errors. Please fix them by running ./Tools/fix_code_style.sh $FILE"
+      exit $FAILED
+    fi
+  done
+fi
+exit 0

Tools/px_uploader.py

@@ -160,6 +160,7 @@ class uploader(object):
         GET_CRC         = b'\x29'     # rev3+
         GET_OTP         = b'\x2a'     # rev4+  , get a word from OTP area
         GET_SN          = b'\x2b'     # rev4+  , get a word from SN area
+        GET_CHIP        = b'\x2c'     # rev5+  , get chip version
         REBOOT          = b'\x30'
         
         INFO_BL_REV     = b'\x01'        # bootloader protocol revision
@@ -258,7 +259,7 @@ class uploader(object):
                 self.__getSync()
                 return value
 
-        # send the GET_OTP command and wait for an info parameter
+        # send the GET_SN command and wait for an info parameter
         def __getSN(self, param):
                 t = struct.pack("I", param) # int param as 32bit ( 4 byte ) char array.
                 self.__send(uploader.GET_SN + t + uploader.EOC)
@@ -266,6 +267,13 @@ class uploader(object):
                 self.__getSync()
                 return value
 
+        # send the GET_CHIP command
+        def __getCHIP(self):
+                self.__send(uploader.GET_CHIP + uploader.EOC)
+                value = self.__recv_int()
+                self.__getSync()
+                return value
+
         def __drawProgressBar(self, label, progress, maxVal):
                 if maxVal < progress:
                     progress = maxVal
@@ -416,7 +424,12 @@ class uploader(object):
         def upload(self, fw):
                 # Make sure we are doing the right thing
                 if self.board_type != fw.property('board_id'):
-                        raise RuntimeError("Firmware not suitable for this board")
+                        msg = "Firmware not suitable for this board (board_type=%u board_id=%u)" % (
+                                self.board_type, fw.property('board_id'))
+                        if args.force:
+                                print("WARNING: %s" % msg)
+                        else:
+                                raise IOError(msg)
                 if self.fw_maxsize < fw.property('image_size'):
                         raise RuntimeError("Firmware image is too large for this board")
 
@@ -446,6 +459,7 @@ class uploader(object):
                                     self.sn  = self.sn + x
                                     print(binascii.hexlify(x).decode('Latin-1'), end='') # show user
                             print('')
+                            print("chip: %08x" % self.__getCHIP())
                     except Exception:
                             # ignore bad character encodings
                             pass
@@ -486,6 +500,7 @@ else:
 parser = argparse.ArgumentParser(description="Firmware uploader for the PX autopilot system.")
 parser.add_argument('--port', action="store", required=True, help="Serial port(s) to which the FMU may be attached")
 parser.add_argument('--baud', action="store", type=int, default=115200, help="Baud rate of the serial port (default is 115200), only required for true serial ports.")
+parser.add_argument('--force', action='store_true', default=False, help='Override board type check and continue loading')
 parser.add_argument('firmware', action="store", help="Firmware file to be uploaded")
 args = parser.parse_args()
 
@@ -564,10 +579,13 @@ try:
                             up.upload(fw)
 
                     except RuntimeError as ex:
-
                             # print the error
                             print("\nERROR: %s" % ex.args)
 
+                    except IOError as e:
+                            up.close();
+                            continue
+
                     finally:
                             # always close the port
                             up.close()

integrationtests/demo_tests/direct_manual_input_test.py

@@ -0,0 +1,87 @@
+#!/usr/bin/env python
+#***************************************************************************
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+#***************************************************************************/
+
+#
+# @author Andreas Antener <andreas@uaventure.com>
+#
+PKG = 'px4'
+
+import sys
+import unittest
+import rospy
+
+from px4.msg import actuator_armed
+from px4.msg import vehicle_control_mode
+from manual_input import ManualInput
+
+#
+# Tests if commander reacts to manual input and sets control flags accordingly
+#
+class ManualInputTest(unittest.TestCase):
+
+    #
+    # General callback functions used in tests
+    #
+    def actuator_armed_callback(self, data):
+        self.actuatorStatus = data
+
+    def vehicle_control_mode_callback(self, data):
+        self.controlMode = data
+    
+    #
+    # Test arming
+    #
+    def test_manual_input(self):
+        rospy.init_node('test_node', anonymous=True)
+        rospy.Subscriber('px4_multicopter/actuator_armed', actuator_armed, self.actuator_armed_callback)
+        rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback)
+
+        man = ManualInput()
+
+        # Test arming
+        man.arm()
+        self.assertEquals(self.actuatorStatus.armed, True, "did not arm")
+
+        # Test posctl
+        man.posctl()
+        self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set")
+
+        # Test offboard
+        man.offboard()
+        self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set")
+    
+
+if __name__ == '__main__':
+    import rostest
+    rostest.rosrun(PKG, 'direct_manual_input_test', ManualInputTest)

integrationtests/demo_tests/direct_offboard_posctl_test.py

@@ -0,0 +1,167 @@
+#!/usr/bin/env python
+#***************************************************************************
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+#***************************************************************************/
+
+#
+# @author Andreas Antener <andreas@uaventure.com>
+#
+PKG = 'px4'
+
+import sys
+import unittest
+import rospy
+
+from numpy import linalg
+import numpy as np
+
+from px4.msg import vehicle_local_position
+from px4.msg import vehicle_control_mode
+from px4.msg import actuator_armed
+from px4.msg import position_setpoint_triplet
+from px4.msg import position_setpoint
+from sensor_msgs.msg import Joy
+from std_msgs.msg import Header
+
+from manual_input import ManualInput
+from flight_path_assertion import FlightPathAssertion
+
+#
+# Tests flying a path in offboard control by directly sending setpoints
+# to the position controller (position_setpoint_triplet).
+#
+# For the test to be successful it needs to stay on the predefined path
+# and reach all setpoints in a certain time.
+#
+class DirectOffboardPosctlTest(unittest.TestCase):
+
+    def setUp(self):
+        rospy.init_node('test_node', anonymous=True)
+        rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback)
+        rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback)
+        self.pubSpt = rospy.Publisher('px4_multicopter/position_setpoint_triplet', position_setpoint_triplet, queue_size=10)
+        self.rate = rospy.Rate(10) # 10hz
+
+    def tearDown(self):
+        if (self.fpa):
+            self.fpa.stop()
+
+    #
+    # General callback functions used in tests
+    #
+    def position_callback(self, data):
+        self.hasPos = True
+        self.localPosition = data
+
+    def vehicle_control_mode_callback(self, data):
+        self.controlMode = data
+
+
+    #
+    # Helper methods
+    #
+    def is_at_position(self, x, y, z, offset):
+        rospy.logdebug("current position %f, %f, %f" % (self.localPosition.x, self.localPosition.y, self.localPosition.z))
+        desired = np.array((x, y, z))
+        pos = np.array((self.localPosition.x, self.localPosition.y, self.localPosition.z))
+        return linalg.norm(desired - pos) < offset
+
+    def reach_position(self, x, y, z, timeout):
+        # set a position setpoint
+        pos = position_setpoint()
+        pos.valid = True
+        pos.x = x
+        pos.y = y
+        pos.z = z
+        pos.position_valid = True
+        stp = position_setpoint_triplet()
+        stp.current = pos
+        self.pubSpt.publish(stp)
+
+        # does it reach the position in X seconds?
+        count = 0
+        while(count < timeout):
+            if(self.is_at_position(pos.x, pos.y, pos.z, 0.5)):
+                break
+            count = count + 1
+            self.rate.sleep()
+
+        self.assertTrue(count < timeout, "took too long to get to position")
+
+    #
+    # Test offboard position control
+    #
+    def test_posctl(self):
+        manIn = ManualInput()
+
+        # arm and go into offboard
+        manIn.arm()
+        manIn.offboard()
+        self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set")
+        self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set")
+        self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set")
+
+        # prepare flight path
+        positions = (
+            (0,0,0),
+            (2,2,-2),
+            (2,-2,-2),
+            (-2,-2,-2),
+            (2,2,-2))
+
+        # flight path assertion
+        self.fpa = FlightPathAssertion(positions, 1, 0)
+        self.fpa.start()
+
+        for i in range(0, len(positions)):
+            self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120)
+            self.assertFalse(self.fpa.failed, "breached flight path tunnel (%d)" % i)
+        
+        # does it hold the position for Y seconds?
+        positionHeld = True
+        count = 0
+        timeout = 50
+        while(count < timeout):
+            if(not self.is_at_position(2, 2, -2, 0.5)):
+                positionHeld = False
+                break
+            count = count + 1
+            self.rate.sleep()
+
+        self.assertTrue(count == timeout, "position could not be held")
+        self.fpa.stop()
+    
+
+if __name__ == '__main__':
+    import rostest
+    rostest.rosrun(PKG, 'direct_offboard_posctl_test', DirectOffboardPosctlTest)
+    #unittest.main()

integrationtests/demo_tests/direct_tests.launch

@@ -0,0 +1,18 @@
+<launch>
+    <arg name="headless" default="true"/>
+    <arg name="gui" default="false"/>
+    <arg name="enable_logging" default="false"/>
+    <arg name="enable_ground_truth" default="false"/>
+    <arg name="log_file" default="iris"/>
+
+    <include file="$(find px4)/launch/gazebo_iris_empty_world.launch">
+        <arg name="headless" value="$(arg headless)"/>
+        <arg name="gui" value="$(arg gui)"/>
+        <arg name="enable_logging" value="$(arg enable_logging)" />
+        <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
+        <arg name="log_file" value="$(arg log_file)"/>
+    </include>
+
+    <test test-name="direct_manual_input_test" pkg="px4" type="direct_manual_input_test.py" />
+    <test test-name="direct_offboard_posctl_test" pkg="px4" type="direct_offboard_posctl_test.py" />
+</launch>

integrationtests/demo_tests/flight_path_assertion.py

@@ -0,0 +1,171 @@
+#!/usr/bin/env python
+#***************************************************************************
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+#***************************************************************************/
+
+#
+# @author Andreas Antener <andreas@uaventure.com>
+#
+import sys
+import rospy
+import threading
+
+from px4.msg import vehicle_local_position
+from gazebo_msgs.srv import SpawnModel
+from gazebo_msgs.srv import SetModelState
+from gazebo_msgs.srv import DeleteModel
+from geometry_msgs.msg import Pose
+from geometry_msgs.msg import Twist
+
+from numpy import linalg
+import numpy as np
+import math
+
+#
+# Helper to test if vehicle stays on expected flight path.
+#
+class FlightPathAssertion(threading.Thread):
+
+    #
+    # Arguments
+    # - positions: tuple of tuples in the form (x, y, z, heading)
+    #
+    # TODO: yaw validation
+    # TODO: fail main test thread
+    #
+    def __init__(self, positions, tunnelRadius = 1, yawOffset = 0.2):
+        threading.Thread.__init__(self)
+        rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback)
+        self.spawnModel = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel)
+        self.setModelState = rospy.ServiceProxy('gazebo/set_model_state', SetModelState)
+        self.deleteModel = rospy.ServiceProxy('gazebo/delete_model', DeleteModel)
+
+        self.positions = positions
+        self.tunnelRadius = tunnelRadius
+        self.yawOffset = yawOffset
+        self.hasPos = False
+        self.shouldStop = False
+        self.center = positions[0]
+        self.endOfSegment = False
+        self.failed = False
+
+    def position_callback(self, data):
+        self.hasPos = True
+        self.localPosition = data
+
+    def spawn_indicator(self):
+        self.deleteModel("indicator")
+        xml = "<?xml version='1.0'?><sdf version='1.4'><model name='indicator'><static>true</static><link name='link'><visual name='visual'><transparency>0.7</transparency><geometry><sphere><radius>%f</radius></sphere></geometry><material><ambient>1 0 0 0.5</ambient><diffuse>1 0 0 0.5</diffuse></material></visual></link></model></sdf>" % self.tunnelRadius
+        self.spawnModel("indicator", xml, "", Pose(), "")
+
+    def position_indicator(self):
+        state = SetModelState()
+        state.model_name = "indicator"
+        pose = Pose()
+        pose.position.x = self.center[0]
+        pose.position.y = (-1) * self.center[1]
+        pose.position.z = (-1) * self.center[2]
+        state.pose = pose
+        state.twist = Twist()
+        state.reference_frame = ""
+        self.setModelState(state)
+
+    def distance_to_line(self, a, b, pos):
+        v = b - a
+        w = pos - a
+
+        c1 = np.dot(w, v)
+        if c1 <= 0: # before a
+            self.center = a
+            return linalg.norm(pos - a)
+
+        c2 = np.dot(v, v)
+        if c2 <= c1: # after b
+            self.center = b
+            self.endOfSegment = True
+            return linalg.norm(pos - b)
+
+        x = c1 / c2
+        l = a + x * v
+        self.center = l
+        return linalg.norm(pos - l)
+    
+    def stop(self):
+        self.shouldStop = True
+
+    def run(self):
+        rate = rospy.Rate(10) # 10hz
+        self.spawn_indicator()
+
+        current = 0
+
+        while not self.shouldStop:
+            if (self.hasPos):
+                # calculate distance to line segment between first two points
+                # if distances > tunnelRadius
+                #   exit with error
+                # advance current pos if not on the line anymore or distance to next point < tunnelRadius
+                # exit if current pos is now the last position
+
+                self.position_indicator()
+
+                pos = np.array((self.localPosition.x,
+                    self.localPosition.y, 
+                    self.localPosition.z))
+                aPos = np.array((self.positions[current][0],
+                    self.positions[current][1], 
+                    self.positions[current][2]))
+                bPos = np.array((self.positions[current + 1][0],
+                    self.positions[current + 1][1], 
+                    self.positions[current + 1][2]))
+
+                dist = self.distance_to_line(aPos, bPos, pos)
+                bDist = linalg.norm(pos - bPos)
+
+                rospy.logdebug("distance to line: %f, distance to end: %f" % (dist, bDist))
+
+                if (dist > self.tunnelRadius):
+                    msg = "left tunnel at position (%f, %f, %f)" % (self.localPosition.x, self.localPosition.y, self.localPosition.z)
+                    rospy.logerr(msg)
+                    self.failed = True
+                    break
+
+                if (self.endOfSegment or bDist < self.tunnelRadius):
+                    rospy.loginfo("next segment")
+                    self.endOfSegment = False
+                    current = current + 1
+
+                if (current == len(self.positions) - 1):
+                    rospy.loginfo("no more positions")
+                    break
+
+            rate.sleep()

integrationtests/demo_tests/manual_input.py

@@ -0,0 +1,175 @@
+#!/usr/bin/env python
+#***************************************************************************
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+#***************************************************************************/
+
+#
+# @author Andreas Antener <andreas@uaventure.com>
+#
+import sys
+import rospy
+
+from px4.msg import manual_control_setpoint
+from px4.msg import offboard_control_mode
+from mav_msgs.msg import CommandAttitudeThrust
+from std_msgs.msg import Header
+
+#
+# Manual input control helper
+#
+# FIXME: this is not the way to do it! ATM it fakes input to iris/command/attitude because else
+# the simulator does not instantiate our controller. Probably this whole class will disappear once
+# arming works correctly.
+#
+class ManualInput:
+
+    def __init__(self):
+        rospy.init_node('test_node', anonymous=True)
+        self.pubMcsp = rospy.Publisher('px4_multicopter/manual_control_setpoint', manual_control_setpoint, queue_size=10)
+        self.pubOff = rospy.Publisher('px4_multicopter/offboard_control_mode', offboard_control_mode, queue_size=10)
+        self.pubAtt = rospy.Publisher('iris/command/attitude', CommandAttitudeThrust, queue_size=10)
+
+    def arm(self):
+        rate = rospy.Rate(10) # 10hz
+
+        att = CommandAttitudeThrust()
+        att.header = Header()
+
+        pos = manual_control_setpoint()
+        pos.x = 0
+        pos.z = 0
+        pos.y = 0
+        pos.r = 0
+        pos.mode_switch = 3
+        pos.return_switch = 3
+        pos.posctl_switch = 3
+        pos.loiter_switch = 3
+        pos.acro_switch = 0
+        pos.offboard_switch = 3
+
+        count = 0
+        while not rospy.is_shutdown() and count < 5:
+            rospy.loginfo("zeroing")
+            time = rospy.get_rostime().now()
+            pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
+            self.pubMcsp.publish(pos)
+            # Fake input to iris commander
+            self.pubAtt.publish(att)
+            rate.sleep()
+            count = count + 1 
+
+        pos.r = 1
+        count = 0
+        while not rospy.is_shutdown() and count < 5:
+            rospy.loginfo("arming")
+            time = rospy.get_rostime().now()
+            pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
+            self.pubMcsp.publish(pos)
+            rate.sleep()
+            count = count + 1
+
+    def posctl(self):
+        rate = rospy.Rate(10) # 10hz
+
+        # triggers posctl
+        pos = manual_control_setpoint()
+        pos.x = 0
+        pos.z = 0
+        pos.y = 0
+        pos.r = 0
+        pos.mode_switch = 2
+        pos.return_switch = 3
+        pos.posctl_switch = 1
+        pos.loiter_switch = 3
+        pos.acro_switch = 0
+        pos.offboard_switch = 3
+
+        count = 0
+        while not rospy.is_shutdown() and count < 5:
+            rospy.loginfo("triggering posctl")
+            time = rospy.get_rostime().now()
+            pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
+            self.pubMcsp.publish(pos)
+            rate.sleep()
+            count = count + 1
+
+
+    def offboard_attctl(self):
+        self.offboard(False, False, True, True, True, True)
+
+    def offboard_posctl(self):
+        self.offboard(False, False, True, False, True, True)
+
+    # Trigger offboard and set offboard control mode before
+    def offboard(self, ignore_thrust=False, ignore_attitude=False, ignore_bodyrate=True,
+                        ignore_position=False, ignore_velocity=True, ignore_acceleration_force=True):
+        rate = rospy.Rate(10) # 10hz
+
+        mode = offboard_control_mode()
+        mode.ignore_thrust = ignore_thrust
+        mode.ignore_attitude = ignore_attitude
+        mode.ignore_bodyrate = ignore_bodyrate
+        mode.ignore_position = ignore_position
+        mode.ignore_velocity = ignore_velocity
+        mode.ignore_acceleration_force = ignore_acceleration_force
+
+        count = 0
+        while not rospy.is_shutdown() and count < 5:
+            rospy.loginfo("setting offboard mode")
+            time = rospy.get_rostime().now()
+            mode.timestamp = time.secs * 1e6 + time.nsecs / 1000
+            self.pubOff.publish(mode)
+            rate.sleep()
+            count = count + 1
+
+        # triggers offboard
+        pos = manual_control_setpoint()
+        pos.x = 0
+        pos.z = 0
+        pos.y = 0
+        pos.r = 0
+        pos.mode_switch = 3
+        pos.return_switch = 3
+        pos.posctl_switch = 3
+        pos.loiter_switch = 3
+        pos.acro_switch = 0
+        pos.offboard_switch = 1
+
+        count = 0
+        while not rospy.is_shutdown() and count < 5:
+            rospy.loginfo("triggering offboard")
+            time = rospy.get_rostime().now()
+            pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
+            self.pubMcsp.publish(pos)
+            rate.sleep()
+            count = count + 1
+

integrationtests/demo_tests/mavros_offboard_attctl_test.py

@@ -0,0 +1,143 @@
+#!/usr/bin/env python
+#***************************************************************************
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+#***************************************************************************/
+
+#
+# @author Andreas Antener <andreas@uaventure.com>
+#
+PKG = 'px4'
+
+import sys
+import unittest
+import rospy
+import math
+
+from numpy import linalg
+import numpy as np
+
+from px4.msg import vehicle_control_mode
+from std_msgs.msg import Header
+from std_msgs.msg import Float64
+from geometry_msgs.msg import PoseStamped, Quaternion
+from tf.transformations import quaternion_from_euler
+from mavros.srv import CommandBool
+
+from manual_input import ManualInput
+
+#
+# Tests flying a path in offboard control by sending position setpoints
+# over MAVROS.
+#
+# For the test to be successful it needs to reach all setpoints in a certain time.
+# FIXME: add flight path assertion (needs transformation from ROS frame to NED)
+#
+class MavrosOffboardAttctlTest(unittest.TestCase):
+
+    def setUp(self):
+        rospy.init_node('test_node', anonymous=True)
+        rospy.wait_for_service('mavros/cmd/arming', 30)
+        rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback)
+        rospy.Subscriber("mavros/position/local", PoseStamped, self.position_callback)
+        self.pubAtt = rospy.Publisher('mavros/setpoint/attitude', PoseStamped, queue_size=10)
+        self.pubThr = rospy.Publisher('mavros/setpoint/att_throttle', Float64, queue_size=10)
+        self.cmdArm = rospy.ServiceProxy("mavros/cmd/arming", CommandBool)
+        self.rate = rospy.Rate(10) # 10hz
+        self.rateSec = rospy.Rate(1)
+        self.hasPos = False
+        self.controlMode = vehicle_control_mode()
+
+    #
+    # General callback functions used in tests
+    #
+    def position_callback(self, data):
+        self.hasPos = True
+        self.localPosition = data
+
+    def vehicle_control_mode_callback(self, data):
+        self.controlMode = data
+
+
+    #
+    # Helper methods
+    #
+    def arm(self):
+        return self.cmdArm(value=True)
+
+    #
+    # Test offboard position control
+    #
+    def test_attctl(self):
+        # FIXME: this must go ASAP when arming is implemented
+        manIn = ManualInput()
+        manIn.arm()
+        manIn.offboard_attctl()
+
+        self.assertTrue(self.arm(), "Could not arm")
+        self.rateSec.sleep()
+        self.rateSec.sleep()
+        self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set after 2 seconds")
+
+        # set some attitude and thrust
+        att = PoseStamped()
+        att.header = Header() 
+        att.header.frame_id = "base_footprint"
+        att.header.stamp = rospy.Time.now()
+        quaternion = quaternion_from_euler(0.15, 0.15, 0)
+        att.pose.orientation = Quaternion(*quaternion)
+
+        throttle = Float64()
+        throttle.data = 0.6
+
+        # does it cross expected boundaries in X seconds?
+        count = 0
+        timeout = 120
+        while(count < timeout):
+            # update timestamp for each published SP
+            att.header.stamp = rospy.Time.now()
+            self.pubAtt.publish(att)
+            self.pubThr.publish(throttle)
+            
+            if (self.localPosition.pose.position.x > 5
+                and self.localPosition.pose.position.z > 5
+                and self.localPosition.pose.position.y < -5):
+                break
+            count = count + 1
+            self.rate.sleep()
+
+        self.assertTrue(count < timeout, "took too long to cross boundaries")
+    
+
+if __name__ == '__main__':
+    import rostest
+    rostest.rosrun(PKG, 'mavros_offboard_attctl_test', MavrosOffboardAttctlTest)
+    #unittest.main()

integrationtests/demo_tests/mavros_offboard_posctl_test.py

@@ -0,0 +1,174 @@
+#!/usr/bin/env python
+#***************************************************************************
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+#***************************************************************************/
+
+#
+# @author Andreas Antener <andreas@uaventure.com>
+#
+PKG = 'px4'
+
+import sys
+import unittest
+import rospy
+import math
+
+from numpy import linalg
+import numpy as np
+
+from px4.msg import vehicle_control_mode
+from std_msgs.msg import Header
+from geometry_msgs.msg import PoseStamped, Quaternion
+from tf.transformations import quaternion_from_euler
+from mavros.srv import CommandBool
+
+from manual_input import ManualInput
+
+#
+# Tests flying a path in offboard control by sending position setpoints
+# over MAVROS.
+#
+# For the test to be successful it needs to reach all setpoints in a certain time.
+# FIXME: add flight path assertion (needs transformation from ROS frame to NED)
+#
+class MavrosOffboardPosctlTest(unittest.TestCase):
+
+    def setUp(self):
+        rospy.init_node('test_node', anonymous=True)
+        rospy.wait_for_service('mavros/cmd/arming', 30)
+        rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback)
+        rospy.Subscriber("mavros/position/local", PoseStamped, self.position_callback)
+        self.pubSpt = rospy.Publisher('mavros/setpoint/local_position', PoseStamped, queue_size=10)
+        self.cmdArm = rospy.ServiceProxy("mavros/cmd/arming", CommandBool)
+        self.rate = rospy.Rate(10) # 10hz
+        self.rateSec = rospy.Rate(1)
+        self.hasPos = False
+        self.controlMode = vehicle_control_mode()
+
+    #
+    # General callback functions used in tests
+    #
+    def position_callback(self, data):
+        self.hasPos = True
+        self.localPosition = data
+
+    def vehicle_control_mode_callback(self, data):
+        self.controlMode = data
+
+
+    #
+    # Helper methods
+    #
+    def is_at_position(self, x, y, z, offset):
+        if(not self.hasPos):
+            return False
+
+        rospy.logdebug("current position %f, %f, %f" % (self.localPosition.pose.position.x, self.localPosition.pose.position.y, self.localPosition.pose.position.z))
+        desired = np.array((x, y, z))
+        pos = np.array((self.localPosition.pose.position.x, self.localPosition.pose.position.y, self.localPosition.pose.position.z))
+        return linalg.norm(desired - pos) < offset
+
+    def reach_position(self, x, y, z, timeout):
+        # set a position setpoint
+        pos = PoseStamped()
+        pos.header = Header() 
+        pos.header.frame_id = "base_footprint"
+        pos.pose.position.x = x
+        pos.pose.position.y = y
+        pos.pose.position.z = z
+
+        # For demo purposes we will lock yaw/heading to north.
+        yaw_degrees = 0  # North
+        yaw = math.radians(yaw_degrees)
+        quaternion = quaternion_from_euler(0, 0, yaw)
+        pos.pose.orientation = Quaternion(*quaternion)
+
+        # does it reach the position in X seconds?
+        count = 0
+        while(count < timeout):
+            # update timestamp for each published SP
+            pos.header.stamp = rospy.Time.now()
+            self.pubSpt.publish(pos)
+            
+            if(self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5)):
+                break
+            count = count + 1
+            self.rate.sleep()
+
+        self.assertTrue(count < timeout, "took too long to get to position")
+
+    def arm(self):
+        return self.cmdArm(value=True)
+
+    #
+    # Test offboard position control
+    #
+    def test_posctl(self):
+        # FIXME: this must go ASAP when arming is implemented
+        manIn = ManualInput()
+        manIn.arm()
+        manIn.offboard_posctl()
+
+        self.assertTrue(self.arm(), "Could not arm")
+        self.rateSec.sleep()
+        self.rateSec.sleep()
+        self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set after 2 seconds")
+
+        # prepare flight path
+        positions = (
+            (0,0,0),
+            (2,2,2),
+            (2,-2,2),
+            (-2,-2,2),
+            (2,2,2))
+
+        for i in range(0, len(positions)):
+            self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120)
+        
+        # does it hold the position for Y seconds?
+        positionHeld = True
+        count = 0
+        timeout = 50
+        while(count < timeout):
+            if(not self.is_at_position(2, 2, 2, 0.5)):
+                positionHeld = False
+                break
+            count = count + 1
+            self.rate.sleep()
+
+        self.assertTrue(count == timeout, "position could not be held")
+    
+
+if __name__ == '__main__':
+    import rostest
+    rostest.rosrun(PKG, 'mavros_offboard_posctl_test', MavrosOffboardPosctlTest)
+    #unittest.main()

integrationtests/demo_tests/mavros_tests.launch

@@ -0,0 +1,19 @@
+<launch>
+    <arg name="headless" default="true"/>
+    <arg name="gui" default="false"/>
+    <arg name="enable_logging" default="false"/>
+    <arg name="enable_ground_truth" default="false"/>
+    <arg name="log_file" default="iris"/>
+
+    <include file="$(find px4)/launch/gazebo_iris_empty_world.launch">
+        <arg name="headless" value="$(arg headless)"/>
+        <arg name="gui" value="$(arg gui)"/>
+        <arg name="enable_logging" value="$(arg enable_logging)" />
+        <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
+        <arg name="log_file" value="$(arg log_file)"/>
+    </include>
+    <include file="$(find px4)/launch/mavros_sitl.launch" />
+
+    <test test-name="mavros_offboard_posctl_test" pkg="px4" type="mavros_offboard_posctl_test.py" />
+    <test test-name="mavros_offboard_attctl_test" pkg="px4" type="mavros_offboard_attctl_test.py" />
+</launch>

launch/gazebo_ardrone_empty_world.launch

@@ -3,7 +3,7 @@
     <arg name="headless" default="false"/>
     <arg name="gui" default="true"/>
     <arg name="enable_logging" default="false"/>
-    <arg name="enable_ground_truth" default="true"/>
+    <arg name="enable_ground_truth" default="false"/>
     <arg name="log_file" default="ardrone"/>
 
     <include file="$(find rotors_gazebo)/launch/ardrone_empty_world_with_joy.launch">

launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch

@@ -0,0 +1,9 @@
+
+<launch>
+
+<include file="$(find px4)/launch/gazebo_ardrone_empty_world.launch" />
+<include file="$(find px4)/launch/mavros_sitl.launch" />
+
+<node pkg="px4" name="demo_offboard_attitude_setpoints" type="demo_offboard_attitude_setpoints"/>
+
+</launch>

launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch

@@ -0,0 +1,9 @@
+
+<launch>
+
+<include file="$(find px4)/launch/gazebo_ardrone_empty_world.launch" />
+<include file="$(find px4)/launch/mavros_sitl.launch" />
+
+<node pkg="px4" name="demo_offboard_position_setpoints" type="demo_offboard_position_setpoints"/>
+
+</launch>

launch/mavros_sitl.launch

@@ -0,0 +1,21 @@
+<launch>
+	<!-- vim: set ft=xml noet : -->
+	<!-- example launch script for PX4 based FCU's -->
+
+	<arg name="fcu_url" default="udp://localhost:14560@localhost:14565" />
+	<arg name="gcs_url" default="" />
+	<arg name="tgt_system" default="1" />
+	<arg name="tgt_component" default="50" />
+
+	<param name="mavros/setpoint/attitude/listen_twist" type="bool" value="false" />
+
+	<include file="$(find mavros)/launch/node.launch">
+		<arg name="blacklist_yaml" value="$(find mavros)/launch/px4_blacklist.yaml" />
+		<arg name="config_yaml" value="$(find mavros)/launch/px4_config.yaml" />
+
+		<arg name="fcu_url" value="$(arg fcu_url)" />
+		<arg name="gcs_url" value="$(arg gcs_url)" />
+		<arg name="tgt_system" value="$(arg tgt_system)" />
+		<arg name="tgt_component" value="$(arg tgt_component)" />
+	</include>
+</launch>

launch/multicopter.launch

@@ -9,6 +9,7 @@
 	<node pkg="px4" name="position_estimator" type="position_estimator"/>
 	<node pkg="px4" name="mc_att_control" type="mc_att_control"/>
 	<node pkg="px4" name="mc_pos_control" type="mc_pos_control"/>
+	<node pkg="px4" name="mavlink" type="mavlink"/>
 	<!-- <node pkg="rosbag" type="record" name="record" output="screen" args="-a -O px4_multicopter"/> -->
 </group>
 

makefiles/config_px4fmu-v1_default.mk

@@ -67,9 +67,12 @@ MODULES 	+= modules/land_detector
 #
 # Estimation modules (EKF / other filters)
 #
-MODULES		+= modules/attitude_estimator_ekf
+# Too high RAM usage due to static allocations
+# MODULES		+= modules/attitude_estimator_ekf
 MODULES		+= modules/ekf_att_pos_estimator
-MODULES		+= modules/position_estimator_inav
+# Since attitude_estimator_ekf is disabled, this app won't be
+# worthwhile on its own
+# MODULES		+= modules/position_estimator_inav
 
 #
 # Vehicle Control

makefiles/config_px4fmu-v2_default.mk

@@ -44,6 +44,7 @@ MODULES		+= modules/sensors
 MODULES		+= drivers/mkblctrl
 MODULES		+= drivers/px4flow
 MODULES		+= drivers/oreoled
+MODULES		+= drivers/gimbal
 
 #
 # System commands

msg/offboard_control_mode.msg

@@ -0,0 +1,9 @@
+# Off-board control mode
+uint64 timestamp
+
+bool ignore_thrust
+bool ignore_attitude
+bool ignore_bodyrate
+bool ignore_position
+bool ignore_velocity
+bool ignore_acceleration_force

msg/vehicle_force_setpoint.msg

@@ -0,0 +1,8 @@
+# Definition of force (NED) setpoint uORB topic. Typically this can be used
+# by a position control app together with an attitude control app.
+
+
+float32 x		# in N NED
+float32 y		# in N NED
+float32 z		# in N NED
+float32 yaw		# right-hand rotation around downward axis (rad, equivalent to Tait-Bryan yaw)

nuttx-configs/aerocore/nsh/defconfig

@@ -332,7 +332,7 @@ CONFIG_BOARD_LOOPSPERMSEC=16717
 CONFIG_DRAM_START=0x20000000
 CONFIG_DRAM_SIZE=262144
 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y
-CONFIG_ARCH_INTERRUPTSTACK=4096
+CONFIG_ARCH_INTERRUPTSTACK=1500
 
 #
 # Boot options
@@ -415,8 +415,8 @@ CONFIG_PREALLOC_TIMERS=50
 #
 # Stack and heap information
 #
-CONFIG_IDLETHREAD_STACKSIZE=6000
-CONFIG_USERMAIN_STACKSIZE=4096
+CONFIG_IDLETHREAD_STACKSIZE=1000
+CONFIG_USERMAIN_STACKSIZE=3000
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_PTHREAD_STACK_DEFAULT=2048
 

nuttx-configs/px4fmu-v1/nsh/defconfig

@@ -325,7 +325,8 @@ CONFIG_BOARD_LOOPSPERMSEC=16717
 CONFIG_DRAM_START=0x20000000
 CONFIG_DRAM_SIZE=196608
 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y
-CONFIG_ARCH_INTERRUPTSTACK=4096
+# The actual usage is 420 bytes
+CONFIG_ARCH_INTERRUPTSTACK=1500
 
 #
 # Boot options
@@ -416,8 +417,8 @@ CONFIG_PREALLOC_TIMERS=50
 #
 # Stack and heap information
 #
-CONFIG_IDLETHREAD_STACKSIZE=3500
-CONFIG_USERMAIN_STACKSIZE=2600
+CONFIG_IDLETHREAD_STACKSIZE=1000
+CONFIG_USERMAIN_STACKSIZE=3000
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_PTHREAD_STACK_DEFAULT=2048
 

nuttx-configs/px4fmu-v2/nsh/defconfig

@@ -367,7 +367,7 @@ CONFIG_BOARD_LOOPSPERMSEC=16717
 CONFIG_DRAM_START=0x20000000
 CONFIG_DRAM_SIZE=262144
 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y
-CONFIG_ARCH_INTERRUPTSTACK=4096
+CONFIG_ARCH_INTERRUPTSTACK=1500
 
 #
 # Boot options
@@ -451,8 +451,8 @@ CONFIG_PREALLOC_TIMERS=50
 #
 # Stack and heap information
 #
-CONFIG_IDLETHREAD_STACKSIZE=3500
-CONFIG_USERMAIN_STACKSIZE=2600
+CONFIG_IDLETHREAD_STACKSIZE=1000
+CONFIG_USERMAIN_STACKSIZE=3000
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_PTHREAD_STACK_DEFAULT=2048
 

package.xml

@@ -44,10 +44,15 @@
   <build_depend>rospy</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>eigen</build_depend>
+  <build_depend>libmavconn</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>rostest</build_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>rospy</run_depend>
   <run_depend>std_msgs</run_depend>
   <run_depend>eigen</run_depend>
+  <run_depend>libmavconn</run_depend>
+  <run_depend>tf</run_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

src/drivers/batt_smbus/batt_smbus.cpp

@@ -452,10 +452,7 @@ BATT_SMBUS::read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_
 	uint8_t pec = get_PEC(reg, true, buff, bufflen + 1);
 
 	if (pec != buff[bufflen + 1]) {
-		// debug
-		warnx("CurrPEC:%x vs MyPec:%x", (int)buff[bufflen + 1], (int)pec);
 		return 0;
-
 	}
 
 	// copy data

src/drivers/drv_gpio.h

@@ -159,4 +159,6 @@
 
 #define GPIO_SENSOR_RAIL_RESET	GPIOC(13)
 
+#define GPIO_PERIPHERAL_RAIL_RESET	GPIOC(14)
+
 #endif /* _DRV_GPIO_H */

src/drivers/drv_oreoled.h

@@ -58,6 +58,9 @@
 /** run macro */
 #define OREOLED_RUN_MACRO		_OREOLEDIOC(2)
 
+/** send bytes */
+#define OREOLED_SEND_BYTES		_OREOLEDIOC(3)
+
 /* Oreo LED driver supports up to 4 leds */
 #define OREOLED_NUM_LEDS		4
 
@@ -65,7 +68,7 @@
 #define OREOLED_ALL_INSTANCES	0xff
 
 /* maximum command length that can be sent to LEDs */
-#define OREOLED_CMD_LENGTH_MAX	10
+#define OREOLED_CMD_LENGTH_MAX	24
 
 /* enum passed to OREOLED_SET_MODE ioctl()
  *	defined by hardware */

src/drivers/gimbal/gimbal.cpp

@@ -0,0 +1,541 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014, 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 gimbal.cpp
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Matosov <anton.matosov@gmail.com>
+ *
+ * Driver to control a gimbal - relies on input via telemetry or RC
+ * and output via the standardized control group #2 and a mixer.
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+#include <termios.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_range_finder.h>
+#include <drivers/device/device.h>
+#include <drivers/device/ringbuffer.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_attitude.h>
+
+#include <board_config.h>
+#include <mathlib/math/test/test.hpp>
+#include <mathlib/math/Quaternion.hpp>
+
+/* Configuration Constants */
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+#define GIMBAL_DEVICE_PATH	"/dev/gimbal"
+
+#define GIMBAL_UPDATE_INTERVAL (5 * 1000)
+
+#define GIMBALIOCATTCOMPENSATE		1
+
+class Gimbal : public device::CDev
+{
+public:
+	Gimbal();
+	virtual ~Gimbal();
+
+	virtual int 			init();
+
+	virtual ssize_t			read(struct file *filp, char *buffer, size_t buflen);
+	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/**
+	* Diagnostics - print some basic information about the driver.
+	*/
+	void				print_info();
+
+protected:
+	virtual int			probe();
+
+private:
+	float				_min_distance;
+	float				_max_distance;
+	work_s				_work;
+	int				_vehicle_command_sub;
+	int				_att_sub;
+
+	bool				_attitude_compensation;
+	bool				_initialized;
+
+	orb_advert_t			_actuator_controls_2_topic;
+
+	perf_counter_t			_sample_perf;
+	perf_counter_t			_comms_errors;
+	perf_counter_t			_buffer_overflows;
+
+	/**
+	* Initialise the automatic measurement state machine and start it.
+	*
+	* @note This function is called at open and error time.  It might make sense
+	*       to make it more aggressive about resetting the bus in case of errors.
+	*/
+	void				start();
+
+	/**
+	* Stop the automatic measurement state machine.
+	*/
+	void				stop();
+
+	/**
+	* Perform a poll cycle; collect from the previous measurement
+	* and start a new one.
+	*/
+	void				cycle();
+
+	/**
+	* Static trampoline from the workq context; because we don't have a
+	* generic workq wrapper yet.
+	*
+	* @param arg		Instance pointer for the driver that is polling.
+	*/
+	static void			cycle_trampoline(void *arg);
+
+
+};
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int gimbal_main(int argc, char *argv[]);
+
+Gimbal::Gimbal() :
+	CDev("Gimbal", GIMBAL_DEVICE_PATH),
+	_vehicle_command_sub(-1),
+	_att_sub(-1),
+	_attitude_compensation(true),
+	_initialized(false),
+	_actuator_controls_2_topic(-1),
+	_sample_perf(perf_alloc(PC_ELAPSED, "gimbal_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "gimbal_comms_errors")),
+	_buffer_overflows(perf_alloc(PC_COUNT, "gimbal_buffer_overflows"))
+{
+	// disable debug() calls
+	_debug_enabled = false;
+
+	// work_cancel in the dtor will explode if we don't do this...
+	memset(&_work, 0, sizeof(_work));
+}
+
+Gimbal::~Gimbal()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	::close(_actuator_controls_2_topic);
+	::close(_vehicle_command_sub);
+}
+
+int
+Gimbal::init()
+{
+	int ret = ERROR;
+
+	/* do regular cdev init */
+	if (CDev::init() != OK) {
+		goto out;
+	}
+
+	start();
+	ret = OK;
+
+out:
+	return ret;
+}
+
+int
+Gimbal::probe()
+{
+	return OK;
+}
+
+int
+Gimbal::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case GIMBALIOCATTCOMPENSATE:
+		_attitude_compensation = (arg != 0);
+		return OK;
+
+	default:
+		/* give it to the superclass */
+		return CDev::ioctl(filp, cmd, arg);
+	}
+}
+
+ssize_t
+Gimbal::read(struct file *filp, char *buffer, size_t buflen)
+{
+	return 0;
+}
+
+void
+Gimbal::start()
+{
+	/* schedule a cycle to start things */
+	work_queue(LPWORK, &_work, (worker_t)&Gimbal::cycle_trampoline, this, 1);
+}
+
+void
+Gimbal::stop()
+{
+	work_cancel(LPWORK, &_work);
+}
+
+void
+Gimbal::cycle_trampoline(void *arg)
+{
+	Gimbal *dev = static_cast<Gimbal *>(arg);
+
+	dev->cycle();
+}
+
+void
+Gimbal::cycle()
+{
+	if (!_initialized) {
+		/* get a subscription handle on the vehicle command topic */
+		_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
+
+		/* get a publication handle on actuator output topic */
+		struct actuator_controls_s zero_report;
+		memset(&zero_report, 0, sizeof(zero_report));
+		zero_report.timestamp = hrt_absolute_time();
+		_actuator_controls_2_topic = orb_advertise(ORB_ID(actuator_controls_2), &zero_report);
+
+		if (_actuator_controls_2_topic < 0) {
+			warnx("advert err");
+		}
+
+		_initialized = true;
+	}
+
+	bool	updated = false;
+
+	perf_begin(_sample_perf);
+
+	float roll = 0.0f;
+	float pitch = 0.0f;
+	float yaw = 0.0f;
+
+
+	if (_attitude_compensation) {
+		if (_att_sub < 0) {
+			_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+		}
+
+		vehicle_attitude_s att;
+
+		orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att);
+
+		roll = -att.roll;
+		pitch = -att.pitch;
+		yaw = att.yaw;
+
+		updated = true;
+	}
+
+	struct vehicle_command_s cmd;
+
+	bool cmd_updated;
+
+	orb_check(_vehicle_command_sub, &cmd_updated);
+
+	if (cmd_updated) {
+
+		orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
+
+		VEHICLE_MOUNT_MODE mountMode = (VEHICLE_MOUNT_MODE)cmd.param7;
+		debug("cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", cmd.command, mountMode, (double)cmd.param1, (double)cmd.param2);
+
+		if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
+
+			/* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */
+			roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param1;
+			pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param2;
+			yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param3;
+			
+			updated = true;
+
+		}
+
+		if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
+			float gimbalDirectionQuat[] = {cmd.param1, cmd.param2, cmd.param3, cmd.param4};
+			math::Vector<3> gimablDirectionEuler = math::Quaternion(gimbalDirectionQuat).to_dcm().to_euler();
+
+			roll += gimablDirectionEuler(0);
+			pitch += gimablDirectionEuler(1);
+			yaw += gimablDirectionEuler(2);
+
+			updated = true;
+		}
+	}
+
+	if (updated) {
+
+		struct actuator_controls_s controls;
+
+		// debug("publishing | roll: %8.4f pitch: %8.4f yaw: %8.4f", (double)roll, (double)pitch, (double)yaw);
+
+		/* fill in the final control values */
+		controls.timestamp = hrt_absolute_time();
+		controls.control[0] = roll;
+		controls.control[1] = pitch;
+		controls.control[2] = yaw;
+
+		/* publish it */
+		orb_publish(ORB_ID(actuator_controls_2), _actuator_controls_2_topic, &controls);
+
+	}
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	perf_end(_sample_perf);
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(LPWORK,
+		   &_work,
+		   (worker_t)&Gimbal::cycle_trampoline,
+		   this,
+		   USEC2TICK(GIMBAL_UPDATE_INTERVAL));
+}
+
+void
+Gimbal::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	perf_print_counter(_buffer_overflows);
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace gimbal
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+const int ERROR = -1;
+
+Gimbal	*g_dev;
+
+void	start();
+void	stop();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start()
+{
+	if (g_dev != nullptr) {
+		errx(1, "already started");
+	}
+
+	/* create the driver */
+	g_dev = new Gimbal();
+
+	if (g_dev == nullptr) {
+		goto fail;
+	}
+
+	if (OK != g_dev->init()) {
+		goto fail;
+	}
+
+	exit(0);
+
+fail:
+
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Stop the driver
+ */
+void stop()
+{
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+
+	} else {
+		errx(1, "driver not running");
+	}
+
+	exit(0);
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+	int fd = open(GIMBAL_DEVICE_PATH, O_RDONLY);
+
+	if (ioctl(fd, GIMBALIOCATTCOMPENSATE, 1) < 0) {
+		err(1, "failed enabling compensation");
+	}
+
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(GIMBAL_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		err(1, "failed ");
+	}
+
+	// if (ioctl(fd, GIMBALIOCATTCOMPENSATE, 1) < 0) {
+	// 	err(1, "failed enabling compensation");
+	// }
+
+	exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr) {
+		errx(1, "driver not running");
+	}
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	exit(0);
+}
+
+} // namespace
+
+int
+gimbal_main(int argc, char *argv[])
+{
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[1], "start")) {
+		gimbal::start();
+	}
+
+	/*
+	 * Stop the driver
+	 */
+	if (!strcmp(argv[1], "stop")) {
+		gimbal::stop();
+	}
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test")) {
+		gimbal::test();
+	}
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset")) {
+		gimbal::reset();
+	}
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
+		gimbal::info();
+	}
+
+	errx(1, "unrecognized command, try 'start', 'stop', 'reset', 'test' or 'info'");
+}

src/drivers/gimbal/module.mk

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2014, 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.
+#
+############################################################################
+
+#
+# Makefile to build the gimbal high-level controller
+#
+
+MODULE_COMMAND	= gimbal
+
+SRCS		= gimbal.cpp
+
+MAXOPTIMIZATION = -Os

src/drivers/mkblctrl/mkblctrl.cpp

@@ -96,6 +96,9 @@
 #define ESC_UORB_PUBLISH_DELAY			500000
 
 #define CONTROL_INPUT_DROP_LIMIT_MS		20
+#define RC_MIN_VALUE					1010
+#define RC_MAX_VALUE					2100
+
 
 struct MotorData_t {
 	unsigned int Version;                        // the version of the BL (0 = old)
@@ -141,6 +144,8 @@ public:
 	void		set_px4mode(int px4mode);
 	void		set_frametype(int frametype);
 	unsigned int		mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C);
+	void		set_rc_min_value(unsigned value);
+	void		set_rc_max_value(unsigned value);
 
 private:
 	static const unsigned	_max_actuators = MAX_MOTORS;
@@ -164,7 +169,9 @@ private:
 	bool					_armed;
 	unsigned long			debugCounter;
 	MixerGroup				*_mixers;
-	bool					_indicate_esc; 
+	bool					_indicate_esc;
+	unsigned				_rc_min_value;
+	unsigned				_rc_max_value;
 	param_t					_param_indicate_esc;
 	actuator_controls_s 	_controls;
 	MotorData_t 			Motor[MAX_MOTORS];
@@ -227,7 +234,9 @@ MK::MK(int bus, const char *_device_path) :
 	_task_should_exit(false),
 	_armed(false),
 	_mixers(nullptr),
-	_indicate_esc(false)
+	_indicate_esc(false),
+	_rc_min_value(RC_MIN_VALUE),
+	_rc_max_value(RC_MAX_VALUE)
 {
 	strncpy(_device, _device_path, sizeof(_device));
 	/* enforce null termination */
@@ -296,7 +305,7 @@ MK::init(unsigned motors)
 	_task = task_spawn_cmd("mkblctrl",
 			       SCHED_DEFAULT,
 			       SCHED_PRIORITY_MAX - 20,
-			       2048,
+			       1500,
 			       (main_t)&MK::task_main_trampoline,
 			       nullptr);
 
@@ -327,6 +336,19 @@ MK::set_frametype(int frametype)
 	_frametype = frametype;
 }
 
+void
+MK::set_rc_min_value(unsigned value)
+{
+	_rc_min_value = value;
+	fprintf(stderr, "[mkblctrl] rc_min = %i\n", _rc_min_value);
+}
+
+void
+MK::set_rc_max_value(unsigned value)
+{
+	_rc_max_value = value;
+	fprintf(stderr, "[mkblctrl] rc_max = %i\n", _rc_max_value);
+}
 
 int
 MK::set_motor_count(unsigned count)
@@ -984,7 +1006,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
 		if (arg < 2150) {
 			Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue = (unsigned short)arg;
-			mk_servo_set(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 2047));
+			mk_servo_set(cmd - PWM_SERVO_SET(0), scaling(arg, _rc_min_value, _rc_max_value, 0, 2047));
 
 		} else {
 			ret = -EINVAL;
@@ -1064,6 +1086,37 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			break;
 		}
 
+	case PWM_SERVO_SET_MIN_PWM: {
+		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
+		if (pwm->channel_count > _max_actuators)
+			/* fail with error */
+			return -E2BIG;
+
+		set_rc_min_value((unsigned)pwm->values[0]);
+		ret = OK;
+		break;
+	}
+
+	case PWM_SERVO_GET_MIN_PWM:
+		ret = OK;
+		break;
+
+	case PWM_SERVO_SET_MAX_PWM: {
+		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
+		if (pwm->channel_count > _max_actuators)
+			/* fail with error */
+			return -E2BIG;
+
+		set_rc_max_value((unsigned)pwm->values[0]);
+		ret = OK;
+		break;
+	}
+
+	case PWM_SERVO_GET_MAX_PWM:
+		ret = OK;
+		break;
+
+
 	default:
 		ret = -ENOTTY;
 		break;
@@ -1094,7 +1147,7 @@ MK::write(file *filp, const char *buffer, size_t len)
 
 	for (uint8_t i = 0; i < count; i++) {
 		Motor[i].RawPwmValue = (unsigned short)values[i];
-		mk_servo_set(i, scaling(values[i], 1010, 2100, 0, 2047));
+		mk_servo_set(i, scaling(values[i], _rc_min_value, _rc_max_value, 0, 2047));
 	}
 
 	return count * 2;
@@ -1116,10 +1169,16 @@ enum FrameType {
 
 
 int
-mk_new_mode(int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks)
+mk_new_mode(int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks, unsigned rcmin, unsigned rcmax)
 {
 	int shouldStop = 0;
 
+	/* set rc min pulse value */
+	g_mk->set_rc_min_value(rcmin);
+
+	/* set rc max pulse value */
+	g_mk->set_rc_max_value(rcmax);
+
 	/* native PX4 addressing) */
 	g_mk->set_px4mode(px4mode);
 
@@ -1209,6 +1268,9 @@ mkblctrl_main(int argc, char *argv[])
 	bool showHelp = false;
 	bool newMode = true;
 	const char *devicepath = "";
+	unsigned rc_min_value = RC_MIN_VALUE;
+	unsigned rc_max_value = RC_MAX_VALUE;
+	char *ep;
 
 	/*
 	 * optional parameters
@@ -1268,6 +1330,35 @@ mkblctrl_main(int argc, char *argv[])
 			}
 		}
 
+		/* look for the optional -rc_min parameter */
+		if (strcmp(argv[i], "-rc_min") == 0 ) {
+			if (argc > i + 1) {
+				rc_min_value = strtoul(argv[i + 1], &ep, 0);
+				if (*ep != '\0') {
+					errx(1, "bad pwm val (-rc_min)");
+					return 1;
+				}
+			} else {
+				errx(1, "missing value (-rc_min)");
+				return 1;
+			}
+		}
+
+		/* look for the optional -rc_max parameter */
+		if (strcmp(argv[i], "-rc_max") == 0 ) {
+			if (argc > i + 1) {
+				rc_max_value = strtoul(argv[i + 1], &ep, 0);
+				if (*ep != '\0') {
+					errx(1, "bad pwm val (-rc_max)");
+					return 1;
+				}
+			} else {
+				errx(1, "missing value (-rc_max)");
+				return 1;
+			}
+		}
+
+
 	}
 
 	if (showHelp) {
@@ -1276,6 +1367,8 @@ mkblctrl_main(int argc, char *argv[])
 		fprintf(stderr, "\t -mkmode {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n");
 		fprintf(stderr, "\t -d {devicepath & name}\t\t Create alternate pwm device.\n");
 		fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n");
+		fprintf(stderr, "\t -rcmin {pwn-value}\t\t Set RC_MIN Value.\n");
+		fprintf(stderr, "\t -rcmax {pwn-value}\t\t Set RC_MAX Value.\n");
 		fprintf(stderr, "\n");
 		fprintf(stderr, "Motortest:\n");
 		fprintf(stderr, "First you have to start mkblctrl, the you can enter Motortest Mode with:\n");
@@ -1294,7 +1387,7 @@ mkblctrl_main(int argc, char *argv[])
 			/* parameter set ? */
 			if (newMode) {
 				/* switch parameter */
-				return mk_new_mode(motorcount, motortest, px4mode, frametype, overrideSecurityChecks);
+				return mk_new_mode(motorcount, motortest, px4mode, frametype, overrideSecurityChecks, rc_min_value, rc_max_value);
 			}
 
 			exit(0);

src/drivers/mpu6000/mpu6000.cpp

@@ -1881,6 +1881,7 @@ MPU6000	*g_dev_int; // on internal bus
 MPU6000	*g_dev_ext; // on external bus
 
 void	start(bool, enum Rotation);
+void	stop(bool);
 void	test(bool);
 void	reset(bool);
 void	info(bool);
@@ -1946,6 +1947,20 @@ fail:
 	errx(1, "driver start failed");
 }
 
+void
+stop(bool external_bus)
+{
+	MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int;
+	if (*g_dev_ptr != nullptr) {
+		delete *g_dev_ptr;
+		*g_dev_ptr = nullptr;
+	} else {
+		/* warn, but not an error */
+		warnx("already stopped.");
+	}
+	exit(0);
+}
+
 /**
  * Perform some basic functional tests on the driver;
  * make sure we can collect data from the sensor in polled
@@ -2111,7 +2126,7 @@ factorytest(bool external_bus)
 void
 usage()
 {
-	warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'factorytest', 'testerror'");
+	warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'factorytest', 'testerror'");
 	warnx("options:");
 	warnx("    -X    (external bus)");
 	warnx("    -R rotation");
@@ -2147,38 +2162,50 @@ mpu6000_main(int argc, char *argv[])
 	 * Start/load the driver.
 
 	 */
-	if (!strcmp(verb, "start"))
+	if (!strcmp(verb, "start")) {
 		mpu6000::start(external_bus, rotation);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		mpu6000::stop(external_bus);
+	}
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(verb, "test"))
+	if (!strcmp(verb, "test")) {
 		mpu6000::test(external_bus);
+	}
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(verb, "reset"))
+	if (!strcmp(verb, "reset")) {
 		mpu6000::reset(external_bus);
+	}
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(verb, "info"))
+	if (!strcmp(verb, "info")) {
 		mpu6000::info(external_bus);
+	}
 
 	/*
 	 * Print register information.
 	 */
-	if (!strcmp(verb, "regdump"))
+	if (!strcmp(verb, "regdump")) {
 		mpu6000::regdump(external_bus);
+	}
 
-	if (!strcmp(verb, "factorytest"))
+	if (!strcmp(verb, "factorytest")) {
 		mpu6000::factorytest(external_bus);
+	}
 
-	if (!strcmp(verb, "testerror"))
+	if (!strcmp(verb, "testerror")) {
 		mpu6000::testerror(external_bus);
+	}
 
-	errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'regdump', 'factorytest' or 'testerror'");
+	mpu6000::usage();
+	exit(1);
 }

src/drivers/oreoled/oreoled.cpp

@@ -368,6 +368,31 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 		return ret;
 
+	case OREOLED_SEND_BYTES:
+		/* send bytes */
+		new_cmd = *((oreoled_cmd_t *) arg);
+
+		/* special handling for request to set all instances */
+		if (new_cmd.led_num == OREOLED_ALL_INSTANCES) {
+			for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
+				/* add command to queue for all healthy leds */
+				if (_healthy[i]) {
+					new_cmd.led_num = i;
+					_cmd_queue->force(&new_cmd);
+					ret = OK;
+				}
+			}
+
+		} else if (new_cmd.led_num < OREOLED_NUM_LEDS) {
+			/* request to set individual instance's rgb value */
+			if (_healthy[new_cmd.led_num]) {
+				_cmd_queue->force(&new_cmd);
+				ret = OK;
+			}
+		}
+
+		return ret;
+
 	default:
 		/* see if the parent class can make any use of it */
 		ret = CDev::ioctl(filp, cmd, arg);

src/drivers/px4fmu/fmu.cpp

@@ -182,6 +182,7 @@ private:
 
 	void		gpio_reset(void);
 	void		sensor_reset(int ms);
+	void		peripheral_reset(int ms);
 	void		gpio_set_function(uint32_t gpios, int function);
 	void		gpio_write(uint32_t gpios, int function);
 	uint32_t	gpio_read(void);
@@ -1376,6 +1377,29 @@ PX4FMU::sensor_reset(int ms)
 #endif
 }
 
+void
+PX4FMU::peripheral_reset(int ms)
+{
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+
+	if (ms < 1) {
+		ms = 10;
+	}
+
+	/* set the peripheral rails off */
+	stm32_configgpio(GPIO_VDD_5V_PERIPH_EN);
+	stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 1);
+
+	/* wait for the peripheral rail to reach GND */
+	usleep(ms * 1000);
+	warnx("reset done, %d ms", ms);
+
+	/* re-enable power */
+
+	/* switch the peripheral rail back on */
+	stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 0);
+#endif
+}
 
 void
 PX4FMU::gpio_reset(void)
@@ -1488,6 +1512,10 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
 		sensor_reset(arg);
 		break;
 
+	case GPIO_PERIPHERAL_RAIL_RESET:
+		peripheral_reset(arg);
+		break;
+
 	case GPIO_SET_OUTPUT:
 	case GPIO_SET_INPUT:
 	case GPIO_SET_ALT_1:
@@ -1674,6 +1702,24 @@ sensor_reset(int ms)
 	close(fd);
 }
 
+void
+peripheral_reset(int ms)
+{
+	int	 fd;
+
+	fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
+
+	if (fd < 0) {
+		errx(1, "open fail");
+	}
+
+	if (ioctl(fd, GPIO_PERIPHERAL_RAIL_RESET, ms) < 0) {
+		warnx("peripheral rail reset failed");
+	}
+
+	close(fd);
+}
+
 void
 test(void)
 {
@@ -1895,6 +1941,19 @@ fmu_main(int argc, char *argv[])
 		exit(0);
 	}
 
+	if (!strcmp(verb, "peripheral_reset")) {
+		if (argc > 2) {
+			int reset_time = strtol(argv[2], 0, 0);
+			peripheral_reset(reset_time);
+
+		} else {
+			peripheral_reset(0);
+			warnx("resettet default time");
+		}
+
+		exit(0);
+	}
+
 	if (!strcmp(verb, "i2c")) {
 		if (argc > 3) {
 			int bus = strtol(argv[2], 0, 0);

src/lib/ecl/attitude_fw/ecl_controller.cpp

@@ -49,6 +49,7 @@
 #include "ecl_controller.h"
 
 #include <stdio.h>
+#include <mathlib/mathlib.h>
 
 ECL_Controller::ECL_Controller(const char *name) :
 	_last_run(0),
@@ -126,3 +127,14 @@ float ECL_Controller::get_desired_bodyrate()
 {
 	return _bodyrate_setpoint;
 }
+
+float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float maxspeed) {
+	float airspeed_result = airspeed;
+	if (!isfinite(airspeed)) {
+		/* airspeed is NaN, +- INF or not available, pick center of band */
+		airspeed = 0.5f * (minspeed + maxspeed);
+	} else if (airspeed < minspeed) {
+		airspeed = minspeed;
+	}
+	return airspeed_result;
+}

src/lib/ecl/attitude_fw/ecl_controller.h

@@ -119,4 +119,5 @@ protected:
 	perf_counter_t _nonfinite_input_perf;
 	static const uint8_t _perf_name_max = 40;
 	char _perf_name[_perf_name_max];
+	float constrain_airspeed(float airspeed, float minspeed, float maxspeed);
 };

src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp

@@ -94,9 +94,11 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da
 		}
 	}
 
+	/* input conditioning */
+	float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max);
 	/* calculate the offset in the rate resulting from rolling  */
 	//xxx needs explanation and conversion to body angular rates or should be removed
-	float turn_offset = fabsf((CONSTANTS_ONE_G / ctl_data.airspeed) *
+	float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) *
 				  tanf(roll) * sinf(roll)) * _roll_ff;
 
 	if (inverted) {
@@ -154,17 +156,6 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
 		lock_integrator = true;
 	}
 
-	/* input conditioning */
-	float airspeed = ctl_data.airspeed;
-
-	if (!isfinite(airspeed)) {
-		/* airspeed is NaN, +- INF or not available, pick center of band */
-		airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max);
-
-	} else if (airspeed < ctl_data.airspeed_min) {
-		airspeed = ctl_data.airspeed_min;
-	}
-
 	/* Transform setpoint to body angular rates (jacobian) */
 	_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
 			     cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
@@ -175,7 +166,7 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
 
 	_rate_error = _bodyrate_setpoint - pitch_bodyrate;
 
-	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) {
+	if (!lock_integrator && _k_i > 0.0f) {
 
 		float id = _rate_error * dt * ctl_data.scaler;
 

src/lib/ecl/attitude_fw/ecl_roll_controller.cpp

@@ -106,17 +106,6 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
 		lock_integrator = true;
 	}
 
-	/* input conditioning */
-	float airspeed = ctl_data.airspeed;
-
-	if (!isfinite(airspeed)) {
-		/* airspeed is NaN, +- INF or not available, pick center of band */
-		airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max);
-
-	} else if (airspeed < ctl_data.airspeed_min) {
-		airspeed = ctl_data.airspeed_min;
-	}
-
 	/* Transform setpoint to body angular rates (jacobian) */
 	_bodyrate_setpoint = _rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;
 
@@ -126,7 +115,7 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
 	/* Calculate body angular rate error */
 	_rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error
 
-	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) {
+	if (!lock_integrator && _k_i > 0.0f) {
 
 		float id = _rate_error * dt * ctl_data.scaler;
 
@@ -157,3 +146,4 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
 
 	return math::constrain(_last_output, -1.0f, 1.0f);
 }
+

src/lib/mathlib/math/Quaternion.hpp

@@ -115,6 +115,35 @@ public:
 		return Vector<3>(&data[1]);
 	}
 
+	/**
+	 * inverse of quaternion
+	 */
+	math::Quaternion inverse() {
+		Quaternion res;
+		memcpy(res.data,data,sizeof(res.data));
+		res.data[1] = -res.data[1];
+		res.data[2] = -res.data[2];
+		res.data[3] = -res.data[3];
+		return res;
+	}
+
+
+	/**
+	* rotate vector by quaternion
+	*/
+	Vector<3> rotate(const Vector<3> &w) {
+		Quaternion q_w;	// extend vector to quaternion
+		Quaternion q = {data[0],data[1],data[2],data[3]};
+		Quaternion q_rotated;	// quaternion representation of rotated vector
+		q_w(0) = 0;
+		q_w(1) = w.data[0];
+		q_w(2) = w.data[1];
+		q_w(3) = w.data[2];
+		q_rotated = q*q_w*q.inverse();
+		Vector<3> res = {q_rotated.data[1],q_rotated.data[2],q_rotated.data[3]};
+		return res;
+	}
+
 	/**
 	 * set quaternion to rotation defined by euler angles
 	 */

src/lib/rc/module.mk

@@ -33,8 +33,10 @@
 
 #
 # Yuntec ST24 transmitter protocol decoder
+# Graupnet HoTT transmitter protocol decoder
 #
 
-SRCS		 =	st24.c
+SRCS		 =	st24.c \
+				sumd.c
 
 MAXOPTIMIZATION	 = -Os

src/lib/rc/sumd.c

@@ -0,0 +1,319 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file sumd.h
+ *
+ * RC protocol definition for Graupner HoTT transmitter (SUMD/SUMH Protocol)
+ *
+ * @author Marco Bauer <marco@wtns.de>
+ */
+
+#include <stdbool.h>
+#include <stdio.h>
+#include "sumd.h"
+
+
+enum SUMD_DECODE_STATE {
+	SUMD_DECODE_STATE_UNSYNCED = 0,
+	SUMD_DECODE_STATE_GOT_HEADER,
+	SUMD_DECODE_STATE_GOT_STATE,
+	SUMD_DECODE_STATE_GOT_LEN,
+	SUMD_DECODE_STATE_GOT_DATA,
+	SUMD_DECODE_STATE_GOT_CRC,
+	SUMD_DECODE_STATE_GOT_CRC16_BYTE_1,
+	SUMD_DECODE_STATE_GOT_CRC16_BYTE_2
+};
+
+/*
+const char *decode_states[] = {"UNSYNCED",
+			       "GOT_HEADER",
+			       "GOT_STATE",
+			       "GOT_LEN",
+			       "GOT_DATA",
+			       "GOT_CRC",
+			       "GOT_CRC16_BYTE_1",
+			       "GOT_CRC16_BYTE_2"
+			      };
+*/
+
+uint8_t 	_crc8 	= 0x00;
+uint16_t 	_crc16 = 0x0000;
+bool 		_sumd 		= true;
+bool		_crcOK		= false;
+bool		_debug		= false;
+
+
+/* define range mapping here, -+100% -> 1000..2000 */
+#define SUMD_RANGE_MIN 0.0f
+#define SUMD_RANGE_MAX 4096.0f
+
+#define SUMD_TARGET_MIN 1000.0f
+#define SUMD_TARGET_MAX 2000.0f
+
+/* pre-calculate the floating point stuff as far as possible at compile time */
+#define SUMD_SCALE_FACTOR ((SUMD_TARGET_MAX - SUMD_TARGET_MIN) / (SUMD_RANGE_MAX - SUMD_RANGE_MIN))
+#define SUMD_SCALE_OFFSET (int)(SUMD_TARGET_MIN - (SUMD_SCALE_FACTOR * SUMD_RANGE_MIN + 0.5f))
+
+static enum SUMD_DECODE_STATE _decode_state = SUMD_DECODE_STATE_UNSYNCED;
+static uint8_t _rxlen;
+
+static ReceiverFcPacketHoTT _rxpacket;
+
+
+uint16_t sumd_crc16(uint16_t crc, uint8_t value)
+{
+	int i;
+	crc ^= (uint16_t)value << 8;
+	for (i = 0; i < 8; i++) {
+		crc = (crc & 0x8000) ? (crc << 1) ^ 0x1021 : (crc << 1);
+	}
+	return crc;
+}
+
+uint8_t sumd_crc8(uint8_t crc, uint8_t value)
+{
+	crc += value;
+	return crc;
+}
+
+int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels,
+		    uint16_t max_chan_count)
+{
+
+	int ret = 1;
+	switch (_decode_state) {
+	case SUMD_DECODE_STATE_UNSYNCED:
+		if(_debug)
+			printf( " SUMD_DECODE_STATE_UNSYNCED \n") ;
+			
+		if (byte == SUMD_HEADER_ID) {
+			_rxpacket.header = byte;
+			_sumd = true;
+			_rxlen = 0;
+			_crc16 = 0x0000;
+			_crc8 = 0x00;
+			_crcOK = false;
+			_crc16 = sumd_crc16(_crc16, byte);
+			_crc8 = sumd_crc8(_crc8, byte);
+			_decode_state = SUMD_DECODE_STATE_GOT_HEADER;
+			if(_debug)
+				printf( " SUMD_DECODE_STATE_GOT_HEADER: %x \n", byte) ;
+				
+		} else {
+			ret = 3;
+		}
+
+		break;
+
+	case SUMD_DECODE_STATE_GOT_HEADER:
+		if (byte == SUMD_ID_SUMD || byte == SUMD_ID_SUMH) {
+			_rxpacket.status = byte;
+			if (byte == SUMD_ID_SUMH) {
+				_sumd = false;
+			}
+			if (_sumd) {
+				_crc16 = sumd_crc16(_crc16, byte);
+			} else {
+				_crc8 = sumd_crc8(_crc8, byte);	
+			}
+			_decode_state = SUMD_DECODE_STATE_GOT_STATE;
+			if(_debug)
+				printf( " SUMD_DECODE_STATE_GOT_STATE: %x \n", byte) ;
+
+		} else {
+			_decode_state = SUMD_DECODE_STATE_UNSYNCED;
+		}
+
+		break;
+
+	case SUMD_DECODE_STATE_GOT_STATE:
+		if (byte >= 2 && byte <= SUMD_MAX_CHANNELS) {
+			_rxpacket.length = byte;
+			if (_sumd) {
+				_crc16 = sumd_crc16(_crc16, byte);
+			} else {
+				_crc8 = sumd_crc8(_crc8, byte);	
+			}
+			_rxlen++;
+			_decode_state = SUMD_DECODE_STATE_GOT_LEN;
+			if(_debug)
+				printf( " SUMD_DECODE_STATE_GOT_LEN: %x (%d) \n", byte, byte) ;
+				
+		} else {
+			_decode_state = SUMD_DECODE_STATE_UNSYNCED;
+		}
+		break;
+
+	case SUMD_DECODE_STATE_GOT_LEN:
+		_rxpacket.sumd_data[_rxlen] = byte;
+		if (_sumd) {
+			_crc16 = sumd_crc16(_crc16, byte);
+		} else {
+			_crc8 = sumd_crc8(_crc8, byte);	
+		}
+		_rxlen++;
+		
+		if (_rxlen <= ((_rxpacket.length*2) )) {
+			if(_debug)
+				printf( " SUMD_DECODE_STATE_GOT_DATA[%d]: %x\n", _rxlen-2, byte) ;
+
+		} else {
+			_decode_state = SUMD_DECODE_STATE_GOT_DATA;
+			if(_debug)
+				printf( " SUMD_DECODE_STATE_GOT_DATA -- finish --\n") ;
+				
+		}
+
+		break;
+
+	case SUMD_DECODE_STATE_GOT_DATA:
+		_rxpacket.crc16_high = byte;
+		if(_debug)
+			printf( " SUMD_DECODE_STATE_GOT_CRC16[1]: %x   [%x]\n", byte, ((_crc16 >> 8) & 0xff)) ;
+			
+		if (_sumd) {
+			_decode_state = SUMD_DECODE_STATE_GOT_CRC;
+		} else {
+			_decode_state = SUMD_DECODE_STATE_GOT_CRC16_BYTE_1;
+		}
+
+		break;
+
+	case SUMD_DECODE_STATE_GOT_CRC16_BYTE_1:
+		_rxpacket.crc16_low = byte;
+		if(_debug)
+			printf( " SUMD_DECODE_STATE_GOT_CRC16[2]: %x   [%x]\n", byte, (_crc16 & 0xff)) ;
+			
+		_decode_state = SUMD_DECODE_STATE_GOT_CRC16_BYTE_2;
+
+		break;
+
+	case SUMD_DECODE_STATE_GOT_CRC16_BYTE_2:
+		_rxpacket.telemetry = byte;
+		if(_debug)
+			printf( " SUMD_DECODE_STATE_GOT_SUMH_TELEMETRY: %x\n", byte) ;
+			
+		_decode_state = SUMD_DECODE_STATE_GOT_CRC;
+
+		break;
+		
+	case SUMD_DECODE_STATE_GOT_CRC:
+		if (_sumd) {
+			_rxpacket.crc16_low = byte;
+			if(_debug)
+				printf( " SUMD_DECODE_STATE_GOT_CRC[2]: %x   [%x]\n\n", byte, (_crc16 & 0xff)) ;
+				
+			if (_crc16 == (uint16_t)(_rxpacket.crc16_high<<8)+_rxpacket.crc16_low) {
+				_crcOK = true;
+			}
+		} else {
+			_rxpacket.crc8 = byte;
+			if(_debug)
+				printf( " SUMD_DECODE_STATE_GOT_CRC8_SUMH: %x   [%x]\n\n", byte, _crc8) ;
+				
+			if (_crc8 == _rxpacket.crc8) {
+				_crcOK = true;
+			}
+		}
+		
+		if (_crcOK) {
+			if(_debug)
+				printf( " CRC - OK \n") ;
+
+			if (_sumd) {
+				if(_debug)
+					printf( " Got valid SUMD Packet\n") ;
+					
+			} else {
+				if(_debug)
+					printf( " Got valid SUMH Packet\n") ;
+					
+			}
+			
+			if(_debug)
+				printf( " RXLEN: %d  [Chans: %d] \n\n", _rxlen-1, (_rxlen-1)/2) ;
+
+			ret = 0;
+			unsigned i;
+			uint8_t _cnt = *rx_count + 1;
+			*rx_count = _cnt;
+		
+			*rssi = 255;
+
+			/* received Channels */
+			if ((uint16_t)_rxpacket.length > max_chan_count) {
+				_rxpacket.length = (uint8_t) max_chan_count;
+			}
+			*channel_count = (uint16_t)_rxpacket.length;
+
+			/* decode the actual packet */
+			/* reorder first 4 channels */
+
+			/* ch1 = roll -> sumd = ch2 */
+			channels[0] = (uint16_t)((_rxpacket.sumd_data[1*2+1]<<8) | _rxpacket.sumd_data[1*2+2])>>3;
+			/* ch2 = pitch -> sumd = ch2 */
+			channels[1] = (uint16_t)((_rxpacket.sumd_data[2*2+1]<<8) | _rxpacket.sumd_data[2*2+2])>>3;
+			/* ch3 = throttle -> sumd = ch2 */
+			channels[2] = (uint16_t)((_rxpacket.sumd_data[0*2+1]<<8) | _rxpacket.sumd_data[0*2+2])>>3;
+			/* ch4 = yaw -> sumd = ch2 */
+			channels[3] = (uint16_t)((_rxpacket.sumd_data[3*2+1]<<8) | _rxpacket.sumd_data[3*2+2])>>3;
+
+			/* we start at channel 5(index 4) */
+			unsigned chan_index = 4;
+
+			for (i = 4; i < _rxpacket.length; i++) {
+				if(_debug)
+					printf( "ch[%d] : %x %x [ %x    %d ]\n", i+1, _rxpacket.sumd_data[i*2+1], _rxpacket.sumd_data[i*2+2], ((_rxpacket.sumd_data[i*2+1]<<8) | _rxpacket.sumd_data[i*2+2])>>3, ((_rxpacket.sumd_data[i*2+1]<<8) | _rxpacket.sumd_data[i*2+2])>>3);
+					
+				channels[chan_index] = (uint16_t)((_rxpacket.sumd_data[i*2+1]<<8) | _rxpacket.sumd_data[i*2+2])>>3;
+				/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
+				//channels[chan_index] = (uint16_t)(channels[chan_index] * SUMD_SCALE_FACTOR + .5f) + SUMD_SCALE_OFFSET;
+
+				chan_index++;
+			}
+
+		} else {
+			/* decoding failed */
+			ret = 4;
+			if(_debug)
+				printf( " CRC - fail \n") ;
+
+		}
+
+		_decode_state = SUMD_DECODE_STATE_UNSYNCED;
+		break;
+	}
+
+	return ret;
+}

src/lib/rc/sumd.h

@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file sumd.h
+ *
+ * RC protocol definition for Graupner HoTT transmitter
+ *
+ * @author Marco Bauer <marco@wtns.de>
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+#define SUMD_MAX_CHANNELS	32
+#define SUMD_HEADER_LENGTH	3
+#define SUMD_HEADER_ID		0xA8
+#define SUMD_ID_SUMH		0x00
+#define SUMD_ID_SUMD		0x01
+#define SUMD_ID_FAILSAFE	0x81
+
+
+
+
+#pragma pack(push, 1)
+typedef struct {
+	uint8_t	header;							///< 0xA8 for a valid packet
+	uint8_t	status;							///< 0x01 valid and live SUMD data frame / 0x00 = SUMH / 0x81 = Failsafe
+	uint8_t	length;							///< Channels
+	uint8_t	sumd_data[SUMD_MAX_CHANNELS*2];	///< ChannelData (High Byte/ Low Byte)
+	uint8_t	crc16_high;						///< High Byte of 16 Bit CRC
+	uint8_t	crc16_low;						///< Low Byte of 16 Bit CRC
+	uint8_t	telemetry;						///< Telemetry request
+	uint8_t	crc8;							///< SUMH CRC8
+} ReceiverFcPacketHoTT;
+#pragma pack(pop)
+
+
+/**
+ * CRC16 implementation for SUMD protocol
+ *
+ * @param crc Initial CRC Value
+ * @param value to accumulate in the checksum
+ * @return the checksum
+ */
+uint16_t sumd_crc16(uint16_t crc, uint8_t value);
+
+/**
+ * CRC8 implementation for SUMH protocol
+ *
+ * @param crc Initial CRC Value
+ * @param value to accumulate in the checksum
+ * @return the checksum
+ */
+uint8_t sumd_crc8(uint8_t crc, uint8_t value);
+
+/**
+ * Decoder for SUMD/SUMH protocol
+ *
+ * @param byte current char to read
+ * @param rssi pointer to a byte where the RSSI value is written back to
+ * @param rx_count pointer to a byte where the receive count of packets signce last wireless frame is written back to
+ * @param channels pointer to a datastructure of size max_chan_count where channel values (12 bit) are written back to
+ * @param max_chan_count maximum channels to decode - if more channels are decoded, the last n are skipped and success (0) is returned
+ * @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 2 for unknown packet, 3 for out of sync, 4 for checksum error
+ */
+/*
+__EXPORT int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count,
+				 uint16_t *channels, uint16_t max_chan_count);
+*/
+int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count,
+				 uint16_t *channels, uint16_t max_chan_count);
+
+				 
+__END_DECLS

src/modules/commander/accelerometer_calibration.cpp

@@ -358,7 +358,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se
 
 		if (orient < 0) {
 			mavlink_and_console_log_info(mavlink_fd, "invalid motion, hold still...");
-			sleep(3);
+			sleep(2);
 			continue;
 		}
 
@@ -372,6 +372,8 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se
 		mavlink_and_console_log_info(mavlink_fd, "Hold still, starting to measure %s side", orientation_strs[orient]);
 		sleep(1);
 		read_accelerometer_avg(subs, accel_ref, orient, samples_num);
+		mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]);
+		usleep(100000);
 		mavlink_and_console_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient],
 				 (double)accel_ref[0][orient][0],
 				 (double)accel_ref[0][orient][1],

src/modules/commander/commander.cpp

@@ -65,7 +65,7 @@
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/offboard_control_setpoint.h>
+#include <uORB/topics/offboard_control_mode.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
@@ -133,7 +133,7 @@ extern struct system_load_s system_load;
 #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
 
-#define POSITION_TIMEOUT		(2 * 1000 * 1000)	/**< consider the local or global position estimate invalid after 600ms */
+#define POSITION_TIMEOUT		(1 * 1000 * 1000)	/**< consider the local or global position estimate invalid after 1000ms */
 #define FAILSAFE_DEFAULT_TIMEOUT	(3 * 1000 * 1000)	/**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
 #define OFFBOARD_TIMEOUT		500000
 #define DIFFPRESS_TIMEOUT		2000000
@@ -180,7 +180,7 @@ static struct vehicle_status_s status;
 static struct actuator_armed_s armed;
 static struct safety_s safety;
 static struct vehicle_control_mode_s control_mode;
-static struct offboard_control_setpoint_s sp_offboard;
+static struct offboard_control_mode_s offboard_control_mode;
 
 /* tasks waiting for low prio thread */
 typedef enum {
@@ -966,7 +966,7 @@ int commander_thread_main(int argc, char *argv[])
 
 	pthread_attr_t commander_low_prio_attr;
 	pthread_attr_init(&commander_low_prio_attr);
-	pthread_attr_setstacksize(&commander_low_prio_attr, 2900);
+	pthread_attr_setstacksize(&commander_low_prio_attr, 2000);
 
 	struct sched_param param;
 	(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
@@ -1016,8 +1016,8 @@ int commander_thread_main(int argc, char *argv[])
 	memset(&sp_man, 0, sizeof(sp_man));
 
 	/* Subscribe to offboard control data */
-	int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
-	memset(&sp_offboard, 0, sizeof(sp_offboard));
+	int offboard_control_mode_sub = orb_subscribe(ORB_ID(offboard_control_mode));
+	memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));
 
 	/* Subscribe to telemetry status topics */
 	int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM];
@@ -1227,14 +1227,14 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
 		}
 
-		orb_check(sp_offboard_sub, &updated);
+		orb_check(offboard_control_mode_sub, &updated);
 
 		if (updated) {
-			orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
+			orb_copy(ORB_ID(offboard_control_mode), offboard_control_mode_sub, &offboard_control_mode);
 		}
 
-		if (sp_offboard.timestamp != 0 &&
-		    sp_offboard.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) {
+		if (offboard_control_mode.timestamp != 0 &&
+		    offboard_control_mode.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) {
 			if (status.offboard_control_signal_lost) {
 				status.offboard_control_signal_lost = false;
 				status_changed = true;
@@ -2051,7 +2051,7 @@ int commander_thread_main(int argc, char *argv[])
 	led_deinit();
 	buzzer_deinit();
 	close(sp_man_sub);
-	close(sp_offboard_sub);
+	close(offboard_control_mode_sub);
 	close(local_position_sub);
 	close(global_position_sub);
 	close(gps_sub);
@@ -2426,56 +2426,30 @@ set_control_mode()
 		control_mode.flag_control_auto_enabled = false;
 		control_mode.flag_control_offboard_enabled = true;
 
-		switch (sp_offboard.mode) {
-		case OFFBOARD_CONTROL_MODE_DIRECT_RATES:
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = false;
-			control_mode.flag_control_altitude_enabled = false;
-			control_mode.flag_control_climb_rate_enabled = false;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-			break;
+		/*
+		 * The control flags depend on what is ignored according to the offboard control mode topic
+		 * Inner loop flags (e.g. attitude) also depend on outer loop ignore flags (e.g. position)
+		 */
+		control_mode.flag_control_rates_enabled = !offboard_control_mode.ignore_bodyrate ||
+			!offboard_control_mode.ignore_attitude ||
+			!offboard_control_mode.ignore_position ||
+			!offboard_control_mode.ignore_velocity ||
+			!offboard_control_mode.ignore_acceleration_force;
 
-		case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = true;
-			control_mode.flag_control_altitude_enabled = false;
-			control_mode.flag_control_climb_rate_enabled = false;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-			break;
+		control_mode.flag_control_attitude_enabled = !offboard_control_mode.ignore_attitude ||
+			!offboard_control_mode.ignore_position ||
+			!offboard_control_mode.ignore_velocity ||
+			!offboard_control_mode.ignore_acceleration_force;
 
-		case OFFBOARD_CONTROL_MODE_DIRECT_FORCE:
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = false;
-			control_mode.flag_control_force_enabled = true;
-			control_mode.flag_control_altitude_enabled = false;
-			control_mode.flag_control_climb_rate_enabled = false;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-			break;
+		control_mode.flag_control_velocity_enabled = !offboard_control_mode.ignore_velocity ||
+			!offboard_control_mode.ignore_position;
 
-		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED:
-		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED:
-		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED:
-		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED:
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = true;
-			control_mode.flag_control_altitude_enabled = true;
-			control_mode.flag_control_climb_rate_enabled = true;
-			control_mode.flag_control_position_enabled = true;
-			control_mode.flag_control_velocity_enabled = true;
-			//XXX: the flags could depend on sp_offboard.ignore
-			break;
+		control_mode.flag_control_climb_rate_enabled = !offboard_control_mode.ignore_velocity ||
+			!offboard_control_mode.ignore_position;
 
-		default:
-			control_mode.flag_control_rates_enabled = false;
-			control_mode.flag_control_attitude_enabled = false;
-			control_mode.flag_control_altitude_enabled = false;
-			control_mode.flag_control_climb_rate_enabled = false;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-		}
+		control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position;
+
+		control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_position;
 
 		break;
 

src/modules/commander/mag_calibration.cpp

@@ -137,6 +137,9 @@ int do_mag_calibration(int mavlink_fd)
 	}
 
 	if (calibrated_ok) {
+
+		mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100);
+		usleep(100000);
 		mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
 
 		/* auto-save to EEPROM */
@@ -155,7 +158,7 @@ int do_mag_calibration(int mavlink_fd)
 int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id)
 {
 	/* 45 seconds */
-	uint64_t calibration_interval = 45 * 1000 * 1000;
+	uint64_t calibration_interval = 25 * 1000 * 1000;
 
 	/* maximum 500 values */
 	const unsigned int calibration_maxcount = 240;

src/modules/commander/state_machine_helper.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,7 +35,7 @@
  * @file state_machine_helper.cpp
  * State machine helper functions implementations
  *
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Thomas Gubler <thomas@px4.io>
  * @author Julian Oes <julian@oes.ch>
  */
 

src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h

@@ -143,6 +143,7 @@ private:
     int     _mission_sub;
     int     _home_sub;          /**< home position as defined by commander / user */
     int     _landDetectorSub;
+    int     _armedSub;
 
     orb_advert_t    _att_pub;           /**< vehicle attitude */
     orb_advert_t    _global_pos_pub;        /**< global position */
@@ -163,6 +164,7 @@ private:
     struct wind_estimate_s              _wind;          /**< wind estimate */
     struct range_finder_report          _distance;      /**< distance estimate */
     struct vehicle_land_detected_s      _landDetector;
+    struct actuator_armed_s             _armed;
 
     struct gyro_scale               _gyro_offsets[3];
     struct accel_scale              _accel_offsets[3];

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -96,13 +96,13 @@ uint64_t getMicros()
 namespace estimator
 {
 
-	/* oddly, ERROR is not defined for c++ */
-	#ifdef ERROR
-	# undef ERROR
-	#endif
-	static const int ERROR = -1;
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
 
-	AttitudePositionEstimatorEKF	*g_estimator = nullptr;
+AttitudePositionEstimatorEKF	*g_estimator = nullptr;
 }
 
 AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
@@ -110,7 +110,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_task_running(false),
 	_estimator_task(-1),
 
-	/* subscriptions */
+/* subscriptions */
 	_sensor_combined_sub(-1),
 	_distance_sub(-1),
 	_airspeed_sub(-1),
@@ -122,8 +122,9 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_mission_sub(-1),
 	_home_sub(-1),
 	_landDetectorSub(-1),
+	_armedSub(-1),
 
-	/* publications */
+/* publications */
 	_att_pub(-1),
 	_global_pos_pub(-1),
 	_local_pos_pub(-1),
@@ -131,71 +132,72 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_wind_pub(-1),
 
 	_att({}),
-	_gyro({}),
-	_accel({}),
-	_mag({}),
-	_airspeed({}),
-	_baro({}),
-	_vstatus({}),
-	_global_pos({}),
-	_local_pos({}),
-	_gps({}),
-	_wind({}),
-	_distance{},
-	_landDetector{},
+     _gyro({}),
+     _accel({}),
+     _mag({}),
+     _airspeed({}),
+     _baro({}),
+     _vstatus({}),
+     _global_pos({}),
+     _local_pos({}),
+     _gps({}),
+     _wind({}),
+     _distance {},
+     _landDetector {},
+     _armed {},
 
-	_gyro_offsets({}),
-	_accel_offsets({}),
-	_mag_offsets({}),
+     _gyro_offsets({}),
+     _accel_offsets({}),
+     _mag_offsets({}),
 
-	_sensor_combined{},
+     _sensor_combined {},
 
-	_pos_ref{},
-	_baro_ref(0.0f),
-	_baro_ref_offset(0.0f),
-	_baro_gps_offset(0.0f),
+     _pos_ref {},
+     _baro_ref(0.0f),
+     _baro_ref_offset(0.0f),
+     _baro_gps_offset(0.0f),
 
-	/* performance counters */
-	_loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")),
-	_loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")),
-	_perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")),
-	_perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")),
-	_perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")),
-	_perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")),
-	_perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")),
-	_perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")),
+     /* performance counters */
+     _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")),
+     _loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")),
+     _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")),
+     _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")),
+     _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")),
+     _perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")),
+     _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")),
+     _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")),
 
-	/* states */
-	_gps_alt_filt(0.0f),
-	_baro_alt_filt(0.0f),
-	_covariancePredictionDt(0.0f),
-	_gpsIsGood(false),
-	_previousGPSTimestamp(0),
-	_baro_init(false),
-	_gps_initialized(false),
-	_filter_start_time(0),
-	_last_sensor_timestamp(0),
-	_last_run(0),
-	_distance_last_valid(0),
-	_gyro_valid(false),
-	_accel_valid(false),
-	_mag_valid(false),
-	_gyro_main(0),
-	_accel_main(0),
-	_mag_main(0),
-	_ekf_logging(true),
-	_debug(0),
+     /* states */
+     _gps_alt_filt(0.0f),
+     _baro_alt_filt(0.0f),
+     _covariancePredictionDt(0.0f),
+     _gpsIsGood(false),
+     _previousGPSTimestamp(0),
+     _baro_init(false),
+     _gps_initialized(false),
+     _filter_start_time(0),
+     _last_sensor_timestamp(0),
+     _last_run(0),
+     _distance_last_valid(0),
+     _gyro_valid(false),
+     _accel_valid(false),
+     _mag_valid(false),
+     _gyro_main(0),
+     _accel_main(0),
+     _mag_main(0),
+     _ekf_logging(true),
+     _debug(0),
 
-	_newDataGps(false),
-	_newHgtData(false),
-	_newAdsData(false),
-	_newDataMag(false),
-	_newRangeData(false),
+     _newDataGps(false),
+     _newHgtData(false),
+     _newAdsData(false),
+     _newDataMag(false),
+     _newRangeData(false),
 
-	_mavlink_fd(-1),
-	_parameters{},
-	_parameter_handles{},
-	_ekf(nullptr)
+     _mavlink_fd(-1),
+     _parameters {},
+     _parameter_handles {},
+     _ekf(nullptr)
 {
 	_last_run = hrt_absolute_time();
 
@@ -334,7 +336,7 @@ int AttitudePositionEstimatorEKF::parameters_update()
 		_ekf->posDSigma = _parameters.posd_noise;
 		_ekf->magMeasurementSigma = _parameters.mag_noise;
 		_ekf->gyroProcessNoise = _parameters.gyro_pnoise;
-        	_ekf->accelProcessNoise = _parameters.acc_pnoise;
+		_ekf->accelProcessNoise = _parameters.acc_pnoise;
 		_ekf->airspeedMeasurementSigma = _parameters.eas_noise;
 		_ekf->rngFinderPitch = 0.0f; // XXX base on SENS_BOARD_Y_OFF
 	}
@@ -365,14 +367,15 @@ int AttitudePositionEstimatorEKF::check_filter_state()
 
 	int check = _ekf->CheckAndBound(&ekf_report);
 
-	const char* const feedback[] = { 0,
-					"NaN in states, resetting",
-					"stale sensor data, resetting",
-					"got initial position lock",
-					"excessive gyro offsets",
-					"velocity diverted, check accel config",
-					"excessive covariances",
-					"unknown condition, resetting"};
+	const char *const feedback[] = { 0,
+					 "NaN in states, resetting",
+					 "stale sensor data, resetting",
+					 "got initial position lock",
+					 "excessive gyro offsets",
+					 "velocity diverted, check accel config",
+					 "excessive covariances",
+					 "unknown condition, resetting"
+				       };
 
 	// Print out error condition
 	if (check) {
@@ -391,6 +394,7 @@ int AttitudePositionEstimatorEKF::check_filter_state()
 	}
 
 	struct estimator_status_report rep;
+
 	memset(&rep, 0, sizeof(rep));
 
 	// If error flag is set, we got a filter reset
@@ -433,18 +437,18 @@ int AttitudePositionEstimatorEKF::check_filter_state()
 
 			if (rep.health_flags < ((1 << 0) | (1 << 1) | (1 << 2) | (1 << 3))) {
 				warnx("health: VEL:%s POS:%s HGT:%s OFFS:%s",
-					((rep.health_flags & (1 << 0)) ? "OK" : "ERR"),
-					((rep.health_flags & (1 << 1)) ? "OK" : "ERR"),
-					((rep.health_flags & (1 << 2)) ? "OK" : "ERR"),
-					((rep.health_flags & (1 << 3)) ? "OK" : "ERR"));
+				      ((rep.health_flags & (1 << 0)) ? "OK" : "ERR"),
+				      ((rep.health_flags & (1 << 1)) ? "OK" : "ERR"),
+				      ((rep.health_flags & (1 << 2)) ? "OK" : "ERR"),
+				      ((rep.health_flags & (1 << 3)) ? "OK" : "ERR"));
 			}
 
 			if (rep.timeout_flags) {
 				warnx("timeout: %s%s%s%s",
-					((rep.timeout_flags & (1 << 0)) ? "VEL " : ""),
-					((rep.timeout_flags & (1 << 1)) ? "POS " : ""),
-					((rep.timeout_flags & (1 << 2)) ? "HGT " : ""),
-					((rep.timeout_flags & (1 << 3)) ? "IMU " : ""));
+				      ((rep.timeout_flags & (1 << 0)) ? "VEL " : ""),
+				      ((rep.timeout_flags & (1 << 1)) ? "POS " : ""),
+				      ((rep.timeout_flags & (1 << 2)) ? "HGT " : ""),
+				      ((rep.timeout_flags & (1 << 3)) ? "IMU " : ""));
 			}
 		}
 
@@ -457,12 +461,11 @@ int AttitudePositionEstimatorEKF::check_filter_state()
 			rep.states[i] = ekf_report.states[i];
 		}
 
-		for (size_t i = 0; i < rep.n_states; i++) {
-			rep.states[i] = ekf_report.states[i];
-		}
+
 
 		if (_estimator_status_pub > 0) {
 			orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep);
+
 		} else {
 			_estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep);
 		}
@@ -499,6 +502,7 @@ void AttitudePositionEstimatorEKF::task_main()
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_home_sub = orb_subscribe(ORB_ID(home_position));
 	_landDetectorSub = orb_subscribe(ORB_ID(vehicle_land_detected));
+	_armedSub = orb_subscribe(ORB_ID(actuator_armed));
 
 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_vstatus_sub, 200);
@@ -532,8 +536,9 @@ void AttitudePositionEstimatorEKF::task_main()
 		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
 		/* timed out - periodic check for _task_should_exit, etc. */
-		if (pret == 0)
+		if (pret == 0) {
 			continue;
+		}
 
 		/* this is undesirable but not much we can do - might want to flag unhappy status */
 		if (pret < 0) {
@@ -592,7 +597,7 @@ void AttitudePositionEstimatorEKF::task_main()
 			/**
 			 *    PART ONE: COLLECT ALL DATA
 			 **/
-			 pollData();
+			pollData();
 
 			/*
 			 *    CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY
@@ -630,10 +635,10 @@ void AttitudePositionEstimatorEKF::task_main()
 				/* Initialize the filter first */
 				if (!_gps_initialized && _gpsIsGood) {
 					initializeGPS();
-				} 
-				else if (!_ekf->statesInitialised) {
+
+				} else if (!_ekf->statesInitialised) {
 					// North, East Down position (m)
-					float initVelNED[3] = {0.0f, 0.0f, 0.0f}; 
+					float initVelNED[3] = {0.0f, 0.0f, 0.0f};
 
 					_ekf->posNE[0] = 0.0f;
 					_ekf->posNE[1] = 0.0f;
@@ -645,8 +650,7 @@ void AttitudePositionEstimatorEKF::task_main()
 
 					_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
 
-				} 
-				else if (_ekf->statesInitialised) {
+				} else if (_ekf->statesInitialised) {
 
 					// Check if on ground - status is used by covariance prediction
 					_ekf->setOnGround(_landDetector.landed);
@@ -654,6 +658,7 @@ void AttitudePositionEstimatorEKF::task_main()
 					// We're apparently initialized in this case now
 					// check (and reset the filter as needed)
 					int check = check_filter_state();
+
 					if (check) {
 						// Let the system re-initialize itself
 						continue;
@@ -669,8 +674,7 @@ void AttitudePositionEstimatorEKF::task_main()
 					publishLocalPosition();
 
 					//Publish Global Position, but only if it's any good
-					if(_gps_initialized && _gpsIsGood)
-					{
+					if (_gps_initialized && (_gpsIsGood || _global_pos.dead_reckoning)) {
 						publishGlobalPosition();
 					}
 
@@ -738,12 +742,14 @@ void AttitudePositionEstimatorEKF::initializeGPS()
 	map_projection_init(&_pos_ref, lat, lon);
 	mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
 
-	#if 0
+#if 0
 	warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt,
-		(double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
-	warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_ref_offset);
-	warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination));
-	#endif
+	      (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
+	warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref,
+	      (double)_baro_ref_offset);
+	warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv,
+	      (double)math::degrees(declination));
+#endif
 
 	_gps_initialized = true;
 }
@@ -774,14 +780,14 @@ void AttitudePositionEstimatorEKF::publishAttitude()
 	_att.pitch = euler(1);
 	_att.yaw = euler(2);
 
-	_att.rollspeed = _ekf->angRate.x - _ekf->states[10];
-	_att.pitchspeed = _ekf->angRate.y - _ekf->states[11];
-	_att.yawspeed = _ekf->angRate.z - _ekf->states[12];
+	_att.rollspeed = _ekf->angRate.x - _ekf->states[10] / _ekf->dtIMU;
+	_att.pitchspeed = _ekf->angRate.y - _ekf->states[11] / _ekf->dtIMU;
+	_att.yawspeed = _ekf->angRate.z - _ekf->states[12] / _ekf->dtIMU;
 
 	// gyro offsets
-	_att.rate_offsets[0] = _ekf->states[10];
-	_att.rate_offsets[1] = _ekf->states[11];
-	_att.rate_offsets[2] = _ekf->states[12];
+	_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMU;
+	_att.rate_offsets[1] = _ekf->states[11] / _ekf->dtIMU;
+	_att.rate_offsets[2] = _ekf->states[12] / _ekf->dtIMU;
 
 	/* lazily publish the attitude only once available */
 	if (_att_pub > 0) {
@@ -842,6 +848,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
 	if (_local_pos.v_xy_valid) {
 		_global_pos.vel_n = _local_pos.vx;
 		_global_pos.vel_e = _local_pos.vy;
+
 	} else {
 		_global_pos.vel_n = 0.0f;
 		_global_pos.vel_e = 0.0f;
@@ -857,7 +864,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
 	/* terrain altitude */
 	_global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1];
 	_global_pos.terrain_alt_valid = (_distance_last_valid > 0) &&
-		(hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000);
+					(hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000);
 
 	_global_pos.yaw = _local_pos.yaw;
 	_global_pos.eph = _gps.eph;
@@ -897,8 +904,9 @@ void AttitudePositionEstimatorEKF::publishWindEstimate()
 
 }
 
-void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const bool fuseMag, const bool fuseRangeSensor, 
-	const bool fuseBaro, const bool fuseAirSpeed)
+void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const bool fuseMag,
+		const bool fuseRangeSensor,
+		const bool fuseBaro, const bool fuseAirSpeed)
 {
 	// Run the strapdown INS equations every IMU update
 	_ekf->UpdateStrapdownEquationsNED();
@@ -913,8 +921,8 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
 
 	// perform a covariance prediction if the total delta angle has exceeded the limit
 	// or the time limit will be exceeded at the next IMU update
-	if ((_covariancePredictionDt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) 
-		|| (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) {
+	if ((_covariancePredictionDt >= (_ekf->covTimeStepMax - _ekf->dtIMU))
+	    || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) {
 		_ekf->CovariancePrediction(_covariancePredictionDt);
 		_ekf->summedDelAng.zero();
 		_ekf->summedDelVel.zero();
@@ -936,8 +944,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
 		// run the fusion step
 		_ekf->FuseVelposNED();
 
-	} 
-	else if (!_gps_initialized) {
+	} else if (!_gps_initialized) {
 
 		// force static mode
 		_ekf->staticMode = true;
@@ -961,8 +968,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
 		// run the fusion step
 		_ekf->FuseVelposNED();
 
-	} 
-	else {
+	} else {
 		_ekf->fuseVelData = false;
 		_ekf->fusePosData = false;
 	}
@@ -978,34 +984,33 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
 		// run the fusion step
 		_ekf->FuseVelposNED();
 
-	} 
-	else {
+	} else {
 		_ekf->fuseHgtData = false;
 	}
 
 	// Fuse Magnetometer Measurements
 	if (fuseMag) {
 		_ekf->fuseMagData = true;
-		_ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
+		_ekf->RecallStates(_ekf->statesAtMagMeasTime,
+				   (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
 
 		_ekf->magstate.obsIndex = 0;
 		_ekf->FuseMagnetometer();
 		_ekf->FuseMagnetometer();
 		_ekf->FuseMagnetometer();
 
-	} 
-	else {
+	} else {
 		_ekf->fuseMagData = false;
 	}
 
 	// Fuse Airspeed Measurements
 	if (fuseAirSpeed && _ekf->VtasMeas > 7.0f) {
 		_ekf->fuseVtasData = true;
-		_ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
+		_ekf->RecallStates(_ekf->statesAtVtasMeasTime,
+				   (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
 		_ekf->FuseAirspeed();
 
-	} 
-	else {
+	} else {
 		_ekf->fuseVtasData = false;
 	}
 
@@ -1056,53 +1061,74 @@ void AttitudePositionEstimatorEKF::print_status()
 	// 4-6: Velocity - m/sec (North, East, Down)
 	// 7-9: Position - m (North, East, Down)
 	// 10-12: Delta Angle bias - rad (X,Y,Z)
-	// 13:    Accelerometer offset
+	// 13:    Delta Velocity Bias - m/s (Z)
 	// 14-15: Wind Vector  - m/sec (North,East)
 	// 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
 	// 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
 
 	printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec);
 	printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f));
-	printf("ref alt: %8.4f baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref, (double)_baro_ref_offset, (double)_baro_gps_offset);
-	printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
-	printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z);
-	printf("states (quat)        [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]);
-	printf("states (vel m/s)     [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]);
-	printf("states (pos m)      [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]);
-	printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]);
+	printf("ref alt: %8.4f baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref, (double)_baro_ref_offset,
+	       (double)_baro_gps_offset);
+	printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y,
+	       (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
+	printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y,
+	       (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y,
+	       (double)_ekf->correctedDelAng.z);
+	printf("states (quat)        [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1],
+	       (double)_ekf->states[2], (double)_ekf->states[3]);
+	printf("states (vel m/s)     [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5],
+	       (double)_ekf->states[6]);
+	printf("states (pos m)      [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8],
+	       (double)_ekf->states[9]);
+	printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11],
+	       (double)_ekf->states[12]);
 
 	if (EKF_STATE_ESTIMATES == 23) {
 		printf("states (accel offs)   [13]: %8.4f\n", (double)_ekf->states[13]);
 		printf("states (wind)      [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]);
-		printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]);
-		printf("states (body mag)  [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], (double)_ekf->states[21]);
+		printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17],
+		       (double)_ekf->states[18]);
+		printf("states (body mag)  [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20],
+		       (double)_ekf->states[21]);
 		printf("states (terrain)      [22]: %8.4f\n", (double)_ekf->states[22]);
 
 	} else {
 		printf("states (wind)      [13-14]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]);
-		printf("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]);
-		printf("states (body mag)  [18-20]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]);
+		printf("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16],
+		       (double)_ekf->states[17]);
+		printf("states (body mag)  [18-20]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19],
+		       (double)_ekf->states[20]);
 	}
+
 	printf("states: %s %s %s %s %s %s %s %s %s %s\n",
-		       (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT",
-		       (_landDetector.landed) ? "ON_GROUND" : "AIRBORNE",
-		       (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL",
-		       (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS",
-		       (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
-		       (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG",
-		       (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS",
-		       (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
-		       (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
-		       (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
+	       (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT",
+	       (_landDetector.landed) ? "ON_GROUND" : "AIRBORNE",
+	       (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL",
+	       (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS",
+	       (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
+	       (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG",
+	       (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS",
+	       (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
+	       (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
+	       (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
 }
 
 void AttitudePositionEstimatorEKF::pollData()
 {
+	//Update arming status
+	bool armedUpdate;
+	orb_check(_armedSub, &armedUpdate);
+
+	if (armedUpdate) {
+		orb_copy(ORB_ID(actuator_armed), _armedSub, &_armed);
+	}
+
+	//Update Gyro and Accelerometer
 	static Vector3f lastAngRate;
 	static Vector3f lastAccel;
 	bool accel_updated = false;
 
-	//Update Gyro and Accelerometer
 	orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
 
 	static hrt_abstime last_accel = 0;
@@ -1110,6 +1136,7 @@ void AttitudePositionEstimatorEKF::pollData()
 
 	if (last_accel != _sensor_combined.accelerometer_timestamp) {
 		accel_updated = true;
+
 	} else {
 		accel_updated = false;
 	}
@@ -1138,9 +1165,9 @@ void AttitudePositionEstimatorEKF::pollData()
 	int last_gyro_main = _gyro_main;
 
 	if (isfinite(_sensor_combined.gyro_rad_s[0]) &&
-		isfinite(_sensor_combined.gyro_rad_s[1]) &&
-		isfinite(_sensor_combined.gyro_rad_s[2]) &&
-		(_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) {
+	    isfinite(_sensor_combined.gyro_rad_s[1]) &&
+	    isfinite(_sensor_combined.gyro_rad_s[2]) &&
+	    (_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) {
 
 		_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
 		_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
@@ -1149,8 +1176,8 @@ void AttitudePositionEstimatorEKF::pollData()
 		_gyro_valid = true;
 
 	} else if (isfinite(_sensor_combined.gyro1_rad_s[0]) &&
-		isfinite(_sensor_combined.gyro1_rad_s[1]) &&
-		isfinite(_sensor_combined.gyro1_rad_s[2])) {
+		   isfinite(_sensor_combined.gyro1_rad_s[1]) &&
+		   isfinite(_sensor_combined.gyro1_rad_s[2])) {
 
 		_ekf->angRate.x = _sensor_combined.gyro1_rad_s[0];
 		_ekf->angRate.y = _sensor_combined.gyro1_rad_s[1];
@@ -1169,6 +1196,7 @@ void AttitudePositionEstimatorEKF::pollData()
 	if (!_gyro_valid) {
 		/* keep last value if he hit an out of band value */
 		lastAngRate = _ekf->angRate;
+
 	} else {
 		perf_count(_perf_gyro);
 	}
@@ -1183,6 +1211,7 @@ void AttitudePositionEstimatorEKF::pollData()
 			_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
 			_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
 			_accel_main = 0;
+
 		} else {
 			_ekf->accel.x = _sensor_combined.accelerometer1_m_s2[0];
 			_ekf->accel.y = _sensor_combined.accelerometer1_m_s2[1];
@@ -1220,12 +1249,14 @@ void AttitudePositionEstimatorEKF::pollData()
 	//Update Land Detector
 	bool newLandData;
 	orb_check(_landDetectorSub, &newLandData);
-	if(newLandData) {
+
+	if (newLandData) {
 		orb_copy(ORB_ID(vehicle_land_detected), _landDetectorSub, &_landDetector);
 	}
 
 	//Update AirSpeed
 	orb_check(_airspeed_sub, &_newAdsData);
+
 	if (_newAdsData) {
 		orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
 		perf_count(_perf_airspeed);
@@ -1235,6 +1266,7 @@ void AttitudePositionEstimatorEKF::pollData()
 
 
 	orb_check(_gps_sub, &_newDataGps);
+
 	if (_newDataGps) {
 
 		orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
@@ -1242,57 +1274,64 @@ void AttitudePositionEstimatorEKF::pollData()
 
 		//We are more strict for our first fix
 		float requiredAccuracy = _parameters.pos_stddev_threshold;
-		if(_gpsIsGood) {
+
+		if (_gpsIsGood) {
 			requiredAccuracy = _parameters.pos_stddev_threshold * 2.0f;
 		}
 
 		//Check if the GPS fix is good enough for us to use
-		if(_gps.fix_type >= 3 && _gps.eph < requiredAccuracy && _gps.epv < requiredAccuracy) {
+		if (_gps.fix_type >= 3 && _gps.eph < requiredAccuracy && _gps.epv < requiredAccuracy) {
 			_gpsIsGood = true;
-		}
-		else {
+
+		} else {
 			_gpsIsGood = false;
 		}
 
 		if (_gpsIsGood) {
 
 			//Calculate time since last good GPS fix
-			const float dtGoodGPS = static_cast<float>(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f;
+			const float dtLastGoodGPS = static_cast<float>(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f;
 
-			_ekf->updateDtGpsFilt(math::constrain(dtGoodGPS, 0.01f, POS_RESET_THRESHOLD));
+			//Stop dead-reckoning mode
+			if (_global_pos.dead_reckoning) {
+				mavlink_log_info(_mavlink_fd, "[ekf] stop dead-reckoning");
+			}
 
-			/* fuse GPS updates */
+			_global_pos.dead_reckoning = false;
 
-			//_gps.timestamp / 1e3;
+			//Fetch new GPS data
 			_ekf->GPSstatus = _gps.fix_type;
 			_ekf->velNED[0] = _gps.vel_n_m_s;
 			_ekf->velNED[1] = _gps.vel_e_m_s;
 			_ekf->velNED[2] = _gps.vel_d_m_s;
 
-			// warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]);
-
 			_ekf->gpsLat = math::radians(_gps.lat / (double)1e7);
 			_ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
 			_ekf->gpsHgt = _gps.alt / 1e3f;
 
+			if (_previousGPSTimestamp != 0) {
+				//Calculate average time between GPS updates
+				_ekf->updateDtGpsFilt(math::constrain(dtLastGoodGPS, 0.01f, POS_RESET_THRESHOLD));
+
+				// update LPF
+				_gps_alt_filt += (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt);
+			}
+
 			//check if we had a GPS outage for a long time
-			if(_gps_initialized) {
+			if (_gps_initialized) {
 
 				//Convert from global frame to local frame
-				float posNED[3] = {0.0f, 0.0f, 0.0f}; 
+				float posNED[3] = {0.0f, 0.0f, 0.0f};
 				_ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef);
 				_ekf->posNE[0] = posNED[0];
 				_ekf->posNE[1] = posNED[1];
 
-				if (dtGoodGPS > POS_RESET_THRESHOLD) {
+				if (dtLastGoodGPS > POS_RESET_THRESHOLD) {
 					_ekf->ResetPosition();
 					_ekf->ResetVelocity();
 				}
 			}
 
-			// update LPF
-			_gps_alt_filt += (dtGoodGPS / (rc + dtGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt);
-
 			//warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)dtGoodGPS);
 
 			// if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) {
@@ -1310,8 +1349,8 @@ void AttitudePositionEstimatorEKF::pollData()
 			// warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m);
 
 			_previousGPSTimestamp = _gps.timestamp_position;
-		}
-		else {
+
+		} else {
 			//Too poor GPS fix to use
 			_newDataGps = false;
 		}
@@ -1319,8 +1358,30 @@ void AttitudePositionEstimatorEKF::pollData()
 
 	// If it has gone more than POS_RESET_THRESHOLD amount of seconds since we received a GPS update,
 	// then something is very wrong with the GPS (possibly a hardware failure or comlink error)
-	else if( (static_cast<float>(hrt_elapsed_time(&_gps.timestamp_position)) / 1e6f) > POS_RESET_THRESHOLD) {
+	const float dtLastGoodGPS = static_cast<float>(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f;
+
+	if (dtLastGoodGPS >= POS_RESET_THRESHOLD) {
+
+		if (_global_pos.dead_reckoning) {
+			mavlink_log_info(_mavlink_fd, "[ekf] gave up dead-reckoning after long timeout");
+		}
+
 		_gpsIsGood = false;
+		_global_pos.dead_reckoning = false;
+	}
+
+	//If we have no good GPS fix for half a second, then enable dead-reckoning mode while armed (for up to POS_RESET_THRESHOLD seconds)
+	else if (dtLastGoodGPS >= 0.5f) {
+		if (_armed.armed) {
+			if (!_global_pos.dead_reckoning) {
+				mavlink_log_info(_mavlink_fd, "[ekf] dead-reckoning enabled");
+			}
+
+			_global_pos.dead_reckoning = true;
+
+		} else {
+			_global_pos.dead_reckoning = false;
+		}
 	}
 
 	//Update barometer
@@ -1333,18 +1394,20 @@ void AttitudePositionEstimatorEKF::pollData()
 
 		// init lowpass filters for baro and gps altitude
 		float baro_elapsed;
-		if(baro_last == 0) {
+
+		if (baro_last == 0) {
 			baro_elapsed = 0.0f;
-		}
-		else {
+
+		} else {
 			baro_elapsed = (_baro.timestamp - baro_last) / 1e6f;
 		}
+
 		baro_last = _baro.timestamp;
 
 		_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f));
 
 		_ekf->baroHgt = _baro.altitude;
-		_baro_alt_filt += (baro_elapsed/(rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt);
+		_baro_alt_filt += (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt);
 
 		if (!_baro_init) {
 			_baro_ref = _baro.altitude;
@@ -1379,6 +1442,7 @@ void AttitudePositionEstimatorEKF::pollData()
 			_ekf->magData.z = _sensor_combined.magnetometer_ga[2];
 			_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
 			_mag_main = 0;
+
 		} else {
 			_ekf->magData.x = _sensor_combined.magnetometer1_ga[0];
 			_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
@@ -1398,31 +1462,30 @@ void AttitudePositionEstimatorEKF::pollData()
 
 	//Update range data
 	orb_check(_distance_sub, &_newRangeData);
+
 	if (_newRangeData) {
 		orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance);
 
 		if (_distance.valid) {
 			_ekf->rngMea = _distance.distance;
 			_distance_last_valid = _distance.timestamp;
+
 		} else {
 			_newRangeData = false;
 		}
 	}
 }
 
-int AttitudePositionEstimatorEKF::trip_nan() {
+int AttitudePositionEstimatorEKF::trip_nan()
+{
 
 	int ret = 0;
 
 	// If system is not armed, inject a NaN value into the filter
-	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
-
-	struct actuator_armed_s armed;
-	orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
-
-	if (armed.armed) {
+	if (_armed.armed) {
 		warnx("ACTUATORS ARMED! NOT TRIPPING SYSTEM");
 		ret = 1;
+
 	} else {
 
 		float nan_val = 0.0f / 0.0f;
@@ -1455,24 +1518,26 @@ int AttitudePositionEstimatorEKF::trip_nan() {
 		print_status();
 	}
 
-	close(armed_sub);
 	return ret;
 }
 
 int ekf_att_pos_estimator_main(int argc, char *argv[])
 {
-	if (argc < 1)
+	if (argc < 1) {
 		errx(1, "usage: ekf_att_pos_estimator {start|stop|status|logging}");
+	}
 
 	if (!strcmp(argv[1], "start")) {
 
-		if (estimator::g_estimator != nullptr)
+		if (estimator::g_estimator != nullptr) {
 			errx(1, "already running");
+		}
 
 		estimator::g_estimator = new AttitudePositionEstimatorEKF();
 
-		if (estimator::g_estimator == nullptr)
+		if (estimator::g_estimator == nullptr) {
 			errx(1, "alloc failed");
+		}
 
 		if (OK != estimator::g_estimator->start()) {
 			delete estimator::g_estimator;
@@ -1486,14 +1551,16 @@ int ekf_att_pos_estimator_main(int argc, char *argv[])
 			printf(".");
 			fflush(stdout);
 		}
+
 		printf("\n");
 
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "stop")) {
-		if (estimator::g_estimator == nullptr)
+		if (estimator::g_estimator == nullptr) {
 			errx(1, "not running");
+		}
 
 		delete estimator::g_estimator;
 		estimator::g_estimator = nullptr;

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -520,7 +520,7 @@ FixedwingAttitudeControl::vehicle_control_mode_poll()
 	orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated);
 
 	if (vcontrol_mode_updated) {
-
+		
 		orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode);
 	}
 }
@@ -795,10 +795,10 @@ FixedwingAttitudeControl::task_main()
 			/* Simple handling of failsafe: deploy parachute if failsafe is on */
 			if (_vcontrol_mode.flag_control_termination_enabled) {
 				_actuators_airframe.control[7] = 1.0f;
-//				warnx("_actuators_airframe.control[1] = 1.0f;");
+				//warnx("_actuators_airframe.control[1] = 1.0f;");
 			} else {
 				_actuators_airframe.control[7] = 0.0f;
-//				warnx("_actuators_airframe.control[1] = -1.0f;");
+				//warnx("_actuators_airframe.control[1] = -1.0f;");
 			}
 
 			/* decide if in stabilized or full manual control */
@@ -1077,20 +1077,25 @@ FixedwingAttitudeControl::task_main()
 			_actuators_airframe.timestamp = hrt_absolute_time();
 			_actuators_airframe.timestamp_sample = _att.timestamp;
 
-			/* publish the actuator controls */
-			if (_actuators_0_pub > 0) {
-				orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
-			} else if (_actuators_id) {
-				_actuators_0_pub= orb_advertise(_actuators_id, &_actuators);
-			}
+			/* Only publish if any of the proper modes are enabled */
+			if(_vcontrol_mode.flag_control_rates_enabled ||
+			   _vcontrol_mode.flag_control_attitude_enabled)
+			{
+				/* publish the actuator controls */
+				if (_actuators_0_pub > 0) {
+					orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
+				} else if (_actuators_id) {
+					_actuators_0_pub= orb_advertise(_actuators_id, &_actuators);
+				}
 
-			if (_actuators_2_pub > 0) {
-				/* publish the actuator controls*/
-				orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe);
+				if (_actuators_2_pub > 0) {
+					/* publish the actuator controls*/
+					orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe);
 
-			} else {
-				/* advertise and publish */
-				_actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe);
+				} else {
+					/* advertise and publish */
+					_actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe);
+				}
 			}
 		}
 
@@ -1114,7 +1119,7 @@ FixedwingAttitudeControl::start()
 	_control_task = task_spawn_cmd("fw_att_control",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       2048,
+				       1600,
 				       (main_t)&FixedwingAttitudeControl::task_main_trampoline,
 				       nullptr);
 

src/modules/fw_att_control/fw_att_control_params.c

@@ -302,10 +302,10 @@ PARAM_DEFINE_INT32(FW_YCO_METHOD, 0);
  *
  * @unit m/s
  * @min 0.0
- * @max 30.0
+ * @max 40
  * @group FW Attitude Control
  */
-PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 13.0f);
+PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f);
 
 /**
  * Trim Airspeed
@@ -314,10 +314,10 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 13.0f);
  *
  * @unit m/s
  * @min 0.0
- * @max 30.0
+ * @max 40
  * @group FW Attitude Control
  */
-PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f);
+PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f);
 
 /**
  * Maximum Airspeed
@@ -327,10 +327,10 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f);
  *
  * @unit m/s
  * @min 0.0
- * @max 30.0
+ * @max 40
  * @group FW Attitude Control
  */
-PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 50.0f);
+PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f);
 
 /**
  * Roll Setpoint Offset

src/modules/fw_att_control/module.mk

@@ -40,6 +40,8 @@ MODULE_COMMAND	= fw_att_control
 SRCS		= fw_att_control_main.cpp \
 		  fw_att_control_params.c
 
+# Startup handler, the actual app stack size is
+# in the task_spawn command
 MODULE_STACKSIZE = 1200
 
 MAXOPTIMIZATION = -Os

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -1624,7 +1624,7 @@ FixedwingPositionControl::start()
 	_control_task = task_spawn_cmd("fw_pos_control_l1",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       2000,
+				       1600,
 				       (main_t)&FixedwingPositionControl::task_main_trampoline,
 				       nullptr);
 

src/modules/fw_pos_control_l1/module.mk

@@ -44,6 +44,8 @@ SRCS		= fw_pos_control_l1_main.cpp \
 		  mtecs/limitoverride.cpp \
 		  mtecs/mTecs_params.c
 
+# Startup handler, the actual app stack size is
+# in the task_spawn command
 MODULE_STACKSIZE = 1200
 
 MAXOPTIMIZATION	 = -Os

src/modules/gpio_led/gpio_led.c

@@ -66,7 +66,7 @@ struct gpio_led_s {
 	int counter;
 };
 
-static struct gpio_led_s gpio_led_data;
+static struct gpio_led_s *gpio_led_data;
 static bool gpio_led_started = false;
 
 __EXPORT int gpio_led_main(int argc, char *argv[]);
@@ -170,10 +170,11 @@ int gpio_led_main(int argc, char *argv[])
 				}
 			}
 
-			memset(&gpio_led_data, 0, sizeof(gpio_led_data));
-			gpio_led_data.use_io = use_io;
-			gpio_led_data.pin = pin;
-			int ret = work_queue(LPWORK, &gpio_led_data.work, gpio_led_start, &gpio_led_data, 0);
+			gpio_led_data = malloc(sizeof(struct gpio_led_s));
+			memset(gpio_led_data, 0, sizeof(struct gpio_led_s));
+			gpio_led_data->use_io = use_io;
+			gpio_led_data->pin = pin;
+			int ret = work_queue(LPWORK, &(gpio_led_data->work), gpio_led_start, gpio_led_data, 0);
 
 			if (ret != 0) {
 				errx(1, "failed to queue work: %d", ret);

src/modules/land_detector/land_detector_main.cpp

@@ -139,7 +139,7 @@ static int land_detector_start(const char *mode)
 	_landDetectorTaskID = task_spawn_cmd("land_detector",
 					     SCHED_DEFAULT,
 					     SCHED_PRIORITY_DEFAULT,
-					     1200,
+					     1000,
 					     (main_t)&land_detector_deamon_thread,
 					     nullptr);
 
@@ -179,8 +179,7 @@ int land_detector_main(int argc, char *argv[])
 {
 
 	if (argc < 1) {
-		warnx("usage: land_detector {start|stop|status} [mode]\nmode can either be 'fixedwing' or 'multicopter'");
-		exit(0);
+		goto exiterr;
 	}
 
 	if (argc >= 2 && !strcmp(argv[1], "start")) {
@@ -209,6 +208,8 @@ int land_detector_main(int argc, char *argv[])
 		}
 	}
 
-	warn("usage: land_detector {start|stop|status} [mode]\nmode can either be 'fixedwing' or 'multicopter'");
+exiterr:
+	warnx("usage: land_detector {start|stop|status} [mode]");
+	warnx("mode can either be 'fixedwing' or 'multicopter'");
 	return 1;
 }

src/modules/land_detector/module.mk

@@ -11,3 +11,7 @@ SRCS		= land_detector_main.cpp \
               FixedwingLandDetector.cpp
 
 EXTRACXXFLAGS   = -Weffc++ -Os
+
+# Startup handler, the actual app stack size is
+# in the task_spawn command
+MODULE_STACKSIZE = 1200

src/modules/mavlink/mavlink_main.cpp

@@ -1621,7 +1621,7 @@ Mavlink::start(int argc, char *argv[])
 	task_spawn_cmd(buf,
 		       SCHED_DEFAULT,
 		       SCHED_PRIORITY_DEFAULT,
-		       2800,
+		       2400,
 		       (main_t)&Mavlink::start_helper,
 		       (char * const *)argv);
 

src/modules/mavlink/mavlink_messages.cpp

@@ -51,7 +51,6 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/offboard_control_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_vicon_position.h>

src/modules/mavlink/mavlink_receiver.cpp

@@ -108,7 +108,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_cmd_pub(-1),
 	_flow_pub(-1),
 	_range_pub(-1),
-	_offboard_control_sp_pub(-1),
+	_offboard_control_mode_pub(-1),
+	_actuator_controls_pub(-1),
 	_global_vel_sp_pub(-1),
 	_att_sp_pub(-1),
 	_rates_sp_pub(-1),
@@ -126,6 +127,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_hil_local_proj_inited(0),
 	_hil_local_alt0(0.0f),
 	_hil_local_proj_ref{},
+	_offboard_control_mode{},
+	_rates_sp{},
 	_time_offset_avg_alpha(0.6),
 	_time_offset(0)
 {
@@ -170,6 +173,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_set_attitude_target(msg);
 		break;
 
+	case MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET:
+		handle_message_set_actuator_control_target(msg);
+		break;
+
 	case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
 		handle_message_vision_position_estimate(msg);
 		break;
@@ -517,8 +524,8 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 	mavlink_set_position_target_local_ned_t set_position_target_local_ned;
 	mavlink_msg_set_position_target_local_ned_decode(msg, &set_position_target_local_ned);
 
-	struct offboard_control_setpoint_s offboard_control_sp;
-	memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));//XXX breaks compatibility with multiple setpoints
+	struct offboard_control_mode_s offboard_control_mode;
+	memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints
 
 	/* Only accept messages which are intended for this system */
 	if ((mavlink_system.sysid == set_position_target_local_ned.target_system ||
@@ -527,64 +534,22 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 			 set_position_target_local_ned.target_component == 0)) {
 
 		/* convert mavlink type (local, NED) to uORB offboard control struct */
-		switch (set_position_target_local_ned.coordinate_frame) {
-			case MAV_FRAME_LOCAL_NED:
-				offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED;
-				break;
-			case MAV_FRAME_LOCAL_OFFSET_NED:
-				offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED;
-				break;
-			case MAV_FRAME_BODY_NED:
-				offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED;
-				break;
-			case MAV_FRAME_BODY_OFFSET_NED:
-				offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED;
-				break;
-			default:
-				/* invalid setpoint, avoid publishing */
-				return;
-		}
-		offboard_control_sp.position[0] = set_position_target_local_ned.x;
-		offboard_control_sp.position[1] = set_position_target_local_ned.y;
-		offboard_control_sp.position[2] = set_position_target_local_ned.z;
-		offboard_control_sp.velocity[0] = set_position_target_local_ned.vx;
-		offboard_control_sp.velocity[1] = set_position_target_local_ned.vy;
-		offboard_control_sp.velocity[2] = set_position_target_local_ned.vz;
-		offboard_control_sp.acceleration[0] = set_position_target_local_ned.afx;
-		offboard_control_sp.acceleration[1] = set_position_target_local_ned.afy;
-		offboard_control_sp.acceleration[2] = set_position_target_local_ned.afz;
-		offboard_control_sp.yaw = set_position_target_local_ned.yaw;
-		offboard_control_sp.yaw_rate = set_position_target_local_ned.yaw_rate;
-		offboard_control_sp.isForceSetpoint = (bool)(set_position_target_local_ned.type_mask & (1 << 9));
+		offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7);
+		offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask & 0x38);
+		offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask & 0x1C0);
+		bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9));
+		/* yaw ignore flag mapps to ignore_attitude */
+		offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400);
+		/* yawrate ignore flag mapps to ignore_bodyrate */
+		offboard_control_mode.ignore_bodyrate = (bool)(set_position_target_local_ned.type_mask & 0x800);
 
-		/* If we are in force control mode, for now set offboard mode to force control */
-		if (offboard_control_sp.isForceSetpoint) {
-			offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_FORCE;
-		}
+		offboard_control_mode.timestamp = hrt_absolute_time();
 
-		/* set ignore flags */
-		for (int i = 0; i < 9; i++) {
-			offboard_control_sp.ignore &=  ~(1 << i);
-			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << i));
-		}
-
-		offboard_control_sp.ignore &=  ~(1 << OFB_IGN_BIT_YAW);
-		if (set_position_target_local_ned.type_mask & (1 << 10)) {
-			offboard_control_sp.ignore |=  (1 << OFB_IGN_BIT_YAW);
-		}
-
-		offboard_control_sp.ignore &=  ~(1 << OFB_IGN_BIT_YAWRATE);
-		if (set_position_target_local_ned.type_mask & (1 << 11)) {
-			offboard_control_sp.ignore |=  (1 << OFB_IGN_BIT_YAWRATE);
-		}
-
-		offboard_control_sp.timestamp = hrt_absolute_time();
-
-		if (_offboard_control_sp_pub < 0) {
-			_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
+		if (_offboard_control_mode_pub < 0) {
+			_offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode);
 
 		} else {
-			orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
+			orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode);
 		}
 
 		/* If we are in offboard control mode and offboard control loop through is enabled
@@ -596,15 +561,14 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 				orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
 			}
 			if (_control_mode.flag_control_offboard_enabled) {
-				if (offboard_control_sp.isForceSetpoint &&
-						offboard_control_sp_ignore_position_all(offboard_control_sp) &&
-						offboard_control_sp_ignore_velocity_all(offboard_control_sp)) {
+				if (is_force_sp && offboard_control_mode.ignore_position &&
+						offboard_control_mode.ignore_velocity) {
 					/* The offboard setpoint is a force setpoint only, directly writing to the force
 					 * setpoint topic and not publishing the setpoint triplet topic */
 					struct vehicle_force_setpoint_s	force_sp;
-					force_sp.x = offboard_control_sp.acceleration[0];
-					force_sp.y = offboard_control_sp.acceleration[1];
-					force_sp.z = offboard_control_sp.acceleration[2];
+					force_sp.x = set_position_target_local_ned.afx;
+					force_sp.y = set_position_target_local_ned.afy;
+					force_sp.z = set_position_target_local_ned.afz;
 					//XXX: yaw
 					if (_force_sp_pub < 0) {
 						_force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp);
@@ -619,62 +583,53 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 					pos_sp_triplet.current.valid = true;
 					pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; //XXX support others
 
-					/* set the local pos values if the setpoint type is 'local pos' and none
-					 * of the local pos fields is set to 'ignore' */
-					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
-							!offboard_control_sp_ignore_position_some(offboard_control_sp)) {
-					pos_sp_triplet.current.position_valid = true;
-					pos_sp_triplet.current.x = offboard_control_sp.position[0];
-					pos_sp_triplet.current.y = offboard_control_sp.position[1];
-					pos_sp_triplet.current.z = offboard_control_sp.position[2];
+					/* set the local pos values */
+					if (!offboard_control_mode.ignore_position) {
+						pos_sp_triplet.current.position_valid = true;
+						pos_sp_triplet.current.x = set_position_target_local_ned.x;
+						pos_sp_triplet.current.y = set_position_target_local_ned.y;
+						pos_sp_triplet.current.z = set_position_target_local_ned.z;
 					} else {
 						pos_sp_triplet.current.position_valid = false;
 					}
 
-					/* set the local vel values if the setpoint type is 'local pos' and none
-					 * of the local vel fields is set to 'ignore' */
-					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
-							!offboard_control_sp_ignore_velocity_some(offboard_control_sp)) {
+					/* set the local vel values */
+					if (!offboard_control_mode.ignore_velocity) {
 						pos_sp_triplet.current.velocity_valid = true;
-						pos_sp_triplet.current.vx = offboard_control_sp.velocity[0];
-						pos_sp_triplet.current.vy = offboard_control_sp.velocity[1];
-						pos_sp_triplet.current.vz = offboard_control_sp.velocity[2];
+						pos_sp_triplet.current.vx = set_position_target_local_ned.vx;
+						pos_sp_triplet.current.vy = set_position_target_local_ned.vy;
+						pos_sp_triplet.current.vz = set_position_target_local_ned.vz;
 					} else {
 						pos_sp_triplet.current.velocity_valid = false;
 					}
 
 					/* set the local acceleration values if the setpoint type is 'local pos' and none
 					 * of the accelerations fields is set to 'ignore' */
-					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
-							!offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) {
+					if (!offboard_control_mode.ignore_acceleration_force) {
 						pos_sp_triplet.current.acceleration_valid = true;
-						pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0];
-						pos_sp_triplet.current.a_y = offboard_control_sp.acceleration[1];
-						pos_sp_triplet.current.a_z = offboard_control_sp.acceleration[2];
+						pos_sp_triplet.current.a_x = set_position_target_local_ned.afx;
+						pos_sp_triplet.current.a_y = set_position_target_local_ned.afy;
+						pos_sp_triplet.current.a_z = set_position_target_local_ned.afz;
 						pos_sp_triplet.current.acceleration_is_force =
-						offboard_control_sp.isForceSetpoint;
+						is_force_sp;
 
 					} else {
 						pos_sp_triplet.current.acceleration_valid = false;
 					}
 
-					/* set the yaw sp value if the setpoint type is 'local pos' and the yaw
-					 * field is not set to 'ignore' */
-					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
-							!offboard_control_sp_ignore_yaw(offboard_control_sp)) {
+					/* set the yaw sp value */
+					if (!offboard_control_mode.ignore_attitude) {
 						pos_sp_triplet.current.yaw_valid = true;
-						pos_sp_triplet.current.yaw = offboard_control_sp.yaw;
+						pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw;
 
 					} else {
 						pos_sp_triplet.current.yaw_valid = false;
 					}
 
-					/* set the yawrate sp value if the setpoint type is 'local pos' and the yawrate
-					 * field is not set to 'ignore' */
-					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
-							!offboard_control_sp_ignore_yawrate(offboard_control_sp)) {
+					/* set the yawrate sp value */
+					if (!offboard_control_mode.ignore_bodyrate) {
 						pos_sp_triplet.current.yawspeed_valid = true;
-						pos_sp_triplet.current.yawspeed = offboard_control_sp.yaw_rate;
+						pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate;
 
 					} else {
 						pos_sp_triplet.current.yawspeed_valid = false;
@@ -698,6 +653,66 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 	}
 }
 
+void
+MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *msg)
+{
+	mavlink_set_actuator_control_target_t set_actuator_control_target;
+	mavlink_msg_set_actuator_control_target_decode(msg, &set_actuator_control_target);
+
+	struct offboard_control_mode_s offboard_control_mode;
+	memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints
+
+	struct actuator_controls_s actuator_controls;
+	memset(&actuator_controls, 0, sizeof(actuator_controls));//XXX breaks compatibility with multiple setpoints
+
+	if ((mavlink_system.sysid == set_actuator_control_target.target_system ||
+		    set_actuator_control_target.target_system == 0) &&
+		(mavlink_system.compid == set_actuator_control_target.target_component ||
+		    set_actuator_control_target.target_component == 0)) {
+
+		/* ignore all since we are setting raw actuators here */
+		offboard_control_mode.ignore_thrust             = true;
+		offboard_control_mode.ignore_attitude           = true;
+		offboard_control_mode.ignore_bodyrate           = true;
+		offboard_control_mode.ignore_position           = true;
+		offboard_control_mode.ignore_velocity           = true;
+		offboard_control_mode.ignore_acceleration_force = true;
+
+		offboard_control_mode.timestamp = hrt_absolute_time();
+
+		if (_offboard_control_mode_pub < 0) {
+			_offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode);
+		} else {
+			orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode);
+		}
+
+
+		/* If we are in offboard control mode, publish the actuator controls */
+		bool updated;
+		orb_check(_control_mode_sub, &updated);
+		if (updated) {
+			orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
+		}
+
+		if (_control_mode.flag_control_offboard_enabled) {
+
+			actuator_controls.timestamp = hrt_absolute_time();
+
+			/* Set duty cycles for the servos in actuator_controls_0 */
+			for(size_t i = 0; i < 8; i++) {
+				actuator_controls.control[i] = set_actuator_control_target.controls[i];
+			}
+
+			if (_actuator_controls_pub < 0) {
+				_actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls);
+			} else {
+				orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pub, &actuator_controls);
+			}
+		}
+	}
+
+}
+
 void
 MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 {
@@ -743,42 +758,50 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 	mavlink_set_attitude_target_t set_attitude_target;
 	mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target);
 
-	struct offboard_control_setpoint_s offboard_control_sp;
-	memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); //XXX breaks compatibility with multiple setpoints
-
 	/* Only accept messages which are intended for this system */
 	if ((mavlink_system.sysid == set_attitude_target.target_system ||
 				set_attitude_target.target_system == 0) &&
 			(mavlink_system.compid == set_attitude_target.target_component ||
 			 set_attitude_target.target_component == 0)) {
-		for (int i = 0; i < 4; i++) {
-			offboard_control_sp.attitude[i] = set_attitude_target.q[i];
-		}
-		offboard_control_sp.attitude_rate[0] = set_attitude_target.body_roll_rate;
-		offboard_control_sp.attitude_rate[1] = set_attitude_target.body_pitch_rate;
-		offboard_control_sp.attitude_rate[2] = set_attitude_target.body_yaw_rate;
 
-		/* set correct ignore flags for body rate fields: copy from mavlink message */
-		for (int i = 0; i < 3; i++) {
-			offboard_control_sp.ignore &=  ~(1 << (i + OFB_IGN_BIT_BODYRATE_X));
-			offboard_control_sp.ignore |=  (set_attitude_target.type_mask & (1 << i)) << OFB_IGN_BIT_BODYRATE_X;
-		}
 		/* set correct ignore flags for thrust field: copy from mavlink message */
-		offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_THRUST);
-		offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 6)) << OFB_IGN_BIT_THRUST);
-		/* set correct ignore flags for attitude field: copy from mavlink message */
-		offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_ATT);
-		offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 7)) << OFB_IGN_BIT_ATT);
+		_offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6));
 
+		/*
+		 * The tricky part in parsing this message is that the offboard sender *can* set attitude and thrust
+		 * using different messages. Eg.: First send set_attitude_target containing the attitude and ignore
+		 * bits set for everything else and then send set_attitude_target containing the thrust and ignore bits
+		 * set for everything else.
+		 */
 
-		offboard_control_sp.timestamp = hrt_absolute_time();
-		offboard_control_sp.mode =OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; //XXX handle rate control mode
+		/*
+		 * if attitude or body rate have been used (not ignored) previously and this message only sends
+		 * throttle and has the ignore bits set for attitude and rates don't change the flags for attitude and
+		 * body rates to keep the controllers running
+		 */
+		bool ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7);
+		bool ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7));
 
-		if (_offboard_control_sp_pub < 0) {
-			_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
+		if (ignore_bodyrate && ignore_attitude && !_offboard_control_mode.ignore_thrust) {
+			/* Message want's us to ignore everything except thrust: only ignore if previously ignored */
+			_offboard_control_mode.ignore_bodyrate = ignore_bodyrate && _offboard_control_mode.ignore_bodyrate;
+			_offboard_control_mode.ignore_attitude = ignore_attitude && _offboard_control_mode.ignore_attitude;
+		} else {
+			_offboard_control_mode.ignore_bodyrate = ignore_bodyrate;
+			_offboard_control_mode.ignore_attitude = ignore_attitude;
+		}
+
+		_offboard_control_mode.ignore_position = true;
+		_offboard_control_mode.ignore_velocity = true;
+		_offboard_control_mode.ignore_acceleration_force = true;
+
+		_offboard_control_mode.timestamp = hrt_absolute_time();
+
+		if (_offboard_control_mode_pub < 0) {
+			_offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &_offboard_control_mode);
 
 		} else {
-			orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
+			orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &_offboard_control_mode);
 		}
 
 		/* If we are in offboard control mode and offboard control loop through is enabled
@@ -793,15 +816,16 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 			if (_control_mode.flag_control_offboard_enabled) {
 
 				/* Publish attitude setpoint if attitude and thrust ignore bits are not set */
-				if (!(offboard_control_sp_ignore_attitude(offboard_control_sp) ||
-							offboard_control_sp_ignore_thrust(offboard_control_sp))) {
-					struct vehicle_attitude_setpoint_s att_sp;
+				if (!(_offboard_control_mode.ignore_attitude)) {
+					static struct vehicle_attitude_setpoint_s att_sp = {};
 					att_sp.timestamp = hrt_absolute_time();
 					mavlink_quaternion_to_euler(set_attitude_target.q,
 							&att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body);
 					mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body);
 					att_sp.R_valid = true;
-					att_sp.thrust = set_attitude_target.thrust;
+					if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
+						att_sp.thrust = set_attitude_target.thrust;
+					}
 					att_sp.yaw_sp_move_rate = 0.0;
 					memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d));
 					att_sp.q_d_valid = true;
@@ -814,19 +838,19 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 
 				/* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */
 				///XXX add support for ignoring individual axes
-				if (!(offboard_control_sp_ignore_bodyrates_some(offboard_control_sp) ||
-							offboard_control_sp_ignore_thrust(offboard_control_sp))) {
-					struct vehicle_rates_setpoint_s rates_sp;
-					rates_sp.timestamp = hrt_absolute_time();
-					rates_sp.roll = set_attitude_target.body_roll_rate;
-					rates_sp.pitch = set_attitude_target.body_pitch_rate;
-					rates_sp.yaw = set_attitude_target.body_yaw_rate;
-					rates_sp.thrust = set_attitude_target.thrust;
+				if (!(_offboard_control_mode.ignore_bodyrate)) {
+					_rates_sp.timestamp = hrt_absolute_time();
+					_rates_sp.roll = set_attitude_target.body_roll_rate;
+					_rates_sp.pitch = set_attitude_target.body_pitch_rate;
+					_rates_sp.yaw = set_attitude_target.body_yaw_rate;
+					if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
+						_rates_sp.thrust = set_attitude_target.thrust;
+					}
 
 					if (_att_sp_pub < 0) {
-						_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
+						_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp);
 					} else {
-						orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp);
+						orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &_rates_sp);
 					}
 				}
 			}
@@ -1524,7 +1548,7 @@ MavlinkReceiver::receive_start(Mavlink *parent)
 	param.sched_priority = SCHED_PRIORITY_MAX - 80;
 	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
 
-	pthread_attr_setstacksize(&receiveloop_attr, 2900);
+	pthread_attr_setstacksize(&receiveloop_attr, 1800);
 	pthread_t thread;
 	pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent);
 

src/modules/mavlink/mavlink_receiver.h

@@ -53,7 +53,7 @@
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/offboard_control_setpoint.h>
+#include <uORB/topics/offboard_control_mode.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_global_velocity_setpoint.h>
@@ -122,6 +122,7 @@ private:
 	void handle_message_vision_position_estimate(mavlink_message_t *msg);
 	void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
 	void handle_message_set_position_target_local_ned(mavlink_message_t *msg);
+	void handle_message_set_actuator_control_target(mavlink_message_t *msg);
 	void handle_message_set_attitude_target(mavlink_message_t *msg);
 	void handle_message_radio_status(mavlink_message_t *msg);
 	void handle_message_manual_control(mavlink_message_t *msg);
@@ -142,7 +143,7 @@ private:
 	/**
 	* Exponential moving average filter to smooth time offset
 	*/
-	void smooth_time_offset(uint64_t offset_ns);	
+	void smooth_time_offset(uint64_t offset_ns);
 
 	mavlink_status_t status;
 	struct vehicle_local_position_s hil_local_pos;
@@ -162,7 +163,8 @@ private:
 	orb_advert_t _cmd_pub;
 	orb_advert_t _flow_pub;
 	orb_advert_t _range_pub;
-	orb_advert_t _offboard_control_sp_pub;
+	orb_advert_t _offboard_control_mode_pub;
+	orb_advert_t _actuator_controls_pub;
 	orb_advert_t _global_vel_sp_pub;
 	orb_advert_t _att_sp_pub;
 	orb_advert_t _rates_sp_pub;
@@ -180,6 +182,8 @@ private:
 	bool _hil_local_proj_inited;
 	float _hil_local_alt0;
 	struct map_projection_reference_s _hil_local_proj_ref;
+	struct offboard_control_mode_s _offboard_control_mode;
+	struct vehicle_rates_setpoint_s _rates_sp;
 	double _time_offset_avg_alpha;
 	uint64_t _time_offset;
 

src/modules/mavlink/module.mk

@@ -51,7 +51,7 @@ INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
 
 MAXOPTIMIZATION	 = -Os
 
-MODULE_STACKSIZE = 1024
+MODULE_STACKSIZE = 1200
 
 EXTRACXXFLAGS	= -Weffc++ -Wno-attributes -Wno-packed
 

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -835,7 +835,7 @@ MulticopterAttitudeControl::start()
 	_control_task = task_spawn_cmd("mc_att_control",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       2000,
+				       1600,
 				       (main_t)&MulticopterAttitudeControl::task_main_trampoline,
 				       nullptr);
 

src/modules/mc_att_control/module.mk

@@ -39,3 +39,7 @@ MODULE_COMMAND	= mc_att_control
 
 SRCS		= mc_att_control_main.cpp \
 			  mc_att_control_params.c
+
+# Startup handler, the actual app stack size is
+# in the task_spawn command
+MODULE_STACKSIZE = 1200

src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp

@@ -71,7 +71,7 @@ int mc_att_control_m_main(int argc, char *argv[])
 		daemon_task = task_spawn_cmd("mc_att_control_m",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       3000,
+				       1900,
 				       main,
 					(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
 

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -761,8 +761,8 @@ void MulticopterPositionControl::control_auto(float dt)
 		orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
 
 		//Make sure that the position setpoint is valid
-		if (!isfinite(_pos_sp_triplet.current.lat) || 
-			!isfinite(_pos_sp_triplet.current.lon) || 
+		if (!isfinite(_pos_sp_triplet.current.lat) ||
+			!isfinite(_pos_sp_triplet.current.lon) ||
 			!isfinite(_pos_sp_triplet.current.alt)) {
 			_pos_sp_triplet.current.valid = false;
 		}
@@ -1367,7 +1367,7 @@ MulticopterPositionControl::task_main()
 
 				} else if (yaw_offs > YAW_OFFSET_MAX) {
 					_att_sp.yaw_body = _wrap_pi(_att.yaw + YAW_OFFSET_MAX);
-				}				
+				}
 			}
 
 			//Control roll and pitch directly if we no aiding velocity controller is active
@@ -1388,15 +1388,22 @@ MulticopterPositionControl::task_main()
 			_att_sp.timestamp = hrt_absolute_time();
 		}
 		else {
-			reset_yaw_sp = true;			
+			reset_yaw_sp = true;
 		}
 
-		// publish attitude setpoint
-		if (_att_sp_pub > 0) {
-			orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
+		/* publish attitude setpoint
+		 * Do not publish if offboard is enabled but position/velocity control is disabled,
+		 * in this case the attitude setpoint is published by the mavlink app
+		 */
+		if (!(_control_mode.flag_control_offboard_enabled &&
+					!(_control_mode.flag_control_position_enabled ||
+						_control_mode.flag_control_velocity_enabled))) {
+			if (_att_sp_pub > 0) {
+				orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
 
-		} else {
-			_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
+			} else {
+				_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
+			}
 		}
 
 		/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
@@ -1419,7 +1426,7 @@ MulticopterPositionControl::start()
 	_control_task = task_spawn_cmd("mc_pos_control",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       2000,
+				       1600,
 				       (main_t)&MulticopterPositionControl::task_main_trampoline,
 				       nullptr);
 

src/modules/mc_pos_control/module.mk

@@ -39,3 +39,7 @@ MODULE_COMMAND	= mc_pos_control
 
 SRCS		= mc_pos_control_main.cpp \
 			  mc_pos_control_params.c
+
+# Startup handler, the actual app stack size is
+# in the task_spawn command
+MODULE_STACKSIZE = 1200

src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp

@@ -1015,12 +1015,19 @@ void  MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti
 		reset_yaw_sp = true;
 	}
 
-	/* publish attitude setpoint */
-	if (_att_sp_pub != nullptr) {
-		_att_sp_pub->publish(_att_sp_msg);
+	/* publish attitude setpoint
+	 * Do not publish if offboard is enabled but position/velocity control is disabled, in this case the attitude setpoint
+	 * is published by the mavlink app
+	 */
+	if (!(_control_mode->data().flag_control_offboard_enabled &&
+				!(_control_mode->data().flag_control_position_enabled ||
+					_control_mode->data().flag_control_velocity_enabled))) {
+		if (_att_sp_pub != nullptr) {
+			_att_sp_pub->publish(_att_sp_msg);
 
-	} else {
-		_att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>();
+		} else {
+			_att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>();
+		}
 	}
 
 	/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */

src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp

@@ -71,7 +71,7 @@ int mc_pos_control_m_main(int argc, char *argv[])
 		daemon_task = task_spawn_cmd("mc_pos_control_m",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       3000,
+				       2500,
 				       main,
 					(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
 

src/modules/navigator/rtl_params.c

@@ -83,7 +83,7 @@ PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 60);
  * @max 100
  * @group RTL
  */
-PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20);
+PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 30);
 
 /**
  * RTL delay

src/modules/position_estimator_inav/position_estimator_inav_params.c

@@ -288,6 +288,20 @@ PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f);
  */
 PARAM_DEFINE_INT32(CBRK_NO_VISION, 0);
 
+/**
+ * INAV enabled
+ *
+ * If set to 1, use INAV for position estimation
+ * the system uses the compined attitude / position
+ * filter framework.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @unit s
+ * @group Position Estimator INAV
+ */
+PARAM_DEFINE_INT32(INAV_ENABLED, 0);
+
 int parameters_init(struct position_estimator_inav_param_handles *h)
 {
 	h->w_z_baro = param_find("INAV_W_Z_BARO");

src/modules/px4iofirmware/controls.c

@@ -45,6 +45,7 @@
 #include <systemlib/perf_counter.h>
 #include <systemlib/ppm_decode.h>
 #include <rc/st24.h>
+#include <rc/sumd.h>
 
 #include "px4io.h"
 
@@ -53,7 +54,7 @@
 #define RC_CHANNEL_LOW_THRESH		-8000	/* 10% threshold */
 
 static bool	ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
-static bool	dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated);
+static bool	dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated);
 
 static perf_counter_t c_gather_dsm;
 static perf_counter_t c_gather_sbus;
@@ -63,7 +64,7 @@ static int _dsm_fd;
 
 static uint16_t rc_value_override = 0;
 
-bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated)
+bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated)
 {
 	perf_begin(c_gather_dsm);
 	uint16_t temp_count = r_raw_rc_count;
@@ -106,7 +107,31 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated)
 		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
 	}
 
-	return (*dsm_updated | *st24_updated);
+
+	/* get data from FD and attempt to parse with SUMD libs */
+	uint8_t sumd_rssi, sumd_rx_count;
+	uint16_t sumd_channel_count = 0;
+
+	*sumd_updated = false;
+
+	for (unsigned i = 0; i < n_bytes; i++) {
+		/* set updated flag if one complete packet was parsed */
+		st24_rssi = RC_INPUT_RSSI_MAX;
+		*sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count,
+					&sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
+	}
+
+	if (*sumd_updated) {
+
+		*rssi = sumd_rssi;
+		r_raw_rc_count = sumd_channel_count;
+
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD;
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+	}
+
+	return (*dsm_updated | *st24_updated | *sumd_updated);
 }
 
 void
@@ -169,14 +194,17 @@ controls_tick() {
 #endif
 
 	perf_begin(c_gather_dsm);
-	bool dsm_updated, st24_updated;
-	(void)dsm_port_input(&rssi, &dsm_updated, &st24_updated);
+	bool dsm_updated, st24_updated, sumd_updated;
+	(void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &sumd_updated);
 	if (dsm_updated) {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
 	}
 	if (st24_updated) {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24;
 	}
+	if (sumd_updated) {
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD;
+	}
 	perf_end(c_gather_dsm);
 
 	perf_begin(c_gather_sbus);
@@ -238,7 +266,7 @@ controls_tick() {
 	/*
 	 * If we received a new frame from any of the RC sources, process it.
 	 */
-	if (dsm_updated || sbus_updated || ppm_updated || st24_updated) {
+	if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated) {
 
 		/* record a bitmask of channels assigned */
 		unsigned assigned_channels = 0;
@@ -438,7 +466,7 @@ controls_tick() {
 			r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
 
 			/* mix new RC input control values to servos */
-			if (dsm_updated || sbus_updated || ppm_updated || st24_updated)
+			if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated)
 				mixer_tick();
 
 		} else {

src/modules/px4iofirmware/module.mk

@@ -14,7 +14,8 @@ SRCS		= adc.c \
 		  ../systemlib/mixer/mixer_multirotor.cpp \
 		  ../systemlib/mixer/mixer_simple.cpp \
 		  ../systemlib/pwm_limit/pwm_limit.c \
-		  ../../lib/rc/st24.c
+		  ../../lib/rc/st24.c \
+		  ../../lib/rc/sumd.c
 
 ifeq ($(BOARD),px4io-v1)
 SRCS		+= i2c.c

src/modules/px4iofirmware/protocol.h

@@ -115,6 +115,7 @@
 #define PX4IO_P_STATUS_FLAGS_SAFETY_OFF		(1 << 12) /* safety is off */
 #define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED	(1 << 13) /* FMU was initialized and OK once */
 #define PX4IO_P_STATUS_FLAGS_RC_ST24		(1 << 14) /* ST24 input is valid */
+#define PX4IO_P_STATUS_FLAGS_RC_SUMD		(1 << 15) /* SUMD input is valid */
 
 #define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
 #define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* [1] VBatt is very close to regulator dropout */

src/modules/sensors/sensor_params.c

@@ -130,7 +130,7 @@ PARAM_DEFINE_INT32(CAL_MAG0_ID, 0);
  * @max 30
  * @group Sensor Calibration
  */
-PARAM_DEFINE_INT32(CAL_MAG0_ROT, 0);
+PARAM_DEFINE_INT32(CAL_MAG0_ROT, -1);
 
 /**
  * Magnetometer X-axis offset
@@ -308,7 +308,7 @@ PARAM_DEFINE_INT32(CAL_MAG1_ID, 0);
  * @max 30
  * @group Sensor Calibration
  */
-PARAM_DEFINE_INT32(CAL_MAG1_ROT, 0);
+PARAM_DEFINE_INT32(CAL_MAG1_ROT, -1);
 
 /**
  * Magnetometer X-axis offset
@@ -486,7 +486,7 @@ PARAM_DEFINE_INT32(CAL_MAG2_ID, 0);
  * @max 30
  * @group Sensor Calibration
  */
-PARAM_DEFINE_INT32(CAL_MAG2_ROT, 0);
+PARAM_DEFINE_INT32(CAL_MAG2_ROT, -1);
 
 /**
  * Magnetometer X-axis offset
@@ -993,6 +993,36 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f);
 PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124);	/* scaling for 3DR power brick */
 
 
+/**
+ * RC channel count
+ *
+ * This parameter is used by Ground Station software to save the number
+ * of channels which were used during RC calibration. It is only meant
+ * for ground station use.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+
+PARAM_DEFINE_INT32(RC_CHAN_CNT, 0);
+
+/**
+ * RC mode switch threshold automaic distribution
+ *
+ * This parameter is used by Ground Station software to specify whether
+ * the threshold values for flight mode switches were automatically calculated.
+ * 0 indicates that the threshold values were set by the user. Any other value
+ * indicates that the threshold value where automatically set by the ground
+ * station software. It is only meant for ground station use.
+ *
+ * @min 0
+ * @max 1
+ * @group Radio Calibration
+ */
+
+PARAM_DEFINE_INT32(RC_TH_USER, 1);
+
 /**
  * Roll control channel mapping.
  *

src/modules/sensors/sensors.cpp

@@ -1511,14 +1511,21 @@ Sensors::parameter_update_poll(bool forced)
 					if (ioctl(fd, MAGIOCGEXTERNAL, 0) <= 0) {
 						/* mag is internal */
 						_mag_rotation[s] = _board_rotation;
-						/* reset param to -1 to indicate external mag */
+						/* reset param to -1 to indicate internal mag */
 						int32_t minus_one = MAG_ROT_VAL_INTERNAL;
 						param_set_no_notification(param_find(str), &minus_one);
 					} else {
 
-						int32_t mag_rot = 0;
+						int32_t mag_rot;
 						param_get(param_find(str), &mag_rot);
 
+						/* check if this mag is still set as internal */
+						if (mag_rot < 0) {
+							/* it was marked as internal, change to external with no rotation */
+							mag_rot = 0;
+							param_set_no_notification(param_find(str), &mag_rot);
+						}
+
 						/* handling of old setups, will be removed later (noted Feb 2015) */
 						int32_t deprecated_mag_rot = 0;
 						param_get(param_find("SENS_EXT_MAG_ROT"), &deprecated_mag_rot);
@@ -1536,6 +1543,9 @@ Sensors::parameter_update_poll(bool forced)
 						if ((deprecated_mag_rot != 0) && (mag_rot <= 0)) {
 							mag_rot = deprecated_mag_rot;
 							param_set_no_notification(param_find(str), &mag_rot);
+							/* clear the old param, not supported in GUI anyway */
+							deprecated_mag_rot = 0;
+							param_set_no_notification(param_find("SENS_EXT_MAG_ROT"), &deprecated_mag_rot);
 						}
 
 						/* handling of transition from internal to external */

src/modules/systemlib/system_params.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -97,3 +97,15 @@ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2);
 * @group System
 */
 PARAM_DEFINE_INT32(SYS_COMPANION, 0);
+
+/**
+* Parameter version
+*
+* This monotonically increasing number encodes the parameter compatibility set.
+* whenever it increases parameters might not be backwards compatible and
+* ground control stations should suggest a fresh configuration.
+*
+* @min 0
+* @group System
+*/
+PARAM_DEFINE_INT32(SYS_PARAM_VER, 1);

src/modules/uORB/objects_common.cpp

@@ -163,8 +163,8 @@ ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s);
 #include "topics/vehicle_control_debug.h"
 ORB_DEFINE(vehicle_control_debug, struct vehicle_control_debug_s);
 
-#include "topics/offboard_control_setpoint.h"
-ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s);
+#include "topics/offboard_control_mode.h"
+ORB_DEFINE(offboard_control_mode, struct offboard_control_mode_s);
 
 #include "topics/optical_flow.h"
 ORB_DEFINE(optical_flow, struct optical_flow_s);

src/modules/uORB/topics/offboard_control_setpoint.h

@@ -1,276 +0,0 @@
-/****************************************************************************
- *
- *	 Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *	 Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *		notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *		notice, this list of conditions and the following disclaimer in
- *		the documentation and/or other materials provided with the
- *		distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *		used to endorse or promote products derived from this software
- *		without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file offboard_control_setpoint.h
- * Definition of the manual_control_setpoint uORB topic.
- */
-
-#ifndef TOPIC_OFFBOARD_CONTROL_SETPOINT_H_
-#define TOPIC_OFFBOARD_CONTROL_SETPOINT_H_
-
-#include <stdint.h>
-#include "../uORB.h"
-
-/**
- * Off-board control inputs.
- *
- * Typically sent by a ground control station / joystick or by
- * some off-board controller via C or SIMULINK.
- */
-enum OFFBOARD_CONTROL_MODE {
-	OFFBOARD_CONTROL_MODE_DIRECT = 0,
-	OFFBOARD_CONTROL_MODE_DIRECT_RATES = 1,
-	OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE = 2,
-	OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED = 3,
-	OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED = 4,
-	OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED = 5,
-	OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED = 6,
-	OFFBOARD_CONTROL_MODE_DIRECT_GLOBAL = 7,
-	OFFBOARD_CONTROL_MODE_DIRECT_FORCE = 8,
-	OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 9,
-	OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 10,
-	OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 11 /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
-};
-
-enum OFFBOARD_CONTROL_FRAME {
-	OFFBOARD_CONTROL_FRAME_LOCAL_NED = 0,
-	OFFBOARD_CONTROL_FRAME_LOCAL_OFFSET_NED = 1,
-	OFFBOARD_CONTROL_FRAME_BODY_NED = 2,
-	OFFBOARD_CONTROL_FRAME_BODY_OFFSET_NED = 3,
-	OFFBOARD_CONTROL_FRAME_GLOBAL = 4
-};
-
-/* mappings for the ignore bitmask */
-enum {OFB_IGN_BIT_POS_X,
-	OFB_IGN_BIT_POS_Y,
-	OFB_IGN_BIT_POS_Z,
-	OFB_IGN_BIT_VEL_X,
-	OFB_IGN_BIT_VEL_Y,
-	OFB_IGN_BIT_VEL_Z,
-	OFB_IGN_BIT_ACC_X,
-	OFB_IGN_BIT_ACC_Y,
-	OFB_IGN_BIT_ACC_Z,
-	OFB_IGN_BIT_BODYRATE_X,
-	OFB_IGN_BIT_BODYRATE_Y,
-	OFB_IGN_BIT_BODYRATE_Z,
-	OFB_IGN_BIT_ATT,
-	OFB_IGN_BIT_THRUST,
-	OFB_IGN_BIT_YAW,
-	OFB_IGN_BIT_YAWRATE,
-};
-
-/**
- * @addtogroup topics
- * @{
- */
-
-struct offboard_control_setpoint_s {
-	uint64_t timestamp;
-
-	enum OFFBOARD_CONTROL_MODE mode;		 /**< The current control inputs mode */
-
-	double position[3];	/**< lat, lon, alt / x, y, z */
-	float velocity[3];	/**< x vel, y vel, z vel */
-	float acceleration[3];	/**< x acc, y acc, z acc */
-	float attitude[4];	/**< attitude of vehicle (quaternion) */
-	float attitude_rate[3];	/**< body angular rates (x, y, z) */
-	float thrust;	/**< thrust */
-	float yaw;	/**< yaw: this is the yaw from the position_target message
-			  (not from the full attitude_target message) */
-	float yaw_rate;	/**< yaw rate: this is the yaw from the position_target message
-			  (not from the full attitude_target message) */
-
-	uint16_t ignore; /**< if field i is set to true, the value should be ignored, see definition at top of file
-			   for mapping */
-
-	bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */
-
-	float override_mode_switch;
-
-	float aux1_cam_pan_flaps;
-	float aux2_cam_tilt;
-	float aux3_cam_zoom;
-	float aux4_cam_roll;
-
-}; /**< offboard control inputs */
-/**
- * @}
- */
-
-/**
- * Returns true if the position setpoint at index should be ignored
- */
-inline bool offboard_control_sp_ignore_position(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
-	return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_POS_X +  index)));
-}
-
-/**
- * Returns true if all position setpoints should be ignored
- */
-inline bool offboard_control_sp_ignore_position_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	for (int i = 0; i < 3; i++) {
-		if (!offboard_control_sp_ignore_position(offboard_control_sp, i))	{
-			return false;
-		}
-	}
-	return true;
-}
-
-/**
- * Returns true if some position setpoints should be ignored
- */
-inline bool offboard_control_sp_ignore_position_some(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	for (int i = 0; i < 3; i++) {
-		if (offboard_control_sp_ignore_position(offboard_control_sp, i))	{
-			return true;
-		}
-	}
-	return false;
-}
-
-/**
- * Returns true if the velocity setpoint at index should be ignored
- */
-inline bool offboard_control_sp_ignore_velocity(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
-	return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_VEL_X + index)));
-}
-
-/**
- * Returns true if all velocity setpoints should be ignored
- */
-inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	for (int i = 0; i < 3; i++) {
-		if (!offboard_control_sp_ignore_velocity(offboard_control_sp, i))	{
-			return false;
-		}
-	}
-	return true;
-}
-
-/**
- * Returns true if some velocity setpoints should be ignored
- */
-inline bool offboard_control_sp_ignore_velocity_some(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	for (int i = 0; i < 3; i++) {
-		if (offboard_control_sp_ignore_velocity(offboard_control_sp, i))	{
-			return true;
-		}
-	}
-	return false;
-}
-
-/**
- * Returns true if the acceleration setpoint at index should be ignored
- */
-inline bool offboard_control_sp_ignore_acceleration(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
-	return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_ACC_X + index)));
-}
-
-/**
- * Returns true if all acceleration setpoints should be ignored
- */
-inline bool offboard_control_sp_ignore_acceleration_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	for (int i = 0; i < 3; i++) {
-		if (!offboard_control_sp_ignore_acceleration(offboard_control_sp, i))	{
-			return false;
-		}
-	}
-	return true;
-}
-
-/**
- * Returns true if some acceleration setpoints should be ignored
- */
-inline bool offboard_control_sp_ignore_acceleration_some(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	for (int i = 0; i < 3; i++) {
-		if (offboard_control_sp_ignore_acceleration(offboard_control_sp, i))	{
-			return true;
-		}
-	}
-	return false;
-}
-
-/**
- * Returns true if the bodyrate setpoint at index should be ignored
- */
-inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
-	return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_BODYRATE_X + index)));
-}
-
-/**
- * Returns true if some of the bodyrate setpoints should be ignored
- */
-inline bool offboard_control_sp_ignore_bodyrates_some(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	for (int i = 0; i < 3; i++) {
-		if (offboard_control_sp_ignore_bodyrates(offboard_control_sp, i))	{
-			return true;
-		}
-	}
-	return false;
-}
-
-/**
- * Returns true if the attitude setpoint should be ignored
- */
-inline bool offboard_control_sp_ignore_attitude(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_ATT));
-}
-
-/**
- * Returns true if the thrust setpoint should be ignored
- */
-inline bool offboard_control_sp_ignore_thrust(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_THRUST));
-}
-
-/**
- * Returns true if the yaw setpoint should be ignored
- */
-inline bool offboard_control_sp_ignore_yaw(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_YAW));
-}
-
-/**
- * Returns true if the yaw rate setpoint should be ignored
- */
-inline bool offboard_control_sp_ignore_yawrate(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_YAWRATE));
-}
-
-
-/* register this as object request broker structure */
-ORB_DECLARE(offboard_control_setpoint);
-
-#endif

src/modules/uORB/topics/vehicle_command.h

@@ -38,6 +38,7 @@
  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
  * @author Julian Oes <joes@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Matosov <anton.matosov@gmail.com>
  */
 
 #ifndef TOPIC_VEHICLE_COMMAND_H_
@@ -85,6 +86,15 @@ enum VEHICLE_CMD {
 	VEHICLE_CMD_DO_REPEAT_SERVO = 184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty|  */
 	VEHICLE_CMD_DO_FLIGHTTERMINATION=185, /* Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty|  */
 	VEHICLE_CMD_DO_CONTROL_VIDEO = 200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|  */
+	VEHICLE_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty|  */
+	VEHICLE_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value|  */
+	VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty|  */
+	VEHICLE_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty|  */
+	VEHICLE_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty|  */
+	VEHICLE_CMD_DO_INVERTED_FLIGHT=210, /* Change to/from inverted flight |inverted (0=normal, 1=inverted)| Empty| Empty| Empty| Empty| Empty| Empty|  */
+	VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1, w (1 in null-rotation)| q2 - quaternion param #2, x (0 in null-rotation)| q3 - quaternion param #3, y (0 in null-rotation)| q4 - quaternion param #4, z (0 in null-rotation)| Empty| Empty| Empty|  */
+	VEHICLE_CMD_DO_GUIDED_MASTER=221, /* set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty|  */
+	VEHICLE_CMD_DO_GUIDED_LIMITS=222, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue.  0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue.  0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty|  */
 	VEHICLE_CMD_DO_LAST = 240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
 	VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty|  */
 	VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS = 242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units|  */
@@ -101,7 +111,7 @@ enum VEHICLE_CMD {
 /**
  * Commands for commander app.
  *
- * Should contain all of MAVLink's VEHICLE_CMD_RESULT values
+ * Should contain all of MAVLink's MAV_CMD_RESULT values
  * but can contain additional ones.
  */
 enum VEHICLE_CMD_RESULT {
@@ -113,6 +123,22 @@ enum VEHICLE_CMD_RESULT {
 	VEHICLE_CMD_RESULT_ENUM_END = 5, /*  | */
 };
 
+/**
+ * Commands for gimbal app.
+ *
+ * Should contain all of MAVLink's MAV_MOUNT_MODE values
+ * but can contain additional ones.
+ */
+typedef enum VEHICLE_MOUNT_MODE
+{
+	VEHICLE_MOUNT_MODE_RETRACT=0, /* Load and keep safe position (Roll,Pitch,Yaw) from permant memory and stop stabilization | */
+	VEHICLE_MOUNT_MODE_NEUTRAL=1, /* Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. | */
+	VEHICLE_MOUNT_MODE_MAVLINK_TARGETING=2, /* Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization | */
+	VEHICLE_MOUNT_MODE_RC_TARGETING=3, /* Load neutral position and start RC Roll,Pitch,Yaw control with stabilization | */
+	VEHICLE_MOUNT_MODE_GPS_POINT=4, /* Load neutral position and start to point to Lat,Lon,Alt | */
+	VEHICLE_MOUNT_MODE_ENUM_END=5, /*  | */
+} VEHICLE_MOUNT_MODE;
+
 /**
  * @addtogroup topics
  * @{

src/modules/uORB/topics/vehicle_global_position.h

@@ -74,6 +74,7 @@ struct vehicle_global_position_s {
 	float epv;			/**< Standard deviation of position vertically */
 	float terrain_alt;		/**< Terrain altitude in m, WGS84 */
 	bool terrain_alt_valid;		/**< Terrain altitude estimate is valid */
+	bool dead_reckoning;		/**< True if this position is estimated through dead-reckoning*/
 };
 
 /**

src/modules/uORB/uORB.cpp

@@ -684,6 +684,8 @@ namespace
 
 ORBDevMaster	*g_dev;
 bool pubsubtest_passed = false;
+bool pubsubtest_print = false;
+int pubsubtest_res = OK;
 
 struct orb_test {
 	int val;
@@ -693,6 +695,22 @@ struct orb_test {
 ORB_DEFINE(orb_test, struct orb_test);
 ORB_DEFINE(orb_multitest, struct orb_test);
 
+struct orb_test_medium {
+	int val;
+	hrt_abstime time;
+	char junk[64];
+};
+
+ORB_DEFINE(orb_test_medium, struct orb_test_medium);
+
+struct orb_test_large {
+	int val;
+	hrt_abstime time;
+	char junk[512];
+};
+
+ORB_DEFINE(orb_test_large, struct orb_test_large);
+
 int
 test_fail(const char *fmt, ...)
 {
@@ -727,21 +745,44 @@ int pubsublatency_main(void)
 	float latency_integral = 0.0f;
 
 	/* wakeup source(s) */
-	struct pollfd fds[1];
+	struct pollfd fds[3];
 
-	int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_multitest), 0);
+	int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_test), 0);
+	int test_multi_sub_medium = orb_subscribe_multi(ORB_ID(orb_test_medium), 0);
+	int test_multi_sub_large = orb_subscribe_multi(ORB_ID(orb_test_large), 0);
+
+	struct orb_test_large t;
+
+	/* clear all ready flags */
+	orb_copy(ORB_ID(orb_test), test_multi_sub, &t);
+	orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t);
+	orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t);
 
 	fds[0].fd = test_multi_sub;
 	fds[0].events = POLLIN;
+	fds[1].fd = test_multi_sub_medium;
+	fds[1].events = POLLIN;
+	fds[2].fd = test_multi_sub_large;
+	fds[2].events = POLLIN;
 
-	struct orb_test t;
+	const unsigned maxruns = 1000;
+	unsigned timingsgroup = 0;
 
-	const unsigned maxruns = 10;
+	unsigned *timings = new unsigned[maxruns];
 
 	for (unsigned i = 0; i < maxruns; i++) {
 		/* wait for up to 500ms for data */
 		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
-		orb_copy(ORB_ID(orb_multitest), test_multi_sub, &t);
+		if (fds[0].revents & POLLIN) {
+			orb_copy(ORB_ID(orb_test), test_multi_sub, &t);
+			timingsgroup = 0;
+		} else if (fds[1].revents & POLLIN) {
+			orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t);
+			timingsgroup = 1;
+		} else if (fds[2].revents & POLLIN) {
+			orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t);
+			timingsgroup = 2;
+		}
 
 		if (pret < 0) {
 			warn("poll error %d, %d", pret, errno);
@@ -750,17 +791,46 @@ int pubsublatency_main(void)
 
 		hrt_abstime elt = hrt_elapsed_time(&t.time);
 		latency_integral += elt;
+		timings[i] = elt;
 	}
 
 	orb_unsubscribe(test_multi_sub);
+	orb_unsubscribe(test_multi_sub_medium);
+	orb_unsubscribe(test_multi_sub_large);
+
+	if (pubsubtest_print) {
+		char fname[32];
+		sprintf(fname, "/fs/microsd/timings%u.txt", timingsgroup);
+		FILE *f = fopen(fname, "w");
+		if (f == NULL) {
+			warnx("Error opening file!\n");
+			return ERROR;
+    		}
+
+		for (unsigned i = 0; i < maxruns; i++) {
+			fprintf(f, "%u\n", timings[i]);
+		}
+
+		fclose(f);
+	}
+
+	delete[] timings;
 
 	warnx("mean: %8.4f", static_cast<double>(latency_integral / maxruns));
 
 	pubsubtest_passed = true;
 
-	return OK;
+	if (static_cast<float>(latency_integral / maxruns) > 30.0f) {
+		pubsubtest_res = ERROR;
+	} else {
+		pubsubtest_res = OK;
+	}
+
+	return pubsubtest_res;
 }
 
+template<typename S> int latency_test(orb_id_t T, bool print);
+
 int
 test()
 {
@@ -874,6 +944,25 @@ test()
 	if (prio != ORB_PRIO_MIN)
 		return test_fail("prio: %d", prio);
 
+	if (OK != latency_test<struct orb_test>(ORB_ID(orb_test), false))
+		return test_fail("latency test failed");
+
+	return test_note("PASS");
+}
+
+template<typename S> int
+latency_test(orb_id_t T, bool print)
+{
+	S t;
+	t.val = 308;
+	t.time = hrt_absolute_time();
+
+	int pfd0 = orb_advertise(T, &t);
+
+	pubsubtest_print = print;
+
+	pubsubtest_passed = false;
+
 	/* test pub / sub latency */
 
 	int pubsub_task = task_spawn_cmd("uorb_latency",
@@ -885,20 +974,22 @@ test()
 
 	/* give the test task some data */
 	while (!pubsubtest_passed) {
-		t.val = 303;
+		t.val = 308;
 		t.time = hrt_absolute_time();
-		if (OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t))
+		if (OK != orb_publish(T, pfd0, &t))
 			return test_fail("mult. pub0 timing fail");
 
 		/* simulate >800 Hz system operation */
 		usleep(1000);
 	}
 
+	close(pfd0);
+
 	if (pubsub_task < 0) {
 		return test_fail("failed launching task");
 	}
 
-	return test_note("PASS");
+	return pubsubtest_res;
 }
 
 int
@@ -955,13 +1046,27 @@ uorb_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "test"))
 		return test();
 
+	/*
+	 * Test the latency.
+	 */
+	if (!strcmp(argv[1], "latency_test")) {
+
+		if (argc > 2 && !strcmp(argv[2], "medium")) {
+			return latency_test<struct orb_test_medium>(ORB_ID(orb_test_medium), true);
+		} else if (argc > 2 && !strcmp(argv[2], "large")) {
+			return latency_test<struct orb_test_large>(ORB_ID(orb_test_large), true);
+		} else {
+			return latency_test<struct orb_test>(ORB_ID(orb_test), true);
+		}
+	}
+
 	/*
 	 * Print driver information.
 	 */
 	if (!strcmp(argv[1], "status"))
 		return info();
 
-	errx(-EINVAL, "unrecognized command, try 'start', 'test' or 'status'");
+	errx(-EINVAL, "unrecognized command, try 'start', 'test', 'latency_test' or 'status'");
 }
 
 /*

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -782,18 +782,23 @@ void VtolAttitudeControl::task_main()
 				fill_mc_att_control_output();
 				fill_mc_att_rates_sp();
 
-				if (_actuators_0_pub > 0) {
-					orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
+				/* Only publish if the proper mode(s) are enabled */
+				if(_v_control_mode.flag_control_attitude_enabled ||
+				   _v_control_mode.flag_control_rates_enabled)
+				{
+					if (_actuators_0_pub > 0) {
+						orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
 
-				} else {
-					_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
-				}
+					} else {
+						_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
+					}
 
-				if (_actuators_1_pub > 0) {
-					orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
+					if (_actuators_1_pub > 0) {
+						orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
 
-				} else {
-					_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
+					} else {
+						_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
+					}
 				}
 			}
 		}
@@ -813,18 +818,23 @@ void VtolAttitudeControl::task_main()
 				fill_fw_att_control_output();
 				fill_fw_att_rates_sp();
 
-				if (_actuators_0_pub > 0) {
-					orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
+				/* Only publish if the proper mode(s) are enabled */
+				if(_v_control_mode.flag_control_attitude_enabled ||
+				   _v_control_mode.flag_control_rates_enabled)
+				{
+					if (_actuators_0_pub > 0) {
+						orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
 
-				} else {
-					_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
-				}
+					} else {
+						_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
+					}
 
-				if (_actuators_1_pub > 0) {
-					orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
+					if (_actuators_1_pub > 0) {
+						orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
 
-				} else {
-					_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
+					} else {
+						_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
+					}
 				}
 			}
 		}

src/platforms/px4_includes.h

@@ -66,6 +66,8 @@
 #include <px4_vehicle_global_velocity_setpoint.h>
 #include <px4_vehicle_local_position.h>
 #include <px4_position_setpoint_triplet.h>
+#include <px4_offboard_control_mode.h>
+#include <px4_vehicle_force_setpoint.h>
 #endif
 
 #else
@@ -93,6 +95,8 @@
 #include <platforms/nuttx/px4_messages/px4_vehicle_global_velocity_setpoint.h>
 #include <platforms/nuttx/px4_messages/px4_vehicle_local_position.h>
 #include <platforms/nuttx/px4_messages/px4_position_setpoint_triplet.h>
+#include <platforms/nuttx/px4_messages/px4_offboard_control_mode.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_force_setpoint.h>
 #endif
 #include <systemlib/err.h>
 #include <systemlib/param/param.h>

src/platforms/ros/nodes/commander/commander.cpp

@@ -45,18 +45,25 @@
 Commander::Commander() :
 	_n(),
 	_man_ctrl_sp_sub(_n.subscribe("manual_control_setpoint", 10, &Commander::ManualControlInputCallback, this)),
+	_offboard_control_mode_sub(_n.subscribe("offboard_control_mode", 10, &Commander::OffboardControlModeCallback, this)),
 	_vehicle_control_mode_pub(_n.advertise<px4::vehicle_control_mode>("vehicle_control_mode", 10)),
 	_actuator_armed_pub(_n.advertise<px4::actuator_armed>("actuator_armed", 10)),
 	_vehicle_status_pub(_n.advertise<px4::vehicle_status>("vehicle_status", 10)),
 	_parameter_update_pub(_n.advertise<px4::parameter_update>("parameter_update", 10)),
 	_msg_parameter_update(),
 	_msg_actuator_armed(),
-	_msg_vehicle_control_mode()
+	_msg_vehicle_control_mode(),
+	_msg_offboard_control_mode(),
+	_got_manual_control(false)
 {
+
+	/* Default to offboard control: when no joystick is connected offboard control should just work */
+
 }
 
 void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg)
 {
+	_got_manual_control = true;
 	px4::vehicle_status msg_vehicle_status;
 
 	/* fill vehicle control mode based on (faked) stick positions*/
@@ -101,12 +108,51 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon
 	}
 }
 
+void Commander::SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode,
+			px4::vehicle_control_mode &msg_vehicle_control_mode)
+{
+	msg_vehicle_control_mode.flag_control_manual_enabled = false;
+	msg_vehicle_control_mode.flag_control_offboard_enabled = true;
+	msg_vehicle_control_mode.flag_control_auto_enabled = false;
+
+	msg_vehicle_control_mode.flag_control_rates_enabled = !msg_offboard_control_mode.ignore_bodyrate ||
+		!msg_offboard_control_mode.ignore_attitude ||
+		!msg_offboard_control_mode.ignore_position ||
+		!msg_offboard_control_mode.ignore_velocity ||
+		!msg_offboard_control_mode.ignore_acceleration_force;
+
+	msg_vehicle_control_mode.flag_control_attitude_enabled = !msg_offboard_control_mode.ignore_attitude ||
+		!msg_offboard_control_mode.ignore_position ||
+		!msg_offboard_control_mode.ignore_velocity ||
+		!msg_offboard_control_mode.ignore_acceleration_force;
+
+
+	msg_vehicle_control_mode.flag_control_velocity_enabled = !msg_offboard_control_mode.ignore_velocity ||
+		!msg_offboard_control_mode.ignore_position;
+
+	msg_vehicle_control_mode.flag_control_climb_rate_enabled = !msg_offboard_control_mode.ignore_velocity ||
+		!msg_offboard_control_mode.ignore_position;
+
+	msg_vehicle_control_mode.flag_control_position_enabled = !msg_offboard_control_mode.ignore_position;
+
+	msg_vehicle_control_mode.flag_control_altitude_enabled = !msg_offboard_control_mode.ignore_position;
+}
+
 void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg,
 		px4::vehicle_status &msg_vehicle_status,
 		px4::vehicle_control_mode &msg_vehicle_control_mode) {
 	// XXX this is a minimal implementation. If more advanced functionalities are
 	// needed consider a full port of the commander
 
+
+	if (msg->offboard_switch == px4::manual_control_setpoint::SWITCH_POS_ON)
+	{
+		SetOffboardControl(_msg_offboard_control_mode, msg_vehicle_control_mode);
+		return;
+	}
+
+	msg_vehicle_control_mode.flag_control_offboard_enabled = false;
+
 	switch (msg->mode_switch) {
 		case px4::manual_control_setpoint::SWITCH_POS_NONE:
 		ROS_WARN("Joystick button mapping error, main mode not set");
@@ -152,6 +198,35 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg,
 
 }
 
+void Commander::OffboardControlModeCallback(const px4::offboard_control_modeConstPtr &msg)
+{
+	_msg_offboard_control_mode = *msg;
+
+	/* Force system into offboard control mode */
+	if (!_got_manual_control) {
+		SetOffboardControl(_msg_offboard_control_mode, _msg_vehicle_control_mode);
+		
+		px4::vehicle_status msg_vehicle_status;
+		msg_vehicle_status.timestamp = px4::get_time_micros();
+		msg_vehicle_status.hil_state = msg_vehicle_status.HIL_STATE_OFF;
+		msg_vehicle_status.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR;
+		msg_vehicle_status.is_rotary_wing = true;
+		msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED;
+		
+
+		_msg_actuator_armed.armed = true;
+		_msg_actuator_armed.timestamp = px4::get_time_micros();
+
+		_msg_vehicle_control_mode.timestamp = px4::get_time_micros();
+		_msg_vehicle_control_mode.flag_armed = true;
+		
+
+		_vehicle_control_mode_pub.publish(_msg_vehicle_control_mode);
+		_actuator_armed_pub.publish(_msg_actuator_armed);
+		_vehicle_status_pub.publish(msg_vehicle_status);
+	}
+}
+
 int main(int argc, char **argv)
 {
 	ros::init(argc, argv, "commander");

src/platforms/ros/nodes/commander/commander.h

@@ -44,6 +44,7 @@
 #include <px4/vehicle_status.h>
 #include <px4/parameter_update.h>
 #include <px4/actuator_armed.h>
+#include <px4/offboard_control_mode.h>
 
 class Commander
 {
@@ -58,6 +59,11 @@ protected:
 	 */
 	void ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg);
 
+	/**
+	 * Stores the offboard control mode
+	 */
+	void OffboardControlModeCallback(const px4::offboard_control_modeConstPtr &msg);
+
 	/**
 	 * Set control mode flags based on stick positions (equiv to code in px4 commander)
 	 */
@@ -65,8 +71,15 @@ protected:
 			px4::vehicle_status &msg_vehicle_status,
 			px4::vehicle_control_mode &msg_vehicle_control_mode);
 
+	/**
+	 * Sets offboard controll flags in msg_vehicle_control_mode
+	 */
+	void SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode,
+			px4::vehicle_control_mode &msg_vehicle_control_mode);
+
 	ros::NodeHandle _n;
 	ros::Subscriber _man_ctrl_sp_sub;
+	ros::Subscriber _offboard_control_mode_sub;
 	ros::Publisher _vehicle_control_mode_pub;
 	ros::Publisher _actuator_armed_pub;
 	ros::Publisher _vehicle_status_pub;
@@ -75,5 +88,8 @@ protected:
 	px4::parameter_update _msg_parameter_update;
 	px4::actuator_armed _msg_actuator_armed;
 	px4::vehicle_control_mode _msg_vehicle_control_mode;
+	px4::offboard_control_mode _msg_offboard_control_mode;
+
+	bool _got_manual_control;
 
 };