IO: Remove hint that parameter change requires reboot

The change is effective immediately so no reboot is required. This makes the whole configuration a lot easier.

.github_changelog_generator

@@ -0,0 +1,33 @@
+# How to install:
+#   gem install github_changelog_generator
+# How to run:
+#   github_changelog_generator -u PX4 -p Firmware
+# Description:
+#   The following params are sensible defaults for the PX4 project,
+#   if you want to do a changelog before a release you need to update since-tag and future-releases,
+
+# Params:
+#   github_changelog_generator --help for all options
+
+# max-issues
+#   max threshold for github api queries
+#   make sure you set your CHANGELOG_GITHUB_TOKEN before
+#   running
+max-issues=1500
+
+# exclude-tags-regex
+#   excludes release candidates
+exclude-tags-regex=rc[0-9]{1,}|beta[0-9]{1,}
+
+# since-tag
+#   version of last stable release
+#   you need to change this depending on what you need
+#   if you want a changelog between versions this is the lowest version
+since-tag=1.6.5
+
+# future-release
+#   version you are about to release
+#   if you want a changelog between a version and all unreleased changes grouped as a release
+#     eg: v1.6.5 to v1.7.0
+future-release=v1.7.0
+

CMakeLists.txt

@@ -243,11 +243,11 @@ set(BUILD_SHARED_LIBS OFF)
 #=============================================================================
 # ccache
 #
-option(CCACHE "Use ccache if available" OFF)
+option(CCACHE "Use ccache if available" ON)
 find_program(CCACHE_PROGRAM ccache)
-if (CCACHE AND CCACHE_PROGRAM)
-	message(STATUS "Enabled ccache: ${CCACHE_PROGRAM}")
+if (CCACHE AND CCACHE_PROGRAM AND NOT DEFINED ENV{CCACHE_DISABLE})
 	set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE "${CCACHE_PROGRAM}")
+else()
 endif()
 
 #=============================================================================
@@ -413,6 +413,27 @@ if (all_posix_cmake_targets)
 		)
 endif()
 
+
+#=============================================================================
+# uORB graph generation: add a custom target 'uorb_graph'
+#
+set(uorb_graph_config ${BOARD})
+
+set(graph_module_list "")
+foreach(module ${config_module_list})
+	set(graph_module_list "${graph_module_list}" "--src-path" "src/${module}")
+endforeach()
+
+add_custom_command(OUTPUT ${uorb_graph_config}
+	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/uorb_graph/create.py
+		${module_list}
+		--exclude-path src/examples
+		--file ${PX4_SOURCE_DIR}/Tools/uorb_graph/graph_${uorb_graph_config}
+	WORKING_DIRECTORY ${PX4_SOURCE_DIR}
+	COMMENT "Generating uORB graph"
+)
+add_custom_target(uorb_graph DEPENDS ${uorb_graph_config})
+
 #=============================================================================
 # packaging
 #

Documentation/px4_hil/UserGuide.md

@@ -1,98 +0,0 @@
-[TOC]
-
-# Introduction
-
-The HIL architecture allows you to test the flight stack replacing the real physical vehicle and sensors with a simulator of vehicle dynamics and sensor outputs.  The flight stack "is not aware" that it is not on a real vehicle.  This is a powerful tool for develping and testing code rapidly in a benchtop environment. 
-
-The flight stack can be run anywhere that supports a network connection to the simulator (with sufficient bandwidth and latency to transport the sensor and actuator messages).  This can be on a standard linux workstation, an on-target linux image, or the on-target DSP image.  These modes can be selected based on the goals of the testing.  Workstation is useful for rapid testing in a tool-rich environment.  DSP image testing is the closest to the final implementation, so is useful for testing actual HW operation, other than the physical sensing and actuation.
-
-## Px4 High-level HIL Architecture
-
-A diagram of the setup described is shown here.  Note that UDP port numbers are only displayed on the socket server and are left blank on the socket client. 
-
-(???NOTES:  This diagram needs to be updated to use control inputs over UDP, either from QGC or from other)
-
-![SITL Diagram](./SITL_Diagram_QGC.png "SITL Diagram")  
-
-## Requirements
-The simulator that is currently supported is jMAVSim.  The setup described here requires PX4 and jMAVSim installed and running.  qGroundControl (QGC) is also required because it is the supported method of providing manual control commands.  
-
-## Assumptions
-
-# Compiling Code
-
-## JMAVSim
-
-### Platform Requirements
-Linux with java-1.7.x or greater
-
-### Build Instructions
-In a clean directory
-```
-> git clone https://github.com/PX4/jMAVSim.git
-> cd jMAVSim
-> git submodule init
-> git submodule update
-> ant
-```
-
-## qGroundControl
-
-### Platform Requirements
-Windows 7
-Logitech Gamepad F310 joystick controller
-
-### Download/Install Instructions
-Download QGC from http://qgroundcontrol.org/downloads and install using the windows executable.
-
-
-## PX4
-
-### Platform Requirements
-Linux or Eagle with a working IP interface (?? does this need further instructions?)
-
-### Build Host Requirements
-(???Notes:  Windows?)
-
-### Download & Build Instructions
-
-### Installing binaries on the Qualcomm Target
-
-# Running PX4 in HIL Mode
-
-## Starting PX4 on Qualcomm Eagle
-
-```
-> adb shell
-# bash
-root@linaro-developer:/# cd ???
-root@linaro-developer:/# ./px4
-App name: px4
-Enter a command and its args:
-uorb start
-muorb start
-mavlink start -u 14556
-simulator start -p
-```
-
-## Starting jMAVSim
-In the directory where jMAVSim is installed
-```
- java -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp <IPADDR>:14560 -n 100
-```
-replacing <IPADDR> with the IP address of the machine running PX4 (Eagle).  This can be found by running "ifconfig" on that machine.
-
-## Starting qGroundControl
-Launch the qGroundControl application
-1. Set up the communication to the flight stack.  In the menu File:Settings:CommLinks, select Add.  Enter a Link Name of your choice.  Select Link Type: UDP.  Set the listening port to an unused port (example: 14561).  Select Add.  Enter the IP address and port of the PX4 Mavlink app, which is <IPADDR>:14556 with <IPADDR> being the IP address of the Eagle board. Select OK.
-1. Set up the joystick.  Plug in the joystick to your Windows machine.  In the menu File:Settings:CommLinks, check Enable Controllers.  Select "Gamepad F310".  Select "Manual".  Set the axes/channel mapping to 0:Yaw, 1:Throttle, 2:unset, 3:Pitch, 4:Roll.  Seletct "Inverted" for the throttle axis.  Click "Calibrate range".  Move the right joystick through its full range of motion.  Move the left joystick full left then full right.  Move the left joystick full forward (but not full backward).  Click "end calibration."
-1. Connect to the flight stack.  Click Analyze.  Click the "Connect" button in the upper right, and select the connection that you created in the first step.
-
-You should now be connected to the flight stack.  You can see incoming Mavlink packets using the MAVLink Instpector (from Advanced:Tool Widgets)
-
-
-## Controlling PX4 flight in HIL Mode
-The joystick can now be used to fly the simulated vehicle.  The jMAVSim world visualization gives a FPV view, and QGC can be used to display instruments such as artificial horizon and maps (if GPS simulation is enabled).
-
-
-# Debugging/FAQ

Documentation/versionfilter.sh

@@ -1,2 +0,0 @@
-#!/bin/sh
-git log --pretty=format:"Last change: commit %h - %aN, %ar : %s" -1 $1 || echo no git

Images/aerocore2.prototype

@@ -7,6 +7,7 @@
     "summary": "AEROCORE2",
     "version": "0.1",
     "image_size": 0,
+    "image_maxsize": 1032192,
     "git_identity": "",
     "board_revision": 0
 }

Images/aerofc-v1.prototype

@@ -7,6 +7,7 @@
     "summary": "AEROFCv1",
     "version": "0.1",
     "image_size": 0,
+    "image_maxsize": 999424,
     "git_identity": "",
     "board_revision": 0
 }

Images/auav-x21.prototype

@@ -7,6 +7,7 @@
     "summary": "AUAV X2.1",
     "version": "0.1",
     "image_size": 0,
+    "image_maxsize": 2080768,
     "git_identity": "",
     "board_revision": 0
 }

Images/crazyflie.prototype

@@ -7,6 +7,7 @@
     "summary": "CRAZYFLIE",
     "version": "0.1",
     "image_size": 0,
+    "image_maxsize": 1032192,
     "git_identity": "",
     "board_revision": 0
 }

Images/esc35-v1.prototype

@@ -7,6 +7,7 @@
     "summary": "ESC35v1",
     "version": "0.1",
     "image_size": 0,
+    "image_maxsize": 229376,
     "git_identity": "",
     "board_revision": 0
 }

Images/mindpx-v2.prototype

@@ -7,6 +7,7 @@
     "summary": "MindPXFMUv2",
     "version": "2.1",
     "image_size": 0,
+    "image_maxsize": 2080768,
     "git_identity": "",
     "board_revision": 0
 }

Images/nxphlite-v3.prototype

@@ -7,6 +7,7 @@
     "summary": "NXPHLITEv3",
     "version": "0.1",
     "image_size": 0,
+    "image_maxsize": 2096112,
     "git_identity": "",
     "board_revision": 0
 }

Images/px4-same70xplained-v1.prototype

@@ -7,6 +7,7 @@
     "summary": "PX4/SAME70xplained",
     "version": "0.1",
     "image_size": 0,
+    "image_maxsize": 2097152,
     "git_identity": "",
     "board_revision": 0
 }

Images/px4-stm32f4discovery.prototype

@@ -7,6 +7,7 @@
     "summary": "PX4/STM32F4Discovery",
     "version": "0.1",
     "image_size": 0,
+    "image_maxsize": 1032192,
     "git_identity": "",
     "board_revision": 0
 }

Images/px4cannode-v1.prototype

@@ -7,6 +7,7 @@
     "summary": "PX4CANNODEv1",
     "version": "0.1",
     "image_size": 0,
+    "image_maxsize": 122880,
     "git_identity": "",
     "board_revision": 0
 }

Images/px4esc-v1.prototype

@@ -7,6 +7,7 @@
     "summary": "PX4ESCv1",
     "version": "0.1",
     "image_size": 0,
+    "image_maxsize": 475136,
     "git_identity": "",
     "board_revision": 0
 }

Images/px4flow-v2.prototype

@@ -1,12 +1,13 @@
 {
     "board_id": 24,
     "magic": "FLOWv1",
-    "description": "Firmware for the PX4FLowV1 board",
+    "description": "Firmware for the PX4FlowV2 board",
     "image": "",
     "build_time": 0,
-    "summary": "PX4FLOWv1",
+    "summary": "PX4FLOWv2",
     "version": "0.1",
     "image_size": 0,
+    "image_maxsize": 2080768,
     "git_identity": "",
     "board_revision": 0
 }

Images/px4fmu-v2.prototype

@@ -7,6 +7,7 @@
     "summary": "PX4FMUv2",
     "version": "0.1",
     "image_size": 0,
+    "image_maxsize": 1032192,
     "git_identity": "",
     "board_revision": 0
 }

Images/px4fmu-v3.prototype

@@ -7,6 +7,7 @@
     "summary": "PX4FMUv3",
     "version": "0.1",
     "image_size": 0,
+    "image_maxsize": 2080768,
     "git_identity": "",
     "board_revision": 0
 }

Images/px4fmu-v4.prototype

@@ -7,6 +7,7 @@
     "summary": "PX4FMUv4",
     "version": "0.1",
     "image_size": 0,
+    "image_maxsize": 2080768,
     "git_identity": "",
     "board_revision": 0
 }

Images/px4fmu-v4pro.prototype

@@ -7,6 +7,7 @@
     "summary": "PX4FMUv4PRO",
     "version": "0.1",
     "image_size": 0,
+    "image_maxsize": 2080768,
     "git_identity": "",
     "board_revision": 0
 }

Images/px4fmu-v5.prototype

@@ -7,6 +7,7 @@
     "summary": "PX4FMUv5",
     "version": "0.1",
     "image_size": 0,
+    "image_maxsize": 2064384,
     "git_identity": "",
     "board_revision": 0
 }

Images/px4io-v2.prototype

@@ -7,6 +7,7 @@
     "summary": "PX4IOv2",
     "version": "2.0",
     "image_size": 0,
+    "image_maxsize": 61440,
     "git_identity": "",
     "board_revision": 0
 }

Images/px4nucleoF767ZI-v1.prototype

@@ -7,6 +7,7 @@
     "summary": "PX4NUCLEOF767ZIv1",
     "version": "0.1",
     "image_size": 0,
+    "image_maxsize": 2097152,
     "git_identity": "",
     "board_revision": 0
 }

Images/s2740vc-v1.prototype

@@ -7,6 +7,7 @@
     "summary": "S2740VCv1",
     "version": "0.1",
     "image_size": 0,
+    "image_maxsize": 65536,
     "git_identity": "",
     "board_revision": 0
 }

Images/tap-v1.prototype

@@ -7,6 +7,7 @@
     "summary": "TAPv1",
     "version": "0.1",
     "image_size": 0,
+    "image_maxsize": 999424,
     "git_identity": "",
     "board_revision": 0
 }

Images/zubaxgnss-v1.prototype

@@ -7,6 +7,7 @@
     "summary": "ZUBAXGNSSv1",
     "version": "0.0",
     "image_size": 0,
+    "image_maxsize": 253952,
     "git_identity": "",
     "board_revision": 0
 }

Jenkinsfile

@@ -1,17 +1,6 @@
 pipeline {
   agent none
   stages {
-    stage('Quality Checks') {
-      agent {
-        docker {
-          image 'px4io/px4-dev-base:2017-10-23'
-          args '-e CI=true'
-        }
-      }
-      steps {
-        sh 'make check_format'
-      }
-    }
 
     stage('Build') {
       steps {
@@ -55,7 +44,7 @@ pipeline {
                     sh "make clean"
                     sh "ccache -z"
                     sh "git fetch --tags"
-                    sh "make nuttx_px4io-v2_default"
+                    sh "make px4io-v2_default"
                     sh "make nuttx_px4fmu-v2_default"
                     sh "make nuttx_px4fmu-v2_lpe"
                     sh "make nuttx_px4fmu-v3_default"
@@ -118,6 +107,28 @@ pipeline {
           }
 
 
+          // posix_sitl
+          for (def option in ["sitl_default", "sitl_rtps"]) {
+            def node_name = "${option}"
+
+            builds["${node_name}"] = {
+              node {
+                stage("Build Test ${node_name}") {
+                  docker.image('px4io/px4-dev-base:2017-12-30').inside('-e CI=true -e CCACHE_BASEDIR=$WORKSPACE -e CCACHE_DIR=/tmp/ccache -v /tmp/ccache:/tmp/ccache:rw') {
+                    stage("${node_name}") {
+                      checkout scm
+                      sh "make clean"
+                      sh "ccache -z"
+                      sh "make posix_${node_name}"
+                      sh "ccache -s"
+                    }
+                  }
+                }
+              }
+            }
+          }
+
+
           // raspberry pi and bebop (armhf)
           for (def option in ["rpi_cross", "bebop_default"]) {
             def node_name = "${option}"
@@ -125,7 +136,7 @@ pipeline {
             builds["${node_name}"] = {
               node {
                 stage("Build Test ${node_name}") {
-                  docker.image('px4io/px4-dev-raspi:2017-10-23').inside('-e CI=true -e CCACHE_BASEDIR=$WORKSPACE -e CCACHE_DIR=/tmp/ccache -v /tmp/ccache:/tmp/ccache:rw') {
+                  docker.image('px4io/px4-dev-raspi:2017-12-30').inside('-e CI=true -e CCACHE_BASEDIR=$WORKSPACE -e CCACHE_DIR=/tmp/ccache -v /tmp/ccache:/tmp/ccache:rw') {
                     stage("${node_name}") {
                       checkout scm
                       sh "make clean"
@@ -147,7 +158,7 @@ pipeline {
             builds["${node_name}"] = {
               node {
                 stage("Build Test ${node_name}") {
-                  docker.image('px4io/px4-dev-armhf:2017-10-23').inside('-e CI=true -e CCACHE_BASEDIR=$WORKSPACE -e CCACHE_DIR=/tmp/ccache -v /tmp/ccache:/tmp/ccache:rw') {
+                  docker.image('px4io/px4-dev-armhf:2017-12-30').inside('-e CI=true -e CCACHE_BASEDIR=$WORKSPACE -e CCACHE_DIR=/tmp/ccache -v /tmp/ccache:/tmp/ccache:rw') {
                     stage("${node_name}") {
                       checkout scm
                       sh "make clean"
@@ -170,7 +181,7 @@ pipeline {
               node {
                 stage("Build Test ${node_name}") {
                   docker.withRegistry('https://registry.hub.docker.com', 'docker_hub_dagar') {
-                    docker.image("lorenzmeier/px4-dev-snapdragon:2017-10-23").inside {
+                    docker.image("lorenzmeier/px4-dev-snapdragon:2017-12-29").inside('-e CI=true -e CCACHE_BASEDIR=$WORKSPACE -e CCACHE_DIR=/tmp/ccache -v /tmp/ccache:/tmp/ccache:rw') {
                       stage("${node_name}") {
                         checkout scm
                         sh "make clean"
@@ -186,8 +197,29 @@ pipeline {
           }
 
 
-          // GCC7 tests
-          for (def option in ["posix_sitl_default", "nuttx_px4fmu-v5_default"]) {
+          // GCC7 posix
+          for (def option in ["sitl_default"]) {
+            def node_name = "${option}"
+
+            builds["${node_name} (GCC7)"] = {
+              node {
+                stage("Build Test ${node_name} (GCC7)") {
+                  docker.image('px4io/px4-dev-base-archlinux:2017-12-30').inside('-e CI=true -e CCACHE_BASEDIR=$WORKSPACE -e CCACHE_DIR=/tmp/ccache -v /tmp/ccache:/tmp/ccache:rw') {
+                    stage("${node_name}") {
+                      checkout scm
+                      sh "make clean"
+                      sh "ccache -z"
+                      sh "make posix_${node_name}"
+                      sh "ccache -s"
+                    }
+                  }
+                }
+              }
+            }
+          }
+
+          // GCC7 nuttx
+          for (def option in ["px4fmu-v5_default"]) {
             def node_name = "${option}"
 
             builds["${node_name} (GCC7)"] = {
@@ -198,7 +230,7 @@ pipeline {
                       checkout scm
                       sh "make clean"
                       sh "ccache -z"
-                      sh "make ${node_name}"
+                      sh "make nuttx_${node_name}"
                       sh "ccache -s"
                     }
                   }
@@ -215,24 +247,95 @@ pipeline {
     stage('Test') {
       parallel {
 
-        // temporarily disabled until build resources are available
-        //stage('clang-tidy') {
-        //  agent {
-        //    docker {
-        //      image 'px4io/px4-dev-clang:2017-10-23'
-        //      args '-e CI=true -e CCACHE_BASEDIR=$WORKSPACE -e CCACHE_DIR=/tmp/ccache -v /tmp/ccache:/tmp/ccache:rw'
-        //    }
-        //  }
-        //  steps {
-        //    sh 'make clean'
-        //    sh 'make clang-tidy-quiet'
-        //  }
-        //}
+        stage('check style') {
+          agent {
+            docker {
+              image 'px4io/px4-dev-base:2017-12-30'
+              args '-e CI=true'
+            }
+          }
+          steps {
+            sh 'make check_format'
+          }
+        }
+
+        stage('clang analyzer') {
+          agent {
+            docker {
+              image 'px4io/px4-dev-clang:2017-12-30'
+              args '-e CI=true -e CCACHE_BASEDIR=$WORKSPACE -e CCACHE_DIR=/tmp/ccache -v /tmp/ccache:/tmp/ccache:rw'
+            }
+          }
+          steps {
+            sh 'make clean'
+            sh 'make scan-build'
+            // publish html
+            publishHTML target: [
+              reportTitles: 'clang static analyzer',
+              allowMissing: false,
+              alwaysLinkToLastBuild: true,
+              keepAll: true,
+              reportDir: 'build/scan-build/report_latest',
+              reportFiles: '*',
+              reportName: 'Clang Static Analyzer'
+            ]
+          }
+          when {
+            anyOf {
+              branch 'master'
+              branch 'beta'
+              branch 'stable'
+            }
+          }
+        }
+
+        stage('clang tidy') {
+          agent {
+            docker {
+              image 'px4io/px4-dev-clang:2017-12-30'
+              args '-e CI=true -e CCACHE_BASEDIR=$WORKSPACE -e CCACHE_DIR=/tmp/ccache -v /tmp/ccache:/tmp/ccache:rw'
+            }
+          }
+          steps {
+            sh 'make clean'
+            sh 'make clang-tidy-quiet'
+          }
+        }
+
+        stage('cppcheck') {
+          agent {
+            docker {
+              image 'px4io/px4-dev-base:ubuntu17.10'
+              args '-e CI=true -e CCACHE_BASEDIR=$WORKSPACE -e CCACHE_DIR=/tmp/ccache -v /tmp/ccache:/tmp/ccache:rw'
+            }
+          }
+          steps {
+            sh 'make clean'
+            sh 'make cppcheck'
+            // publish html
+            publishHTML target: [
+              reportTitles: 'Cppcheck',
+              allowMissing: false,
+              alwaysLinkToLastBuild: true,
+              keepAll: true,
+              reportDir: 'build/cppcheck/',
+              reportFiles: '*',
+              reportName: 'Cppcheck'
+            ]
+          }
+          when {
+            anyOf {
+              branch 'master'
+              branch 'beta'
+              branch 'stable'
+            }
+          }
+        }
 
         stage('tests') {
           agent {
             docker {
-              image 'px4io/px4-dev-base:2017-10-23'
+              image 'px4io/px4-dev-base:2017-12-30'
               args '-e CI=true -e CCACHE_BASEDIR=$WORKSPACE -e CCACHE_DIR=/tmp/ccache -v /tmp/ccache:/tmp/ccache:rw'
             }
           }
@@ -243,11 +346,179 @@ pipeline {
           }
         }
 
+        stage('ROS vtol mission new 1') {
+          agent {
+            docker {
+              image 'px4io/px4-dev-ros:2017-12-31'
+              args '-e CCACHE_DIR=/tmp/ccache -v /tmp/ccache:/tmp/ccache:rw -e HOME=$WORKSPACE'
+            }
+          }
+          steps {
+            sh 'make clean; rm -rf .ros; rm -rf .gazebo'
+            sh 'make posix_sitl_default'
+            sh 'make posix_sitl_default sitl_gazebo'
+            sh './test/rostest_px4_run.sh mavros_posix_test_mission.test mission:=vtol_new_1.txt vehicle:=vtol_standard'
+          }
+          post {
+            always {
+              archiveArtifacts '**/*.ulg'
+              sh './Tools/upload_log.py -q --description "${CHANGE_TITLE} ${CHANGE_URL}" --source CI .ros/rootfs/fs/microsd/log/*/*.ulg'
+            }
+          }
+        }
+
+        stage('ROS vtol mission new 2') {
+          agent {
+            docker {
+              image 'px4io/px4-dev-ros:2017-12-31'
+              args '-e CCACHE_DIR=/tmp/ccache -v /tmp/ccache:/tmp/ccache:rw -e HOME=$WORKSPACE'
+            }
+          }
+          steps {
+            sh 'make clean; rm -rf .ros; rm -rf .gazebo'
+            sh 'make posix_sitl_default'
+            sh 'make posix_sitl_default sitl_gazebo'
+            sh './test/rostest_px4_run.sh mavros_posix_test_mission.test mission:=vtol_new_2.txt vehicle:=vtol_standard'
+          }
+          post {
+            always {
+              archiveArtifacts '**/*.ulg'
+              sh './Tools/upload_log.py -q --description "${CHANGE_TITLE} ${CHANGE_URL}" --source CI .ros/rootfs/fs/microsd/log/*/*.ulg'
+            }
+          }
+        }
+
+        stage('ROS vtol mission old 1') {
+          agent {
+            docker {
+              image 'px4io/px4-dev-ros:2017-12-31'
+              args '-e CCACHE_DIR=/tmp/ccache -v /tmp/ccache:/tmp/ccache:rw -e HOME=$WORKSPACE'
+            }
+          }
+          steps {
+            sh 'make clean; rm -rf .ros; rm -rf .gazebo'
+            sh 'make posix_sitl_default'
+            sh 'make posix_sitl_default sitl_gazebo'
+            sh './test/rostest_px4_run.sh mavros_posix_test_mission.test mission:=vtol_old_1.txt vehicle:=vtol_standard'
+          }
+          post {
+            always {
+              archiveArtifacts '**/*.ulg'
+              sh './Tools/upload_log.py -q --description "${CHANGE_TITLE} ${CHANGE_URL}" --source CI .ros/rootfs/fs/microsd/log/*/*.ulg'
+            }
+          }
+        }
+
+        stage('ROS vtol mission old 2') {
+          agent {
+            docker {
+              image 'px4io/px4-dev-ros:2017-12-31'
+              args '-e CCACHE_DIR=/tmp/ccache -v /tmp/ccache:/tmp/ccache:rw -e HOME=$WORKSPACE'
+            }
+          }
+          steps {
+            sh 'make clean; rm -rf .ros; rm -rf .gazebo'
+            sh 'make posix_sitl_default'
+            sh 'make posix_sitl_default sitl_gazebo'
+            sh './test/rostest_px4_run.sh mavros_posix_test_mission.test mission:=vtol_old_2.txt vehicle:=vtol_standard'
+          }
+          post {
+            always {
+              archiveArtifacts '**/*.ulg'
+              sh './Tools/upload_log.py -q --description "${CHANGE_TITLE} ${CHANGE_URL}" --source CI .ros/rootfs/fs/microsd/log/*/*.ulg'
+            }
+          }
+        }
+
+        stage('ROS vtol mission old 3') {
+          agent {
+            docker {
+              image 'px4io/px4-dev-ros:2017-12-31'
+              args '-e CCACHE_DIR=/tmp/ccache -v /tmp/ccache:/tmp/ccache:rw -e HOME=$WORKSPACE'
+            }
+          }
+          steps {
+            sh 'make clean; rm -rf .ros; rm -rf .gazebo'
+            sh 'make posix_sitl_default'
+            sh 'make posix_sitl_default sitl_gazebo'
+            sh './test/rostest_px4_run.sh mavros_posix_test_mission.test mission:=vtol_old_3.txt vehicle:=vtol_standard'
+          }
+          post {
+            always {
+              archiveArtifacts '**/*.ulg'
+              sh './Tools/upload_log.py -q --description "${CHANGE_TITLE} ${CHANGE_URL}" --source CI .ros/rootfs/fs/microsd/log/*/*.ulg'
+            }
+          }
+        }
+
+        stage('ROS MC mission box') {
+          agent {
+            docker {
+              image 'px4io/px4-dev-ros:2017-12-31'
+              args '-e CCACHE_DIR=/tmp/ccache -v /tmp/ccache:/tmp/ccache:rw -e HOME=$WORKSPACE'
+            }
+          }
+          steps {
+            sh 'make clean; rm -rf .ros; rm -rf .gazebo'
+            sh 'make posix_sitl_default'
+            sh 'make posix_sitl_default sitl_gazebo'
+            sh './test/rostest_px4_run.sh mavros_posix_test_mission.test mission:=multirotor_box.mission vehicle:=iris'
+          }
+          post {
+            always {
+              archiveArtifacts '**/*.ulg'
+              sh './Tools/upload_log.py -q --description "${CHANGE_TITLE} ${CHANGE_URL}" --source CI .ros/rootfs/fs/microsd/log/*/*.ulg'
+            }
+          }
+        }
+
+        stage('ROS offboard att') {
+          agent {
+            docker {
+              image 'px4io/px4-dev-ros:2017-12-31'
+              args '-e CCACHE_DIR=/tmp/ccache -v /tmp/ccache:/tmp/ccache:rw -e HOME=$WORKSPACE'
+            }
+          }
+          steps {
+            sh 'make clean; rm -rf .ros; rm -rf .gazebo'
+            sh 'make posix_sitl_default'
+            sh 'make posix_sitl_default sitl_gazebo'
+            sh './test/rostest_px4_run.sh mavros_posix_tests_offboard_attctl.test'
+          }
+          post {
+            always {
+              archiveArtifacts '**/*.ulg'
+              sh './Tools/upload_log.py -q --description "${CHANGE_TITLE} ${CHANGE_URL}" --source CI .ros/rootfs/fs/microsd/log/*/*.ulg'
+            }
+          }
+        }
+
+        stage('ROS offboard pos') {
+          agent {
+            docker {
+              image 'px4io/px4-dev-ros:2017-12-31'
+              args '-e CCACHE_DIR=/tmp/ccache -v /tmp/ccache:/tmp/ccache:rw -e HOME=$WORKSPACE'
+            }
+          }
+          steps {
+            sh 'make clean; rm -rf .ros; rm -rf .gazebo'
+            sh 'make posix_sitl_default'
+            sh 'make posix_sitl_default sitl_gazebo'
+            sh './test/rostest_px4_run.sh mavros_posix_tests_offboard_posctl.test'
+          }
+          post {
+            always {
+              archiveArtifacts '**/*.ulg'
+              sh './Tools/upload_log.py -q --description "${CHANGE_TITLE} ${CHANGE_URL}" --source CI .ros/rootfs/fs/microsd/log/*/*.ulg'
+            }
+          }
+        }
+
         // temporarily disabled until stable
         //stage('tests coverage') {
         //  agent {
         //    docker {
-        //      image 'px4io/px4-dev-base:2017-10-23'
+        //      image 'px4io/px4-dev-base:2017-12-30'
         //      args '-e CI=true -e CCACHE_BASEDIR=$WORKSPACE -e CCACHE_DIR=/tmp/ccache -v /tmp/ccache:/tmp/ccache:rw'
         //    }
         //  }
@@ -275,7 +546,7 @@ pipeline {
 
         stage('airframe') {
           agent {
-            docker { image 'px4io/px4-dev-base:2017-10-23' }
+            docker { image 'px4io/px4-dev-base:2017-12-30' }
           }
           steps {
             sh 'make airframe_metadata'
@@ -285,7 +556,7 @@ pipeline {
 
         stage('parameter') {
           agent {
-            docker { image 'px4io/px4-dev-base:2017-10-23' }
+            docker { image 'px4io/px4-dev-base:2017-12-30' }
           }
           steps {
             sh 'make parameters_metadata'
@@ -295,7 +566,7 @@ pipeline {
 
         stage('module') {
           agent {
-            docker { image 'px4io/px4-dev-base:2017-10-23' }
+            docker { image 'px4io/px4-dev-base:2017-12-30' }
           }
           steps {
             sh 'make module_documentation'
@@ -307,7 +578,7 @@ pipeline {
 
     stage('S3 Upload') {
       agent {
-        docker { image 'px4io/px4-dev-base:2017-10-23' }
+        docker { image 'px4io/px4-dev-base:2017-12-30' }
       }
 
       when {

Makefile

@@ -218,7 +218,7 @@ check_rtps: \
 	check_posix_sitl_rtps \
 	sizes
 
-.PHONY: sizes check quick_check check_rtps
+.PHONY: sizes check quick_check check_rtps uorb_graphs
 
 sizes:
 	@-find build -name *.elf -type f | xargs size 2> /dev/null || :
@@ -235,6 +235,14 @@ check_%:
 	@$(MAKE) --no-print-directory $(subst check_,,$@)
 	@echo
 
+uorb_graphs:
+	@./Tools/uorb_graph/create_from_startupscript.sh
+	@./Tools/uorb_graph/create.py --src-path src --exclude-path src/examples --file Tools/uorb_graph/graph_full
+	@$(MAKE) --no-print-directory px4fmu-v2_default uorb_graph
+	@$(MAKE) --no-print-directory px4fmu-v4_default uorb_graph
+	@$(MAKE) --no-print-directory posix_sitl_default uorb_graph
+
+
 .PHONY: coverity_scan
 
 coverity_scan: posix_sitl_default
@@ -256,37 +264,6 @@ module_documentation:
 
 px4_metadata: parameters_metadata airframe_metadata module_documentation
 
-# S3 upload helpers
-# --------------------------------------------------------------------
-# s3cmd uses these ENV variables
-#  AWS_ACCESS_KEY_ID
-#  AWS_SECRET_ACCESS_KEY
-#  AWS_S3_BUCKET
-.PHONY: s3put_firmware s3put_qgc_firmware s3put_px4fmu_firmware s3put_misc_qgc_extra_firmware s3put_metadata s3put_scan-build s3put_cppcheck s3put_coverage
-
-s3put_qgc_firmware: s3put_px4fmu_firmware s3put_misc_qgc_extra_firmware
-
-s3put_px4fmu_firmware: px4fmu_firmware
-	@find $(SRC_DIR)/build -name "*.px4" -exec $(SRC_DIR)/Tools/s3put.sh "{}" \;
-
-s3put_misc_qgc_extra_firmware: misc_qgc_extra_firmware
-	@find $(SRC_DIR)/build -name "*.px4" -exec $(SRC_DIR)/Tools/s3put.sh "{}" \;
-
-s3put_metadata: px4_metadata
-	@$(SRC_DIR)/Tools/s3put.sh airframes.md
-	@$(SRC_DIR)/Tools/s3put.sh airframes.xml
-	@$(SRC_DIR)/Tools/s3put.sh parameters.xml
-	@$(SRC_DIR)/Tools/s3put.sh parameters.md
-
-s3put_scan-build: scan-build
-	@$(SRC_DIR)/Tools/s3put.sh `find $(SRC_DIR)/build/scan-build -mindepth 1 -maxdepth 1 -type d`/
-
-s3put_cppcheck: cppcheck
-	@$(SRC_DIR)/Tools/s3put.sh $(SRC_DIR)/build/cppcheck/
-
-s3put_coverage: tests_coverage
-	@$(SRC_DIR)/Tools/s3put.sh $(SRC_DIR)/build/posix_sitl_default/coverage-html/
-
 # Astyle
 # --------------------------------------------------------------------
 .PHONY: check_format format
@@ -302,7 +279,7 @@ format:
 
 # Testing
 # --------------------------------------------------------------------
-.PHONY: tests tests_coverage
+.PHONY: tests tests_coverage tests_mission tests_offboard rostest
 
 tests:
 	@$(MAKE) --no-print-directory posix_sitl_default test_results \
@@ -310,9 +287,25 @@ tests:
 	UBSAN_OPTIONS="color=always"
 
 tests_coverage:
+	@$(MAKE) clean
+	@$(MAKE) --no-print-directory posix_sitl_default PX4_CMAKE_BUILD_TYPE=Coverage
+	@$(MAKE) --no-print-directory posix_sitl_default sitl_gazebo PX4_CMAKE_BUILD_TYPE=Coverage
+	@$(SRC_DIR)/test/rostest_px4_run.sh mavros_posix_tests_missions.test
+	@$(SRC_DIR)/test/rostest_px4_run.sh mavros_posix_tests_offboard_attctl.test
+	@$(SRC_DIR)/test/rostest_px4_run.sh mavros_posix_tests_offboard_posctl.test
 	@$(MAKE) --no-print-directory posix_sitl_default test_coverage_genhtml PX4_CMAKE_BUILD_TYPE=Coverage
 	@echo "Open $(SRC_DIR)/build/posix_sitl_default/coverage-html/index.html to see coverage"
 
+rostest: posix_sitl_default
+	@$(MAKE) --no-print-directory posix_sitl_default sitl_gazebo
+
+tests_mission: rostest
+	@$(SRC_DIR)/test/rostest_px4_run.sh mavros_posix_tests_missions.test
+
+tests_offboard: rostest
+	@$(SRC_DIR)/test/rostest_px4_run.sh mavros_posix_tests_offboard_attctl.test
+	@$(SRC_DIR)/test/rostest_px4_run.sh mavros_posix_tests_offboard_posctl.test
+
 # static analyzers (scan-build, clang-tidy, cppcheck)
 # --------------------------------------------------------------------
 .PHONY: scan-build posix_sitl_default-clang clang-tidy clang-tidy-fix clang-tidy-quiet cppcheck check_stack
@@ -320,9 +313,12 @@ tests_coverage:
 scan-build:
 	@export CCC_CC=clang
 	@export CCC_CXX=clang++
+	@rm -rf $(SRC_DIR)/build/posix_sitl_default-scan-build
+	@rm -rf $(SRC_DIR)/build/scan-build/report_latest
 	@mkdir -p $(SRC_DIR)/build/posix_sitl_default-scan-build
 	@cd $(SRC_DIR)/build/posix_sitl_default-scan-build && scan-build cmake $(SRC_DIR) -GNinja -DCONFIG=posix_sitl_default
 	@scan-build -o $(SRC_DIR)/build/scan-build cmake --build $(SRC_DIR)/build/posix_sitl_default-scan-build
+	@find $(SRC_DIR)/build/scan-build -maxdepth 1 -mindepth 1 -type d -exec cp -r "{}" $(SRC_DIR)/build/scan-build/report_latest \;
 
 posix_sitl_default-clang:
 	@mkdir -p $(SRC_DIR)/build/posix_sitl_default-clang

README.md

@@ -1,34 +1,42 @@
-## PX4 Pro Drone Autopilot ##
+# PX4 Pro Drone Autopilot
 
-[![Releases](https://img.shields.io/github/release/PX4/Firmware.svg)](https://github.com/PX4/Firmware/releases) [![DOI](https://zenodo.org/badge/22634/PX4/Firmware.svg)](https://zenodo.org/badge/latestdoi/22634/PX4/Firmware) [![Build Status](https://travis-ci.org/PX4/Firmware.svg?branch=master)](https://travis-ci.org/PX4/Firmware) [![Coverity Scan](https://scan.coverity.com/projects/3966/badge.svg?flat=1)](https://scan.coverity.com/projects/3966?tab=overview)
+[![Releases](https://img.shields.io/github/release/PX4/Firmware.svg)](https://github.com/PX4/Firmware/releases) [![DOI](https://zenodo.org/badge/22634/PX4/Firmware.svg)](https://zenodo.org/badge/latestdoi/22634/PX4/Firmware)
+
+[![Build Status](http://ci.px4.io:8080/buildStatus/icon?job=Firmware/master)](http://ci.px4.io:8080/blue/organizations/jenkins/Firmware/activity) [![Coverity Scan](https://scan.coverity.com/projects/3966/badge.svg?flat=1)](https://scan.coverity.com/projects/3966?tab=overview)
 
 [![Slack](https://px4-slack.herokuapp.com/badge.svg)](http://slack.px4.io)
 
 This repository holds the [PX4 Pro](http://px4.io) flight control solution for drones, with the main applications located in the [src/modules](https://github.com/PX4/Firmware/tree/master/src/modules) directory. It also contains the PX4 Drone Middleware Platform, which provides drivers and middleware to run drones.
 
-  * Official Website: http://px4.io (License: BSD 3-clause, [LICENSE.md](https://github.com/PX4/Firmware/blob/master/LICENSE.md))
-  * Supported airframes:
-    * [Multicopters](http://px4.io/portfolio_category/multicopter/)
-    * [Fixed wing](http://px4.io/portfolio_category/plane/)
-    * [VTOL](http://px4.io/portfolio_category/vtol/)
-    * many more experimental types (Rovers, Blimps, Boats, Submarines, etc)
-  * Releases: [Downloads](https://github.com/PX4/Firmware/releases)
+* Official Website: http://px4.io (License: BSD 3-clause, [LICENSE](https://github.com/PX4/Firmware/blob/master/LICENSE))
+* [Supported airframes](https://docs.px4.io/en/airframes/airframe_reference.html) ([portfolio](http://px4.io/#airframes)):
+  * [Multicopters](https://docs.px4.io/en/airframes/airframe_reference.html#copter)
+  * [Fixed wing](https://docs.px4.io/en/airframes/airframe_reference.html#plane)
+  * [VTOL](https://docs.px4.io/en/airframes/airframe_reference.html#vtol)
+  * many more experimental types (Rovers, Blimps, Boats, Submarines, etc)
+* Releases: [Downloads](https://github.com/PX4/Firmware/releases)
+
+
+## PX4 Users
+
+The [PX4 User Guide](https://docs.px4.io/en/) explains how to assemble [supported vehicles](https://docs.px4.io/en/airframes/airframe_reference.html) and fly drones with PX4. 
+See the [forum and chat](https://docs.px4.io/en/#support) if you need help!
+
+
+## PX4 Developers
+
+This [Developer Guide](https://dev.px4.io/) is for software developers who want to modify the flight stack and middleware (e.g. to add new flight modes), hardware integrators who want to support new flight controller boards and peripherals, any anyone who wants to get PX4 working on a new (unsupported) airframe/vehicle.
+
+Developers should read the [Guide for Contributions](https://dev.px4.io/en/contribute/).
+See the [forum and chat](https://dev.px4.io/en/#support) if you need help!
 
-Please refer to the [user documentation](https://docs.px4.io/en/) and [user forum](http://discuss.px4.io) for flying drones with the PX4 flight stack.
 
 ### Weekly Dev Call
 
-The PX4 Dev Team syncs up on a [weekly dev call](https://dev.px4.io/en/contribute/).
+The PX4 Dev Team syncs up on a [weekly dev call](https://dev.px4.io/en/contribute/#dev_call).
 
-  * [Wednesday 17:00 Central European Time, 11:00 Eastern Time, 08:00 Pacific Standard Time](https://www.google.com/calendar/embed?src=bGludXhmb3VuZGF0aW9uLm9yZ19nMjF0dmFtMjRtN3BtN2poZXYwMWJ2bHFoOEBncm91cC5jYWxlbmRhci5nb29nbGUuY29t)
-  * [Uber conference (dial-in or web client)](https://www.uberconference.com/lf-dronecode)
-  * The agenda is announced in advance on the [PX4 Discuss](http://discuss.px4.io/c/weekly-dev-call)
-  * Issues and PRs may be labelled [devcall](https://github.com/PX4/Firmware/issues?q=is%3Aopen+is%3Aissue+label%3Adevcall) to flag them for discussion
+> **Note** The dev call is open to all interested developers (not just the core dev team). This is a great opportunity to meet the team and contribute to the ongoing development of the platform.
 
-### Developers ###
-  * [Developer Guide](https://dev.px4.io/)
-    * [Build instructions](https://dev.px4.io/en/setup/building_px4.html)
-    * [Guide for Contributions](https://dev.px4.io/en/contribute/)
 
 ## Maintenance Team
 
@@ -71,28 +79,32 @@ The PX4 Dev Team syncs up on a [weekly dev call](https://dev.px4.io/en/contribut
   * [Airmind MindPX / MindRacer](https://github.com/PX4/Firmware/labels/mindpx) - [Henry Zhang](https://github.com/iZhangHui)
   * RTPS/ROS2 Interface - [Vicente Monge](https://github.com/vicenteeprosima)
 
+See also [About Us](http://px4.io/about-us/#development_team) (px4.io) and the [contributors list](https://github.com/PX4/Firmware/graphs/contributors) (Github).
+
 ## Supported Hardware
 
 This repository contains code supporting these boards:
-  * [Snapdragon Flight](https://dev.px4.io/en/flight_controller/snapdragon_flight.html)
-  * [Intel Aero](https://dev.px4.io/en/flight_controller/intel_aero.html)
-  * [Raspberry PI with Navio 2](https://dev.px4.io/en/flight_controller/raspberry_pi.html)
+  * [Snapdragon Flight](https://docs.px4.io/en/flight_controller/snapdragon_flight.html)
+  * [Intel Aero](https://docs.px4.io/en/flight_controller/intel_aero.html)
+  * [Raspberry PI with Navio 2](https://docs.px4.io/en/flight_controller/raspberry_pi_navio2.html)
   * [Parrot Bebop 2](https://dev.px4.io/en/advanced/parrot_bebop.html)
   * FMUv2.x
-    * [Pixhawk](https://dev.px4.io/en/flight_controller/pixhawk.html)
-    * Pixhawk Mini
-    * [Pixfalcon](https://dev.px4.io/en/flight_controller/pixfalcon.html)
+    * [Pixhawk](https://docs.px4.io/en/flight_controller/pixhawk.html)
+    * [Pixhawk Mini](https://docs.px4.io/en/flight_controller/pixhawk_mini.html)
+    * [Pixfalcon](https://docs.px4.io/en/flight_controller/pixfalcon.html)
   * FMUv3.x [Pixhawk 2](https://pixhawk.org/modules/pixhawk2)
   * FMUv4.x
-    * [Pixracer](https://dev.px4.io/en/flight_controller/pixracer.html)
-    * Pixhawk 3 Pro
+    * [Pixracer](https://docs.px4.io/en/flight_controller/pixracer.html)
+    * [Pixhawk 3 Pro](https://docs.px4.io/en/flight_controller/pixhawk3_pro.html)
   * FMUv5.x (ARM Cortex M7, future Pixhawk)
-  * STM32F4Discovery (basic support) [Tutorial](https://pixhawk.org/modules/stm32f4discovery)
-  * Gumstix AeroCore (v1 and v2)
+  * [STM32F4Discovery](http://www.st.com/en/evaluation-tools/stm32f4discovery.html) (basic support) [Tutorial](https://pixhawk.org/modules/stm32f4discovery)
+  * [Gumstix AeroCore](https://www.gumstix.com/aerocore-2/) (only v2)
   * [Airmind MindPX V2.8](http://www.mindpx.net/assets/accessories/UserGuide_MindPX.pdf)
   * [Airmind MindRacer V1.2](http://mindpx.net/assets/accessories/mindracer_user_guide_v1.2.pdf)
-  * [Bitcraze Crazyflie 2.0](https://dev.px4.io/en/flight_controller/crazyflie2.html)
+  * [Bitcraze Crazyflie 2.0](https://docs.px4.io/en/flight_controller/crazyflie2.html)
 
-## Project Milestones
+Additional information about supported hardware can be found in [PX4 user Guide > Autopilot Hardware](https://docs.px4.io/en/flight_controller/).
 
-The PX4 software and Pixhawk hardware (which has been designed for it) has been created in 2011 by [Lorenz Meier](https://github.com/LorenzMeier).
+## Project Roadmap
+
+A high level project roadmap is available [here](https://www.dronecode.org/roadmap/).

ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil

@@ -8,7 +8,6 @@
 # @maintainer Lorenz Meier <lorenz@px4.io>
 #
 
-sh /etc/init.d/rc.mc_defaults
+sh /etc/init.d/4001_quad_x
 
-set MIXER quad_x
 param set SYS_HITL 1

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -372,6 +372,12 @@ then
 	teraranger start
 fi
 
+# Benewake TFMini
+if param greater SENS_EN_TFMINI 0
+then
+	tfmini start
+fi
+
 # Wait 20 ms for sensors (because we need to wait for the HRT and work queue callbacks to fire)
 usleep 20000
 sensors start

ROMFS/px4fmu_common/init.d/rc.vtol_defaults

@@ -4,20 +4,17 @@ set VEHICLE_TYPE vtol
 
 if [ $AUTOCNF == yes ]
 then
-	param set MC_ROLL_P 6.0
-	param set MC_PITCH_P 6.0
-	param set MC_YAW_P 4
-
-	#
-	# Default parameters for mission and position handling
-	#
-	param set NAV_ACC_RAD 3
-	param set MPC_TKO_SPEED 1.0
-	param set MPC_LAND_SPEED 0.7
-	param set MPC_Z_VEL_MAX_DN 1.5
-	param set MPC_XY_VEL_MAX 4.0
+	param set MIS_TAKEOFF_ALT 20
 	param set MIS_YAW_TMT 10
+
 	param set MPC_ACC_HOR_MAX 2.0
+	param set MPC_LAND_SPEED 0.7
+	param set MPC_TKO_SPEED 1.0
+	param set MPC_XY_VEL_MAX 4.0
+	param set MPC_Z_VEL_MAX_DN 1.5
+
+	param set NAV_ACC_RAD 3
+
 	param set RTL_LAND_DELAY 0
 fi
 

ROMFS/px4fmu_common/init.d/rcS

@@ -194,7 +194,7 @@ then
 	set MAV_TYPE none
 	set FAILSAFE none
 	set USE_IO no
-	set LOGGER_BUF 16
+	set LOGGER_BUF 14
 
 	if ver hwcmp PX4FMU_V4
 	then

Tools/check_submodules.sh

@@ -3,8 +3,17 @@
 function check_git_submodule {
 
 # The .git exists in a submodule if init and update have been done.
-if [ "$CI" != "true" ] && [[ -f $1"/.git" || -d $1"/.git" ]];
+if [[ -f $1"/.git" || -d $1"/.git" ]];
 then
+
+	if [ "$CI" == "true" ];
+	then
+		git submodule sync --recursive -- $1
+		git submodule update --init --recursive --force -- $1  || true
+		git submodule update --init --recursive --force -- $1
+		exit 0
+	fi
+
 	SUBMODULE_STATUS=$(git submodule summary "$1")
 	STATUSRETVAL=$(echo $SUBMODULE_STATUS | grep -A20 -i "$1")
 	if ! [[ -z "$STATUSRETVAL" ]];
@@ -35,8 +44,8 @@ then
 		elif [ "$user_cmd" == "u" ]
 		then
 			git submodule sync --recursive -- $1
-			git submodule update --init --recursive --force --quiet -- $1 || true
-			git submodule update --init --recursive -- $1
+			git submodule update --init --recursive -- $1 || true
+			git submodule update --init --recursive --force -- $1
 			echo "Submodule fixed, continuing build.."
 		else
 			echo "Build aborted."
@@ -44,9 +53,9 @@ then
 		fi
 	fi
 else
-	git submodule sync --recursive -- $1
-	git submodule update --init --recursive --force -- $1  || true
-	git submodule update --init --recursive --force -- $1
+	git submodule sync --recursive --quiet -- $1
+	git submodule update --init --recursive -- $1  || true
+	git submodule update --init --recursive -- $1
 fi
 
 }

Tools/docker_run.sh

@@ -7,22 +7,22 @@ if [ -z ${PX4_DOCKER_REPO+x} ]; then
 		PX4_DOCKER_REPO="px4io/px4-dev-nuttx:2017-10-23"
 	elif [[ $@ =~ .*rpi.* ]] || [[ $@ =~ .*bebop.* ]]; then
 		# posix_rpi_cross, posix_bebop_default
-		PX4_DOCKER_REPO="px4io/px4-dev-raspi:2017-10-23"
+		PX4_DOCKER_REPO="px4io/px4-dev-raspi:2017-12-30"
 	elif [[ $@ =~ .*eagle.* ]] || [[ $@ =~ .*excelsior.* ]]; then
 		# eagle, excelsior
-		PX4_DOCKER_REPO="lorenzmeier/px4-dev-snapdragon:2017-10-23"
+		PX4_DOCKER_REPO="lorenzmeier/px4-dev-snapdragon:2017-12-29"
 	elif [[ $@ =~ .*ocpoc.* ]]; then
 		# posix_ocpoc_ubuntu
-		PX4_DOCKER_REPO="px4io/px4-dev-armhf:2017-10-23"
+		PX4_DOCKER_REPO="px4io/px4-dev-armhf:2017-12-30"
 	elif [[ $@ =~ .*clang.* ]] || [[ $@ =~ .*scan-build.* ]]; then	
 		# clang tools
-		PX4_DOCKER_REPO="px4io/px4-dev-clang:2017-10-23"
+		PX4_DOCKER_REPO="px4io/px4-dev-clang:2017-12-30"
 	elif [[ $@ =~ .*cppcheck.* ]]; then
 		# TODO: remove this once px4io/px4-dev-base updates
 		PX4_DOCKER_REPO="px4io/px4-dev-base:ubuntu17.10"
 	elif [[ $@ =~ .*tests* ]]; then
 		# run all tests with simulation
-		PX4_DOCKER_REPO="px4io/px4-dev-simulation:2017-10-23"
+		PX4_DOCKER_REPO="px4io/px4-dev-simulation:2017-12-30"
 	fi
 else
 	echo "PX4_DOCKER_REPO is set to '$PX4_DOCKER_REPO'";

Tools/px_uploader.py

@@ -488,9 +488,18 @@ class uploader(object):
                 print("FORCED WRITE, FLASHING ANYWAY!")
             else:
                 raise IOError(msg)
+
+        # Prevent uploads where the image would overflow the flash
         if self.fw_maxsize < fw.property('image_size'):
             raise RuntimeError("Firmware image is too large for this board")
 
+        # Prevent uploads where the maximum image size of the board config is smaller than the flash
+        # of the board. This is a hint the user chose the wrong config and will lack features
+        # for this particular board.
+        if self.fw_maxsize > fw.property('image_maxsize'):
+            raise RuntimeError("Board can accept larger flash images (%u bytes) than board config (%u bytes). Please use the correct board configuration to avoid lacking critical functionality."
+                % (self.fw_maxsize, fw.property('image_maxsize')))
+
         # OTP added in v4:
         if self.bl_rev > 3:
             for byte in range(0, 32*6, 4):

Tools/s3put.sh

@@ -1,23 +0,0 @@
-#!/bin/bash
-
-filename=${1}
-
-# Requires these ENV variables
-# AWS_ACCESS_KEY_ID
-# AWS_SECRET_ACCESS_KEY
-# AWS_S3_BUCKET
-
-[ -z "$AWS_ACCESS_KEY_ID" ] && { echo "ERROR: Need to set AWS_ACCESS_KEY_ID"; exit 1; }
-[ -z "$AWS_SECRET_ACCESS_KEY" ] && { echo "ERROR: Need to set AWS_SECRET_ACCESS_KEY"; exit 1; }
-[ -z "$AWS_S3_BUCKET" ] && { echo "ERROR: Need to set AWS_S3_BUCKET"; exit 1; }
-
-if [ -f ${filename} ]; then
-	base_file_name=`basename $filename`
-	s3cmd --access_key=${AWS_ACCESS_KEY_ID} --secret_key=${AWS_SECRET_ACCESS_KEY} put ${filename} s3://${AWS_S3_BUCKET}/${base_file_name}
-elif [ -d ${filename} ]; then
-	dir_name=$filename
-	s3cmd --access_key=${AWS_ACCESS_KEY_ID} --secret_key=${AWS_SECRET_ACCESS_KEY} put -r ${dir_name} s3://${AWS_S3_BUCKET}/
-else
-	echo "ERROR: ${file} doesn't exist"
-	exit 1
-fi

Tools/uorb_graph/.gitignore

@@ -0,0 +1,2 @@
+*.json
+

Tools/uorb_graph/create.py

@@ -0,0 +1,641 @@
+#! /usr/bin/env python
+
+from __future__ import print_function
+
+import argparse
+import os
+import codecs
+import re
+import colorsys
+import json
+
+
+
+parser = argparse.ArgumentParser(
+    description='Generate uORB pub/sub dependency graph from source code')
+
+parser.add_argument('-s', '--src-path', action='append',
+                    help='Source path(s) (default=src, can be specified multiple times)',
+                    default=[])
+parser.add_argument('-e', '--exclude-path', action='append',
+                    help='Excluded path(s), can be specified multiple times',
+                    default=[])
+parser.add_argument('-f', '--file', metavar='file', action='store',
+                    help='output file name prefix',
+                    default='graph')
+parser.add_argument('-o', '--output', metavar='output', action='store',
+                    help='output format (json or graphviz)',
+                    default='json')
+parser.add_argument('--use-topic-union', action='store_true',
+                    help='''
+Use the union of all publication and subscription topics (useful for complete
+graphs or only few/single module(s)). The default is to use the intersection
+(remove topics that have no subscriber or no publisher)''')
+parser.add_argument('-m', '--modules', action='store',
+                    help='Comma-separated whitelist of modules (the module\'s '+
+                    'MAIN, e.g. from a startup script)',
+                    default='')
+
+args = parser.parse_args()
+
+
+g_debug = False
+def dbg_print(string):
+    if g_debug:
+        print(string)
+
+
+def get_N_colors(N, s=0.8, v=0.9):
+    """ get N distinct colors as a list of hex strings """
+    HSV_tuples = [(x*1.0/N, s, v) for x in range(N)]
+    hex_out = []
+    for rgb in HSV_tuples:
+        rgb = map(lambda x: int(x*255), colorsys.hsv_to_rgb(*rgb))
+        hex_out.append("#"+"".join(map(lambda x: format(x, '02x'), rgb)))
+    return hex_out
+
+
+class PubSub:
+    """ Collects either publication or subscription information for nodes
+    (modules and topics) & edges """
+
+    def __init__(self, is_publication, topic_blacklist, orb_pub_sub_regexes, special_cases):
+        """
+            :param is_publication: if True, publications, False for
+            subscriptions
+            :param topic_blacklist: list of topics to blacklist
+            :param orb_pub_sub_regexes: list of regexes to extract orb calls
+                (e.g. orb_subscribe). They need to have 2 captures, the second
+                one is the one capturing ORB_ID(<topic>
+        """
+        self._module_pubsubs = {} # key = module name, value = set of topic names
+        self._special_cases = special_cases
+        self._special_cases_matched = None
+        self._topic_blacklist = topic_blacklist
+        self._orb_pub_sub_regexes = orb_pub_sub_regexes
+        if is_publication:
+            self._method = 'Publication'
+        else:
+            self._method = 'Subscription'
+
+    def reset(self):
+        self._special_cases_matched = [False]*len(self._special_cases)
+
+    def filter_modules(self, module_whitelist):
+        remove = [k for k in self._module_pubsubs if k not in module_whitelist]
+        for k in remove: del self._module_pubsubs[k]
+
+
+    def check_if_match_found(self, modules):
+        """ check if all special cases got a match (if not, it means the source
+            code got changed)
+        """
+        for i, (module_match, file_match_re, src_match_re, _) in enumerate(self._special_cases):
+            if module_match in modules and src_match_re is not None:
+                if not self._special_cases_matched[i]:
+                    raise Exception('Module '+module_match+
+                            ': no match for '+self._method+' special case'+
+                            src_match_re.pattern+'. The case needs to be updated')
+
+
+    def extract(self, file_name, src_str, module, orb_id_vehicle_attitude_controls_topic):
+        """ Extract subscribed/published topics from a source string
+        :param src_str: string of C/C++ code with comments and whitespace removed
+        """
+
+        orb_pubsub_matches = []
+        for regex in self._orb_pub_sub_regexes:
+            orb_pubsub_matches += re.findall(regex, src_str)
+
+        orb_id = 'ORB_ID('
+
+        for _, match in orb_pubsub_matches:
+            if match == 'ORB_ID_VEHICLE_ATTITUDE_CONTROLS': # special case
+                match = orb_id+orb_id_vehicle_attitude_controls_topic
+
+            # match has the form: '[ORB_ID(]<topic_name>'
+            if match.startswith(orb_id):
+                topic_name = match[len(orb_id):]
+                self._add_topic(topic_name, file_name, module)
+            else:
+                ignore_found = False
+                for module_match, file_match_re, _, ignore_re in self._special_cases:
+                    if module == module_match:
+                        if file_match_re.search(file_name):
+                            if ignore_re.search(match):
+                                ignore_found = True
+                if not ignore_found:
+                    # If we land here, we need to add another special case
+                    raise Exception(self._method+' w/o ORB_ID(): '+match+' in '
+                            +file_name+' ('+module+'). You need to add another special case.')
+
+        # handle special cases
+        for i, (module_match, file_match_re, src_match_re, _) in enumerate(self._special_cases):
+            if src_match_re is None:
+                continue
+            if module == module_match:
+                if file_match_re.search(file_name):
+                    matches = src_match_re.findall(src_str)
+                    for match in matches:
+                        # match has the form: '[ORB_ID(]<topic_name>'
+                        if match.startswith(orb_id):
+                            topic_name = match[len(orb_id):]
+                            dbg_print('Found '+self._method+' for special case in '
+                                      +module+': '+topic_name)
+                            self._add_topic(topic_name, file_name, module)
+                            self._special_cases_matched[i] = True
+                        else:
+                            # this is not fatal, as it could be a method delaration/definition
+                            dbg_print('Special case '+self._method+' w/o ORB_ID(): '
+                                      +match+' in '+file_name+' ('+module+')')
+
+    def _add_topic(self, topic_name, file_name, module):
+        """ add a subscription/publication for a module
+        """
+        if topic_name in self._topic_blacklist:
+            dbg_print('ignoring blacklisted topic '+topic_name)
+            return
+
+        if module is None:
+            if not file_name.endswith('hott/messages.cpp'): # hott has a special module structure. just ignore it
+                print('Warning: found '+self._method+' without associated module: '
+                      +topic_name+' in '+file_name)
+            return
+
+        if not module in self._module_pubsubs:
+            self._module_pubsubs[module] = set()
+        self._module_pubsubs[module].add(topic_name)
+
+
+    def get_topics(self, modules):
+        """ get the set of topics
+            :param modules: list of modules to take into account
+        """
+        topics = set()
+        for module in modules:
+            if module in self._module_pubsubs:
+                topics |= self._module_pubsubs[module]
+        return topics
+
+    @property
+    def pubsubs(self):
+        """ get dict of all publication/subscriptions (key=modules, value=set of
+            topic names"""
+        return self._module_pubsubs
+
+
+class Graph:
+    """ Collects Node and Edge information by parsing the source tree """
+    def __init__(self, module_whitelist=[], topic_blacklist=[]):
+        self._current_module = [] # stack with current module (they can be nested)
+        self._all_modules = set() # set of all found modules
+
+        self._comment_remove_pattern = re.compile(
+            r'//.*?$|/\*.*?\*/|\'(?:\\.|[^\\\'])*\'|"(?:\\.|[^\\"])*"',
+            re.DOTALL | re.MULTILINE)
+        self._whitespace_pattern = re.compile(r'\s+')
+        self._module_whitelist = module_whitelist
+        self._excluded_paths = []
+
+        self._orb_id_vehicle_attitude_controls_topic = 'actuator_controls_0'
+        self._orb_id_vehicle_attitude_controls_re = \
+            re.compile(r'\#define\s+ORB_ID_VEHICLE_ATTITUDE_CONTROLS\s+([^,)]+)')
+
+        self._module_subscriptions = {} # key = module name, value = set of topic names
+        self._module_publications = {} # key = module name, value = set of topic names
+
+        self._modules = set() # all modules
+        self._topics = set() # all topics
+        self._topic_colors = {} # key = topic, value = color (html string)
+
+    # handle special cases
+    # format: list of tuples with 4 entries:
+    # - module name to match (module MAIN)
+    # - regex for file name(s) to match within the module (matched against the full path)
+    # - regex to extract the topic name: the match must be ORB_ID(<topic_name>
+    #   Note: whitespace is removed from source code, so it does not need to be
+    #   accounted for in the regex.
+    #   If this is None, it will just be ignored
+    # - regex to ignore matches in the form orb_[subscribe|advertise](<match>
+    #   (the expectation is that the previous matching ORB_ID() will be passed
+    #   to this, so that we can ignore it)
+        special_cases_sub = [
+    ('sensors', r'voted_sensors_update\.cpp$', r'\binit_sensor_class\b\(([^,)]+)', r'^meta$'),
+    ('mavlink', r'.*', r'\badd_orb_subscription\b\(([^,)]+)', r'^_topic$'),
+    ('sdlog2', r'.*', None, r'^topic$'),
+    ('logger', r'.*', None, r'^(topic|sub\.metadata|_polling_topic_meta)$'),
+
+    ('uavcan', r'uavcan_main\.cpp$', r'\b_control_topics\[[0-9]\]=([^,)]+)', r'^_control_topics\[i\]$'),
+    ('tap_esc', r'.*', r'\b_control_topics\[[0-9]\]=([^,)]+)', r'^_control_topics\[i\]$'),
+    ('pwm_out_sim', r'.*', r'\b_control_topics\[[0-9]\]=([^,)]+)', r'^_control_topics\[i\]$'),
+    ('snapdragon_pwm_out', r'.*', r'\b_controls_topics\[[0-9]\]=([^,)]+)', r'^_controls_topics\[i\]$'),
+    ('fmu', r'.*', r'\b_control_topics\[[0-9]\]=([^,)]+)', r'^_control_topics\[i\]$'),
+    ('linux_pwm_out', r'.*', r'\b_controls_topics\[[0-9]\]=([^,)]+)', r'^_controls_topics\[i\]$'),
+    ]
+        special_cases_sub = [(a, re.compile(b), re.compile(c) if c is not None else None, re.compile(d))
+                                   for a,b,c,d in special_cases_sub]
+
+        self._subscriptions = PubSub(False, topic_blacklist,
+                [r"\borb_subscribe(_multi|)\b\(([^,)]+)"],
+                special_cases_sub)
+
+
+        special_cases_pub = [
+    ('replay', r'replay_main\.cpp$', None, r'^sub\.orb_meta$'),
+    ('fw_pos_control_l1', r'FixedwingPositionControl\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'),
+    
+    ('mc_pos_control', r'mc_pos_control_main\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'),
+
+    ('mc_att_control', r'mc_att_control_main\.cpp$', r'\b_rates_sp_id=([^,)]+)', r'^_rates_sp_id$'),
+    ('mc_att_control', r'mc_att_control_main\.cpp$', r'\b_actuators_id=([^,)]+)', r'^_actuators_id$'),
+
+    ('fw_att_control', r'fw_att_control_main\.cpp$', r'\b_rates_sp_id=([^,)]+)', r'^_rates_sp_id$'),
+    ('fw_att_control', r'fw_att_control_main\.cpp$', r'\b_actuators_id=([^,)]+)', r'^_actuators_id$'),
+    ('fw_att_control', r'fw_att_control_main\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'),
+
+    ('uavcan', r'sensors/.*\.cpp$', r'\bUavcanCDevSensorBridgeBase\([^{]*DEVICE_PATH,([^,)]+)', r'^_orb_topic$'),
+    ('batt_smbus', r'batt_smbus\.cpp$', r'\b_batt_orb_id=([^,)]+)', r'^_batt_orb_id$'),
+    ]
+        special_cases_pub = [(a, re.compile(b), re.compile(c) if c is not None else None, re.compile(d))
+                                   for a,b,c,d in special_cases_pub]
+        self._publications = PubSub(True, topic_blacklist,
+                [r"\borb_advertise(_multi|_queue|_multi_queue|)\b\(([^,)]+)",
+                 r"\borb_publish_auto()\b\(([^,)]+)"],
+                special_cases_pub)
+
+
+    def _get_current_module(self):
+        if len(self._current_module) == 0:
+            return None
+        return self._current_module[-1]
+
+    def build(self, src_path_list, excluded_paths=[], use_topic_pubsub_union=True):
+        """ parse the source tree & extract pub/sub information.
+            :param use_topic_pubsub_union: if true, use all topics that have a
+            publisher or subscriber. If false, use only topics with at least one
+            publisher and subscriber.
+
+            fill in self._module_subsciptions & self._module_publications
+        """
+
+        self._subscriptions.reset()
+        self._publications.reset()
+
+        self._excluded_paths = [os.path.normpath(p) for p in excluded_paths]
+        for path in src_path_list:
+            self._build_recursive(path)
+
+        # filter by whitelist
+        if len(self._module_whitelist) > 0:
+            self._subscriptions.filter_modules(self._module_whitelist)
+            self._publications.filter_modules(self._module_whitelist)
+
+        # modules & topics sets
+        self._modules = set(self._publications.pubsubs.keys() +
+                self._subscriptions.pubsubs.keys())
+        print('number of modules: '+str(len(self._modules)))
+        self._topics = self._get_topics(use_topic_pubsub_union=use_topic_pubsub_union)
+        print('number of topics: '+str(len(self._topics)))
+
+        # initialize colors
+        color_list = get_N_colors(len(self._topics), 0.7, 0.85)
+        self._topic_colors = {}
+        for i, topic in enumerate(self._topics):
+            self._topic_colors[topic] = color_list[i]
+
+
+        # validate that all special rules got used
+        self._subscriptions.check_if_match_found(self._all_modules)
+        self._publications.check_if_match_found(self._all_modules)
+
+
+    def _get_topics(self, use_topic_pubsub_union=True):
+        """ get the set of topics
+        """
+        subscribed_topics = self._subscriptions.get_topics(self._modules)
+        published_topics = self._publications.get_topics(self._modules)
+        if use_topic_pubsub_union:
+            return subscribed_topics | published_topics
+        return subscribed_topics & published_topics
+
+    def _build_recursive(self, path):
+
+        if os.path.normpath(path) in self._excluded_paths:
+            dbg_print('ignoring excluded path '+path)
+            return
+
+        entries = os.listdir(path)
+
+        # check if entering a new module
+        cmake_file = 'CMakeLists.txt'
+        new_module = False
+        if cmake_file in entries:
+            new_module = self._extract_module_name(os.path.join(path, cmake_file))
+
+        # iterate directories recursively
+        for entry in entries:
+            file_name = os.path.join(path, entry)
+            if os.path.isdir(file_name):
+                self._build_recursive(file_name)
+
+
+        # iterate source files
+        # Note: we could skip the entries if we're not in a module, but we don't
+        # so that we get appropriate error messages to know where we miss subs
+        # or pubs
+        for entry in entries:
+            file_name = os.path.join(path, entry)
+            if os.path.isfile(file_name):
+                _, ext = os.path.splitext(file_name)
+                if ext in ['.cpp', '.c', '.h', '.hpp']:
+                    self._process_source_file(file_name)
+
+
+        if new_module:
+            self._current_module.pop()
+
+
+    def _extract_module_name(self, file_name):
+        """ extract the module name from a CMakeLists.txt file and store
+            in self._current_module if there is any """
+        datafile = file(file_name)
+        found_module_def = False
+        for line in datafile:
+            if 'px4_add_module' in line: # must contain 'px4_add_module'
+                found_module_def = True
+
+            words = line.split()
+            # get the definition of MAIN
+            if found_module_def and 'MAIN' in words and len(words) >= 2:
+                self._current_module.append(words[1])
+                self._all_modules.add(words[1])
+                dbg_print('Found module name: '+words[1])
+                return True
+        return False
+
+
+    def _process_source_file(self, file_name):
+        """ extract information from a single source file """
+
+        with codecs.open(file_name, 'r', 'utf-8') as f:
+            try:
+                content = f.read()
+            except:
+                print('Failed reading file: %s, skipping content.' % path)
+                return
+
+
+            current_module = self._get_current_module()
+            if current_module == 'uorb_tests': # skip this
+                return
+            if current_module == 'uorb':
+
+                # search and validate the ORB_ID_VEHICLE_ATTITUDE_CONTROLS define
+                matches = self._orb_id_vehicle_attitude_controls_re.findall(content)
+                for match in matches:
+                    if match != 'ORB_ID('+self._orb_id_vehicle_attitude_controls_topic:
+                        # if we land here, you need to change _orb_id_vehicle_attitude_controls_topic
+                        raise Exception(
+                            'The extracted define for ORB_ID_VEHICLE_ATTITUDE_CONTROLS '
+                            'is '+match+' but expected ORB_ID('+
+                            self._orb_id_vehicle_attitude_controls_topic)
+
+                return # skip uorb module for the rest
+
+
+
+            if content.lower().find('orb_') != -1: # approximative filter to quickly
+                                           # discard files we're not interested in
+                                           # (speedup the parsing)
+                src = self._comment_remover(content)
+                src = re.sub(self._whitespace_pattern, '', src) # remove all whitespace
+
+
+                # subscriptions
+                self._subscriptions.extract(file_name, src, current_module,
+                        self._orb_id_vehicle_attitude_controls_topic)
+
+                # publications
+                self._publications.extract(file_name, src, current_module,
+                        self._orb_id_vehicle_attitude_controls_topic)
+
+                # TODO: handle Publication & Subscription template classes
+
+
+
+    def _comment_remover(self, text):
+        """ remove C++ & C style comments.
+            Source: https://stackoverflow.com/a/241506 """
+        def replacer(match):
+            s = match.group(0)
+            if s.startswith('/'):
+                return " " # note: a space and not an empty string
+            else:
+                return s
+        return re.sub(self._comment_remove_pattern, replacer, text)
+
+
+    @property
+    def modules(self):
+        """ get the set of all modules """
+        return self._modules
+
+    @property
+    def topics(self):
+        """ get set set of all topics """
+        return self._topics
+
+    @property
+    def topic_colors(self):
+        """ get a dict of all topic colors with key=topic, value=color """
+        return self._topic_colors
+
+    @property
+    def module_subscriptions(self):
+        """ get a dict of all subscriptions with key=module name, value=set(topic names) """
+        return self._subscriptions.pubsubs
+
+    @property
+    def module_publications(self):
+        """ get a dict of all publications with key=module name, value=set(topic names) """
+        return self._publications.pubsubs
+
+
+
+class OutputGraphviz:
+    """ write graph using Graphviz """
+
+    def __init__(self, graph):
+        self._graph = graph
+
+    def write(self, file_name, engine='fdp',
+              show_publications=True, show_subscriptions=True):
+        """ write the graph to a file
+            :param engine: graphviz engine
+                - fdp works for large graphs
+                - neato works better for smaller graphs
+                - circo works for single modules
+                CLI: fdp graph.fv -Tpdf -o test.pdf
+        """
+
+        print('Writing to '+file_name)
+
+        ratio = 1 # aspect ratio
+
+        modules = self._graph.modules
+        topics = self._graph.topics
+        topic_colors = self._graph.topic_colors
+        module_publications = self._graph.module_publications
+        module_subscriptions = self._graph.module_subscriptions
+
+        graph_attr={'splines': 'true', 'ratio': str(ratio), 'overlap': 'false'}
+        graph_attr['sep'] = '"+15,15"' # increase spacing between nodes
+        graph = Digraph(comment='autogenerated graph with graphviz using uorb_graph.py',
+                engine=engine, graph_attr=graph_attr)
+
+
+        # nodes
+        for module in modules:
+            graph.node('m_'+module, module, shape='box', fontcolor='#ffffff',
+                    style='filled', color='#666666', fontsize='16')
+
+        for topic in topics:
+            graph.node('t_'+topic, topic, shape='ellipse', fontcolor='#ffffff',
+                    style='filled', color=topic_colors[topic])
+
+
+        # edges
+        if show_publications:
+            for module in modules:
+                if module in module_publications:
+                    for topic in module_publications[module]:
+                        if topic in topics:
+                            graph.edge('m_'+module, 't_'+topic,
+                                    color=topic_colors[topic], style='dashed')
+
+        if show_subscriptions:
+            for module in modules:
+                if module in module_subscriptions:
+                    for topic in module_subscriptions[module]:
+                        if topic in topics:
+                            graph.edge('t_'+topic, 'm_'+module,
+                                    color=topic_colors[topic])
+
+	graph.render(file_name, view=False)
+
+
+class OutputJSON:
+    """ write graph to a JSON file (that can be used with D3.js) """
+
+    def __init__(self, graph):
+        self._graph = graph
+
+    def write(self, file_name):
+
+        print('Writing to '+file_name)
+
+        modules = self._graph.modules
+        topics = self._graph.topics
+        topic_colors = self._graph.topic_colors
+        module_publications = self._graph.module_publications
+        module_subscriptions = self._graph.module_subscriptions
+
+        data = {}
+        nodes = []
+
+        # nodes
+        # (sort by length, such that short names are last. The rendering order
+        # will be the same, so that in case of an overlap, the shorter label
+        # will be on top)
+        for module in sorted(modules, key=len, reverse=True):
+            node = {}
+            node['id'] = 'm_'+module
+            node['name'] = module
+            node['type'] = 'module'
+            node['color'] = '#666666'
+            # TODO: add url to open module documentation?
+            nodes.append(node)
+
+        for topic in sorted(topics, key=len, reverse=True):
+            node = {}
+            node['id'] = 't_'+topic
+            node['name'] = topic
+            node['type'] = 'topic'
+            node['color'] = topic_colors[topic]
+            # url is opened when double-clicking on the node
+            # TODO: does not work for multi-topics
+            node['url'] = 'https://github.com/PX4/Firmware/blob/master/msg/'+topic+'.msg'
+            nodes.append(node)
+
+        data['nodes'] = nodes
+
+        edges = []
+
+        # edges
+        for module in modules:
+            if module in module_publications:
+                for topic in module_publications[module]:
+                    if topic in topics:
+                        edge = {}
+                        edge['source'] = 'm_'+module
+                        edge['target'] = 't_'+topic
+                        edge['color'] = topic_colors[topic]
+                        edge['style'] = 'dashed'
+                        edges.append(edge)
+
+        for module in modules:
+            if module in module_subscriptions:
+                for topic in module_subscriptions[module]:
+                    if topic in topics:
+                        edge = {}
+                        edge['source'] = 't_'+topic
+                        edge['target'] = 'm_'+module
+                        edge['color'] = topic_colors[topic]
+                        edge['style'] = 'normal'
+                        edges.append(edge)
+
+        data['links'] = edges
+
+        with open(file_name, 'w') as outfile:
+            json.dump(data, outfile) # add indent=2 for readable formatting
+
+
+
+# ignore topics that are subscribed/published by many topics, but are not really
+# useful to show in the graph
+topic_blacklist = [ 'parameter_update', 'mavlink_log', 'log_message' ]
+print('Excluded topics: '+str(topic_blacklist))
+
+if len(args.modules) == 0:
+    module_whitelist = []
+else:
+    module_whitelist = [ m.strip() for m in args.modules.split(',')]
+
+graph = Graph(module_whitelist=module_whitelist, topic_blacklist=topic_blacklist)
+if len(args.src_path) == 0:
+    args.src_path = ['src']
+graph.build(args.src_path, args.exclude_path, use_topic_pubsub_union=args.use_topic_union)
+
+
+if args.output == 'json':
+    output_json = OutputJSON(graph)
+    output_json.write(args.file+'.json')
+
+elif args.output == 'graphviz':
+    try:
+        from graphviz import Digraph
+    except:
+        print("Failed to import graphviz.")
+        print("You may need to install it with 'pip install graphviz'")
+        print("")
+        raise
+    output_graphviz = OutputGraphviz(graph)
+    engine='fdp' # use neato or fdp
+    output_graphviz.write(args.file+'.fv', engine=engine)
+    output_graphviz.write(args.file+'_subs.fv', show_publications=False, engine=engine)
+    output_graphviz.write(args.file+'_pubs.fv', show_subscriptions=False, engine=engine)
+else:
+    print('Error: unknown output format '+args.output)
+
+
+

Tools/uorb_graph/create_from_startupscript.sh

@@ -0,0 +1,13 @@
+#! /bin/bash
+# create the graph from a posix (e.g. SITL) startup script
+
+SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
+
+startup_file="$SCRIPT_DIR"/../../posix-configs/SITL/init/ekf2/typhoon_h480
+[ -n "$1" ] && startup_file=$1
+# get the modules as comma-separated list
+modules=$(cat "$startup_file"|cut -f1 -d' '|sort|uniq|tr '\n' ,)
+
+cd "$SCRIPT_DIR/../.."
+"$SCRIPT_DIR"/create.py --src-path src -m "$modules" -f "$SCRIPT_DIR/graph_runtime_sitl"
+

cmake/configs/nuttx_auav-x21_default.cmake

@@ -49,6 +49,7 @@ set(config_module_list
 	drivers/bst
 	drivers/snapdragon_rc_pwm
 	drivers/lis3mdl
+	drivers/tfmini
 
 	#
 	# System commands

cmake/configs/nuttx_mindpx-v2_default.cmake

@@ -52,6 +52,7 @@ set(config_module_list
 	drivers/camera_trigger
 	drivers/bst
 	drivers/snapdragon_rc_pwm
+	drivers/tfmini
 
 	#
 	# System commands

cmake/configs/nuttx_nxphlite-v3_default.cmake

@@ -59,6 +59,7 @@ set(config_module_list
 	drivers/teraranger
 	drivers/vmount
 	modules/sensors
+	drivers/tfmini
 
 	#
 	# System commands

cmake/configs/nuttx_px4-same70xplained-v1_default.cmake

@@ -48,6 +48,8 @@ set(config_module_list
 	drivers/bst
 	drivers/snapdragon_rc_pwm
 	drivers/lis3mdl
+	drivers/tfmini
+
 
 	#
 	# System commands
@@ -167,4 +169,4 @@ set(config_module_list
 
 	# Hardware test
 	#examples/hwtest
-)
+)

cmake/configs/nuttx_px4fmu-v2_default.cmake

@@ -59,6 +59,7 @@ set(config_module_list
 	#drivers/ulanding
 	drivers/vmount
 	modules/sensors
+	#drivers/tfmini
 
 	#
 	# System commands

cmake/configs/nuttx_px4fmu-v2_test.cmake

@@ -48,6 +48,7 @@ set(config_module_list
 	#drivers/bst
 	#drivers/snapdragon_rc_pwm
 	#drivers/lis3mdl
+	drivers/tfmini
 
 	#
 	# System commands

cmake/configs/nuttx_px4fmu-v3_default.cmake

@@ -2,6 +2,7 @@
 # FMUv3 is FMUv2 with access to the full 2MB flash
 set(BOARD px4fmu-v2 CACHE string "" FORCE)
 set(FW_NAME nuttx_px4fmu-v3_default.elf CACHE string "" FORCE)
+set(FW_PROTOTYPE px4fmu-v3 CACHE string "" FORCE)
 set(LD_SCRIPT ld_full.script CACHE string "" FORCE)
 
 include(nuttx/px4_impl_nuttx)
@@ -64,6 +65,7 @@ set(config_module_list
 	drivers/ulanding
 	drivers/vmount
 	modules/sensors
+	drivers/tfmini
 
 	#
 	# System commands

cmake/configs/nuttx_px4fmu-v4_default.cmake

@@ -56,6 +56,7 @@ set(config_module_list
 	drivers/teraranger
 	drivers/vmount
 	modules/sensors
+	drivers/tfmini
 
 	#
 	# System commands

cmake/configs/nuttx_px4fmu-v4pro_default.cmake

@@ -56,6 +56,7 @@ set(config_module_list
 	drivers/teraranger
 	drivers/vmount
 	modules/sensors
+	drivers/tfmini
 
 	#
 	# System commands

cmake/configs/nuttx_px4fmu-v5_default.cmake

@@ -56,6 +56,7 @@ set(config_module_list
 	drivers/teraranger
 	drivers/vmount
 	modules/sensors
+	drivers/tfmini
 
 	#
 	# System commands

cmake/configs/nuttx_px4nucleoF767ZI-v1_default.cmake

@@ -50,6 +50,7 @@ set(config_module_list
 	drivers/tap_esc
 	drivers/teraranger
 	modules/sensors
+	drivers/tfmini
 
 	#
 	# System commands

integrationtests/python_src/px4_it/mavros/mavros_offboard_attctl_test.py

@@ -37,21 +37,19 @@
 #
 # The shebang of this file is currently Python2 because some
 # dependencies such as pymavlink don't play well with Python3 yet.
+from __future__ import division
 
 PKG = 'px4'
 
 import unittest
 import rospy
-import rosbag
-import time
-
-from std_msgs.msg import Header
-from std_msgs.msg import Float64
-from geometry_msgs.msg import PoseStamped, Quaternion
+from threading import Thread
 from tf.transformations import quaternion_from_euler
-from mavros_msgs.srv import CommandLong
-from sensor_msgs.msg import NavSatFix
-#from px4_test_helper import PX4TestHelper
+from geometry_msgs.msg import PoseStamped, Quaternion, Vector3
+from mavros_msgs.msg import AttitudeTarget, HomePosition, State
+from mavros_msgs.srv import CommandBool, SetMode
+from std_msgs.msg import Header
+
 
 class MavrosOffboardAttctlTest(unittest.TestCase):
     """
@@ -62,90 +60,212 @@ class MavrosOffboardAttctlTest(unittest.TestCase):
     """
 
     def setUp(self):
-        rospy.init_node('test_node', anonymous=True)
-        rospy.wait_for_service('mavros/cmd/arming', 30)
-        #self.helper = PX4TestHelper("mavros_offboard_attctl_test")
-        #self.helper.setUp()
-
-        rospy.Subscriber("mavros/local_position/pose", PoseStamped, self.position_callback)
-        rospy.Subscriber("mavros/global_position/global", NavSatFix, self.global_position_callback)
-        self.pub_att = rospy.Publisher('mavros/setpoint_attitude/attitude', PoseStamped, queue_size=10)
-        self.pub_thr = rospy.Publisher('mavros/setpoint_attitude/att_throttle', Float64, queue_size=10)
-        rospy.wait_for_service('mavros/cmd/command', 30)
-        self._srv_cmd_long = rospy.ServiceProxy('mavros/cmd/command', CommandLong, persistent=True)
-        self.rate = rospy.Rate(10) # 10hz
-        self.has_global_pos = False
         self.local_position = PoseStamped()
+        self.state = State()
+        self.att = AttitudeTarget()
+        self.sub_topics_ready = {
+            key: False
+            for key in ['local_pos', 'home_pos', 'state']
+        }
+
+        # setup ROS topics and services
+        try:
+            rospy.wait_for_service('mavros/cmd/arming', 30)
+            rospy.wait_for_service('mavros/set_mode', 30)
+        except rospy.ROSException:
+            self.fail("failed to connect to mavros services")
+
+        self.set_arming_srv = rospy.ServiceProxy('mavros/cmd/arming',
+                                                 CommandBool)
+        self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode)
+        self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose',
+                                              PoseStamped,
+                                              self.local_position_callback)
+        self.home_pos_sub = rospy.Subscriber('mavros/home_position/home',
+                                             HomePosition,
+                                             self.home_position_callback)
+        self.state_sub = rospy.Subscriber('mavros/state', State,
+                                          self.state_callback)
+        self.att_setpoint_pub = rospy.Publisher(
+            'mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=10)
+
+        # send setpoints in seperate thread to better prevent failsafe
+        self.att_thread = Thread(target=self.send_att, args=())
+        self.att_thread.daemon = True
+        self.att_thread.start()
 
     def tearDown(self):
-        #self.helper.tearDown()
         pass
 
     #
-    # General callback functions used in tests
+    # Callback functions
     #
-    def position_callback(self, data):
+    def local_position_callback(self, data):
         self.local_position = data
 
-    def global_position_callback(self, data):
-        self.has_global_pos = True
+        if not self.sub_topics_ready['local_pos']:
+            self.sub_topics_ready['local_pos'] = True
 
+    def home_position_callback(self, data):
+        # this topic publishing seems to be a better indicator that the sim
+        # is ready, it's not actually needed
+        self.home_pos_sub.unregister()
+
+        if not self.sub_topics_ready['home_pos']:
+            self.sub_topics_ready['home_pos'] = True
+
+    def state_callback(self, data):
+        self.state = data
+
+        if not self.sub_topics_ready['state']:
+            self.sub_topics_ready['state'] = True
+
+    #
+    # Helper methods
+    #
+    def send_att(self):
+        rate = rospy.Rate(10)  # Hz
+        self.att.body_rate = Vector3()
+        self.att.header = Header()
+        self.att.header.frame_id = "base_footprint"
+        self.att.orientation = Quaternion(*quaternion_from_euler(0.25, 0.25,
+                                                                 0))
+        self.att.thrust = 0.7
+        self.att.type_mask = 7  # ignore body rate
+
+        while not rospy.is_shutdown():
+            self.att.header.stamp = rospy.Time.now()
+            self.att_setpoint_pub.publish(self.att)
+            try:  # prevent garbage in console output when thread is killed
+                rate.sleep()
+            except rospy.ROSInterruptException:
+                pass
+
+    def set_mode(self, mode, timeout):
+        """mode: PX4 mode string, timeout(int): seconds"""
+        old_mode = self.state.mode
+        loop_freq = 1  # Hz
+        rate = rospy.Rate(loop_freq)
+        mode_set = False
+        for i in xrange(timeout * loop_freq):
+            if self.state.mode == mode:
+                mode_set = True
+                rospy.loginfo(
+                    "set mode success | new mode: {0}, old mode: {1} | seconds: {2} of {3}".
+                    format(mode, self.state.mode, i / loop_freq, timeout))
+                break
+            else:
+                try:
+                    res = self.set_mode_srv(0, mode)  # 0 is custom mode
+                    if not res.mode_sent:
+                        rospy.logerr("failed to send mode command")
+                except rospy.ServiceException as e:
+                    rospy.logerr(e)
+
+            rate.sleep()
+
+        self.assertTrue(mode_set, (
+            "failed to set mode | new mode: {0}, old mode: {1} | timeout(seconds): {2}".
+            format(mode, old_mode, timeout)))
+
+    def set_arm(self, arm, timeout):
+        """arm: True to arm or False to disarm, timeout(int): seconds"""
+        old_arm = self.state.armed
+        loop_freq = 1  # Hz
+        rate = rospy.Rate(loop_freq)
+        arm_set = False
+        for i in xrange(timeout * loop_freq):
+            if self.state.armed == arm:
+                arm_set = True
+                rospy.loginfo(
+                    "set arm success | new arm: {0}, old arm: {1} | seconds: {2} of {3}".
+                    format(arm, old_arm, i / loop_freq, timeout))
+                break
+            else:
+                try:
+                    res = self.set_arming_srv(arm)
+                    if not res.success:
+                        rospy.logerr("failed to send arm command")
+                except rospy.ServiceException as e:
+                    rospy.logerr(e)
+
+            rate.sleep()
+
+        self.assertTrue(arm_set, (
+            "failed to set arm | new arm: {0}, old arm: {1} | timeout(seconds): {2}".
+            format(arm, self.state.armed, timeout)))
+
+    def wait_for_topics(self, timeout):
+        """wait for simulation to be ready, make sure we're getting topic info
+        from all topics by checking dictionary of flag values set in callbacks,
+        timeout(int): seconds"""
+        rospy.loginfo("waiting for simulation topics to be ready")
+        loop_freq = 1  # Hz
+        rate = rospy.Rate(loop_freq)
+        simulation_ready = False
+        for i in xrange(timeout * loop_freq):
+            if all(value for value in self.sub_topics_ready.values()):
+                simulation_ready = True
+                rospy.loginfo("simulation topics ready | seconds: {0} of {1}".
+                              format(i / loop_freq, timeout))
+                break
+
+            rate.sleep()
+
+        self.assertTrue(simulation_ready, (
+            "failed to hear from all subscribed simulation topics | topic ready flags: {0} | timeout(seconds): {1}".
+            format(self.sub_topics_ready, timeout)))
+
+    #
+    # Test method
+    #
     def test_attctl(self):
         """Test offboard attitude control"""
+        # boundary to cross
+        boundary_x = 5
+        boundary_y = 5
+        boundary_z = -5
 
-        # FIXME: hack to wait for simulation to be ready
-        while not self.has_global_pos:
-            self.rate.sleep()
+        # delay starting the mission
+        self.wait_for_topics(30)
 
-        # 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.25, 0.25, 0)
-        att.pose.orientation = Quaternion(*quaternion)
+        rospy.loginfo("seting mission mode")
+        self.set_mode("OFFBOARD", 5)
+        rospy.loginfo("arming")
+        self.set_arm(True, 5)
 
-        throttle = Float64()
-        throttle.data = 0.7
-        armed = False
-
-        # 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.pub_att.publish(att)
-            #self.helper.bag_write('mavros/setpoint_attitude/attitude', att)
-            self.pub_thr.publish(throttle)
-            #self.helper.bag_write('mavros/setpoint_attitude/att_throttle', throttle)
-            self.rate.sleep()
-
-            # FIXME: arm and switch to offboard
-            # (need to wait the first few rounds until PX4 has the offboard stream)
-            if not armed and count > 5:
-                self._srv_cmd_long(False, 176, False,
-                                   1, 6, 0, 0, 0, 0, 0)
-                # make sure the first command doesn't get lost
-                time.sleep(1)
-
-                self._srv_cmd_long(False, 400, False,
-                                   # arm
-                                   1, 0, 0, 0, 0, 0, 0)
-
-                armed = True
-
-            if (self.local_position.pose.position.x > 5
-                    and self.local_position.pose.position.z > 5
-                    and self.local_position.pose.position.y < -5):
+        rospy.loginfo("run mission")
+        rospy.loginfo("attempting to cross boundary | x: {0}, y: {1}, z: {2}".
+                      format(boundary_x, boundary_y, boundary_z))
+        # does it cross expected boundaries in 'timeout' seconds?
+        timeout = 12  # (int) seconds
+        loop_freq = 10  # Hz
+        rate = rospy.Rate(loop_freq)
+        crossed = False
+        for i in xrange(timeout * loop_freq):
+            if (self.local_position.pose.position.x > boundary_x and
+                    self.local_position.pose.position.z > boundary_y and
+                    self.local_position.pose.position.y < boundary_z):
+                rospy.loginfo("boundary crossed | seconds: {0} of {1}".format(
+                    i / loop_freq, timeout))
+                crossed = True
                 break
-            count = count + 1
 
-        self.assertTrue(count < timeout, "took too long to cross boundaries")
+            rate.sleep()
+
+        self.assertTrue(crossed, (
+            "took too long to cross boundaries | current position x: {0}, y: {1}, z: {2} | timeout(seconds): {3}".
+            format(self.local_position.pose.position.x,
+                   self.local_position.pose.position.y,
+                   self.local_position.pose.position.z, timeout)))
+
+        rospy.loginfo("disarming")
+        self.set_arm(False, 5)
 
 
 if __name__ == '__main__':
     import rostest
-    rostest.rosrun(PKG, 'mavros_offboard_attctl_test', MavrosOffboardAttctlTest)
-    #unittest.main()
+    rospy.init_node('test_node', anonymous=True)
+
+    rostest.rosrun(PKG, 'mavros_offboard_attctl_test',
+                   MavrosOffboardAttctlTest)

integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py

@@ -37,24 +37,21 @@
 #
 # The shebang of this file is currently Python2 because some
 # dependencies such as pymavlink don't play well with Python3 yet.
+from __future__ import division
 
 PKG = 'px4'
 
 import unittest
 import rospy
 import math
-import rosbag
-import time
-
-from numpy import linalg
 import numpy as np
-
-from std_msgs.msg import Header
-from geometry_msgs.msg import PoseStamped, Quaternion
+from threading import Thread
 from tf.transformations import quaternion_from_euler
-from mavros_msgs.srv import CommandLong
-from sensor_msgs.msg import NavSatFix
-#from px4_test_helper import PX4TestHelper
+from geometry_msgs.msg import PoseStamped, Quaternion
+from mavros_msgs.msg import HomePosition, State
+from mavros_msgs.srv import CommandBool, SetMode
+from std_msgs.msg import Header
+
 
 class MavrosOffboardPosctlTest(unittest.TestCase):
     """
@@ -62,115 +59,236 @@ class MavrosOffboardPosctlTest(unittest.TestCase):
     via 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)
     """
 
     def setUp(self):
-        rospy.init_node('test_node', anonymous=True)
-        #self.helper = PX4TestHelper("mavros_offboard_posctl_test")
-        #self.helper.setUp()
-
-        rospy.Subscriber("mavros/local_position/pose", PoseStamped, self.position_callback)
-        rospy.Subscriber("mavros/global_position/global", NavSatFix, self.global_position_callback)
-        self.pub_spt = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10)
-        rospy.wait_for_service('mavros/cmd/command', 30)
-        self._srv_cmd_long = rospy.ServiceProxy('mavros/cmd/command', CommandLong, persistent=True)
-        self.rate = rospy.Rate(10) # 10hz
-        self.has_global_pos = False
         self.local_position = PoseStamped()
-        self.armed = False
+        self.state = State()
+        self.pos = PoseStamped()
+        self.sub_topics_ready = {
+            key: False
+            for key in ['local_pos', 'home_pos', 'state']
+        }
+
+        # setup ROS topics and services
+        try:
+            rospy.wait_for_service('mavros/cmd/arming', 30)
+            rospy.wait_for_service('mavros/set_mode', 30)
+        except rospy.ROSException:
+            self.fail("failed to connect to mavros services")
+
+        self.set_arming_srv = rospy.ServiceProxy('/mavros/cmd/arming',
+                                                 CommandBool)
+        self.set_mode_srv = rospy.ServiceProxy('/mavros/set_mode', SetMode)
+        self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose',
+                                              PoseStamped,
+                                              self.local_position_callback)
+        self.home_pos_sub = rospy.Subscriber('mavros/home_position/home',
+                                             HomePosition,
+                                             self.home_position_callback)
+        self.state_sub = rospy.Subscriber('mavros/state', State,
+                                          self.state_callback)
+        self.pos_setpoint_pub = rospy.Publisher(
+            'mavros/setpoint_position/local', PoseStamped, queue_size=10)
+
+        # send setpoints in seperate thread to better prevent failsafe
+        self.pos_thread = Thread(target=self.send_pos, args=())
+        self.pos_thread.daemon = True
+        self.pos_thread.start()
 
     def tearDown(self):
-        #self.helper.tearDown()
         pass
 
     #
-    # General callback functions used in tests
+    # Callback functions
     #
-    def position_callback(self, data):
+    def local_position_callback(self, data):
         self.local_position = data
 
-    def global_position_callback(self, data):
-        self.has_global_pos = True
+        if not self.sub_topics_ready['local_pos']:
+            self.sub_topics_ready['local_pos'] = True
+
+    def home_position_callback(self, data):
+        # this topic publishing seems to be a better indicator that the sim
+        # is ready, it's not actually needed
+        self.home_pos_sub.unregister()
+
+        if not self.sub_topics_ready['home_pos']:
+            self.sub_topics_ready['home_pos'] = True
+
+    def state_callback(self, data):
+        self.state = data
+
+        if not self.sub_topics_ready['state']:
+            self.sub_topics_ready['state'] = True
 
     #
     # Helper methods
     #
+    def send_pos(self):
+        rate = rospy.Rate(10)  # Hz
+        self.pos.header = Header()
+        self.pos.header.frame_id = "base_footprint"
+
+        while not rospy.is_shutdown():
+            self.pos.header.stamp = rospy.Time.now()
+            self.pos_setpoint_pub.publish(self.pos)
+            try:  # prevent garbage in console output when thread is killed
+                rate.sleep()
+            except rospy.ROSInterruptException:
+                pass
+
+    def set_mode(self, mode, timeout):
+        """mode: PX4 mode string, timeout(int): seconds"""
+        old_mode = self.state.mode
+        loop_freq = 1  # Hz
+        rate = rospy.Rate(loop_freq)
+        mode_set = False
+        for i in xrange(timeout * loop_freq):
+            if self.state.mode == mode:
+                mode_set = True
+                rospy.loginfo(
+                    "set mode success | new mode: {0}, old mode: {1} | seconds: {2} of {3}".
+                    format(mode, old_mode, i / loop_freq, timeout))
+                break
+            else:
+                try:
+                    res = self.set_mode_srv(0, mode)  # 0 is custom mode
+                    if not res.mode_sent:
+                        rospy.logerr("failed to send mode command")
+                except rospy.ServiceException as e:
+                    rospy.logerr(e)
+
+            rate.sleep()
+
+        self.assertTrue(mode_set, (
+            "failed to set mode | new mode: {0}, old mode: {1} | timeout(seconds): {2}".
+            format(mode, self.state.mode, timeout)))
+
+    def set_arm(self, arm, timeout):
+        """arm: True to arm or False to disarm, timeout(int): seconds"""
+        old_arm = self.state.armed
+        loop_freq = 1  # Hz
+        rate = rospy.Rate(loop_freq)
+        arm_set = False
+        for i in xrange(timeout * loop_freq):
+            if self.state.armed == arm:
+                arm_set = True
+                rospy.loginfo(
+                    "set arm success | new arm: {0}, old arm: {1} | seconds: {2} of {3}".
+                    format(arm, old_arm, i / loop_freq, timeout))
+                break
+            else:
+                try:
+                    res = self.set_arming_srv(arm)
+                    if not res.success:
+                        rospy.logerr("failed to send arm command")
+                except rospy.ServiceException as e:
+                    rospy.logerr(e)
+
+            rate.sleep()
+
+        self.assertTrue(arm_set, (
+            "failed to set arm | new arm: {0}, old arm: {1} | timeout(seconds): {2}".
+            format(arm, self.state.armed, timeout)))
+
+    def wait_for_topics(self, timeout):
+        """wait for simulation to be ready, make sure we're getting topic info
+        from all topics by checking dictionary of flag values set in callbacks,
+        timeout(int): seconds"""
+        rospy.loginfo("waiting for simulation topics to be ready")
+        loop_freq = 1  # Hz
+        rate = rospy.Rate(loop_freq)
+        simulation_ready = False
+        for i in xrange(timeout * loop_freq):
+            if all(value for value in self.sub_topics_ready.values()):
+                simulation_ready = True
+                rospy.loginfo("simulation topics ready | seconds: {0} of {1}".
+                              format(i / loop_freq, timeout))
+                break
+
+            rate.sleep()
+
+        self.assertTrue(simulation_ready, (
+            "failed to hear from all subscribed simulation topics | topic ready flags: {0} | timeout(seconds): {1}".
+            format(self.sub_topics_ready, timeout)))
+
     def is_at_position(self, x, y, z, offset):
-        rospy.logdebug("current position %f, %f, %f" %
-                       (self.local_position.pose.position.x,
-                       self.local_position.pose.position.y,
-                       self.local_position.pose.position.z))
+        rospy.logdebug("current position | x:{0}, y:{1}, z:{2}".format(
+            self.local_position.pose.position.x, self.local_position.pose.
+            position.y, self.local_position.pose.position.z))
 
         desired = np.array((x, y, z))
         pos = np.array((self.local_position.pose.position.x,
                         self.local_position.pose.position.y,
                         self.local_position.pose.position.z))
-        return linalg.norm(desired - pos) < offset
+        return np.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
+        self.pos.pose.position.x = x
+        self.pos.pose.position.y = y
+        self.pos.pose.position.z = z
+        rospy.loginfo("attempting to reach position | x: {0}, y: {1}, z: {2}".
+                      format(x, y, 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)
+        self.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.pub_spt.publish(pos)
-            #self.helper.bag_write('mavros/setpoint_position/local', pos)
-
-            # FIXME: arm and switch to offboard
-            # (need to wait the first few rounds until PX4 has the offboard stream)
-            if not self.armed and count > 5:
-                self._srv_cmd_long(False, 176, False,
-                                   1, 6, 0, 0, 0, 0, 0)
-                # make sure the first command doesn't get lost
-                time.sleep(1)
-
-                self._srv_cmd_long(False, 400, False,
-                                   # arm
-                                   1, 0, 0, 0, 0, 0, 0)
-
-                self.armed = True
-
-            if self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 1):
+        # does it reach the position in 'timeout' seconds?
+        loop_freq = 10  # Hz
+        rate = rospy.Rate(loop_freq)
+        reached = False
+        for i in xrange(timeout * loop_freq):
+            if self.is_at_position(self.pos.pose.position.x,
+                                   self.pos.pose.position.y,
+                                   self.pos.pose.position.z, 1):
+                rospy.loginfo("position reached | seconds: {0} of {1}".format(
+                    i / loop_freq, timeout))
+                reached = True
                 break
-            count = count + 1
-            self.rate.sleep()
 
-        self.assertTrue(count < timeout, "took too long to get to position")
+            rate.sleep()
 
+        self.assertTrue(reached, (
+            "took too long to get to position | current position x: {0}, y: {1}, z: {2} | timeout(seconds): {3}".
+            format(self.local_position.pose.position.x,
+                   self.local_position.pose.position.y,
+                   self.local_position.pose.position.z, timeout)))
+
+    #
+    # Test method
+    #
     def test_posctl(self):
         """Test offboard position control"""
 
-        # FIXME: hack to wait for simulation to be ready
-        while not self.has_global_pos:
-            self.rate.sleep()
+        # delay starting the mission
+        self.wait_for_topics(30)
 
-        positions = (
-            (0, 0, 0),
-            (2, 2, 2),
-            (2, -2, 2),
-            (-2, -2, 2),
-            (2, 2, 2))
+        rospy.loginfo("seting mission mode")
+        self.set_mode("OFFBOARD", 5)
+        rospy.loginfo("arming")
+        self.set_arm(True, 5)
 
-        for i in range(0, len(positions)):
-            self.reach_position(positions[i][0], positions[i][1], positions[i][2], 180)
+        rospy.loginfo("run mission")
+        positions = ((0, 0, 0), (2, 2, 2), (2, -2, 2), (-2, -2, 2), (2, 2, 2))
+
+        for i in xrange(len(positions)):
+            self.reach_position(positions[i][0], positions[i][1],
+                                positions[i][2], 18)
+
+        rospy.loginfo("disarming")
+        self.set_arm(False, 5)
 
 
 if __name__ == '__main__':
     import rostest
-    rostest.rosrun(PKG, 'mavros_offboard_posctl_test', MavrosOffboardPosctlTest)
-    #unittest.main()
+    rospy.init_node('test_node', anonymous=True)
+
+    rostest.rosrun(PKG, 'mavros_offboard_posctl_test',
+                   MavrosOffboardPosctlTest)

integrationtests/python_src/px4_it/mavros/mission_test.py

@@ -38,269 +38,399 @@
 
 # The shebang of this file is currently Python2 because some
 # dependencies such as pymavlink don't play well with Python3 yet.
+from __future__ import division
 
 PKG = 'px4'
 
 import unittest
 import rospy
-import math
-import rosbag
-import sys
-import os
-import time
 import glob
 import json
-
-import mavros
-from pymavlink import mavutil
-from mavros import mavlink
-
+import math
+import os
 import px4tools
-
-from geometry_msgs.msg import PoseStamped
-from mavros_msgs.srv import CommandLong, WaypointPush
-from mavros_msgs.msg import Mavlink, Waypoint, ExtendedState
-from sensor_msgs.msg import NavSatFix
+import sys
+from mavros import mavlink
 from mavros.mission import QGroundControlWP
-#from px4_test_helper import PX4TestHelper
+from pymavlink import mavutil
+from threading import Thread
+from mavros_msgs.msg import Altitude, ExtendedState, HomePosition, Mavlink, \
+                            State, Waypoint
+from mavros_msgs.srv import CommandBool, SetMode, WaypointPush
+from sensor_msgs.msg import NavSatFix
+
 
 def get_last_log():
     try:
         log_path = os.environ['PX4_LOG_DIR']
     except KeyError:
-        log_path = os.path.join(os.environ['HOME'], 'ros/rootfs/fs/microsd/log')
-    last_log_dir = sorted(
-        glob.glob(os.path.join(log_path, '*')))[-1]
+        log_path = os.path.join(os.environ['HOME'],
+                                '.ros/rootfs/fs/microsd/log')
+    last_log_dir = sorted(glob.glob(os.path.join(log_path, '*')))[-1]
     last_log = sorted(glob.glob(os.path.join(last_log_dir, '*.ulg')))[-1]
     return last_log
 
+
 def read_new_mission(f):
     d = json.load(f)
     current = True
     for wp in d['items']:
         yield Waypoint(
-                is_current = current,
-                frame = int(wp['frame']),
-                command = int(wp['command']),
-                param1 = float(wp['param1']),
-                param2 = float(wp['param2']),
-                param3 = float(wp['param3']),
-                param4 = float(wp['param4']),
-                x_lat = float(wp['coordinate'][0]),
-                y_long = float(wp['coordinate'][1]),
-                z_alt = float(wp['coordinate'][2]),
-                autocontinue = bool(wp['autoContinue']))
+            is_current=current,
+            frame=int(wp['frame']),
+            command=int(wp['command']),
+            param1=float(wp['param1']),
+            param2=float(wp['param2']),
+            param3=float(wp['param3']),
+            param4=float(wp['param4']),
+            x_lat=float(wp['coordinate'][0]),
+            y_long=float(wp['coordinate'][1]),
+            z_alt=float(wp['coordinate'][2]),
+            autocontinue=bool(wp['autoContinue']))
         if current:
             current = False
 
+
 class MavrosMissionTest(unittest.TestCase):
     """
     Run a mission
     """
+    # dictionaries correspond to mavros ExtendedState msg
+    LAND_STATES = {
+        0: 'UNDEFINED',
+        1: 'ON_GROUND',
+        2: 'IN_AIR',
+        3: 'TAKEOFF',
+        4: 'LANDING'
+    }
+    VTOL_STATES = {
+        0: 'VTOL UNDEFINED',
+        1: 'VTOL MC->FW',
+        2: 'VTOL FW->MC',
+        3: 'VTOL MC',
+        4: 'VTOL FW'
+    }
 
     def setUp(self):
-        rospy.init_node('test_node', anonymous=True)
-
-        self.rate = rospy.Rate(10) # 10hz
+        self.rate = rospy.Rate(10)  # 10hz
         self.has_global_pos = False
-        self.local_position = PoseStamped()
         self.global_position = NavSatFix()
         self.extended_state = ExtendedState()
-        self.home_alt = 0
+        self.altitude = Altitude()
+        self.state = State()
         self.mc_rad = 5
         self.fw_rad = 60
         self.fw_alt_rad = 10
-        self.last_alt_d = 9999
-        self.last_pos_d = 9999
+        self.last_alt_d = None
+        self.last_pos_d = None
         self.mission_name = ""
+        self.sub_topics_ready = {
+            key: False
+            for key in ['global_pos', 'home_pos', 'ext_state', 'alt', 'state']
+        }
 
-        # need to simulate heartbeat for datalink loss detection
-        rospy.Timer(rospy.Duration(0.5), self.send_heartbeat)
+        # setup ROS topics and services
+        try:
+            rospy.wait_for_service('mavros/mission/push', 30)
+            rospy.wait_for_service('mavros/cmd/arming', 30)
+            rospy.wait_for_service('mavros/set_mode', 30)
+        except rospy.ROSException:
+            self.fail("failed to connect to mavros services")
 
-        rospy.wait_for_service('mavros/cmd/command', 30)
-        self.pub_mavlink = rospy.Publisher('mavlink/to', Mavlink, queue_size=1)
-        self._srv_cmd_long = rospy.ServiceProxy('mavros/cmd/command', CommandLong, persistent=True)
-        self._srv_wp_push = rospy.ServiceProxy('mavros/mission/push', WaypointPush, persistent=True)
+        self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push',
+                                              WaypointPush)
+        self.set_arming_srv = rospy.ServiceProxy('/mavros/cmd/arming',
+                                                 CommandBool)
+        self.set_mode_srv = rospy.ServiceProxy('/mavros/set_mode', SetMode)
+        self.global_pos_sub = rospy.Subscriber('mavros/global_position/global',
+                                               NavSatFix,
+                                               self.global_position_callback)
+        self.home_pos_sub = rospy.Subscriber('mavros/home_position/home',
+                                             HomePosition,
+                                             self.home_position_callback)
+        self.ext_state_sub = rospy.Subscriber('mavros/extended_state',
+                                              ExtendedState,
+                                              self.extended_state_callback)
+        self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude,
+                                        self.altitude_callback)
+        self.state_sub = rospy.Subscriber('mavros/state', State,
+                                          self.state_callback)
+        self.mavlink_pub = rospy.Publisher('mavlink/to', Mavlink, queue_size=1)
 
-        rospy.Subscriber("mavros/local_position/pose", PoseStamped, self.position_callback)
-        rospy.Subscriber("mavros/global_position/global", NavSatFix, self.global_position_callback)
-        rospy.Subscriber("mavros/extended_state", ExtendedState, self.extended_state_callback)
+        # need to simulate heartbeat to prevent datalink loss detection
+        self.hb_mav_msg = mavutil.mavlink.MAVLink_heartbeat_message(
+            mavutil.mavlink.MAV_TYPE_GCS, 0, 0, 0, 0, 0)
+        self.hb_mav_msg.pack(mavutil.mavlink.MAVLink('', 2, 1))
+        self.hb_ros_msg = mavlink.convert_to_rosmsg(self.hb_mav_msg)
+        self.hb_thread = Thread(target=self.send_heartbeat, args=())
+        self.hb_thread.daemon = True
+        self.hb_thread.start()
 
     def tearDown(self):
-        #self.helper.tearDown()
         pass
 
     #
-    # General callback functions used in tests
+    # Callback functions
     #
-    def position_callback(self, data):
-        self.local_position = data
-
     def global_position_callback(self, data):
         self.global_position = data
 
-        if not self.has_global_pos:
-            if data.altitude != 0:
-                self.home_alt = data.altitude
-                self.has_global_pos = True
+        if not self.sub_topics_ready['global_pos']:
+            self.sub_topics_ready['global_pos'] = True
+
+    def home_position_callback(self, data):
+        # this topic publishing seems to be a better indicator that the sim
+        # is ready, it's not actually needed
+        self.home_pos_sub.unregister()
+
+        if not self.sub_topics_ready['home_pos']:
+            self.sub_topics_ready['home_pos'] = True
 
     def extended_state_callback(self, data):
+        if self.extended_state.vtol_state != data.vtol_state:
+            rospy.loginfo("VTOL state changed from {0} to {1}".format(
+                self.VTOL_STATES.get(self.extended_state.vtol_state),
+                self.VTOL_STATES.get(data.vtol_state)))
 
-        prev_state = self.extended_state.vtol_state;
+        if self.extended_state.landed_state != data.landed_state:
+            rospy.loginfo("landed state changed from {0} to {1}".format(
+                self.LAND_STATES.get(self.extended_state.landed_state),
+                self.LAND_STATES.get(data.landed_state)))
 
         self.extended_state = data
-        if (prev_state != self.extended_state.vtol_state):
-            print("VTOL state change: %d" % self.extended_state.vtol_state);
+
+        if not self.sub_topics_ready['ext_state']:
+            self.sub_topics_ready['ext_state'] = True
+
+    def state_callback(self, data):
+        if self.state.armed != data.armed:
+            rospy.loginfo("armed state changed from {0} to {1}".format(
+                self.state.armed, data.armed))
+
+        if self.state.mode != data.mode:
+            rospy.loginfo("mode changed from {0} to {1}".format(
+                self.state.mode, data.mode))
+
+        self.state = data
+
+        # mavros publishes a disconnected state message on init
+        if not self.sub_topics_ready['state'] and data.connected:
+            self.sub_topics_ready['state'] = True
+
+    def altitude_callback(self, data):
+        self.altitude = data
+
+        # amsl has been observed to be nan while other fields are valid
+        if not self.sub_topics_ready['alt'] and not math.isnan(data.amsl):
+            self.sub_topics_ready['alt'] = True
 
     #
     # Helper methods
     #
+    def send_heartbeat(self):
+        rate = rospy.Rate(2)  # Hz
+        while not rospy.is_shutdown():
+            self.mavlink_pub.publish(self.hb_ros_msg)
+            try:  # prevent garbage in console output when thread is killed
+                rate.sleep()
+            except rospy.ROSInterruptException:
+                pass
+
+    def set_mode(self, mode, timeout):
+        """mode: PX4 mode string, timeout(int): seconds"""
+        old_mode = self.state.mode
+        loop_freq = 1  # Hz
+        rate = rospy.Rate(loop_freq)
+        mode_set = False
+        for i in xrange(timeout * loop_freq):
+            if self.state.mode == mode:
+                mode_set = True
+                rospy.loginfo(
+                    "set mode success | new mode: {0}, old mode: {1} | seconds: {2} of {3}".
+                    format(mode, old_mode, i / loop_freq, timeout))
+                break
+            else:
+                try:
+                    res = self.set_mode_srv(0, mode)  # 0 is custom mode
+                    if not res.mode_sent:
+                        rospy.logerr("failed to send mode command")
+                except rospy.ServiceException as e:
+                    rospy.logerr(e)
+
+            rate.sleep()
+
+        self.assertTrue(mode_set, (
+            "failed to set mode | new mode: {0}, old mode: {1} | timeout(seconds): {2}".
+            format(mode, old_mode, timeout)))
+
+    def set_arm(self, arm, timeout):
+        """arm: True to arm or False to disarm, timeout(int): seconds"""
+        old_arm = self.state.armed
+        loop_freq = 1  # Hz
+        rate = rospy.Rate(loop_freq)
+        arm_set = False
+        for i in xrange(timeout * loop_freq):
+            if self.state.armed == arm:
+                arm_set = True
+                rospy.loginfo(
+                    "set arm success | new arm: {0}, old arm: {1} | seconds: {2} of {3}".
+                    format(arm, old_arm, i / loop_freq, timeout))
+                break
+            else:
+                try:
+                    res = self.set_arming_srv(arm)
+                    if not res.success:
+                        rospy.logerr("failed to send arm command")
+                except rospy.ServiceException as e:
+                    rospy.logerr(e)
+
+            rate.sleep()
+
+        self.assertTrue(arm_set, (
+            "failed to set arm | new arm: {0}, old arm: {1} | timeout(seconds): {2}".
+            format(arm, old_arm, timeout)))
+
     def is_at_position(self, lat, lon, alt, xy_offset, z_offset):
-        R = 6371000 # metres
+        """alt(amsl), xy_offset, z_offset: meters"""
+        R = 6371000  # metres
         rlat1 = math.radians(lat)
         rlat2 = math.radians(self.global_position.latitude)
 
         rlat_d = math.radians(self.global_position.latitude - lat)
         rlon_d = math.radians(self.global_position.longitude - lon)
 
-        a = (math.sin(rlat_d / 2) * math.sin(rlat_d / 2) +
-             math.cos(rlat1) * math.cos(rlat2) *
-             math.sin(rlon_d / 2) * math.sin(rlon_d / 2))
-        c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
+        a = (math.sin(rlat_d / 2) * math.sin(rlat_d / 2) + math.cos(rlat1) *
+             math.cos(rlat2) * math.sin(rlon_d / 2) * math.sin(rlon_d / 2))
+        c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
 
         d = R * c
-        alt_d = abs(alt - self.global_position.altitude)
-
-        #rospy.loginfo("d: %f, alt_d: %f", d, alt_d)
+        alt_d = abs(alt - self.altitude.amsl)
 
         # remember best distances
-        if self.last_pos_d > d:
+        if not self.last_pos_d or self.last_pos_d > d:
             self.last_pos_d = d
-        if self.last_alt_d > alt_d:
+        if not self.last_alt_d or self.last_alt_d > alt_d:
             self.last_alt_d = alt_d
 
+        rospy.logdebug("d: {0}, alt_d: {1}".format(d, alt_d))
         return d < xy_offset and alt_d < z_offset
 
     def reach_position(self, lat, lon, alt, timeout, index):
+        """alt(amsl): meters, timeout(int): seconds"""
         # reset best distances
-        self.last_alt_d = 9999
-        self.last_pos_d = 9999
+        self.last_alt_d = None
+        self.last_pos_d = None
 
-        rospy.loginfo("trying to reach waypoint " +
-            "lat: %13.9f, lon: %13.9f, alt: %6.2f, timeout: %d, index: %d" %
-            (lat, lon, alt, timeout, index))
+        rospy.loginfo(
+            "trying to reach waypoint | lat: {0:13.9f}, lon: {1:13.9f}, alt: {2:6.2f}, index: {3}".
+            format(lat, lon, alt, index))
 
-        # does it reach the position in X seconds?
-        count = 0
-        while count < timeout:
+        # does it reach the position in 'timeout' seconds?
+        loop_freq = 10  # Hz
+        rate = rospy.Rate(loop_freq)
+        reached = False
+        for i in xrange(timeout * loop_freq):
             # use MC radius by default
             # FIXME: also check MAV_TYPE from system status, otherwise pure fixed-wing won't work
             xy_radius = self.mc_rad
             z_radius = self.mc_rad
 
             # use FW radius if in FW or in transition
-            if (self.extended_state.vtol_state == ExtendedState.VTOL_STATE_FW or
-                    self.extended_state.vtol_state == ExtendedState.VTOL_STATE_TRANSITION_TO_MC or
-                    self.extended_state.vtol_state == ExtendedState.VTOL_STATE_TRANSITION_TO_FW):
+            if (self.extended_state.vtol_state == ExtendedState.VTOL_STATE_FW
+                    or self.extended_state.vtol_state ==
+                    ExtendedState.VTOL_STATE_TRANSITION_TO_MC or
+                    self.extended_state.vtol_state ==
+                    ExtendedState.VTOL_STATE_TRANSITION_TO_FW):
                 xy_radius = self.fw_rad
                 z_radius = self.fw_alt_rad
 
             if self.is_at_position(lat, lon, alt, xy_radius, z_radius):
-                rospy.loginfo("position reached, index: %d, count: %d, pos_d: %f, alt_d: %f" %
-                    (index, count, self.last_pos_d, self.last_alt_d))
+                reached = True
+                rospy.loginfo(
+                    "position reached | pos_d: {0:.2f}, alt_d: {1:.2f}, index: {2} | seconds: {3} of {4}".
+                    format(self.last_pos_d, self.last_alt_d, index, i /
+                           loop_freq, timeout))
                 break
 
-            count = count + 1
-            self.rate.sleep()
+            rate.sleep()
 
-        vtol_state_string = "VTOL undefined"
+        self.assertTrue(reached, (
+            "({0}) took too long to get to position | lat: {1:13.9f}, lon: {2:13.9f}, alt: {3:6.2f}, xy off: {4}, z off: {5}, pos_d: {6:.2f}, alt_d: {7:.2f}, VTOL state: {8}, index: {9} | timeout(seconds): {10}".
+            format(self.mission_name, lat, lon, alt, xy_radius, z_radius,
+                   self.last_pos_d, self.last_alt_d,
+                   self.VTOL_STATES.get(self.extended_state.vtol_state), index,
+                   timeout)))
 
-        if (self.extended_state.vtol_state == ExtendedState.VTOL_STATE_MC):
-            vtol_state_string = "VTOL MC"
-        if (self.extended_state.vtol_state == ExtendedState.VTOL_STATE_FW):
-            vtol_state_string = "VTOL FW"
-        if (self.extended_state.vtol_state == ExtendedState.VTOL_STATE_TRANSITION_TO_MC):
-            vtol_state_string = "VTOL FW->MC"
-        if (self.extended_state.vtol_state == ExtendedState.VTOL_STATE_TRANSITION_TO_FW):
-            vtol_state_string = "VTOL MC->FW"
-
-        self.assertTrue(count < timeout, (("(%s) took too long to get to position " +
-            "lat: %13.9f, lon: %13.9f, alt: %6.2f, xy off: %f, z off: %f, timeout: %d, index: %d, pos_d: %f, alt_d: %f, VTOL state: %s") %
-            (self.mission_name, lat, lon, alt, xy_radius, z_radius, timeout, index, self.last_pos_d, self.last_alt_d, vtol_state_string)))
-
-    def run_mission(self):
-	# Hack to wait until vehicle is ready
-	# TODO better integration with pre-flight status reporting
-	time.sleep(5)
-	"""switch mode: auto and arm"""
-        self._srv_cmd_long(False, 176, False,
-                           # custom, auto, mission
-                           1, 4, 4, 0, 0, 0, 0)
-        # make sure the first command doesn't get lost
-        time.sleep(1)
-
-        self._srv_cmd_long(False, 400, False,
-                           # arm
-                           1, 0, 0, 0, 0, 0, 0)
-
-    def wait_until_ready(self):
-        """FIXME: hack to wait for simulation to be ready"""
-        while not self.has_global_pos:
-            self.rate.sleep()
-
-    def wait_on_landing(self, timeout, index):
-        """Wait for landed state"""
-
-        rospy.loginfo("waiting for landing " +
-            "timeout: %d, index: %d" %
-            (timeout, index))
-
-        count = 0
-        while count < timeout:
-            if self.extended_state.landed_state == ExtendedState.LANDED_STATE_ON_GROUND:
+    def wait_for_topics(self, timeout):
+        """wait for simulation to be ready, make sure we're getting topic info
+        from all topics by checking dictionary of flag values set in callbacks,
+        timeout(int): seconds"""
+        rospy.loginfo("waiting for simulation topics to be ready")
+        loop_freq = 1  # Hz
+        rate = rospy.Rate(loop_freq)
+        simulation_ready = False
+        for i in xrange(timeout * loop_freq):
+            if all(value for value in self.sub_topics_ready.values()):
+                simulation_ready = True
+                rospy.loginfo("simulation topics ready | seconds: {0} of {1}".
+                              format(i / loop_freq, timeout))
                 break
 
-            count = count + 1
-            self.rate.sleep()
+            rate.sleep()
 
-        self.assertTrue(count < timeout, ("(%s) landing not detected after landing WP " +
-            "timeout: %d, index: %d") %
-            (self.mission_name, timeout, index))
+        self.assertTrue(simulation_ready, (
+            "failed to hear from all subscribed simulation topics | topic ready flags: {0} | timeout(seconds): {1}".
+            format(self.sub_topics_ready, timeout)))
+
+    def wait_on_landed_state(self, desired_landed_state, timeout, index):
+        rospy.loginfo(
+            "waiting for landed state | state: {0}, index: {1}".format(
+                self.LAND_STATES.get(desired_landed_state), index))
+        loop_freq = 10  # Hz
+        rate = rospy.Rate(loop_freq)
+        landed_state_confirmed = False
+        for i in xrange(timeout * loop_freq):
+            if self.extended_state.landed_state == desired_landed_state:
+                landed_state_confirmed = True
+                rospy.loginfo(
+                    "landed state confirmed | state: {0}, index: {1}".format(
+                        self.LAND_STATES.get(desired_landed_state), index))
+                break
+
+            rate.sleep()
+
+        self.assertTrue(landed_state_confirmed, (
+            "({0}) landed state not detected | desired: {1}, current: {2} | index: {3}, timeout(seconds): {4}".
+            format(self.mission_name,
+                   self.LAND_STATES.get(desired_landed_state),
+                   self.LAND_STATES.get(self.extended_state.landed_state),
+                   index, timeout)))
 
     def wait_on_transition(self, transition, timeout, index):
-        """Wait for VTOL transition"""
-
-        rospy.loginfo("waiting for VTOL transition " +
-            "transition: %d, timeout: %d, index: %d" %
-            (transition, timeout, index))
-
-        count = 0
-        while count < timeout:
-            # transition to MC
-            if (transition == ExtendedState.VTOL_STATE_MC and
-                    self.extended_state.vtol_state == ExtendedState.VTOL_STATE_MC):
+        """Wait for VTOL transition, timeout(int): seconds"""
+        rospy.loginfo(
+            "waiting for VTOL transition | transition: {0}, index: {1}".format(
+                self.VTOL_STATES.get(transition), index))
+        loop_freq = 10  # Hz
+        rate = rospy.Rate(loop_freq)
+        transitioned = False
+        for i in xrange(timeout * loop_freq):
+            if transition == self.extended_state.vtol_state:
+                rospy.loginfo(
+                    "transitioned | index: {0} | seconds: {1} of {2}".format(
+                        index, i / loop_freq, timeout))
+                transitioned = True
                 break
 
-            # transition to FW
-            if (transition == ExtendedState.VTOL_STATE_FW and
-                    self.extended_state.vtol_state == ExtendedState.VTOL_STATE_FW):
-                break
+            rate.sleep()
 
-            count = count + 1
-            self.rate.sleep()
-
-        self.assertTrue(count < timeout, ("(%s) transition not detected " +
-            "timeout: %d, index: %d") %
-            (self.mission_name, timeout, index))
-
-    def send_heartbeat(self, event=None):
-        # mav type gcs
-        mavmsg = mavutil.mavlink.MAVLink_heartbeat_message(6, 0, 0, 0, 0, 0)
-        # XXX: hack: using header object to set mav properties
-        mavmsg.pack(mavutil.mavlink.MAVLink_header(0, 0, 0, 2, 1))
-        rosmsg = mavlink.convert_to_rosmsg(mavmsg)
-        self.pub_mavlink.publish(rosmsg)
+        self.assertTrue(transitioned, (
+            "({0}) transition not detected | index: {1} | timeout(seconds): {2}, ".
+            format(self.mission_name, index, timeout)))
 
+    #
+    # Test method
+    #
     def test_mission(self):
         """Test mission"""
 
@@ -309,11 +439,11 @@ class MavrosMissionTest(unittest.TestCase):
             return
 
         self.mission_name = sys.argv[1]
-        mission_file = os.path.dirname(os.path.realpath(__file__)) + "/" + sys.argv[1]
+        mission_file = os.path.dirname(
+            os.path.realpath(__file__)) + "/" + sys.argv[1]
 
-        rospy.loginfo("reading mission %s", mission_file)
+        rospy.loginfo("reading mission {0}".format(mission_file))
         wps = []
-
         with open(mission_file, 'r') as f:
             mission_ext = os.path.splitext(mission_file)[1]
             if mission_ext == '.mission':
@@ -330,62 +460,79 @@ class MavrosMissionTest(unittest.TestCase):
             else:
                 raise IOError('unknown mission file extension', mission_ext)
 
-        rospy.loginfo("wait until ready")
-        self.wait_until_ready()
-
         rospy.loginfo("send mission")
-        res = self._srv_wp_push(wps)
-        rospy.loginfo(res)
-        self.assertTrue(res.success, "(%s) mission could not be transfered" % self.mission_name)
+        result = False
+        try:
+            res = self.wp_push_srv(start_index=0, waypoints=wps)
+            result = res.success
+        except rospy.ServiceException as e:
+            rospy.logerr(e)
+        self.assertTrue(
+            result,
+            "({0}) mission could not be transfered".format(self.mission_name))
+
+        # delay starting the mission
+        self.wait_for_topics(30)
+
+        # make sure the simulation is ready to start the mission
+        self.wait_on_landed_state(ExtendedState.LANDED_STATE_ON_GROUND, 10, -1)
+
+        rospy.loginfo("seting mission mode")
+        self.set_mode("AUTO.MISSION", 5)
+        rospy.loginfo("arming")
+        self.set_arm(True, 5)
 
         rospy.loginfo("run mission")
-        self.run_mission()
-
-        index = 0
-        for waypoint in wps:
+        for index, waypoint in enumerate(wps):
             # only check position for waypoints where this makes sense
             if waypoint.frame == Waypoint.FRAME_GLOBAL_REL_ALT or waypoint.frame == Waypoint.FRAME_GLOBAL:
                 alt = waypoint.z_alt
                 if waypoint.frame == Waypoint.FRAME_GLOBAL_REL_ALT:
-                    alt += self.home_alt
+                    alt += self.altitude.amsl - self.altitude.relative
 
-                self.reach_position(waypoint.x_lat, waypoint.y_long, alt, 600, index)
+                self.reach_position(waypoint.x_lat, waypoint.y_long, alt, 60,
+                                    index)
 
             # check if VTOL transition happens if applicable
             if waypoint.command == 84 or waypoint.command == 85 or waypoint.command == 3000:
                 transition = waypoint.param1
 
-                if waypoint.command == 84: # VTOL takeoff implies transition to FW
+                if waypoint.command == 84:  # VTOL takeoff implies transition to FW
                     transition = ExtendedState.VTOL_STATE_FW
 
-                if waypoint.command == 85: # VTOL takeoff implies transition to MC
+                if waypoint.command == 85:  # VTOL takeoff implies transition to MC
                     transition = ExtendedState.VTOL_STATE_MC
 
-                self.wait_on_transition(transition, 600, index)
+                self.wait_on_transition(transition, 60, index)
 
             # after reaching position, wait for landing detection if applicable
             if waypoint.command == 85 or waypoint.command == 21:
-                self.wait_on_landing(600, index)
+                self.wait_on_landed_state(ExtendedState.LANDED_STATE_ON_GROUND,
+                                          60, index)
 
-            index += 1
+        rospy.loginfo("disarming")
+        self.set_arm(False, 5)
 
         rospy.loginfo("mission done, calculating performance metrics")
         last_log = get_last_log()
-        rospy.loginfo("log file %s", last_log)
-        data = px4tools.ulog.read_ulog(last_log).concat(dt=0.1)
-        data = px4tools.ulog.compute_data(data)
+        rospy.loginfo("log file {0}".format(last_log))
+        data = px4tools.read_ulog(last_log).concat(dt=0.1)
+        data = px4tools.compute_data(data)
         res = px4tools.estimator_analysis(data, False)
 
         # enforce performance
-        self.assertTrue(abs(res['roll_error_mean'])  < 5.0, str(res))
+        self.assertTrue(abs(res['roll_error_mean']) < 5.0, str(res))
         self.assertTrue(abs(res['pitch_error_mean']) < 5.0, str(res))
         self.assertTrue(abs(res['yaw_error_mean']) < 5.0, str(res))
         self.assertTrue(res['roll_error_std'] < 5.0, str(res))
         self.assertTrue(res['pitch_error_std'] < 5.0, str(res))
         self.assertTrue(res['yaw_error_std'] < 5.0, str(res))
 
+
 if __name__ == '__main__':
     import rostest
+    rospy.init_node('test_node', anonymous=True)
+
     name = "mavros_mission_test"
     if len(sys.argv) > 1:
         name += "-%s" % sys.argv[1]

integrationtests/run_container.bash

@@ -1,31 +0,0 @@
-#!/bin/bash
-#
-# Run container and start test execution
-#
-# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
-set -e
-
-if [ -z "$WORKSPACE" ]; then
-    echo "\$WORKSPACE not set"
-    exit 1
-fi
-
-IMAGE=px4io/px4-dev-ros:v1.0
-
-# Pulling latest image
-echo "===> pull latest Docker image"
-docker pull $IMAGE
-
-# removing some images might fail
-set +e
-docker rmi $(docker images --filter "dangling=true" -q --no-trunc)
-set -e
-echo "<==="
-
-#
-# Running SITL testing container
-# Assuming that necessary source projects, including this one, are cloned in the build server workspace of this job.
-#
-echo "===> run container"
-docker run --rm -v "$WORKSPACE:/job:rw" $IMAGE bash "/job/Firmware/integrationtests/run_tests.bash" /job/Firmware
-echo "<==="

integrationtests/run_tests.bash

@@ -1,140 +0,0 @@
-#!/bin/bash
-#
-# Starts tests from within the container
-#
-# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
-set -e
-
-# TODO move to docker image
-pip install --upgrade numpy -q
-pip install px4tools pymavlink toml -q
-
-# A POSIX variable
-OPTIND=1         # Reset in case getopts has been used previously in the shell.
-
-# Initialize our own variables:
-do_clean=true
-gui=false
-
-while getopts "h?og" opt; do
-    case "$opt" in
-    h|\?)
-		echo """
-		$0 [-h] [-o] [-g]
-		-h show help
-		-o don't clean before building (to save time)
-		-g run gazebo gui
-		"""
-        exit 0
-        ;;
-    o)  do_clean=false
-		echo "not cleaning"
-        ;;
-    g)  gui=true
-        ;;
-    esac
-done
-
-
-# determine the directory of the source given the directory of this script
-pushd `dirname $0` > /dev/null
-SCRIPTPATH=`pwd`
-popd > /dev/null
-ORIG_SRC=$(dirname $SCRIPTPATH)
-
-# set paths
-JOB_DIR=$(dirname $ORIG_SRC)
-CATKIN_DIR=$JOB_DIR/catkin
-BUILD_DIR=$CATKIN_DIR/build/px4
-SRC_DIR=${CATKIN_DIR}/src/px4
-
-echo setting up ROS paths
-if [ -f /opt/ros/indigo/setup.bash ]
-then
-	source /opt/ros/indigo/setup.bash
-elif [ -f /opt/ros/kinetic/setup.bash ]
-then
-	source /opt/ros/kinetic/setup.bash
-else
-	echo "could not find /opt/ros/{ros-distro}/setup.bash"
-	exit 1
-fi
-export ROS_HOME=$JOB_DIR/.ros
-export ROS_LOG_DIR=$ROS_HOME/log
-export ROS_TEST_RESULT_DIR=$ROS_HOME/test_results/px4
-export PX4_LOG_DIR=$ROS_HOME/rootfs/fs/microsd/log
-TEST_RESULT_TARGET_DIR=$JOB_DIR/test_results
-
-# TODO
-# BAGS=$ROS_HOME
-# CHARTS=$ROS_HOME/charts
-# EXPORT_CHARTS=/sitl/testing/export_charts.py
-
-if $do_clean
-then
-	echo cleaning
-	rm -rf $CATKIN_DIR
-	rm -rf $ROS_HOME
-	rm -rf $TEST_RESULT_TARGET_DIR
-else
-	echo skipping clean step
-fi
-
-echo "=====> compile ($SRC_DIR)"
-mkdir -p $ROS_HOME
-mkdir -p $CATKIN_DIR/src
-mkdir -p $TEST_RESULT_TARGET_DIR
-if ! [ -d $SRC_DIR ]
-then
-	ln -s $ORIG_SRC $SRC_DIR
-	ln -s $ORIG_SRC/Tools/sitl_gazebo ${CATKIN_DIR}/src/mavlink_sitl_gazebo
-fi
-cd $CATKIN_DIR
-catkin_make
-. ./devel/setup.bash
-echo "<====="
-
-# print paths to user
-echo -e "JOB_DIR\t\t: $JOB_DIR"
-echo -e "ROS_HOME\t: $ROS_HOME"
-echo -e "CATKIN_DIR\t: $CATKIN_DIR"
-echo -e "BUILD_DIR\t: $BUILD_DIR"
-echo -e "SRC_DIR\t\t: $SRC_DIR"
-echo -e "ROS_TEST_RESULT_DIR\t: $ROS_TEST_RESULT_DIR"
-echo -e "ROS_LOG_DIR\t\t: $ROS_LOG_DIR"
-echo -e "PX4_LOG_DIR\t\t: $PX4_LOG_DIR"
-echo -e "TEST_RESULT_TARGET_DIR\t: $TEST_RESULT_TARGET_DIR"
-
-# don't exit on error anymore (because single tests or exports might fail)
-# however, stop executing tests after the first failure
-set +e
-echo "=====> run tests"
-test $? -eq 0 && rostest px4 mavros_posix_tests_iris.launch gui:=$gui
-
-# commented out optical flow test for now since ci server has
-# an issue producing the simulated flow camera currently
-#test $? -eq 0 && rostest px4 mavros_posix_tests_iris_opt_flow.launch gui:=$gui
-
-test $? -eq 0 && rostest px4 mavros_posix_tests_standard_vtol.launch gui:=$gui
-TEST_RESULT=$?
-echo "<====="
-
-# TODO
-echo "=====> process test results"
-# cd $BAGS
-# for bag in `ls *.bag`
-# do
-# 	echo "processing bag: $bag"
-# 	python $EXPORT_CHARTS $CHARTS $bag
-# done
-
-echo "copy build test results to job directory"
-cp -r $ROS_TEST_RESULT_DIR/* ${TEST_RESULT_TARGET_DIR}
-cp -r $ROS_LOG_DIR/* ${TEST_RESULT_TARGET_DIR}
-cp -r $PX4_LOG_DIR/* ${TEST_RESULT_TARGET_DIR}
-# cp $BAGS/*.bag ${TEST_RESULT_TARGET_DIR}/
-# cp -r $CHARTS ${TEST_RESULT_TARGET_DIR}/
-echo "<====="
-
-# need to return error if tests failed, else Jenkins won't notice the failure
-exit $TEST_RESULT

integrationtests/upload_test_logs.bash

@@ -1,19 +0,0 @@
-#!/bin/bash
-#
-# Upload SITL CI logs to Flight Review
-#
-# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
-
-if [ -z "$WORKSPACE" ] || [ -z "${ghprbActualCommitAuthorEmail}" ] || [ -z "${ghprbPullDescription}" ]; then
-    echo "Environment not set. Needs to be called from within Jenkins."
-    exit 1
-fi
-
-echo "Uploading test logs to Flight Review"
-
-CMD="$WORKSPACE/Firmware/Tools/upload_log.py"
-find "$WORKSPACE/test_results" -name \*.ulg -exec "$CMD" -q \
-	--description "${ghprbPullDescription}" --source CI "{}" \;
-
-# XXX: move up if we want email notifications
-#	--email "${ghprbActualCommitAuthorEmail}" \

launch/mavros_posix_sitl.launch

@@ -9,7 +9,7 @@
     <arg name="Y" default="0"/>
 
 
-    <arg name="est" default="lpe"/>
+    <arg name="est" default="ekf2"/>
     <arg name="vehicle" default="iris"/>
     <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
     <arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf"/>

launch/posix_sitl.launch

@@ -7,7 +7,7 @@
     <arg name="R" default="0"/>
     <arg name="P" default="0"/>
     <arg name="Y" default="0"/>
-    <arg name="est" default="lpe"/>
+    <arg name="est" default="ekf2"/>
     <arg name="vehicle" default="iris"/>
     <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
     <arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf"/>
@@ -40,4 +40,4 @@
 
 </launch>
 
-<!-- vim: set et ft=xml fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : -->
+<!-- vim: set et ft=xml fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : -->

launch/single_vehcile_spawn.launch

@@ -7,7 +7,7 @@
     <arg name="R" default="0"/>
     <arg name="P" default="0"/>
     <arg name="Y" default="0"/>
-    <arg name="est" default="lpe"/>
+    <arg name="est" default="ekf2"/>
     <arg name="vehicle" default="iris"/>
     <arg name="ID" default="1"/>
     <arg name="rcS" default="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_$(arg ID)"/>

msg/home_position.msg

@@ -9,9 +9,8 @@ float32 y				# Y coordinate in meters
 float32 z				# Z coordinate in meters
 
 float32 yaw				# Yaw angle in radians
-float32 direction_x		# Takeoff direction in NED X
-float32 direction_y		# Takeoff direction in NED Y
-float32 direction_z		# Takeoff direction in NED Z
 
 bool valid_alt		# true when the altitude has been set
 bool valid_hpos		# true when the latitude and longitude have been set
+
+bool manual_home	# true when home position was set manually

msg/vehicle_status.msg

@@ -63,7 +63,6 @@ uint8 rc_input_mode				# set to 1 to disable the RC input, 2 to enable manual co
 bool data_link_lost				# datalink to GCS lost
 uint8 data_link_lost_counter			# counts unique data link lost events
 bool engine_failure				# Set to true if an engine failure is detected
-bool engine_failure_cmd				# Set to true if an engine failure mode is commanded
 bool mission_failure				# Set to true if mission could not continue/finish
 
 # see SYS_STATUS mavlink message for the following

msg/vehicle_status_flags.msg

@@ -43,15 +43,9 @@ uint16 OFFBOARD_CONTROL_SIGNAL_WEAK_MASK = 8
 uint16 OFFBOARD_CONTROL_SET_BY_COMMAND_MASK = 16
 uint16 OFFBOARD_CONTROL_LOSS_TIMEOUT_MASK = 32
 uint16 RC_SIGNAL_FOUND_ONCE_MASK = 64
-uint16 RC_SIGNAL_LOST_CMD_MASK = 128
 uint16 RC_INPUT_BLOCKED_MASK = 256
-uint16 DATA_LINK_LOST_CMD_MASK = 512
 uint16 VTOL_TRANSITION_FAILURE_MASK = 1024
-uint16 VTOL_TRANSITION_FAILURE_CMD_MASK = 2048
 uint16 GPS_FAILURE_MASK = 4096
-uint16 GPS_FAILURE_CMD_MASK = 8192
-uint16 BAROMETER_FAILURE_MASK = 16384
-uint16 EVER_HAD_BAROMETER_DATA_MASK = 32768
 
 # 0x0001 usb_connected: status of the USB power supply
 # 0x0002 offboard_control_signal_found_once
@@ -60,14 +54,8 @@ uint16 EVER_HAD_BAROMETER_DATA_MASK = 32768
 # 0x0010 offboard_control_set_by_command: true if the offboard mode was set by a mavlink command and should not be overridden by RC
 # 0x0020 offboard_control_loss_timeout: true if offboard is lost for a certain amount of time
 # 0x0040 rc_signal_found_once
-# 0x0080 rc_signal_lost_cmd: true if RC lost mode is commanded
 # 0x0100 rc_input_blocked: set if RC input should be ignored temporarily
-# 0x0200 data_link_lost_cmd: datalink to GCS lost mode commanded
 # 0x0400 vtol_transition_failure: Set to true if vtol transition failed
-# 0x0800 vtol_transition_failure_cmd: Set to true if vtol transition failure mode is commanded
 # 0x1000 gps_failure: Set to true if a gps failure is detected
-# 0x2000 gps_failure_cmd: Set to true if a gps failure mode is commanded
-# 0x4000 barometer_failure: Set to true if a barometer failure is detected
-# 0x8000 ever_had_barometer_data: Set to true if ever had valid barometer data before
 
 uint16 other_flags

msg/vtol_vehicle_status.msg

@@ -4,11 +4,9 @@ uint8 VEHICLE_VTOL_STATE_TRANSITION_TO_FW = 1
 uint8 VEHICLE_VTOL_STATE_TRANSITION_TO_MC = 2
 uint8 VEHICLE_VTOL_STATE_MC = 3
 uint8 VEHICLE_VTOL_STATE_FW = 4
-uint8 VEHICLE_VTOL_STATE_EXTERNAL = 5
 
 bool vtol_in_rw_mode			# true: vtol vehicle is in rotating wing mode
 bool vtol_in_trans_mode
 bool in_transition_to_fw		# True if VTOL is doing a transition from MC to FW
 bool vtol_transition_failsafe	# vtol in transition failsafe mode
 bool fw_permanent_stab			# In fw mode stabilize attitude even if in manual mode
-float32 airspeed_tot			# Estimated airspeed over control surfaces

src/drivers/adis16448/adis16448.cpp

@@ -176,7 +176,7 @@
 #define FW_FILTER									false
 
 #define SPI_BUS_SPEED								1000000
-#define T_STALL										5
+#define T_STALL										9
 
 #define GYROINITIALSENSITIVITY						250
 #define ACCELINITIALSENSITIVITY						(1.0f / 1200.0f)
@@ -293,6 +293,21 @@ private:
 		uint16_t		mag_z;
 		uint16_t		baro;
 		uint16_t		temp;
+
+		ADISReport():
+			cmd(0),
+			status(0),
+			gyro_x(0),
+			gyro_y(0),
+			gyro_z(0),
+			accel_x(0),
+			accel_y(0),
+			accel_z(0),
+			mag_x(0),
+			mag_y(0),
+			mag_z(0),
+			baro(0),
+			temp(0) {}
 	};
 #pragma pack(pop)
 
@@ -711,21 +726,21 @@ int ADIS16448::reset()
 
 	/* Set gyroscope scale to default value */
 	_set_gyro_dyn_range(GYROINITIALSENSITIVITY);
-	usleep(1000);
 
 	/* Set digital FIR filter tap */
 	_set_dlpf_filter(BITS_FIR_16_TAP_CFG);
-	usleep(1000);
 
 	/* Set IMU sample rate */
 	_set_sample_rate(_sample_rate);
-	usleep(1000);
 
 	_accel_range_scale = ADIS16448_ONE_G * ACCELINITIALSENSITIVITY;
 	_accel_range_m_s2  = ADIS16448_ONE_G * ACCELDYNAMICRANGE;
 	_mag_range_scale   = MAGINITIALSENSITIVITY;
 	_mag_range_mgauss  = MAGDYNAMICRANGE;
 
+	/* settling time */
+	up_udelay(50000);
+
 	return OK;
 
 }
@@ -753,6 +768,7 @@ ADIS16448::probe()
 	case ADIS16448_Product:
 		DEVICE_DEBUG("ADIS16448 is detected ID: 0x%02x, Serial: 0x%02x", _product, serial_number);
 		modify_reg16(ADIS16448_GPIO_CTRL, 0x0200, 0x0002);			/* Turn on ADIS16448 adaptor board led */
+		_set_factory_default();										/* Restore factory calibration 		   */
 		return OK;
 	}
 
@@ -782,9 +798,9 @@ ADIS16448::_set_sample_rate(uint16_t desired_sample_rate_hz)
 		smpl_prd = BITS_SMPL_PRD_NO_TAP_CFG;
 	}
 
-	modify_reg16(ADIS16448_SMPL_PRD, 0x0700, smpl_prd);
+	modify_reg16(ADIS16448_SMPL_PRD, 0x1f00, smpl_prd);
 
-	if ((read_reg16(ADIS16448_SMPL_PRD) & 0x0700) != smpl_prd) {
+	if ((read_reg16(ADIS16448_SMPL_PRD) & 0x1f00) != smpl_prd) {
 		DEVICE_DEBUG("failed to set IMU sample rate");
 	}
 
@@ -809,7 +825,6 @@ void
 ADIS16448::_set_factory_default()
 {
 	write_reg16(ADIS16448_GLOB_CMD, 0x02);
-	warnx("Set IMU to factory default!");
 }
 
 /* set the gyroscope dynamic range */
@@ -1096,9 +1111,6 @@ ADIS16448::ioctl(struct file *filp, int cmd, unsigned long arg)
 			return OK;
 		}
 
-	case SENSORIOCGQUEUEDEPTH:
-		return _accel_reports->size();
-
 	case ACCELIOCGSAMPLERATE:
 		return _sample_rate;
 
@@ -1106,15 +1118,6 @@ ADIS16448::ioctl(struct file *filp, int cmd, unsigned long arg)
 		_set_sample_rate(arg);
 		return OK;
 
-	case ACCELIOCGLOWPASS:
-		return _accel_filter_x.get_cutoff_freq();
-
-	case ACCELIOCSLOWPASS:
-		_accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
-		_accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
-		_accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
-		return OK;
-
 	case ACCELIOCSSCALE: {
 			/* copy scale, but only if off by a few percent */
 			struct accel_calibration_s *s = (struct accel_calibration_s *) arg;
@@ -1181,9 +1184,6 @@ ADIS16448::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 			return OK;
 		}
 
-	case SENSORIOCGQUEUEDEPTH:
-		return _gyro_reports->size();
-
 	case GYROIOCGSAMPLERATE:
 		return _sample_rate;
 
@@ -1191,16 +1191,6 @@ ADIS16448::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 		_set_sample_rate(arg);
 		return OK;
 
-	case GYROIOCGLOWPASS:
-		return _gyro_filter_x.get_cutoff_freq();
-
-	case GYROIOCSLOWPASS:
-		_gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
-		_gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
-		_gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
-
-		return OK;
-
 	case GYROIOCSSCALE:
 		/* copy scale in */
 		memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale));
@@ -1259,9 +1249,6 @@ ADIS16448::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
 			return OK;
 		}
 
-	case SENSORIOCGQUEUEDEPTH:
-		return _mag_reports->size();
-
 	case MAGIOCGSAMPLERATE:
 		return _sample_rate;
 
@@ -1269,15 +1256,6 @@ ADIS16448::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
 		_set_sample_rate(arg);
 		return OK;
 
-	case MAGIOCGLOWPASS:
-		return _mag_filter_x.get_cutoff_freq();
-
-	case MAGIOCSLOWPASS:
-		_mag_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
-		_mag_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
-		_mag_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
-		return OK;
-
 	case MAGIOCSSCALE:
 		/* copy scale in */
 		memcpy(&_mag_scale, (struct mag_calibration_s *) arg, sizeof(_mag_scale));
@@ -1316,6 +1294,7 @@ ADIS16448::read_reg16(unsigned reg)
 	transferhword(cmd, nullptr, 1);
 	up_udelay(T_STALL);
 	transferhword(nullptr, cmd, 1);
+	up_udelay(T_STALL);
 
 	return cmd[0];
 }
@@ -1331,6 +1310,7 @@ ADIS16448::write_reg16(unsigned reg, uint16_t value)
 	transferhword(cmd, nullptr, 1);
 	up_udelay(T_STALL);
 	transferhword(cmd + 1, nullptr, 1);
+	up_udelay(T_STALL);
 }
 
 void

src/drivers/bma180/bma180.cpp

@@ -471,9 +471,6 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
 			return OK;
 		}
 
-	case SENSORIOCGQUEUEDEPTH:
-		return _reports->size();
-
 	case SENSORIOCRESET:
 		/* XXX implement */
 		return -EINVAL;
@@ -484,12 +481,6 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case ACCELIOCGSAMPLERATE:
 		return 1200;		/* always operating in low-noise mode */
 
-	case ACCELIOCSLOWPASS:
-		return set_lowpass(arg);
-
-	case ACCELIOCGLOWPASS:
-		return _current_lowpass;
-
 	case ACCELIOCSSCALE:
 		/* copy scale in */
 		memcpy(&_accel_scale, (struct accel_calibration_s *) arg, sizeof(_accel_scale));

src/drivers/bmi055/bmi055_accel.cpp

@@ -434,25 +434,12 @@ BMI055_accel::ioctl(struct file *filp, int cmd, unsigned long arg)
 			return OK;
 		}
 
-	case SENSORIOCGQUEUEDEPTH:
-		return _accel_reports->size();
-
 	case ACCELIOCGSAMPLERATE:
 		return _accel_sample_rate;
 
 	case ACCELIOCSSAMPLERATE:
 		return accel_set_sample_rate(arg);
 
-	case ACCELIOCGLOWPASS:
-		return _accel_filter_x.get_cutoff_freq();
-
-	case ACCELIOCSLOWPASS:
-		// set software filtering
-		_accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
-		_accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
-		_accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
-		return OK;
-
 	case ACCELIOCSSCALE: {
 			/* copy scale, but only if off by a few percent */
 			struct accel_calibration_s *s = (struct accel_calibration_s *) arg;
@@ -481,19 +468,6 @@ BMI055_accel::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case ACCELIOCSELFTEST:
 		return accel_self_test();
 
-#ifdef ACCELIOCSHWLOWPASS
-
-	case ACCELIOCSHWLOWPASS:
-		return OK;
-#endif
-
-#ifdef ACCELIOCGHWLOWPASS
-
-	case ACCELIOCGHWLOWPASS:
-		return _dlpf_freq;
-#endif
-
-
 	default:
 		/* give it to the superclass */
 		return SPI::ioctl(filp, cmd, arg);

src/drivers/bmi055/bmi055_gyro.cpp

@@ -435,25 +435,12 @@ BMI055_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
 			return OK;
 		}
 
-	case SENSORIOCGQUEUEDEPTH:
-		return _gyro_reports->size();
-
 	case GYROIOCGSAMPLERATE:
 		return _gyro_sample_rate;
 
 	case GYROIOCSSAMPLERATE:
 		return gyro_set_sample_rate(arg);
 
-	case GYROIOCGLOWPASS:
-		return _gyro_filter_x.get_cutoff_freq();
-
-	case GYROIOCSLOWPASS:
-		// set software filtering
-		_gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
-		_gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
-		_gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
-		return OK;
-
 	case GYROIOCSSCALE:
 		/* copy scale in */
 		memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale));
@@ -473,18 +460,6 @@ BMI055_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case GYROIOCSELFTEST:
 		return gyro_self_test();
 
-#ifdef GYROIOCSHWLOWPASS
-
-	case GYROIOCSHWLOWPASS:
-		return OK;
-#endif
-
-#ifdef GYROIOCGHWLOWPASS
-
-	case GYROIOCGHWLOWPASS:
-		return _dlpf_freq;
-#endif
-
 	default:
 		/* give it to the superclass */
 		return SPI::ioctl(filp, cmd, arg);

src/drivers/bmi160/bmi160.cpp

@@ -714,25 +714,12 @@ BMI160::ioctl(struct file *filp, int cmd, unsigned long arg)
 			return OK;
 		}
 
-	case SENSORIOCGQUEUEDEPTH:
-		return _accel_reports->size();
-
 	case ACCELIOCGSAMPLERATE:
 		return _accel_sample_rate;
 
 	case ACCELIOCSSAMPLERATE:
 		return accel_set_sample_rate(arg);
 
-	case ACCELIOCGLOWPASS:
-		return _accel_filter_x.get_cutoff_freq();
-
-	case ACCELIOCSLOWPASS:
-		// set software filtering
-		_accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
-		_accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
-		_accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
-		return OK;
-
 	case ACCELIOCSSCALE: {
 			/* copy scale, but only if off by a few percent */
 			struct accel_calibration_s *s = (struct accel_calibration_s *) arg;
@@ -761,20 +748,6 @@ BMI160::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case ACCELIOCSELFTEST:
 		return accel_self_test();
 
-#ifdef ACCELIOCSHWLOWPASS
-
-	case ACCELIOCSHWLOWPASS:
-		_set_dlpf_filter(arg);
-		return OK;
-#endif
-
-#ifdef ACCELIOCGHWLOWPASS
-
-	case ACCELIOCGHWLOWPASS:
-		return _dlpf_freq;
-#endif
-
-
 	default:
 		/* give it to the superclass */
 		return SPI::ioctl(filp, cmd, arg);
@@ -810,25 +783,12 @@ BMI160::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 			return OK;
 		}
 
-	case SENSORIOCGQUEUEDEPTH:
-		return _gyro_reports->size();
-
 	case GYROIOCGSAMPLERATE:
 		return _gyro_sample_rate;
 
 	case GYROIOCSSAMPLERATE:
 		return gyro_set_sample_rate(arg);
 
-	case GYROIOCGLOWPASS:
-		return _gyro_filter_x.get_cutoff_freq();
-
-	case GYROIOCSLOWPASS:
-		// set software filtering
-		_gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
-		_gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
-		_gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
-		return OK;
-
 	case GYROIOCSSCALE:
 		/* copy scale in */
 		memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale));
@@ -848,19 +808,6 @@ BMI160::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 	case GYROIOCSELFTEST:
 		return gyro_self_test();
 
-#ifdef GYROIOCSHWLOWPASS
-
-	case GYROIOCSHWLOWPASS:
-		_set_dlpf_filter(arg);
-		return OK;
-#endif
-
-#ifdef GYROIOCGHWLOWPASS
-
-	case GYROIOCGHWLOWPASS:
-		return _dlpf_freq;
-#endif
-
 	default:
 		/* give it to the superclass */
 		return SPI::ioctl(filp, cmd, arg);

src/drivers/bmm150/bmm150.cpp

@@ -794,9 +794,6 @@ BMM150::ioctl(struct file *filp, int cmd, unsigned long arg)
 			return OK;
 		}
 
-	case SENSORIOCGQUEUEDEPTH:
-		return _reports->size();
-
 	case SENSORIOCRESET:
 		return reset();
 
@@ -827,11 +824,6 @@ BMM150::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case MAGIOCGRANGE:
 		return OK;
 
-	case MAGIOCSLOWPASS:
-	case MAGIOCGLOWPASS:
-		/* not supported, no internal filtering */
-		return -EINVAL;
-
 	default:
 		/* give it to the superclass */
 		return I2C::ioctl(filp, cmd, arg);

src/drivers/bmp280/bmp280.cpp

@@ -411,9 +411,6 @@ BMP280::ioctl(struct file *filp, int cmd, unsigned long arg)
 			return OK;
 		}
 
-	case SENSORIOCGQUEUEDEPTH:
-		return _reports->size();
-
 	case SENSORIOCRESET:
 		/*
 		 * Since we are initialized, we do not need to do anything, since the

src/drivers/drv_accel.h

@@ -82,12 +82,6 @@ struct accel_calibration_s {
 /** return the accel internal sample rate in Hz */
 #define ACCELIOCGSAMPLERATE	_ACCELIOC(1)
 
-/** set the accel internal lowpass filter to no lower than (arg) Hz */
-#define ACCELIOCSLOWPASS	_ACCELIOC(2)
-
-/** return the accel internal lowpass filter in Hz */
-#define ACCELIOCGLOWPASS	_ACCELIOC(3)
-
 /** set the accel scaling constants to the structure pointed to by (arg) */
 #define ACCELIOCSSCALE		_ACCELIOC(5)
 
@@ -103,12 +97,6 @@ struct accel_calibration_s {
 /** get the result of a sensor self-test */
 #define ACCELIOCSELFTEST	_ACCELIOC(9)
 
-/** set the hardware low-pass filter cut-off no lower than (arg) Hz */
-#define ACCELIOCSHWLOWPASS	_ACCELIOC(10)
-
-/** get the hardware low-pass filter cut-off in Hz*/
-#define ACCELIOCGHWLOWPASS	_ACCELIOC(11)
-
 /** determine if hardware is external or onboard */
 #define ACCELIOCGEXTERNAL	_ACCELIOC(12)
 

src/drivers/drv_gpio.h

@@ -68,38 +68,10 @@
 /** configure the board GPIOs in (arg) as outputs */
 #define GPIO_SET_OUTPUT	GPIOC(1)
 
-/** configure the board GPIOs in (arg) as inputs */
-#define GPIO_SET_INPUT	GPIOC(2)
-
-/** configure the board GPIOs in (arg) for the first alternate function (if supported) */
-#define GPIO_SET_ALT_1	GPIOC(3)
-
-/** configure the board GPIO (arg) for the second alternate function (if supported) */
-#define GPIO_SET_ALT_2	GPIOC(4)
-
-/** configure the board GPIO (arg) for the third alternate function (if supported) */
-#define GPIO_SET_ALT_3	GPIOC(5)
-
-/** configure the board GPIO (arg) for the fourth alternate function (if supported) */
-#define GPIO_SET_ALT_4	GPIOC(6)
-
 /** set the GPIOs in (arg) */
 #define GPIO_SET	GPIOC(10)
 
 /** clear the GPIOs in (arg) */
 #define GPIO_CLEAR	GPIOC(11)
 
-/** read all the GPIOs and return their values in *(uint32_t *)arg */
-#define GPIO_GET	GPIOC(12)
-
-#define GPIO_SENSOR_RAIL_RESET	GPIOC(13)
-
-#define GPIO_PERIPHERAL_RAIL_RESET	GPIOC(14)
-
-/** configure the board GPIOs in (arg) as outputs, initially low */
-#define GPIO_SET_OUTPUT_LOW	GPIOC(15)
-
-/** configure the board GPIOs in (arg) as outputs, initially high */
-#define GPIO_SET_OUTPUT_HIGH	GPIOC(16)
-
 #endif /* _DRV_GPIO_H */

src/drivers/drv_gyro.h

@@ -79,12 +79,6 @@ struct gyro_calibration_s {
 /** return the gyro internal sample rate in Hz */
 #define GYROIOCGSAMPLERATE	_GYROIOC(1)
 
-/** set the gyro internal lowpass filter to no lower than (arg) Hz */
-#define GYROIOCSLOWPASS		_GYROIOC(2)
-
-/** set the gyro internal lowpass filter to no lower than (arg) Hz */
-#define GYROIOCGLOWPASS		_GYROIOC(3)
-
 /** set the gyro scaling constants to (arg) */
 #define GYROIOCSSCALE		_GYROIOC(4)
 
@@ -100,15 +94,6 @@ struct gyro_calibration_s {
 /** check the status of the sensor */
 #define GYROIOCSELFTEST		_GYROIOC(8)
 
-/** set the hardware low-pass filter cut-off no lower than (arg) Hz */
-#define GYROIOCSHWLOWPASS	_GYROIOC(9)
-
-/** get the hardware low-pass filter cut-off in Hz*/
-#define GYROIOCGHWLOWPASS	_GYROIOC(10)
-
-/** determine if hardware is external or onboard */
-#define GYROIOCGEXTERNAL	_GYROIOC(12)
-
 /** get the current gyro type */
 #define GYROIOCTYPE			_GYROIOC(13)
 

src/drivers/drv_mag.h

@@ -75,12 +75,6 @@ struct mag_calibration_s {
 /** return the mag internal sample rate in Hz */
 #define MAGIOCGSAMPLERATE	_MAGIOC(1)
 
-/** set the mag internal lowpass filter to no lower than (arg) Hz */
-#define MAGIOCSLOWPASS		_MAGIOC(2)
-
-/** return the mag internal lowpass filter in Hz */
-#define MAGIOCGLOWPASS		_MAGIOC(3)
-
 /** set the mag scaling constants to the structure pointed to by (arg) */
 #define MAGIOCSSCALE		_MAGIOC(4)
 

src/drivers/drv_pwm_output.h

@@ -248,15 +248,6 @@ struct pwm_output_rc_config {
 /** force safety switch on (to enable use of safety switch) */
 #define PWM_SERVO_SET_FORCE_SAFETY_ON		_PX4_IOC(_PWM_SERVO_BASE, 28)
 
-/** set RC config for a channel. This takes a pointer to pwm_output_rc_config */
-#define PWM_SERVO_SET_RC_CONFIG			_PX4_IOC(_PWM_SERVO_BASE, 29)
-
-/** set the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */
-#define PWM_SERVO_SET_OVERRIDE_OK		_PX4_IOC(_PWM_SERVO_BASE, 30)
-
-/** clear the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */
-#define PWM_SERVO_CLEAR_OVERRIDE_OK		_PX4_IOC(_PWM_SERVO_BASE, 31)
-
 /** setup OVERRIDE_IMMEDIATE behaviour on FMU fail */
 #define PWM_SERVO_SET_OVERRIDE_IMMEDIATE	_PX4_IOC(_PWM_SERVO_BASE, 32)
 

src/drivers/drv_range_finder.h

@@ -48,21 +48,4 @@
 #define RANGE_FINDER0_DEVICE_PATH	"/dev/range_finder0"
 #define MB12XX_MAX_RANGEFINDERS	12	// Maximum number of Maxbotix sensors on bus
 
-/*
- * ioctl() definitions
- *
- * Rangefinder drivers also implement the generic sensor driver
- * interfaces from drv_sensor.h
- */
-
-#define _RANGEFINDERIOCBASE			(0x7900)
-#define __RANGEFINDERIOC(_n)		(_IOC(_RANGEFINDERIOCBASE, _n))
-
-/** set the minimum effective distance of the device */
-#define RANGEFINDERIOCSETMINIUMDISTANCE	__RANGEFINDERIOC(1)
-
-/** set the maximum effective distance of the device */
-#define RANGEFINDERIOCSETMAXIUMDISTANCE	__RANGEFINDERIOC(2)
-
-
 #endif /* _DRV_RANGEFINDER_H */

src/drivers/drv_rc_input.h

@@ -88,9 +88,6 @@ typedef uint16_t		rc_input_t;
 
 #define _RC_INPUT_BASE		0x2b00
 
-/** Fetch R/C input values into (rc_input_values *)arg */
-#define RC_INPUT_GET			_IOC(_RC_INPUT_BASE, 0)
-
 /** Enable RSSI input via ADC */
 #define RC_INPUT_ENABLE_RSSI_ANALOG	_IOC(_RC_INPUT_BASE, 1)