logger: add safety (switch) at minimal rate

This fixes a corner case for VTOL RTL with RTL_TYPE 1.
RTL_TYPE 1 means that the VTOL skips all waypoints on RTL and jumps to
the land waypoint at the end.

During a mission in fixedwing mode it will continue to fly in fixedwing
mode and then do a transition before landing.

However, if RTL is engaged right at the moment of the front transition,
the transition waypoint is not inserted and the VTOL will try to land in
fixedwing mode at the mission land location.

This corner case is fixed in this patch by also considering the
"transition after takeoff" state.

.ci/Jenkinsfile-SITL_tests

@@ -8,7 +8,7 @@ pipeline {
     stage('Build') {
       agent {
         docker {
-          image 'px4io/px4-dev-ros-kinetic:2019-10-04'
+          image 'px4io/px4-dev-ros-melodic:2019-10-24'
           args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE'
         }
       }
@@ -130,7 +130,7 @@ def createTestNode(Map test_def) {
   return {
     node {
       cleanWs()
-      docker.image("px4io/px4-dev-ros-kinetic:2019-10-04").inside('-e HOME=${WORKSPACE}') {
+      docker.image("px4io/px4-dev-ros-melodic:2019-10-24").inside('-e HOME=${WORKSPACE}') {
         stage(test_def.name) {
           def run_script = test_def.get('run_script', 'rostest_px4_run.sh')
           def test_ok = true

.ci/Jenkinsfile-SITL_tests_ASan

@@ -8,7 +8,7 @@ pipeline {
     stage('Build') {
       agent {
         docker {
-          image 'px4io/px4-dev-ros-kinetic:2019-10-04'
+          image 'px4io/px4-dev-ros-melodic:2019-10-24'
           args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE'
         }
       }
@@ -131,7 +131,7 @@ def createTestNode(Map test_def) {
   return {
     node {
       cleanWs()
-      docker.image("px4io/px4-dev-ros-kinetic:2019-10-04").inside('-e HOME=${WORKSPACE}') {
+      docker.image("px4io/px4-dev-ros-melodic:2019-10-24").inside('-e HOME=${WORKSPACE}') {
         stage(test_def.name) {
           def run_script = test_def.get('run_script', 'rostest_px4_run.sh')
           def test_ok = true

.ci/Jenkinsfile-SITL_tests_coverage

@@ -79,7 +79,7 @@ pipeline {
         stage('code coverage (python)') {
           agent {
             docker {
-              image 'px4io/px4-dev-base-bionic:2019-10-04'
+              image 'px4io/px4-dev-base-bionic:2019-10-24'
               args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
             }
           }
@@ -99,7 +99,7 @@ pipeline {
         stage('unit tests') {
           agent {
             docker {
-              image 'px4io/px4-dev-base-bionic:2019-10-04'
+              image 'px4io/px4-dev-base-bionic:2019-10-24'
               args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
             }
           }
@@ -137,7 +137,7 @@ def createTestNode(Map test_def) {
   return {
     node {
       cleanWs()
-      docker.image("px4io/px4-dev-ros-kinetic:2019-10-04").inside('-e HOME=${WORKSPACE}') {
+      docker.image("px4io/px4-dev-ros-melodic:2019-10-24").inside('-e HOME=${WORKSPACE}') {
         stage(test_def.name) {
           def test_ok = true
           sh('export')

.ci/Jenkinsfile-compile

@@ -9,10 +9,10 @@ pipeline {
         script {
           def build_nodes = [:]
           def docker_images = [
-            armhf: "px4io/px4-dev-armhf:2019-10-04",
+            armhf: "px4io/px4-dev-armhf:2019-10-24",
-            base: "px4io/px4-dev-base-bionic:2019-10-04",
+            base: "px4io/px4-dev-base-bionic:2019-10-24",
-            nuttx: "px4io/px4-dev-nuttx:2019-10-04",
+            nuttx: "px4io/px4-dev-nuttx:2019-10-24",
-            rpi: "px4io/px4-dev-raspi:2019-10-04",
+            rpi: "px4io/px4-dev-raspi:2019-10-24",
             snapdragon: "lorenzmeier/px4-dev-snapdragon:2018-09-12"
           ]
 
@@ -82,7 +82,7 @@ pipeline {
     // TODO: actually upload artifacts to S3
     // stage('S3 Upload') {
     //   agent {
-    //     docker { image 'px4io/px4-dev-base-bionic:2019-10-04' }
+    //     docker { image 'px4io/px4-dev-base-bionic:2019-10-24' }
     //   }
     //   options {
     //         skipDefaultCheckout()

.ci/Jenkinsfile-hardware

@@ -11,7 +11,7 @@ pipeline {
         stage('px4_fmu-v2_test') {
           agent {
             docker {
-              image 'px4io/px4-dev-nuttx:2019-10-04'
+              image 'px4io/px4-dev-nuttx:2019-10-24'
               args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
             }
           }
@@ -35,7 +35,7 @@ pipeline {
         stage('px4_fmu-v3_default') {
           agent {
             docker {
-              image 'px4io/px4-dev-nuttx:2019-10-04'
+              image 'px4io/px4-dev-nuttx:2019-10-24'
               args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
             }
           }
@@ -59,7 +59,7 @@ pipeline {
         stage('px4_fmu-v4_default') {
           agent {
             docker {
-              image 'px4io/px4-dev-nuttx:2019-10-04'
+              image 'px4io/px4-dev-nuttx:2019-10-24'
               args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
             }
           }
@@ -83,7 +83,7 @@ pipeline {
         stage('px4_fmu-v4pro_default') {
           agent {
             docker {
-              image 'px4io/px4-dev-nuttx:2019-10-04'
+              image 'px4io/px4-dev-nuttx:2019-10-24'
               args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
             }
           }
@@ -107,7 +107,7 @@ pipeline {
         stage('px4_fmu-v5_default') {
           agent {
             docker {
-              image 'px4io/px4-dev-nuttx:2019-10-04'
+              image 'px4io/px4-dev-nuttx:2019-10-24'
               args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
             }
           }
@@ -131,7 +131,7 @@ pipeline {
         stage('px4_fmu-v5_critmonitor') {
           agent {
             docker {
-              image 'px4io/px4-dev-nuttx:2019-10-04'
+              image 'px4io/px4-dev-nuttx:2019-10-24'
               args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
             }
           }
@@ -155,7 +155,7 @@ pipeline {
         stage('px4_fmu-v5_irqmonitor') {
           agent {
             docker {
-              image 'px4io/px4-dev-nuttx:2019-10-04'
+              image 'px4io/px4-dev-nuttx:2019-10-24'
               args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
             }
           }
@@ -179,7 +179,7 @@ pipeline {
         stage('px4_fmu-v5_stackcheck') {
           agent {
             docker {
-              image 'px4io/px4-dev-nuttx:2019-10-04'
+              image 'px4io/px4-dev-nuttx:2019-10-24'
               args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
             }
           }
@@ -203,7 +203,7 @@ pipeline {
         stage('px4_fmu-v5x_default') {
           agent {
             docker {
-              image 'px4io/px4-dev-nuttx:2019-10-04'
+              image 'px4io/px4-dev-nuttx:2019-10-24'
               args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
             }
           }
@@ -227,7 +227,7 @@ pipeline {
         stage('nxp_fmuk66-v3_default') {
           agent {
             docker {
-              image 'px4io/px4-dev-nuttx:2019-10-04'
+              image 'px4io/px4-dev-nuttx:2019-10-24'
               args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
             }
           }

.circleci/config.yml

@@ -2,7 +2,7 @@ version: 2
 jobs:
   build:
     docker:
-      - image: px4io/px4-dev-nuttx:2019-07-29
+      - image: px4io/px4-dev-nuttx:2019-10-24
     steps:
       - checkout
       - run:

.github/workflows/ccpp.yml

@@ -5,7 +5,7 @@ on: [push]
 jobs:
   build:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-bionic:2019-07-29
+    container: px4io/px4-dev-base-bionic:2019-10-24
     steps:
     - uses: actions/checkout@v1
     - name: make

.gitmodules

@@ -33,7 +33,7 @@
 [submodule "src/lib/ecl"]
 	path = src/lib/ecl
 	url = https://github.com/PX4/ecl.git
-	branch = master
+	branch = Release_v1.10
 [submodule "boards/atlflight/cmake_hexagon"]
 	path = boards/atlflight/cmake_hexagon
 	url = https://github.com/PX4/cmake_hexagon.git

.vscode/cmake-variants.yaml

@@ -51,11 +51,11 @@ CONFIG:
       buildType: MinSizeRel
       settings:
         CONFIG: intel_aerofc-v1_default
-    modalai_fc1_default:
+    modalai_fc-v1_default:
-      short: modalai_fc1_default
+      short: modalai_fc-v1_default
       buildType: MinSizeRel
       settings:
-        CONFIG: modalai_fc1_default
+        CONFIG: modalai_fc-v1_default
     mro_ctrl-zero-f7_default:
       short: mro_ctrl-zero-f7_default
       buildType: MinSizeRel

Jenkinsfile

@@ -11,7 +11,7 @@ pipeline {
         stage('Catkin build on ROS workspace') {
           agent {
             docker {
-              image 'px4io/px4-dev-ros-melodic:2019-10-04'
+              image 'px4io/px4-dev-ros-melodic:2019-10-24'
               args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE'
             }
           }
@@ -51,7 +51,7 @@ pipeline {
         stage('Colcon build on ROS2 workspace') {
           agent {
             docker {
-              image 'px4io/px4-dev-ros2-bouncy:2019-10-04'
+              image 'px4io/px4-dev-ros2-dashing:2019-10-24'
               args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE'
             }
           }
@@ -82,7 +82,7 @@ pipeline {
 
         stage('Style check') {
           agent {
-            docker { image 'px4io/px4-dev-base-bionic:2019-10-04' }
+            docker { image 'px4io/px4-dev-base-bionic:2019-10-24' }
           }
           steps {
             sh 'make check_format'
@@ -97,7 +97,7 @@ pipeline {
         stage('px4_fmu-v2 (bloaty)') {
           agent {
             docker {
-              image 'px4io/px4-dev-nuttx:2019-10-04'
+              image 'px4io/px4-dev-nuttx:2019-10-24'
               args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
             }
           }
@@ -130,7 +130,7 @@ pipeline {
         stage('px4_fmu-v5 (bloaty)') {
           agent {
             docker {
-              image 'px4io/px4-dev-nuttx:2019-10-04'
+              image 'px4io/px4-dev-nuttx:2019-10-24'
               args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
             }
           }
@@ -163,7 +163,7 @@ pipeline {
         stage('px4_sitl (bloaty)') {
           agent {
             docker {
-              image 'px4io/px4-dev-nuttx:2019-10-04'
+              image 'px4io/px4-dev-nuttx:2019-10-24'
               args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
             }
           }
@@ -196,7 +196,7 @@ pipeline {
         stage('px4_fmu-v5 (no ninja)') {
           agent {
             docker {
-              image 'px4io/px4-dev-nuttx:2019-10-04'
+              image 'px4io/px4-dev-nuttx:2019-10-24'
               args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
             }
           }
@@ -222,7 +222,7 @@ pipeline {
         stage('px4_sitl (no ninja)') {
           agent {
             docker {
-              image 'px4io/px4-dev-nuttx:2019-10-04'
+              image 'px4io/px4-dev-nuttx:2019-10-24'
               args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
             }
           }
@@ -248,7 +248,7 @@ pipeline {
         stage('SITL unit tests') {
           agent {
             docker {
-              image 'px4io/px4-dev-base-bionic:2019-10-04'
+              image 'px4io/px4-dev-base-bionic:2019-10-24'
               args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
             }
           }
@@ -270,7 +270,7 @@ pipeline {
         stage('Clang analyzer') {
           agent {
             docker {
-              image 'px4io/px4-dev-clang:2019-10-04'
+              image 'px4io/px4-dev-clang:2019-10-24'
               args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
             }
           }
@@ -308,7 +308,7 @@ pipeline {
         // stage('Clang tidy') {
         //   agent {
         //     docker {
-        //       image 'px4io/px4-dev-clang:2019-10-04'
+        //       image 'px4io/px4-dev-clang:2019-10-24'
         //       args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
         //     }
         //   }
@@ -329,7 +329,7 @@ pipeline {
         stage('Cppcheck') {
           agent {
             docker {
-              image 'px4io/px4-dev-base-bionic:2019-10-04'
+              image 'px4io/px4-dev-base-bionic:2019-10-24'
               args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
             }
           }
@@ -367,7 +367,7 @@ pipeline {
         stage('Check stack') {
           agent {
             docker {
-              image 'px4io/px4-dev-nuttx:2019-10-04'
+              image 'px4io/px4-dev-nuttx:2019-10-24'
               args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
             }
           }
@@ -387,7 +387,7 @@ pipeline {
         stage('ShellCheck') {
           agent {
             docker {
-              image 'px4io/px4-dev-nuttx:2019-10-04'
+              image 'px4io/px4-dev-nuttx:2019-10-24'
               args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
             }
           }
@@ -406,7 +406,7 @@ pipeline {
         stage('Module config validation') {
           agent {
             docker {
-              image 'px4io/px4-dev-base-bionic:2019-10-04'
+              image 'px4io/px4-dev-base-bionic:2019-10-24'
               args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
             }
           }
@@ -431,7 +431,7 @@ pipeline {
 
         stage('Airframe') {
           agent {
-            docker { image 'px4io/px4-dev-base-bionic:2019-10-04' }
+            docker { image 'px4io/px4-dev-base-bionic:2019-10-24' }
           }
           steps {
             sh 'make distclean'
@@ -450,7 +450,7 @@ pipeline {
 
         stage('Parameter') {
           agent {
-            docker { image 'px4io/px4-dev-base-bionic:2019-10-04' }
+            docker { image 'px4io/px4-dev-base-bionic:2019-10-24' }
           }
           steps {
             sh 'make distclean'
@@ -469,7 +469,7 @@ pipeline {
 
         stage('Module') {
           agent {
-            docker { image 'px4io/px4-dev-base-bionic:2019-10-04' }
+            docker { image 'px4io/px4-dev-base-bionic:2019-10-24' }
           }
           steps {
             sh 'make distclean'
@@ -489,7 +489,7 @@ pipeline {
         stage('uORB graphs') {
           agent {
             docker {
-              image 'px4io/px4-dev-nuttx:2019-10-04'
+              image 'px4io/px4-dev-nuttx:2019-10-24'
               args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
             }
           }
@@ -518,7 +518,7 @@ pipeline {
 
         stage('Devguide') {
           agent {
-            docker { image 'px4io/px4-dev-base-bionic:2019-10-04' }
+            docker { image 'px4io/px4-dev-base-bionic:2019-10-24' }
           }
           steps {
             sh('export')
@@ -548,7 +548,7 @@ pipeline {
 
         stage('Userguide') {
           agent {
-            docker { image 'px4io/px4-dev-base-bionic:2019-10-04' }
+            docker { image 'px4io/px4-dev-base-bionic:2019-10-24' }
           }
           steps {
             sh('export')
@@ -576,7 +576,7 @@ pipeline {
 
         stage('QGroundControl') {
           agent {
-            docker { image 'px4io/px4-dev-base-bionic:2019-10-04' }
+            docker { image 'px4io/px4-dev-base-bionic:2019-10-24' }
           }
           steps {
             sh('export')
@@ -604,7 +604,7 @@ pipeline {
 
         stage('PX4 ROS msgs') {
           agent {
-            docker { image 'px4io/px4-dev-base-bionic:2019-10-04' }
+            docker { image 'px4io/px4-dev-base-bionic:2019-10-24' }
           }
           steps {
             sh('export')
@@ -633,7 +633,7 @@ pipeline {
 
         stage('PX4 ROS2 bridge') {
           agent {
-            docker { image 'px4io/px4-dev-base-bionic:2019-10-04' }
+            docker { image 'px4io/px4-dev-base-bionic:2019-10-24' }
           }
           steps {
             sh('export')
@@ -674,7 +674,7 @@ pipeline {
 
         stage('S3') {
           agent {
-            docker { image 'px4io/px4-dev-base-bionic:2019-10-04' }
+            docker { image 'px4io/px4-dev-base-bionic:2019-10-24' }
           }
           steps {
             sh('export')

ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480

@@ -10,7 +10,11 @@ sh /etc/init.d/rc.mc_defaults
 if [ $AUTOCNF = yes ]
 then
 	param set MC_PITCHRATE_P 0.1
-	param set MC_ROLLRATE_P 0.05
+	param set MC_PITCHRATE_I 0.05
+	param set MC_PITCH_P 6.0
+	param set MC_ROLLRATE_P 0.15
+	param set MC_ROLLRATE_I 0.1
+	param set MC_ROLL_P 6.0
 	param set MPC_XY_VEL_I 0.2
 	param set MPC_XY_VEL_P 0.15
 

ROMFS/px4fmu_common/init.d/rc.fw_apps

@@ -15,7 +15,7 @@ ekf2 start
 #
 fw_att_control start
 fw_pos_control_l1 start
-airspeed_selector start
+# airspeed_selector start
 #
 # Start Land Detector.
 #

ROMFS/px4fmu_common/init.d/rc.io

@@ -2,9 +2,17 @@
 #
 # PX4IO interface init script.
 #
+
+# If $OUTPUT_MODE indicated Hardware-int-the-loop simulation, px4io should not publish actuator_outputs,
+# instead, pwm_out_sim will publish that uORB
+if [ $OUTPUT_MODE = hil ]
+then
+    set HIL_ARG $OUTPUT_MODE
+fi
+
 if [ $USE_IO = yes -a $IO_PRESENT = yes ]
 then
-	if px4io start
+	if px4io start $HIL_ARG
 	then
 		# Allow PX4IO to recover from midair restarts.
 		px4io recovery

ROMFS/px4fmu_common/init.d/rc.vtol_apps

@@ -21,7 +21,7 @@ mc_att_control start
 mc_pos_control start
 fw_att_control start
 fw_pos_control_l1 start
-airspeed_selector start
+# airspeed_selector start
 
 #
 # Start Land Detector

ROMFS/px4fmu_common/init.d/rcS

@@ -152,8 +152,8 @@ else
 	then
 		if param compare SYS_AUTOCONFIG 1
 		then
-			# Wipe out params except RC*, flight modes, total flight time, accel cal, gyro cal
+			# Wipe out params except RC*, flight modes, total flight time, accel cal, gyro cal, next flight UUID
-			param reset_nostart RC* COM_FLTMODE* LND_FLIGHT_T_* TC_* CAL_ACC* CAL_GYRO*
+			param reset_nostart RC* COM_FLTMODE* LND_FLIGHT_T_* TC_* CAL_ACC* CAL_GYRO* COM_FLIGHT_UUID
 		fi
 
 		set AUTOCNF yes

Tools/docker_run.sh

@@ -4,22 +4,22 @@ if [ -z ${PX4_DOCKER_REPO+x} ]; then
 	echo "guessing PX4_DOCKER_REPO based on input";
 	if [[ $@ =~ .*px4_fmu.* ]]; then
 		# nuttx-px4fmu-v{1,2,3,4,5}
-		PX4_DOCKER_REPO="px4io/px4-dev-nuttx:2019-07-29"
+		PX4_DOCKER_REPO="px4io/px4-dev-nuttx:2019-10-24"
 	elif [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*bebop.* ]]; then
 		# posix_rpi_cross, posix_bebop_default
-		PX4_DOCKER_REPO="px4io/px4-dev-raspi:2019-07-29"
+		PX4_DOCKER_REPO="px4io/px4-dev-raspi:2019-10-24"
 	elif [[ $@ =~ .*eagle.* ]] || [[ $@ =~ .*excelsior.* ]]; then
 		# eagle, excelsior
 		PX4_DOCKER_REPO="lorenzmeier/px4-dev-snapdragon:2018-09-12"
 	elif [[ $@ =~ .*ocpoc.* ]]; then
 		# aerotennaocpoc_ubuntu
-		PX4_DOCKER_REPO="px4io/px4-dev-armhf:2019-07-29"
+		PX4_DOCKER_REPO="px4io/px4-dev-armhf:2019-10-24"
 	elif [[ $@ =~ .*clang.* ]] || [[ $@ =~ .*scan-build.* ]]; then
 		# clang tools
-		PX4_DOCKER_REPO="px4io/px4-dev-clang:2019-07-29"
+		PX4_DOCKER_REPO="px4io/px4-dev-clang:2019-10-24"
 	elif [[ $@ =~ .*tests* ]]; then
 		# run all tests with simulation
-		PX4_DOCKER_REPO="px4io/px4-dev-simulation-xenial:2019-10-04"
+		PX4_DOCKER_REPO="px4io/px4-dev-simulation-bionic:2019-10-24"
 	fi
 else
 	echo "PX4_DOCKER_REPO is set to '$PX4_DOCKER_REPO'";
@@ -27,7 +27,7 @@ fi
 
 # otherwise default to nuttx
 if [ -z ${PX4_DOCKER_REPO+x} ]; then
-	PX4_DOCKER_REPO="px4io/px4-dev-nuttx:2019-07-29"
+	PX4_DOCKER_REPO="px4io/px4-dev-nuttx:2019-10-24"
 fi
 
 # docker hygiene

Tools/setup/arch.sh

@@ -125,7 +125,7 @@ if [[ $INSTALL_SIM == "true" ]]; then
 	# Gazebo setup
 	if [[ $INSTALL_GAZEBO == "true" ]]; then
 		echo
-		echo "Installing gazebo and dependencies for PX4 simulation"
+		echo "Installing gazebo and dependencies for PX4 gazebo simulation"
 
 		# PX4 gazebo simulation dependencies
 		sudo pacman -S --noconfirm --needed \
@@ -143,16 +143,6 @@ if [[ $INSTALL_SIM == "true" ]]; then
 		# install gazebo from AUR
 		yay -S gazebo --noconfirm
 
-		# fix incompatible compile flag to disable default testing that leads to build error
-		# see https://bitbucket.org/ignitionrobotics/ign-cmake/issues/62/compilation-failing-when-performing
-		pushd ~/.cache/yay/ignition-cmake/
-		sed -i 's/-DENABLE_TESTS_COMPILATION:BOOL=False/-DBUILD_TESTING=OFF/g' PKGBUILD
-		makepkg -si --noconfirm
-		popd
-
-		# continue installing gezebo
-		yay -S gazebo --noconfirm
-
 		if sudo dmidecode -t system | grep -q "Manufacturer: VMware, Inc." ; then
 			# fix VMWare 3D graphics acceleration for gazebo
 			echo "export SVGA_VGPU10=0" >> ~/.profile

Tools/sitl_gazebo

@@ -1 +1 @@
-Subproject commit c0634b241eb730cee2ef91e5f5298427a4a328c8
+Subproject commit 169d48217df89922e9fae72ef34fa46ce2e209dd

boards/modalai/fc-v1/default.cmake

@@ -24,6 +24,7 @@ px4_add_board(
 		camera_trigger
 		differential_pressure # all available differential pressure drivers
 		distance_sensor # all available distance sensor drivers
+		dshot
 		gps
 		imu/bmi088
 # TODO		imu/icm42688

boards/modalai/fc-v1/src/board_config.h

@@ -480,6 +480,8 @@
 
 #define BOARD_NUM_IO_TIMERS 5
 
+#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7};
+
 __BEGIN_DECLS
 
 /****************************************************************************************************

boards/modalai/fc-v1/src/timer_config.c

@@ -75,6 +75,13 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
 		.last_channel_index = 3,
 		.handler = io_timer_handler0,
 		.vectorno =  STM32_IRQ_TIM1CC,
+		.dshot = {
+			.dma_base = DSHOT_DMA2_BASE,
+			.channel = DShot_Channel6,
+			.stream = DShot_Stream5,
+			.start_ccr_register = TIM_DMABASE_CCR1,
+			.channels_number = 4u /* CCR1, CCR2, CCR3 and CCR4 */
+		}
 	},
 	{
 		.base = STM32_TIM4_BASE,

boards/nxp/fmuk66-v3/nuttx-config/Kconfig

@@ -3,7 +3,6 @@
 # see misc/tools/kconfig-language.txt.
 #
 
-if ARCH_BOARD_NXP_FMUK66_V3
 config FMUK66_SDHC_AUTOMOUNT
 	bool "SDHC automounter"
 	default n
@@ -21,7 +20,7 @@ config FMUK66_SDHC_AUTOMOUNT_BLKDEV
 
 config FMUK66_SDHC_AUTOMOUNT_MOUNTPOINT
 	string "SDHC mount point"
-	default "/mnt/sdcard"
+	default "/fs/microsd"
 
 config FMUK66_SDHC_AUTOMOUNT_DDELAY
 	int "SDHC debounce delay (milliseconds)"
@@ -46,5 +45,3 @@ config BOARD_USE_PROBES
 
 	---help---
 		Select to use GPIO FMU-CH1-6 to provide timing signals from selected drivers.
-
-endif

boards/nxp/fmuk66-v3/nuttx-config/nsh/defconfig

@@ -1,3 +1,10 @@
+#
+# This file is autogenerated: PLEASE DO NOT EDIT IT.
+#
+# You can use "make menuconfig" to make any modifications to the installed .config file.
+# You can then do "make savedefconfig" to generate a new defconfig file that includes your
+# modifications.
+#
 # CONFIG_DISABLE_OS_API is not set
 # CONFIG_MMCSD_HAVE_WRITEPROTECT is not set
 # CONFIG_MMCSD_SPI is not set
@@ -170,7 +177,6 @@ CONFIG_SDCLONE_DISABLE=y
 CONFIG_SEM_NNESTPRIO=8
 CONFIG_SEM_PREALLOCHOLDERS=0
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
-CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
 CONFIG_SIG_SIGALRM_ACTION=y
 CONFIG_SIG_SIGUSR1_ACTION=y

boards/nxp/fmuk66-v3/src/board_config.h

@@ -187,7 +187,7 @@ __BEGIN_DECLS
  */
 #define SD_CAED_P_EN       (GPIO_LOWDRIVE | GPIO_OUTPUT_ONE  | PIN_PORTD | PIN6)
 
-#define GPIO_SD_CARDDETECT (GPIO_PULLUP | PIN_INT_BOTH | PIN_PORTD | PIN10)
+//#define GPIO_SD_CARDDETECT (GPIO_PULLUP | PIN_INT_BOTH | PIN_PORTD | PIN10)
 
 /* SPI
  *

boards/nxp/fmuk66-v3/src/sdhc.c

@@ -99,6 +99,7 @@ static struct fmuk66_sdhc_state_s g_sdhc;
  * Private Functions
  ****************************************************************************/
 
+#if defined(GPIO_SD_CARDDETECT)
 /****************************************************************************
  * Name: fmuk66_mediachange
  ****************************************************************************/
@@ -143,6 +144,7 @@ static int fmuk66_cdinterrupt(int irq, FAR void *context, FAR void *args)
 	fmuk66_mediachange((struct fmuk66_sdhc_state_s *) args);
 	return OK;
 }
+#endif
 
 /****************************************************************************
  * Public Functions
@@ -164,12 +166,13 @@ int fmuk66_sdhc_initialize(void)
 
 	VDD_3V3_SD_CARD_EN(true);
 
+#if defined(GPIO_SD_CARDDETECT)
 	kinetis_pinconfig(GPIO_SD_CARDDETECT);
 
 	/* Attached the card detect interrupt (but don't enable it yet) */
 
 	kinetis_pinirqattach(GPIO_SD_CARDDETECT, fmuk66_cdinterrupt, sdhc);
-
+#endif
 	/* Configure the write protect GPIO -- None */
 
 	/* Mount the SDHC-based MMC/SD block driver */
@@ -210,6 +213,7 @@ int fmuk66_sdhc_initialize(void)
 
 	syslog(LOG_ERR, "Successfully bound SDHC to the MMC/SD driver\n");
 
+#if defined(GPIO_SD_CARDDETECT)
 	/* Handle the initial card state */
 
 	fmuk66_mediachange(sdhc);
@@ -217,6 +221,9 @@ int fmuk66_sdhc_initialize(void)
 	/* Enable CD interrupts to handle subsequent media changes */
 
 	kinetis_pinirqenable(GPIO_SD_CARDDETECT);
+#else
+	sdhc_mediachange(sdhc->sdhc, true);
+#endif
 	return OK;
 }
 

boards/px4/fmu-v2/init/rc.board_defaults

@@ -25,4 +25,7 @@ unset BL_FILE
 if [ $AUTOCNF = yes ]
 then
 
+	# to minimize cpu usage on older boards limit inner loop to 400 Hz
+	param set IMU_GYRO_RATEMAX 400
+
 fi

boards/px4/fmu-v2/nuttx-config/scripts/script.ld

@@ -1,5 +1,5 @@
 /****************************************************************************
- * nuttx-configs/px4_fmu-v2/scripts/ld.script
+ * scripts/ld.script
  *
  *   Copyright (C) 2011 Gregory Nutt. All rights reserved.
  *   Author: Gregory Nutt <gnutt@nuttx.org>

boards/px4/fmu-v3/init/rc.board_defaults

@@ -4,7 +4,28 @@
 #------------------------------------------------------------------------------
 
 
+#
+# Bootloader upgrade
+#
+set BL_FILE /etc/extras/px4fmuv3_bl.bin
+if [ -f $BL_FILE ]
+then
+	if param compare SYS_BL_UPDATE 1
+	then
+		param set SYS_BL_UPDATE 0
+		param save
+		echo "BL update..." >> $LOG_FILE
+		bl_update $BL_FILE
+		echo "BL update done" >> $LOG_FILE
+		reboot
+	fi
+fi
+unset BL_FILE
+
 if [ $AUTOCNF = yes ]
 then
 
+	# to minimize cpu usage on older boards limit inner loop to 400 Hz
+	param set IMU_GYRO_RATEMAX 400
+
 fi

boards/px4/fmu-v3/src/board_config.h

@@ -496,7 +496,6 @@ __BEGIN_DECLS
 
 extern void stm32_spiinitialize(void);
 
-
 /****************************************************************************************************
  * Name: board_spi_reset board_peripheral_reset
  *

mavlink/include/mavlink/v2.0

@@ -1 +1 @@
-Subproject commit c549ee329842e6182bc0dd72369a83fc0deb8f6e
+Subproject commit 086f176b677b719c1f5b4267a10553ffc5c3a974

msg/estimator_status.msg

@@ -106,8 +106,10 @@ uint16 solution_status_flags # Bitmask indicating which filter kinematic state o
 
 float32 time_slip # cumulative amount of time in seconds that the EKF inertial calculation has slipped relative to system time
 
-bool pre_flt_fail # true when estimator has failed pre-flight checks and the vehicle should not be flown regardless of flight mode
+bool pre_flt_fail_innov_heading
-
+bool pre_flt_fail_innov_vel_horiz
+bool pre_flt_fail_innov_vel_vert
+bool pre_flt_fail_innov_height
 
 # legacy local position estimator (LPE) flags
 uint8 health_flags		# Bitmask to indicate sensor health states (vel, pos, hgt)

platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp

@@ -64,7 +64,7 @@ static constexpr wq_config_t I2C4{"wq:I2C4", 1250, -10};
 
 static constexpr wq_config_t att_pos_ctrl{"wq:att_pos_ctrl", 6600, -11}; // PX4 att/pos controllers, highest priority after sensors
 
-static constexpr wq_config_t hp_default{"wq:hp_default", 1500, -12};
+static constexpr wq_config_t hp_default{"wq:hp_default", 1600, -12};
 static constexpr wq_config_t lp_default{"wq:lp_default", 1700, -50};
 
 static constexpr wq_config_t test1{"wq:test1", 800, 0};

platforms/common/px4_work_queue/WorkQueueManager.cpp

@@ -70,7 +70,7 @@ FindWorkQueueByName(const char *name)
 		return nullptr;
 	}
 
-	auto lg = _wq_manager_wqs_list->getLockGuard();
+	LockGuard lg{_wq_manager_wqs_list->mutex()};
 
 	// search list
 	for (WorkQueue *wq : *_wq_manager_wqs_list) {
@@ -298,7 +298,7 @@ WorkQueueManagerStop()
 		// first ask all WQs to stop
 		if (_wq_manager_wqs_list != nullptr) {
 			{
-				auto lg = _wq_manager_wqs_list->getLockGuard();
+				LockGuard lg{_wq_manager_wqs_list->mutex()};
 
 				// ask all work queues (threads) to stop
 				// NOTE: not currently safe without all WorkItems stopping first
@@ -342,7 +342,7 @@ WorkQueueManagerStatus()
 		const size_t num_wqs = _wq_manager_wqs_list->size();
 		PX4_INFO_RAW("\nWork Queue: %-1zu threads                      RATE        INTERVAL\n", num_wqs);
 
-		auto lg = _wq_manager_wqs_list->getLockGuard();
+		LockGuard lg{_wq_manager_wqs_list->mutex()};
 		size_t i = 0;
 
 		for (WorkQueue *wq : *_wq_manager_wqs_list) {

platforms/nuttx/NuttX/nuttx

@@ -1 +1 @@
-Subproject commit d8da511082646d83a54c6905daca13f0a1a609f0
+Subproject commit 427238133be2b0ecd068a11e886ee8fdbc31f6dc

posix-configs/SITL/init/test/test_shutdown

@@ -29,7 +29,7 @@ mc_pos_control start
 mc_att_control start
 fw_pos_control_l1 start
 fw_att_control start
-airspeed_selector start
+# airspeed_selector start
 
 #mixer load /dev/pwm_output0 ROMFS/sitl/mixers/standard_vtol_sitl.main.mix
 
@@ -56,7 +56,7 @@ mc_pos_control status
 mc_att_control status
 fw_pos_control_l1 status
 fw_att_control status
-airspeed_selector status
+# airspeed_selector status
 dataman status
 uorb status
 
@@ -72,7 +72,7 @@ navigator stop
 commander stop
 land_detector stop
 ekf2 stop
-airspeed_selector stop
+# airspeed_selector stop
 sensors stop
 
 sleep 2

src/drivers/batt_smbus/batt_smbus.cpp

@@ -174,7 +174,7 @@ void BATT_SMBUS::Run()
 	// Temporary variable for storing SMBUS reads.
 	uint16_t result;
 
-	int ret = _interface->read_word(BATT_SMBUS_VOLTAGE, &result);
+	int ret = _interface->read_word(BATT_SMBUS_VOLTAGE, result);
 
 	ret |= get_cell_voltages();
 
@@ -183,13 +183,13 @@ void BATT_SMBUS::Run()
 	new_report.voltage_filtered_v = new_report.voltage_v;
 
 	// Read current.
-	ret |= _interface->read_word(BATT_SMBUS_CURRENT, &result);
+	ret |= _interface->read_word(BATT_SMBUS_CURRENT, result);
 
 	new_report.current_a = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f);
 	new_report.current_filtered_a = new_report.current_a;
 
 	// Read average current.
-	ret |= _interface->read_word(BATT_SMBUS_AVERAGE_CURRENT, &result);
+	ret |= _interface->read_word(BATT_SMBUS_AVERAGE_CURRENT, result);
 
 	float average_current = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f);
 
@@ -200,15 +200,15 @@ void BATT_SMBUS::Run()
 	set_undervoltage_protection(average_current);
 
 	// Read run time to empty.
-	ret |= _interface->read_word(BATT_SMBUS_RUN_TIME_TO_EMPTY, &result);
+	ret |= _interface->read_word(BATT_SMBUS_RUN_TIME_TO_EMPTY, result);
 	new_report.run_time_to_empty = result;
 
 	// Read average time to empty.
-	ret |= _interface->read_word(BATT_SMBUS_AVERAGE_TIME_TO_EMPTY, &result);
+	ret |= _interface->read_word(BATT_SMBUS_AVERAGE_TIME_TO_EMPTY, result);
 	new_report.average_time_to_empty = result;
 
 	// Read remaining capacity.
-	ret |= _interface->read_word(BATT_SMBUS_REMAINING_CAPACITY, &result);
+	ret |= _interface->read_word(BATT_SMBUS_REMAINING_CAPACITY, result);
 
 	// Calculate remaining capacity percent with complementary filter.
 	new_report.remaining = 0.8f * _last_report.remaining + 0.2f * (1.0f - (float)((float)(_batt_capacity - result) /
@@ -239,7 +239,7 @@ void BATT_SMBUS::Run()
 	}
 
 	// Read battery temperature and covert to Celsius.
-	ret |= _interface->read_word(BATT_SMBUS_TEMP, &result);
+	ret |= _interface->read_word(BATT_SMBUS_TEMP, result);
 	new_report.temperature = ((float)result / 10.0f) + CONSTANTS_ABSOLUTE_NULL_CELSIUS;
 
 	new_report.capacity = _batt_capacity;
@@ -287,19 +287,19 @@ int BATT_SMBUS::get_cell_voltages()
 	// Temporary variable for storing SMBUS reads.
 	uint16_t result = 0;
 
-	int ret = _interface->read_word(BATT_SMBUS_CELL_1_VOLTAGE, &result);
+	int ret = _interface->read_word(BATT_SMBUS_CELL_1_VOLTAGE, result);
 	// Convert millivolts to volts.
 	_cell_voltages[0] = ((float)result) / 1000.0f;
 
-	ret = _interface->read_word(BATT_SMBUS_CELL_2_VOLTAGE, &result);
+	ret = _interface->read_word(BATT_SMBUS_CELL_2_VOLTAGE, result);
 	// Convert millivolts to volts.
 	_cell_voltages[1] = ((float)result) / 1000.0f;
 
-	ret = _interface->read_word(BATT_SMBUS_CELL_3_VOLTAGE, &result);
+	ret = _interface->read_word(BATT_SMBUS_CELL_3_VOLTAGE, result);
 	// Convert millivolts to volts.
 	_cell_voltages[2] = ((float)result) / 1000.0f;
 
-	ret = _interface->read_word(BATT_SMBUS_CELL_4_VOLTAGE, &result);
+	ret = _interface->read_word(BATT_SMBUS_CELL_4_VOLTAGE, result);
 	// Convert millivolts to volts.
 	_cell_voltages[3] = ((float)result) / 1000.0f;
 
@@ -414,20 +414,17 @@ int BATT_SMBUS::get_startup_info()
 		_manufacturer_name = new char[sizeof(man_name)];
 	}
 
-	// Temporary variable for storing SMBUS reads.
+	uint16_t serial_num;
-	uint16_t tmp = 0;
+	result = _interface->read_word(BATT_SMBUS_SERIAL_NUMBER, serial_num);
 
-	result = _interface->read_word(BATT_SMBUS_SERIAL_NUMBER, &tmp);
+	uint16_t remaining_cap;
-	uint16_t serial_num = tmp;
+	result |= _interface->read_word(BATT_SMBUS_REMAINING_CAPACITY, remaining_cap);
 
-	result |= _interface->read_word(BATT_SMBUS_REMAINING_CAPACITY, &tmp);
+	uint16_t cycle_count;
-	uint16_t remaining_cap = tmp;
+	result |= _interface->read_word(BATT_SMBUS_CYCLE_COUNT, cycle_count);
 
-	result |= _interface->read_word(BATT_SMBUS_CYCLE_COUNT, &tmp);
+	uint16_t full_cap;
-	uint16_t cycle_count = tmp;
+	result |= _interface->read_word(BATT_SMBUS_FULL_CHARGE_CAPACITY, full_cap);
-
-	result |= _interface->read_word(BATT_SMBUS_FULL_CHARGE_CAPACITY, &tmp);
-	uint16_t full_cap = tmp;
 
 	if (!result) {
 		_serial_number = serial_num;
@@ -463,7 +460,7 @@ uint16_t BATT_SMBUS::get_serial_number()
 {
 	uint16_t serial_num = 0;
 
-	if (_interface->read_word(BATT_SMBUS_SERIAL_NUMBER, &serial_num) == PX4_OK) {
+	if (_interface->read_word(BATT_SMBUS_SERIAL_NUMBER, serial_num) == PX4_OK) {
 		return serial_num;
 	}
 
@@ -472,10 +469,8 @@ uint16_t BATT_SMBUS::get_serial_number()
 
 int BATT_SMBUS::manufacture_date()
 {
-	uint16_t date = PX4_ERROR;
+	uint16_t date;
-	uint8_t code = BATT_SMBUS_MANUFACTURE_DATE;
+	int result = _interface->read_word(BATT_SMBUS_MANUFACTURE_DATE, date);
-
-	int result = _interface->read_word(code, &date);
 
 	if (result != PX4_OK) {
 		return result;
@@ -550,9 +545,9 @@ int BATT_SMBUS::unseal()
 	// See bq40z50 technical reference.
 	uint16_t keys[2] = {0x0414, 0x3672};
 
-	int ret = _interface->write_word(BATT_SMBUS_MANUFACTURER_ACCESS, &keys[0]);
+	int ret = _interface->write_word(BATT_SMBUS_MANUFACTURER_ACCESS, keys[0]);
 
-	ret |= _interface->write_word(BATT_SMBUS_MANUFACTURER_ACCESS, &keys[1]);
+	ret |= _interface->write_word(BATT_SMBUS_MANUFACTURER_ACCESS, keys[1]);
 
 	return ret;
 }

src/drivers/distance_sensor/cm8jl65/CM8JL65.hpp

@@ -43,13 +43,14 @@
 
 #pragma once
 
+#include <termios.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/rangefinder/PX4Rangefinder.hpp>
+#include <perf/perf_counter.h>
 #include <px4_config.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-#include <drivers/drv_hrt.h>
-#include <lib/perf/perf_counter.h>
-#include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
 
-#include <termios.h>
 
 using namespace time_literals;
 

src/drivers/distance_sensor/cm8jl65/CMakeLists.txt

@@ -39,4 +39,6 @@ px4_add_module(
 		cm8jl65_main.cpp
 	MODULE_CONFIG
 		module.yaml
+	DEPENDS
+		drivers_rangefinder
 	)

src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp

@@ -150,7 +150,7 @@ LidarLiteI2C::probe()
 			}
 
 			_retries = 3;
-			return reset_sensor();
+			return OK;
 		}
 
 		PX4_DEBUG("probe failed unit_id=0x%02x hw_version=0x%02x sw_version=0x%02x",

src/drivers/drv_orb_dev.h

@@ -73,7 +73,7 @@
 /** Get the minimum interval at which the topic can be seen to be updated for this subscription */
 #define ORBIOCGETINTERVAL	_ORBIOC(16)
 
-/** Check whether the topic is published, sets *(unsigned long *)arg to 1 if published, 0 otherwise */
+/** Check whether the topic is advertised, sets *(unsigned long *)arg to 1 if advertised, 0 otherwise */
-#define ORBIOCISPUBLISHED	_ORBIOC(17)
+#define ORBIOCISADVERTISED	_ORBIOC(17)
 
 #endif /* _DRV_UORB_H */

src/drivers/dshot/dshot.cpp

@@ -220,7 +220,6 @@ private:
 	bool		_outputs_initialized{false};
 
 	perf_counter_t	_cycle_perf;
-	perf_counter_t	_cycle_interval_perf;
 
 	void		capture_callback(uint32_t chan_index,
 					 hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow);
@@ -247,8 +246,7 @@ px4::atomic_bool DShotOutput::_request_telemetry_init{false};
 DShotOutput::DShotOutput() :
 	CDev("/dev/dshot"),
 	OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default),
-	_cycle_perf(perf_alloc(PC_ELAPSED, "dshot: cycle")),
+	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
-	_cycle_interval_perf(perf_alloc(PC_INTERVAL, "dshot: cycle interval"))
 {
 	_mixing_output.setAllDisarmedValues(DISARMED_VALUE);
 	_mixing_output.setAllMinValues(DISARMED_VALUE + 1);
@@ -265,7 +263,6 @@ DShotOutput::~DShotOutput()
 	unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance);
 
 	perf_free(_cycle_perf);
-	perf_free(_cycle_interval_perf);
 	delete _telemetry;
 }
 
@@ -756,7 +753,6 @@ DShotOutput::Run()
 	}
 
 	perf_begin(_cycle_perf);
-	perf_count(_cycle_interval_perf);
 
 	_mixing_output.update();
 
@@ -1667,7 +1663,6 @@ int DShotOutput::print_status()
 	PX4_INFO("Outputs initialized: %s", _outputs_initialized ? "yes" : "no");
 	PX4_INFO("Outputs on: %s", _outputs_on ? "yes" : "no");
 	perf_print_counter(_cycle_perf);
-	perf_print_counter(_cycle_interval_perf);
 	_mixing_output.printStatus();
 	if (_telemetry) {
 		PX4_INFO("telemetry on: %s", _telemetry_device);

src/drivers/heater/heater.cpp

@@ -188,7 +188,7 @@ void Heater::initialize_topics()
 	for (uint8_t x = 0; x < number_of_imus; x++) {
 		_sensor_accel_sub = uORB::Subscription{ORB_ID(sensor_accel), x};
 
-		if (!_sensor_accel_sub.published()) {
+		if (!_sensor_accel_sub.advertised()) {
 			continue;
 		}
 

src/drivers/magnetometer/ak09916/ak09916.cpp

@@ -32,12 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file mag.cpp
+ * Driver for the standalone AK09916 magnetometer.
- *
- * Driver for the ak09916 magnetometer within the Invensense mpu9250
- *
- * @author Robert Dickenson
- *
  */
 
 #include <px4_config.h>
@@ -51,10 +46,8 @@
 #include "ak09916.hpp"
 
 
-/** driver 'main' command */
 extern "C" { __EXPORT int ak09916_main(int argc, char *argv[]); }
 
-#define AK09916_CONVERSION_INTERVAL	(1000000 / 100)	/* microseconds */
 
 namespace ak09916
 {
@@ -66,25 +59,16 @@ void start(bool, enum Rotation);
 void info(bool);
 void usage();
 
-/**
- * Start the driver.
- *
- * This function only returns if the driver is up and running
- * or failed to detect the sensor.
- */
 void start(bool external_bus, enum Rotation rotation)
 {
 	AK09916 **g_dev_ptr = (external_bus ? &g_dev_ext : &g_dev_int);
 	const char *path = (external_bus ? AK09916_DEVICE_PATH_MAG_EXT : AK09916_DEVICE_PATH_MAG);
 
-	if (*g_dev_ptr != nullptr)
+	if (*g_dev_ptr != nullptr) {
-		/* if already started, the still command succeeded */
-	{
 		PX4_ERR("already started");
 		exit(0);
 	}
 
-	/* create the driver */
 	if (external_bus) {
 #if defined(PX4_I2C_BUS_EXPANSION)
 		*g_dev_ptr = new AK09916(PX4_I2C_BUS_EXPANSION, path, rotation);
@@ -98,25 +82,14 @@ void start(bool external_bus, enum Rotation rotation)
 		exit(0);
 	}
 
-	if (*g_dev_ptr == nullptr) {
+	if (*g_dev_ptr == nullptr || (OK != (*g_dev_ptr)->init())) {
-		goto fail;
+		PX4_ERR("driver start failed");
-	}
+		delete (*g_dev_ptr);
-
+		*g_dev_ptr = nullptr;
-
+		exit(1);
-	if (OK != (*g_dev_ptr)->init()) {
-		goto fail;
 	}
 
 	exit(0);
-fail:
-
-	if (*g_dev_ptr != nullptr) {
-		delete (*g_dev_ptr);
-		*g_dev_ptr = nullptr;
-	}
-
-	PX4_ERR("driver start failed");
-	exit(1);
 }
 
 void
@@ -153,16 +126,14 @@ info(bool external_bus)
 void
 usage()
 {
-	PX4_WARN("missing command: try 'start', 'info', stop'");
+	PX4_INFO("missing command: try 'start', 'info', stop'");
-	PX4_WARN("options:");
+	PX4_INFO("options:");
-	PX4_WARN("    -X    (external bus)");
+	PX4_INFO("    -X    (external bus)");
-
+	PX4_INFO("    -R    (rotation)");
 }
 
-} // namespace AK09916
+} // namespace ak09916
 
-// If interface is non-null, then it will used for interacting with the device.
-// Otherwise, it will passthrough the parent AK09916
 AK09916::AK09916(int bus, const char *path, enum Rotation rotation) :
 	I2C("AK09916", path, bus, AK09916_I2C_ADDR, 400000),
 	ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
@@ -170,8 +141,7 @@ AK09916::AK09916(int bus, const char *path, enum Rotation rotation) :
 	_mag_reads(perf_alloc(PC_COUNT, "ak09916_mag_reads")),
 	_mag_errors(perf_alloc(PC_COUNT, "ak09916_mag_errors")),
 	_mag_overruns(perf_alloc(PC_COUNT, "ak09916_mag_overruns")),
-	_mag_overflows(perf_alloc(PC_COUNT, "ak09916_mag_overflows")),
+	_mag_overflows(perf_alloc(PC_COUNT, "ak09916_mag_overflows"))
-	_mag_duplicates(perf_alloc(PC_COUNT, "ak09916_mag_duplicates"))
 {
 	_px4_mag.set_device_type(DRV_MAG_DEVTYPE_AK09916);
 	_px4_mag.set_scale(AK09916_MAG_RANGE_GA);
@@ -183,7 +153,6 @@ AK09916::~AK09916()
 	perf_free(_mag_errors);
 	perf_free(_mag_overruns);
 	perf_free(_mag_overflows);
-	perf_free(_mag_duplicates);
 }
 
 int
@@ -191,17 +160,15 @@ AK09916::init()
 {
 	int ret = I2C::init();
 
-	/* if cdev init failed, bail now */
 	if (ret != OK) {
-		DEVICE_DEBUG("AK09916 mag init failed");
+		PX4_WARN("AK09916 mag init failed");
-
 		return ret;
 	}
 
 	ret = reset();
 
 	if (ret != PX4_OK) {
-		return PX4_ERROR;
+		return ret;
 	}
 
 	start();
@@ -209,52 +176,56 @@ AK09916::init()
 	return PX4_OK;
 }
 
-bool AK09916::check_duplicate(uint8_t *mag_data)
+void
+AK09916::try_measure()
 {
-	if (memcmp(mag_data, &_last_mag_data, sizeof(_last_mag_data)) == 0) {
+	if (!is_ready()) {
-		// it isn't new data - wait for next timer
+		return;
-		return true;
+	}
 
-	} else {
+	measure();
-		memcpy(&_last_mag_data, mag_data, sizeof(_last_mag_data));
+}
+
+bool
+AK09916::is_ready()
+{
+	uint8_t st1;
+	const int ret = transfer(&AK09916REG_ST1, sizeof(AK09916REG_ST1), &st1, sizeof(st1));
+
+	if (ret != OK) {
 		return false;
 	}
+
+	// Monitor if data overrun flag is ever set.
+	if (st1 & AK09916_ST1_DOR) {
+		perf_count(_mag_overruns);
+	}
+
+	return (st1 & AK09916_ST1_DRDY);
 }
 
 void
 AK09916::measure()
 {
-	uint8_t cmd = AK09916REG_ST1;
+	ak09916_regs regs;
-	struct ak09916_regs raw_data;
 
-	const hrt_abstime timestamp_sample = hrt_absolute_time();
+	const hrt_abstime now = hrt_absolute_time();
-	uint8_t ret = transfer(&cmd, 1, (uint8_t *)(&raw_data), sizeof(struct ak09916_regs));
 
-	if (ret == OK) {
+	const int ret = transfer(&AK09916REG_HXL, sizeof(AK09916REG_HXL),
-		raw_data.st2 = raw_data.st2;
+				 reinterpret_cast<uint8_t *>(&regs), sizeof(regs));
-
-		if (check_duplicate((uint8_t *)&raw_data.x) && !(raw_data.st1 & 0x02)) {
-			perf_count(_mag_duplicates);
-			return;
-		}
-
-		/* monitor for if data overrun flag is ever set */
-		if (raw_data.st1 & 0x02) {
-			perf_count(_mag_overruns);
-		}
-
-		/* monitor for if magnetic sensor overflow flag is ever set noting that st2
-		* is usually not even refreshed, but will always be in the same place in the
-		* mpu's buffers regardless, hence the actual count would be bogus
-		*/
-		if (raw_data.st2 & 0x08) {
-			perf_count(_mag_overflows);
-		}
 
+	if (ret != OK) {
 		_px4_mag.set_error_count(perf_event_count(_mag_errors));
-		_px4_mag.set_external(external());
+		return;
-		_px4_mag.update(timestamp_sample, raw_data.x, raw_data.y, raw_data.z);
 	}
+
+	// Monitor if magnetic sensor overflow flag is set.
+	if (regs.st2 & AK09916_ST2_HOFL) {
+		perf_count(_mag_overflows);
+	}
+
+	_px4_mag.set_external(external());
+	_px4_mag.update(now, regs.x, regs.y, regs.z);
 }
 
 void
@@ -278,9 +249,9 @@ AK09916::read_reg(uint8_t reg)
 }
 
 bool
-AK09916::check_id(uint8_t &deviceid)
+AK09916::check_id()
 {
-	deviceid = read_reg(AK09916REG_WIA);
+	const uint8_t deviceid = read_reg(AK09916REG_WIA);
 
 	return (AK09916_DEVICE_ID_A == deviceid);
 }
@@ -298,10 +269,10 @@ AK09916::reset()
 	int rv = probe();
 
 	if (rv == OK) {
-		// Now reset the mag
+		// Now reset the mag.
 		write_reg(AK09916REG_CNTL3, AK09916_RESET);
 
-		// Then re-initialize the bus/mag
+		// Then re-initialize the bus/mag.
 		rv = setup();
 	}
 
@@ -316,9 +287,7 @@ AK09916::probe()
 	do {
 		write_reg(AK09916REG_CNTL3, AK09916_RESET);
 
-		uint8_t id = 0;
+		if (check_id()) {
-
-		if (check_id(id)) {
 			return OK;
 		}
 
@@ -339,17 +308,16 @@ AK09916::setup()
 void
 AK09916::start()
 {
-	_measure_interval = AK09916_CONVERSION_INTERVAL;
+	_cycle_interval = AK09916_CONVERSION_INTERVAL_us;
 
-	/* schedule a cycle to start things */
 	ScheduleNow();
 }
 
 void
 AK09916::stop()
 {
-	/* ensure no new items are queued while we cancel this one */
+	// Ensure no new items are queued while we cancel this one.
-	_measure_interval = 0;
+	_cycle_interval = 0;
 
 	ScheduleClear();
 }
@@ -357,16 +325,14 @@ AK09916::stop()
 void
 AK09916::Run()
 {
-	if (_measure_interval == 0) {
+	if (_cycle_interval == 0) {
 		return;
 	}
 
-	/* measurement phase */
+	try_measure();
-	measure();
 
-	if (_measure_interval > 0) {
+	if (_cycle_interval > 0) {
-		/* schedule a fresh cycle call when the measurement is done */
+		ScheduleDelayed(_cycle_interval);
-		ScheduleDelayed(_measure_interval);
 	}
 }
 
@@ -402,23 +368,14 @@ ak09916_main(int argc, char *argv[])
 
 	const char *verb = argv[myoptind];
 
-	/*
-	 * Start/load the driver.
-	 */
 	if (!strcmp(verb, "start")) {
 		ak09916::start(external_bus, rotation);
 	}
 
-	/*
-	 * Stop the driver.
-	 */
 	if (!strcmp(verb, "stop")) {
 		ak09916::stop(external_bus);
 	}
 
-	/*
-	 * Print driver information.
-	 */
 	if (!strcmp(verb, "info")) {
 		ak09916::info(external_bus);
 	}

src/drivers/magnetometer/ak09916/ak09916.hpp

@@ -42,31 +42,35 @@
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
 
-#define AK09916_DEVICE_PATH_MAG              "/dev/ak09916_i2c_int"
+static constexpr auto AK09916_DEVICE_PATH_MAG = "/dev/ak09916_i2c_int";
-#define AK09916_DEVICE_PATH_MAG_EXT          "/dev/ak09916_i2c_ext"
+static constexpr auto AK09916_DEVICE_PATH_MAG_EXT = "/dev/ak09916_i2c_ext";
 
-/* in 16-bit sampling mode the mag resolution is 1.5 milli Gauss per bit */
+// in 16-bit sampling mode the mag resolution is 1.5 milli Gauss per bit.
-static constexpr float AK09916_MAG_RANGE_GA{1.5e-3f};
+static constexpr float AK09916_MAG_RANGE_GA = 1.5e-3f;
 
-/* ak09916 deviating register addresses and bit definitions */
+static constexpr uint8_t AK09916_I2C_ADDR = 0x0C;
-#define AK09916_I2C_ADDR         0x0C
 
-#define AK09916_DEVICE_ID_A		0x48
+static constexpr uint8_t AK09916_DEVICE_ID_A = 0x48;
-#define AK09916_DEVICE_ID_B		0x09	// additional ID byte ("INFO" on AK9063 without content specification.)
+static constexpr uint8_t AK09916REG_WIA = 0x00;
 
-#define AK09916REG_WIA           0x00
+static constexpr uint8_t AK09916REG_ST1 = 0x10;
+static constexpr uint8_t AK09916REG_HXL = 0x11;
+static constexpr uint8_t AK09916REG_CNTL2 = 0x31;
+static constexpr uint8_t AK09916REG_CNTL3 = 0x32;
 
-#define AK09916REG_ST1        0x10
+static constexpr uint8_t AK09916_RESET = 0x01;
-#define AK09916REG_CNTL2          0x31
+static constexpr uint8_t AK09916_CNTL2_CONTINOUS_MODE_100HZ = 0x08;
-#define AK09916REG_CNTL3          0x32
 
-#define AK09916_RESET				0x01
+static constexpr uint8_t AK09916_ST1_DRDY = 0x01;
-#define AK09916_CNTL2_SINGLE_MODE               0x01 /* default */
+static constexpr uint8_t AK09916_ST1_DOR = 0x02;
-#define AK09916_CNTL2_CONTINOUS_MODE_100HZ      0x08
+
+static constexpr uint8_t AK09916_ST2_HOFL = 0x08;
+
+// Run at 100 Hz.
+static constexpr unsigned AK09916_CONVERSION_INTERVAL_us = 1000000 / 100;
 
 #pragma pack(push, 1)
 struct ak09916_regs {
-	uint8_t st1;
 	int16_t x;
 	int16_t y;
 	int16_t z;
@@ -75,57 +79,44 @@ struct ak09916_regs {
 };
 #pragma pack(pop)
 
-/**
+
- * Helper class implementing the magnetometer driver node.
- */
 class AK09916 : public device::I2C, px4::ScheduledWorkItem
 {
 public:
 	AK09916(int bus, const char *path, enum Rotation rotation);
 	~AK09916();
 
-	virtual int init();
+	virtual int init() override;
-
+	void start();
-	void read_block(uint8_t reg, uint8_t *val, uint8_t count);
+	void stop();
-
+	void print_info();
-	int reset(void);
+	int probe();
-	int probe(void);
-	int setup(void);
-	void print_info(void);
-	int setup_master_i2c(void);
-	bool check_id(uint8_t &id);
-
-	void Run();
-
-	void start(void);
-	void stop(void);
 
 protected:
-
+	int setup();
-	/* Directly measure from the _interface if possible */
+	int setup_master_i2c();
+	bool check_id();
+	void Run();
+	void try_measure();
+	bool is_ready();
 	void measure();
+	int reset();
 
 	uint8_t read_reg(uint8_t reg);
+	void read_block(uint8_t reg, uint8_t *val, uint8_t count);
 	void write_reg(uint8_t reg, uint8_t value);
 
 private:
 
 	PX4Magnetometer _px4_mag;
 
-	uint32_t _measure_interval{0};
+	uint32_t _cycle_interval{0};
 
 	perf_counter_t _mag_reads;
 	perf_counter_t _mag_errors;
 	perf_counter_t _mag_overruns;
 	perf_counter_t _mag_overflows;
-	perf_counter_t _mag_duplicates;
 
-	bool check_duplicate(uint8_t *mag_data);
+	AK09916(const AK09916 &) = delete;
-
+	AK09916 operator=(const AK09916 &) = delete;
-	// keep last mag reading for duplicate detection
-	uint8_t			_last_mag_data[6] {};
-
-	/* do not allow to copy this class due to pointer data members */
-	AK09916(const AK09916 &);
-	AK09916 operator=(const AK09916 &);
 };

src/drivers/px4fmu/fmu.cpp

@@ -187,7 +187,6 @@ private:
 	unsigned	_num_disarmed_set{0};
 
 	perf_counter_t	_cycle_perf;
-	perf_counter_t	_cycle_interval_perf;
 
 	void		capture_callback(uint32_t chan_index,
 					 hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow);
@@ -212,8 +211,7 @@ private:
 PX4FMU::PX4FMU() :
 	CDev(PX4FMU_DEVICE_PATH),
 	OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default),
-	_cycle_perf(perf_alloc(PC_ELAPSED, "px4fmu: cycle")),
+	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
-	_cycle_interval_perf(perf_alloc(PC_INTERVAL, "px4fmu: cycle interval"))
 {
 	_mixing_output.setAllMinValues(PWM_DEFAULT_MIN);
 	_mixing_output.setAllMaxValues(PWM_DEFAULT_MAX);
@@ -229,7 +227,6 @@ PX4FMU::~PX4FMU()
 	unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance);
 
 	perf_free(_cycle_perf);
-	perf_free(_cycle_interval_perf);
 }
 
 int
@@ -755,7 +752,6 @@ PX4FMU::Run()
 	}
 
 	perf_begin(_cycle_perf);
-	perf_count(_cycle_interval_perf);
 
 	_mixing_output.update();
 
@@ -2305,7 +2301,6 @@ int PX4FMU::print_status()
 	}
 
 	perf_print_counter(_cycle_perf);
-	perf_print_counter(_cycle_interval_perf);
 	_mixing_output.printStatus();
 
 	return 0;

src/drivers/px4io/px4io.cpp

@@ -40,6 +40,7 @@
 
 #include <px4_config.h>
 #include <px4_tasks.h>
+#include <px4_sem.hpp>
 
 #include <sys/types.h>
 #include <stdint.h>
@@ -147,8 +148,9 @@ public:
 	 * Retrieve relevant initial system parameters. Initialize PX4IO registers.
 	 *
 	 * @param disable_rc_handling set to true to forbid override / RC handling on IO
+	 * @param hitl_mode set to suppress publication of actuator_outputs - instead defer to pwm_out_sim
 	 */
-	int			init(bool disable_rc_handling);
+	int			init(bool disable_rc_handling, bool hitl_mode);
 
 	/**
 	 * Detect if a PX4IO is connected.
@@ -285,7 +287,8 @@ private:
 	bool			_analog_rc_rssi_stable; ///< true when analog RSSI input is stable
 	float			_analog_rc_rssi_volt; ///< analog RSSI voltage
 
-	bool			_test_fmu_fail; ///< To test what happens if IO looses FMU
+	bool			_test_fmu_fail; ///< To test what happens if IO loses FMU
+	bool                    _hitl_mode;     ///< Hardware-in-the-loop simulation mode - don't publish actuator_outputs
 
 	/**
 	 * Trampoline to the worker task
@@ -488,7 +491,8 @@ PX4IO::PX4IO(device::Device *interface) :
 	_thermal_control(-1),
 	_analog_rc_rssi_stable(false),
 	_analog_rc_rssi_volt(-1.0f),
-	_test_fmu_fail(false)
+	_test_fmu_fail(false),
+	_hitl_mode(false)
 {
 	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
@@ -558,9 +562,10 @@ PX4IO::detect()
 }
 
 int
-PX4IO::init(bool rc_handling_disabled)
+PX4IO::init(bool rc_handling_disabled, bool hitl_mode)
 {
 	_rc_handling_disabled = rc_handling_disabled;
+	_hitl_mode = hitl_mode;
 	return init();
 }
 
@@ -1836,6 +1841,10 @@ PX4IO::io_publish_raw_rc()
 int
 PX4IO::io_publish_pwm_outputs()
 {
+	if (_hitl_mode) {
+		return OK;
+	}
+
 	/* get servo values from IO */
 	uint16_t ctl[_max_actuators];
 	int ret = io_reg_get(PX4IO_PAGE_SERVOS, 0, ctl, _max_actuators);
@@ -2327,12 +2336,17 @@ PX4IO::print_status(bool extended_status)
 		}
 	}
 
+	if (_hitl_mode) {
+		printf("\nHITL Mode");
+	}
+
 	printf("\n");
 }
 
 int
 PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 {
+	SmartLock lock_guard(_lock);
 	int ret = OK;
 
 	/* regular ioctl? */
@@ -2889,19 +2903,22 @@ start(int argc, char *argv[])
 	}
 
 	bool rc_handling_disabled = false;
+	bool hitl_mode = false;
 
-	/* disable RC handling on request */
+	/* disable RC handling and/or actuator_output publication on request */
-	if (argc > 1) {
+	for (int extra_args = 1; extra_args < argc; extra_args++) {
-		if (!strcmp(argv[1], "norc")) {
+		if (!strcmp(argv[extra_args], "norc")) {
-
 			rc_handling_disabled = true;
 
+		} else if (!strcmp(argv[extra_args], "hil")) {
+			hitl_mode = true;
+
 		} else {
 			warnx("unknown argument: %s", argv[1]);
 		}
 	}
 
-	if (OK != g_dev->init(rc_handling_disabled)) {
+	if (OK != g_dev->init(rc_handling_disabled, hitl_mode)) {
 		delete g_dev;
 		g_dev = nullptr;
 		errx(1, "driver init failed");

src/drivers/rc_input/RCInput.cpp

@@ -46,7 +46,7 @@ constexpr char const *RCInput::RC_SCAN_STRING[];
 
 RCInput::RCInput(bool run_as_task, char *device) :
 	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")),
-	_publish_interval_perf(perf_alloc(PC_INTERVAL,  MODULE_NAME": publish interval"))
+	_publish_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval"))
 {
 	// rc input, published to ORB
 	_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;

src/drivers/uavcan/sensors/flow.cpp

@@ -68,6 +68,7 @@ UavcanFlowBridge::flow_sub_cb(const uavcan::ReceivedDataStructure<com::hex::equi
 	optical_flow_s flow{};
 
 	flow.timestamp = hrt_absolute_time();
+	flow.integration_timespan = 1.e6f * msg.integration_interval; // s -> micros
 	flow.pixel_flow_x_integral = msg.flow_integral[0];
 	flow.pixel_flow_y_integral = msg.flow_integral[1];
 

src/include/containers/BlockingList.hpp

@@ -80,7 +80,7 @@ public:
 		List<T>::clear();
 	}
 
-	LockGuard getLockGuard() { return LockGuard{_mutex}; }
+	pthread_mutex_t &mutex() { return _mutex; }
 
 private:
 

src/include/containers/LockGuard.hpp

@@ -44,6 +44,9 @@ public:
 		pthread_mutex_lock(&_mutex);
 	}
 
+	LockGuard(const LockGuard &other) = delete;
+	LockGuard &operator=(const LockGuard &other) = delete;
+
 	~LockGuard()
 	{
 		pthread_mutex_unlock(&_mutex);

src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp

@@ -168,7 +168,7 @@ bool FlightTaskAutoLineSmoothVel::_generateHeadingAlongTraj()
  * Example: 	- if the constrain is -5, the value will be constrained between -5 and 0
  * 		- if the constrain is 5, the value will be constrained between 0 and 5
  */
-inline float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float constraint)
+float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float constraint)
 {
 	const float min = (constraint < FLT_EPSILON) ? constraint : 0.f;
 	const float max = (constraint > FLT_EPSILON) ? constraint : 0.f;
@@ -176,12 +176,12 @@ inline float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float con
 	return math::constrain(val, min, max);
 }
 
-float FlightTaskAutoLineSmoothVel::_constrainAbs(float val, float min, float max)
+float FlightTaskAutoLineSmoothVel::_constrainAbsPrioritizeMin(float val, float min, float max)
 {
-	return math::sign(val) * math::constrain(fabsf(val), fabsf(min), fabsf(max));
+	return math::sign(val) * math::max(math::min(fabsf(val), fabsf(max)), fabsf(min));
 }
 
-float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget()
+float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget() const
 {
 	// Compute the maximum allowed speed at the waypoint assuming that we want to
 	// connect the two lines (prev-current and current-next)
@@ -217,7 +217,7 @@ float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget()
 	return speed_at_target;
 }
 
-float FlightTaskAutoLineSmoothVel::_getMaxSpeedFromDistance(float braking_distance)
+float FlightTaskAutoLineSmoothVel::_getMaxSpeedFromDistance(float braking_distance) const
 {
 	float max_speed = math::trajectory::computeMaxSpeedFromBrakingDistance(_param_mpc_jerk_auto.get(),
 			  _param_mpc_acc_hor.get(),
@@ -277,8 +277,8 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
 
 			// Constrain the norm of each component using min and max values
 			Vector2f vel_sp_constrained_xy;
-			vel_sp_constrained_xy(0) = _constrainAbs(vel_sp_xy(0), vel_min_xy(0), vel_max_xy(0));
+			vel_sp_constrained_xy(0) = _constrainAbsPrioritizeMin(vel_sp_xy(0), vel_min_xy(0), vel_max_xy(0));
-			vel_sp_constrained_xy(1) = _constrainAbs(vel_sp_xy(1), vel_min_xy(1), vel_max_xy(1));
+			vel_sp_constrained_xy(1) = _constrainAbsPrioritizeMin(vel_sp_xy(1), vel_min_xy(1), vel_max_xy(1));
 
 			for (int i = 0; i < 2; i++) {
 				// If available, constrain the velocity using _velocity_setpoint(.)

src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp

@@ -77,11 +77,21 @@ protected:
 	void _generateHeading();
 	bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */
 
-	inline float _constrainOneSide(float val, float constraint); /**< Constrain val between INF and constraint */
+	static float _constrainOneSide(float val, float constraint); /**< Constrain val between INF and constraint */
-	inline float _constrainAbs(float val, float min, float max); /**< Constrain absolute value of val between min and max */
 
-	float _getSpeedAtTarget();
+	/**
-	float _getMaxSpeedFromDistance(float braking_distance);
+	 * Constrain the abs value below max but above min
+	 * Min can be larger than max and has priority over it
+	 * The whole computation is done on the absolute values but the returned
+	 * value has the sign of val
+	 * @param val the value to constrain and boost
+	 * @param min the minimum value that the function should return
+	 * @param max the value by which val is constrained before the boost is applied
+	 */
+	static float _constrainAbsPrioritizeMin(float val, float min, float max);
+
+	float _getSpeedAtTarget() const;
+	float _getMaxSpeedFromDistance(float braking_distance) const;
 
 	void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */
 	void _updateTrajConstraints();

src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp

@@ -70,6 +70,12 @@ bool FlightTaskAutoMapper::update()
 		_thrust_setpoint = Vector3f(NAN, NAN, NAN);
 	}
 
+	// during mission and reposition, raise the landing gears but only
+	// if altitude is high enough
+	if (_highEnoughForLandingGear()) {
+		_gear.landing_gear = _mission_gear;
+	}
+
 	if (_type == WaypointType::idle) {
 		_generateIdleSetpoints();
 
@@ -92,12 +98,6 @@ bool FlightTaskAutoMapper::update()
 				_yawspeed_setpoint);
 	}
 
-	// during mission and reposition, raise the landing gears but only
-	// if altitude is high enough
-	if (_highEnoughForLandingGear()) {
-		_gear.landing_gear = _mission_gear;
-	}
-
 	// update previous type
 	_type_previous = _type;
 

src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp

@@ -62,6 +62,12 @@ bool FlightTaskAutoMapper2::update()
 		_thrust_setpoint = Vector3f(NAN, NAN, NAN);
 	}
 
+	// during mission and reposition, raise the landing gears but only
+	// if altitude is high enough
+	if (_highEnoughForLandingGear()) {
+		_gear.landing_gear = landing_gear_s::GEAR_UP;
+	}
+
 	switch (_type) {
 	case WaypointType::idle:
 		_prepareIdleSetpoints();
@@ -100,12 +106,6 @@ bool FlightTaskAutoMapper2::update()
 
 	_generateSetpoints();
 
-	// during mission and reposition, raise the landing gears but only
-	// if altitude is high enough
-	if (_highEnoughForLandingGear()) {
-		_gear.landing_gear = landing_gear_s::GEAR_UP;
-	}
-
 	// update previous type
 	_type_previous = _type;
 

src/lib/controllib/BlockDerivative.hpp

@@ -87,6 +87,7 @@ public:
 	float update(float input);
 // accessors
 	void setU(float u) {_u = u;}
+	void reset() { _initialized = false; };
 	float getU() {return _u;}
 	float getLP() {return _lowPass.getFCut();}
 	float getO() { return _lowPass.getState(); }

src/lib/drivers/accelerometer/CMakeLists.txt

@@ -32,4 +32,8 @@
 ############################################################################
 
 px4_add_library(drivers_accelerometer PX4Accelerometer.cpp)
-target_link_libraries(drivers_accelerometer PRIVATE drivers__device)
+target_link_libraries(drivers_accelerometer
+	PRIVATE
+		drivers__device
+		mathlib
+	)

src/lib/drivers/smbus/SMBus.cpp

@@ -54,23 +54,23 @@ SMBus::~SMBus()
 {
 }
 
-int SMBus::read_word(const uint8_t cmd_code, void *data)
+int SMBus::read_word(const uint8_t cmd_code, uint16_t &data)
 {
+	uint8_t buf[6];
 	// 2 data bytes + pec byte
-	int result = transfer(&cmd_code, 1, (uint8_t *)data, 3);
+	int result = transfer(&cmd_code, 1, buf + 3, 3);
 
 	if (result == PX4_OK) {
+		data = buf[3] | ((uint16_t)buf[4] << 8);
 		// Check PEC.
 		uint8_t addr = get_device_address() << 1;
-		uint8_t full_data_packet[5];
+		buf[0] = addr | 0x00;
-		full_data_packet[0] = addr | 0x00;
+		buf[1] = cmd_code;
-		full_data_packet[1] = cmd_code;
+		buf[2] = addr | 0x01;
-		full_data_packet[2] = addr | 0x01;
-		memcpy(&full_data_packet[3], data, 2);
 
-		uint8_t pec = get_pec(full_data_packet, sizeof(full_data_packet) / sizeof(full_data_packet[0]));
+		uint8_t pec = get_pec(buf, sizeof(buf) - 1);
 
-		if (pec != ((uint8_t *)data)[2]) {
+		if (pec != buf[sizeof(buf) - 1]) {
 			result = -EINVAL;
 		}
 	}
@@ -78,14 +78,14 @@ int SMBus::read_word(const uint8_t cmd_code, void *data)
 	return result;
 }
 
-int SMBus::write_word(const uint8_t cmd_code, void *data)
+int SMBus::write_word(const uint8_t cmd_code, uint16_t data)
 {
 	// 2 data bytes + pec byte
-	uint8_t buf[5] = {};
+	uint8_t buf[5];
 	buf[0] = (get_device_address() << 1) | 0x10;
 	buf[1] = cmd_code;
-	buf[2] = ((uint8_t *)data)[0];
+	buf[2] = data & 0xff;
-	buf[3] = ((uint8_t *)data)[1];
+	buf[3] = (data >> 8) & 0xff;
 
 	buf[4] = get_pec(buf, 4);
 
@@ -177,4 +177,4 @@ uint8_t SMBus::get_pec(uint8_t *buff, const uint8_t len)
 	}
 
 	return crc;
-}
+}

src/lib/drivers/smbus/SMBus.hpp

@@ -55,7 +55,7 @@ public:
 	 * @param cmd_code The command code.
 	 * @param data The data to be written.
 	 * @param length The number of bytes being written.
-	 * @return Returns PX4_OK on success, PX4_ERROR on failure.
+	 * @return Returns PX4_OK on success, -errno on failure.
 	 */
 	int block_write(const uint8_t cmd_code, void *data, uint8_t byte_count, bool use_pec);
 
@@ -64,7 +64,7 @@ public:
 	 * @param cmd_code The command code.
 	 * @param data The returned data.
 	 * @param length The number of bytes being read
-	 * @return Returns PX4_OK on success, PX4_ERROR on failure.
+	 * @return Returns PX4_OK on success, -errno on failure.
 	 */
 	int block_read(const uint8_t cmd_code, void *data, const uint8_t length, bool use_pec);
 
@@ -72,23 +72,23 @@ public:
 	 * @brief Sends a read word command.
 	 * @param cmd_code The command code.
 	 * @param data The 2 bytes of returned data plus a 1 byte CRC if used.
-	 * @return Returns PX4_OK on success, PX4_ERROR on failure.
+	 * @return Returns PX4_OK on success, -errno on failure.
 	 */
-	int read_word(const uint8_t cmd_code, void *data);
+	int read_word(const uint8_t cmd_code, uint16_t &data);
 
 	/**
 	 * @brief Sends a write word command.
 	 * @param cmd_code The command code.
 	 * @param data The 2 bytes of data to be transfered.
-	 * @return Returns PX4_OK on success, PX4_ERROR on failure.
+	 * @return Returns PX4_OK on success, -errno on failure.
 	 */
-	int write_word(const uint8_t cmd_code, void *data);
+	int write_word(const uint8_t cmd_code, uint16_t data);
 
 	/**
 	 * @brief Calculates the PEC from the data.
 	 * @param buffer The buffer that stores the data to perform the CRC over.
 	 * @param length The number of bytes being written.
-	 * @return Returns PX4_OK on success, PX4_ERROR on failure.
+	 * @return Returns PX4_OK on success, -errno on failure.
 	 */
 	uint8_t get_pec(uint8_t *buffer, uint8_t length);
 

src/lib/ecl

@@ -1 +1 @@
-Subproject commit f005e0ea8f33d10dd014a0700e712426540bb58a
+Subproject commit dec1ed0aeb395a885b60b1d61cfb57bd932c7d34

src/lib/mixer_module/mixer_module.cpp

@@ -236,6 +236,7 @@ unsigned MixingOutput::motorTest()
 
 	while (_motor_test.test_motor_sub.update(&test_motor)) {
 		if (test_motor.driver_instance != _driver_instance ||
+		    test_motor.timestamp == 0 ||
 		    hrt_elapsed_time(&test_motor.timestamp) > 100_ms) {
 			continue;
 		}

src/lib/parameters/px4params/markdownout.py

@@ -28,6 +28,7 @@ class MarkdownTablesOutput():
                 def_val = param.GetDefault() or ''
                 unit = param.GetFieldValue("unit") or ''
                 type = param.GetType()
+                is_boolean = param.GetBoolean()
                 reboot_required = param.GetFieldValue("reboot_required") or ''
                 #board = param.GetFieldValue("board") or '' ## Disabled as no board values are defined in any parameters!
                 #decimal = param.GetFieldValue("decimal") or '' #Disabled as is intended for GCS not people
@@ -78,8 +79,12 @@ class MarkdownTablesOutput():
                         bitmask_output+='  <li><strong>%s:</strong> %s</li> \n' % (bit, bit_text)
                     bitmask_output+='</ul>\n'
 
+                if is_boolean and def_val=='1':
+                    def_val='Enabled (1)'
+                if is_boolean and def_val=='0':
+                    def_val='Disabled (0)'
                     
-                result += '<tr>\n <td style="vertical-align: top;">%s (%s)</td>\n <td style="vertical-align: top;"><p>%s</p>%s %s %s %s</td>\n <td style="vertical-align: top;">%s</td>\n <td style="vertical-align: top;">%s </td>\n <td style="vertical-align: top;">%s</td>\n</tr>\n' % (code, type, name, long_desc, enum_output, bitmask_output, reboot_required, max_min_combined, def_val, unit)
+                result += '<tr>\n <td style="vertical-align: top;">%s (%s)</td>\n <td style="vertical-align: top;"><p>%s</p>%s %s %s %s</td>\n <td style="vertical-align: top;">%s</td>\n <td style="vertical-align: top;">%s</td>\n <td style="vertical-align: top;">%s</td>\n</tr>\n' % (code, type, name, long_desc, enum_output, bitmask_output, reboot_required, max_min_combined, def_val, unit)
 
             #Close the table.
             result += '</tbody></table>\n\n'

src/lib/parameters/px4params/srcparser.py

@@ -59,6 +59,7 @@ class Parameter(object):
         self.default = default
         self.volatile = "false"
         self.category = ""
+        self.boolean = False
 
     def GetName(self):
         return self.name
@@ -75,6 +76,9 @@ class Parameter(object):
     def GetVolatile(self):
         return self.volatile
 
+    def GetBoolean(self):
+        return self.boolean
+
     def SetField(self, code, value):
         """
         Set named field value
@@ -99,6 +103,12 @@ class Parameter(object):
         """
         self.volatile = "true"
 
+    def SetBoolean(self):
+        """
+        Set boolean flag
+        """
+        self.boolean = True
+
     def SetCategory(self, category):
         """
         Set param category
@@ -305,6 +315,8 @@ class SourceParser(object):
                                 param.SetVolatile()
                             elif tag == "category":
                                 param.SetCategory(tags[tag])
+                            elif tag == "boolean":
+                                param.SetBoolean()
                             elif tag not in self.valid_tags:
                                 sys.stderr.write("Skipping invalid documentation tag: '%s'\n" % tag)
                                 return False

src/modules/battery_status/sensor_params_battery.c

@@ -66,31 +66,6 @@ PARAM_DEFINE_FLOAT(BAT_CNT_V_CURR, -1.0);
  */
 PARAM_DEFINE_FLOAT(BAT_V_OFFS_CURR, 0.0);
 
-/**
- * Battery voltage divider (V divider)
- *
- * This is the divider from battery voltage to 3.3V ADC voltage.
- * If using e.g. Mauch power modules the value from the datasheet
- * can be applied straight here. A value of -1 means to use
- * the board default.
- *
- * @group Battery Calibration
- * @decimal 8
- */
-PARAM_DEFINE_FLOAT(BAT_V_DIV, -1.0);
-
-/**
- * Battery current per volt (A/V)
- *
- * The voltage seen by the 3.3V ADC multiplied by this factor
- * will determine the battery current. A value of -1 means to use
- * the board default.
- *
- * @group Battery Calibration
- * @decimal 8
- */
-PARAM_DEFINE_FLOAT(BAT_A_PER_V, -1.0);
-
 /**
  * Battery monitoring source.
  *

src/modules/commander/PreflightCheck.cpp

@@ -494,9 +494,23 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_s
 	}
 
 	// Check if preflight check performed by estimator has failed
-	if (status.pre_flt_fail) {
+	if (status.pre_flt_fail_innov_heading ||
+	    status.pre_flt_fail_innov_vel_horiz ||
+	    status.pre_flt_fail_innov_vel_vert ||
+	    status.pre_flt_fail_innov_height) {
 		if (report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Position unknown");
+			if (status.pre_flt_fail_innov_heading) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: heading estimate not stable");
+
+			} else if (status.pre_flt_fail_innov_vel_horiz) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: horizontal velocity estimate not stable");
+
+			} else if (status.pre_flt_fail_innov_vel_horiz) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: vertical velocity estimate not stable");
+
+			} else if (status.pre_flt_fail_innov_height) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: height estimate not stable");
+			}
 		}
 
 		success = false;

src/modules/ekf2/CMakeLists.txt

@@ -30,6 +30,9 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 #############################################################################
+
+add_subdirectory(Utility)
+
 px4_add_module(
 	MODULE modules__ekf2
 	MAIN ekf2
@@ -42,4 +45,5 @@ px4_add_module(
 		ecl_EKF
 		ecl_geo
 		perf
+		Ekf2Utility
 	)

src/modules/ekf2/Utility/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+#############################################################################
+
+px4_add_library(Ekf2Utility
+	PreFlightChecker.cpp
+)
+
+target_include_directories(Ekf2Utility
+	PUBLIC
+	${CMAKE_CURRENT_SOURCE_DIR}
+)
+
+target_link_libraries(Ekf2Utility PRIVATE mathlib)
+
+px4_add_unit_gtest(SRC PreFlightCheckerTest.cpp LINKLIBS Ekf2Utility)

src/modules/ekf2/Utility/InnovationLpf.hpp

@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * First order "alpha" IIR digital filter with input saturation
+ */
+
+#include <mathlib/mathlib.h>
+
+class InnovationLpf final
+{
+public:
+	InnovationLpf() = default;
+	~InnovationLpf() = default;
+
+	void reset(float val = 0.f) { _x = val; }
+
+	/**
+	 * Update the filter with a new value and returns the filtered state
+	 * The new value is constained by the limit set in setSpikeLimit
+	 * @param val new input
+	 * @param alpha normalized weight of the new input
+	 * @param spike_limit the amplitude of the saturation at the input of the filter
+	 * @return filtered output
+	 */
+	float update(float val, float alpha, float spike_limit)
+	{
+		float val_constrained = math::constrain(val, -spike_limit, spike_limit);
+		float beta = 1.f - alpha;
+
+		_x = beta * _x + alpha * val_constrained;
+
+		return _x;
+	}
+
+	/**
+	 * Helper function to compute alpha from dt and the inverse of tau
+	 * @param dt sampling time in seconds
+	 * @param tau_inv inverse of the time constant of the filter
+	 * @return alpha, the normalized weight of a new measurement
+	 */
+	static float computeAlphaFromDtAndTauInv(float dt, float tau_inv)
+	{
+		return math::constrain(dt * tau_inv, 0.f, 1.f);
+	}
+
+private:
+	float _x{}; ///< current state of the filter
+};

src/modules/ekf2/Utility/PreFlightChecker.cpp

@@ -0,0 +1,136 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file PreFlightCheckHelper.cpp
+ * Class handling the EKF2 innovation pre flight checks
+ */
+
+#include "PreFlightChecker.hpp"
+
+void PreFlightChecker::update(const float dt, const ekf2_innovations_s &innov)
+{
+	const float alpha = InnovationLpf::computeAlphaFromDtAndTauInv(dt, _innov_lpf_tau_inv);
+
+	_has_heading_failed = preFlightCheckHeadingFailed(innov, alpha);
+	_has_horiz_vel_failed = preFlightCheckHorizVelFailed(innov, alpha);
+	_has_vert_vel_failed = preFlightCheckVertVelFailed(innov, alpha);
+	_has_height_failed = preFlightCheckHeightFailed(innov, alpha);
+}
+
+bool PreFlightChecker::preFlightCheckHeadingFailed(const ekf2_innovations_s &innov, const float alpha)
+{
+	const float heading_test_limit = selectHeadingTestLimit();
+	const float heading_innov_spike_lim = 2.0f * heading_test_limit;
+
+	const float heading_innov_lpf = _filter_heading_innov.update(innov.heading_innov, alpha, heading_innov_spike_lim);
+
+	return checkInnovFailed(innov.heading_innov, heading_innov_lpf, heading_test_limit);
+}
+
+float PreFlightChecker::selectHeadingTestLimit()
+{
+	// Select the max allowed heading innovaton depending on whether we are not aiding navigation using
+	// observations in the NE reference frame and if the vehicle can use GPS course to realign in flight (fixedwing sideslip fusion).
+	const bool is_ne_aiding = _is_using_gps_aiding || _is_using_ev_pos_aiding;
+
+	return (is_ne_aiding && !_can_observe_heading_in_flight)
+	       ? _nav_heading_innov_test_lim // more restrictive test limit
+	       : _heading_innov_test_lim; // less restrictive test limit
+}
+
+bool PreFlightChecker::preFlightCheckHorizVelFailed(const ekf2_innovations_s &innov, const float alpha)
+{
+	bool has_failed = false;
+
+	if (_is_using_gps_aiding || _is_using_ev_pos_aiding) {
+		const Vector2f vel_ne_innov = Vector2f(innov.vel_pos_innov);
+		Vector2f vel_ne_innov_lpf;
+		vel_ne_innov_lpf(0) = _filter_vel_n_innov.update(vel_ne_innov(0), alpha, _vel_innov_spike_lim);
+		vel_ne_innov_lpf(1) = _filter_vel_n_innov.update(vel_ne_innov(1), alpha, _vel_innov_spike_lim);
+		has_failed |= checkInnov2DFailed(vel_ne_innov, vel_ne_innov_lpf, _vel_innov_test_lim);
+	}
+
+	if (_is_using_flow_aiding) {
+		const Vector2f flow_innov = Vector2f(innov.flow_innov);
+		Vector2f flow_innov_lpf;
+		flow_innov_lpf(0) = _filter_flow_x_innov.update(flow_innov(0), alpha, _flow_innov_spike_lim);
+		flow_innov_lpf(1) = _filter_flow_x_innov.update(flow_innov(1), alpha, _flow_innov_spike_lim);
+		has_failed |= checkInnov2DFailed(flow_innov, flow_innov_lpf, _flow_innov_test_lim);
+	}
+
+	return has_failed;
+}
+
+bool PreFlightChecker::preFlightCheckVertVelFailed(const ekf2_innovations_s &innov, const float alpha)
+{
+	const float vel_d_innov = innov.vel_pos_innov[2];
+	const float vel_d_innov_lpf = _filter_vel_d_innov.update(vel_d_innov, alpha, _vel_innov_spike_lim);
+	return checkInnovFailed(vel_d_innov, vel_d_innov_lpf, _vel_innov_test_lim);
+}
+
+bool PreFlightChecker::preFlightCheckHeightFailed(const ekf2_innovations_s &innov, const float alpha)
+{
+	const float hgt_innov = innov.vel_pos_innov[5];
+	const float hgt_innov_lpf = _filter_hgt_innov.update(hgt_innov, alpha, _hgt_innov_spike_lim);
+	return checkInnovFailed(hgt_innov, hgt_innov_lpf, _hgt_innov_test_lim);
+}
+
+bool PreFlightChecker::checkInnovFailed(const float innov, const float innov_lpf, const float test_limit)
+{
+	return fabsf(innov_lpf) > test_limit || fabsf(innov) > 2.0f * test_limit;
+}
+
+bool PreFlightChecker::checkInnov2DFailed(const Vector2f &innov, const Vector2f &innov_lpf, const float test_limit)
+{
+	return innov_lpf.norm_squared() > sq(test_limit)
+	       || innov.norm_squared() > sq(2.0f * test_limit);
+}
+
+void PreFlightChecker::reset()
+{
+	_is_using_gps_aiding = false;
+	_is_using_flow_aiding = false;
+	_is_using_ev_pos_aiding = false;
+	_has_heading_failed = false;
+	_has_horiz_vel_failed = false;
+	_has_vert_vel_failed = false;
+	_has_height_failed = false;
+	_filter_vel_n_innov.reset();
+	_filter_vel_e_innov.reset();
+	_filter_vel_d_innov.reset();
+	_filter_hgt_innov.reset();
+	_filter_heading_innov.reset();
+	_filter_flow_x_innov.reset();
+	_filter_flow_y_innov.reset();
+}

src/modules/ekf2/Utility/PreFlightChecker.hpp

@@ -0,0 +1,176 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file PreFlightChecker.hpp
+ * Class handling the EKF2 innovation pre flight checks
+ *
+ * First call the update(...) function and then get the results
+ * using the hasXxxFailed() getters
+ */
+
+#pragma once
+
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/ekf2_innovations.h>
+
+#include <matrix/matrix/math.hpp>
+
+#include "InnovationLpf.hpp"
+
+using matrix::Vector2f;
+
+class PreFlightChecker
+{
+public:
+	PreFlightChecker() = default;
+	~PreFlightChecker() = default;
+
+	/*
+	 * Reset all the internal states of the class to their default value
+	 */
+	void reset();
+
+	/*
+	 * Update the internal states
+	 * @param dt the sampling time
+	 * @param innov the ekf2_innovation_s struct containing the current innovations
+	 */
+	void update(float dt, const ekf2_innovations_s &innov);
+
+	/*
+	 * If set to true, the checker will use a less conservative heading innovation check
+	 */
+	void setVehicleCanObserveHeadingInFlight(bool val) { _can_observe_heading_in_flight = val; }
+
+	void setUsingGpsAiding(bool val) { _is_using_gps_aiding = val; }
+	void setUsingFlowAiding(bool val) { _is_using_flow_aiding = val; }
+	void setUsingEvPosAiding(bool val) { _is_using_ev_pos_aiding = val; }
+
+	bool hasHeadingFailed() const { return _has_heading_failed; }
+	bool hasHorizVelFailed() const { return _has_horiz_vel_failed; }
+	bool hasVertVelFailed() const { return _has_vert_vel_failed; }
+	bool hasHeightFailed() const { return _has_height_failed; }
+
+	/*
+	 * Overall state of the pre fligh checks
+	 * @return true if any of the check failed
+	 */
+	bool hasFailed() const { return hasHorizFailed() || hasVertFailed(); }
+
+	/*
+	 * Horizontal checks overall result
+	 * @return true if one of the horizontal checks failed
+	 */
+	bool hasHorizFailed() const { return _has_heading_failed || _has_horiz_vel_failed; }
+
+	/*
+	 * Vertical checks overall result
+	 * @return true if one of the vertical checks failed
+	 */
+	bool hasVertFailed() const { return _has_vert_vel_failed || _has_height_failed; }
+
+	/*
+	 * Check if the innovation fails the test
+	 * To pass the test, the following conditions should be true:
+	 * innov <= test_limit
+	 * innov_lpf <= 2 * test_limit
+	 * @param innov the current unfiltered innovation
+	 * @param innov_lpf the low-pass filtered innovation
+	 * @param test_limit the magnitude test limit
+	 * @return true if the check failed the test, false otherwise
+	 */
+	static bool checkInnovFailed(float innov, float innov_lpf, float test_limit);
+
+	/*
+	 * Check if the a innovation of a 2D vector fails the test
+	 * To pass the test, the following conditions should be true:
+	 * innov <= test_limit
+	 * innov_lpf <= 2 * test_limit
+	 * @param innov the current unfiltered innovation
+	 * @param innov_lpf the low-pass filtered innovation
+	 * @param test_limit the magnitude test limit
+	 * @return true if the check failed the test, false otherwise
+	 */
+	static bool checkInnov2DFailed(const Vector2f &innov, const Vector2f &innov_lpf, float test_limit);
+
+	static constexpr float sq(float var) { return var * var; }
+
+private:
+	bool preFlightCheckHeadingFailed(const ekf2_innovations_s &innov, float alpha);
+	float selectHeadingTestLimit();
+
+	bool preFlightCheckHorizVelFailed(const ekf2_innovations_s &innov, float alpha);
+	bool preFlightCheckVertVelFailed(const ekf2_innovations_s &innov, float alpha);
+	bool preFlightCheckHeightFailed(const ekf2_innovations_s &innov, float alpha);
+
+	void resetPreFlightChecks();
+
+	bool _has_heading_failed{};
+	bool _has_horiz_vel_failed{};
+	bool _has_vert_vel_failed{};
+	bool _has_height_failed{};
+
+	bool _can_observe_heading_in_flight{};
+	bool _is_using_gps_aiding{};
+	bool _is_using_flow_aiding{};
+	bool _is_using_ev_pos_aiding{};
+
+	// Low-pass filters for innovation pre-flight checks
+	InnovationLpf _filter_vel_n_innov;	///< Preflight low pass filter N axis velocity innovations (m/sec)
+	InnovationLpf _filter_vel_e_innov;	///< Preflight low pass filter E axis velocity innovations (m/sec)
+	InnovationLpf _filter_vel_d_innov;	///< Preflight low pass filter D axis velocity innovations (m/sec)
+	InnovationLpf _filter_hgt_innov;	///< Preflight low pass filter height innovation (m)
+	InnovationLpf _filter_heading_innov;	///< Preflight low pass filter heading innovation magntitude (rad)
+	InnovationLpf _filter_flow_x_innov;	///< Preflight low pass filter optical flow innovation (rad)
+	InnovationLpf _filter_flow_y_innov;	///< Preflight low pass filter optical flow innovation (rad)
+
+	// Preflight low pass filter time constant inverse (1/sec)
+	static constexpr float _innov_lpf_tau_inv = 0.2f;
+	// Maximum permissible velocity innovation to pass pre-flight checks (m/sec)
+	static constexpr float _vel_innov_test_lim = 0.5f;
+	// Maximum permissible height innovation to pass pre-flight checks (m)
+	static constexpr float _hgt_innov_test_lim = 1.5f;
+	// Maximum permissible yaw innovation to pass pre-flight checks when aiding inertial nav using NE frame observations (rad)
+	static constexpr float _nav_heading_innov_test_lim = 0.25f;
+	// Maximum permissible yaw innovation to pass pre-flight checks when not aiding inertial nav using NE frame observations (rad)
+	static constexpr float _heading_innov_test_lim = 0.52f;
+	// Maximum permissible flow innovation to pass pre-flight checks
+	static constexpr float _flow_innov_test_lim = 0.1f;
+	// Preflight velocity innovation spike limit (m/sec)
+	static constexpr float _vel_innov_spike_lim = 2.0f * _vel_innov_test_lim;
+	// Preflight position innovation spike limit (m)
+	static constexpr float _hgt_innov_spike_lim = 2.0f * _hgt_innov_test_lim;
+	// Preflight flow innovation spike limit (rad)
+	static constexpr float _flow_innov_spike_lim = 2.0f * _flow_innov_test_lim;
+};

src/modules/ekf2/Utility/PreFlightCheckerTest.cpp

@@ -0,0 +1,93 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Test code for PreFlightChecker class
+ * Run this test only using make tests TESTFILTER=PreFlightChecker
+ */
+
+#include <gtest/gtest.h>
+
+#include "PreFlightChecker.hpp"
+
+class PreFlightCheckerTest : public ::testing::Test
+{
+};
+
+TEST_F(PreFlightCheckerTest, testInnovFailed)
+{
+	const float test_limit = 1.0; ///< is the limit for innovation_lpf, the limit for innovation is 2*test_limit
+	const float innovations[9] = 	 {0.0, 1.5, 2.5, -1.5, -2.5, 1.5, -1.5, -2.5, -2.5};
+	const float innovations_lpf[9] = {0.0, 0.9, 0.9, -0.9, -0.9, 1.1, -1.1, -1.1, 1.1};
+	const bool expected_result[9] = {false, false, true, false, true, true, true, true, true};
+
+	for (int i = 0; i < 9; i++) {
+		EXPECT_EQ(PreFlightChecker::checkInnovFailed(innovations[i], innovations_lpf[i], test_limit), expected_result[i]);
+	}
+
+	// Smaller test limit, all the checks should fail except the first
+	EXPECT_FALSE(PreFlightChecker::checkInnovFailed(innovations[0], innovations_lpf[0], 0.0));
+
+	for (int i = 1; i < 9; i++) {
+		EXPECT_TRUE(PreFlightChecker::checkInnovFailed(innovations[i], innovations_lpf[i], 0.0));
+	}
+
+	// Larger test limit, none of the checks should fail
+	for (int i = 0; i < 9; i++) {
+		EXPECT_FALSE(PreFlightChecker::checkInnovFailed(innovations[i], innovations_lpf[i], 2.0));
+	}
+}
+
+TEST_F(PreFlightCheckerTest, testInnov2dFailed)
+{
+	const float test_limit = 1.0;
+	Vector2f innovations[4] = {{0.0, 0.0}, {0.0, 0.0}, {0.0, -2.5}, {1.5, -1.5}};
+	Vector2f innovations_lpf[4] = {{0.0, 0.0}, {1.1, 0.0}, {0.5, 0.5}, {1.0, -1.0}};
+	const bool expected_result[4] = {false, true, true, true};
+
+	for (int i = 0; i < 4; i++) {
+		EXPECT_EQ(PreFlightChecker::checkInnov2DFailed(innovations[i], innovations_lpf[i], test_limit), expected_result[i]);
+	}
+
+	// Smaller test limit, all the checks should fail except the first
+	EXPECT_FALSE(PreFlightChecker::checkInnov2DFailed(innovations[0], innovations_lpf[0], 0.0));
+
+	for (int i = 1; i < 4; i++) {
+		EXPECT_TRUE(PreFlightChecker::checkInnov2DFailed(innovations[i], innovations_lpf[i], 0.0));
+	}
+
+	// Larger test limit, none of the checks should fail
+	for (int i = 0; i < 4; i++) {
+		EXPECT_FALSE(PreFlightChecker::checkInnov2DFailed(innovations[i], innovations_lpf[i], 1.42));
+	}
+}

src/modules/ekf2/ekf2_main.cpp

@@ -78,6 +78,8 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/wind_estimate.h>
 
+#include "Utility/PreFlightChecker.hpp"
+
 // defines used to specify the mask position for use of different accuracy metrics in the GPS blending algorithm
 #define BLEND_MASK_USE_SPD_ACC      1
 #define BLEND_MASK_USE_HPOS_ACC     2
@@ -116,6 +118,12 @@ public:
 private:
 	int getRangeSubIndex(); ///< get subscription index of first downward-facing range sensor
 
+	PreFlightChecker _preflt_checker;
+	void runPreFlightChecks(float dt, const filter_control_status_u &control_status,
+				const vehicle_status_s &vehicle_status,
+				const ekf2_innovations_s &innov);
+	void resetPreFlightChecks();
+
 	template<typename Param>
 	void update_mag_bias(Param &mag_bias_param, int axis_index);
 
@@ -171,8 +179,6 @@ private:
 	uint64_t _start_time_us = 0;		///< system time at EKF start (uSec)
 	int64_t _last_time_slip_us = 0;		///< Last time slip (uSec)
 
-	perf_counter_t _cycle_perf;
-	perf_counter_t _interval_perf;
 	perf_counter_t _ekf_update_perf;
 
 	// Initialise time stamps used to send sensor data to the EKF and for logging
@@ -202,27 +208,6 @@ private:
 	// Used to control saving of mag declination to be used on next startup
 	bool _mag_decl_saved = false;	///< true when the magnetic declination has been saved
 
-	// Used to filter velocity innovations during pre-flight checks
-	bool _preflt_horiz_fail = false;	///< true if preflight horizontal innovation checks are failed
-	bool _preflt_vert_fail = false;		///< true if preflight vertical innovation checks are failed
-	bool _preflt_fail = false;		///< true if any preflight innovation checks are failed
-	Vector2f _vel_ne_innov_lpf = {};	///< Preflight low pass filtered NE axis velocity innovations (m/sec)
-	float _vel_d_innov_lpf = {};		///< Preflight low pass filtered D axis velocity innovations (m/sec)
-	float _hgt_innov_lpf = 0.0f;		///< Preflight low pass filtered height innovation (m)
-	float _yaw_innov_magnitude_lpf = 0.0f;	///< Preflight low pass filtered yaw innovation magntitude (rad)
-
-	static constexpr float _innov_lpf_tau_inv = 0.2f;	///< Preflight low pass filter time constant inverse (1/sec)
-	static constexpr float _vel_innov_test_lim =
-		0.5f;	///< Maximum permissible velocity innovation to pass pre-flight checks (m/sec)
-	static constexpr float _hgt_innov_test_lim =
-		1.5f;	///< Maximum permissible height innovation to pass pre-flight checks (m)
-	static constexpr float _nav_yaw_innov_test_lim =
-		0.25f;	///< Maximum permissible yaw innovation to pass pre-flight checks when aiding inertial nav using NE frame observations (rad)
-	static constexpr float _yaw_innov_test_lim =
-		0.52f;	///< Maximum permissible yaw innovation to pass pre-flight checks when not aiding inertial nav using NE frame observations (rad)
-	const float _vel_innov_spike_lim = 2.0f * _vel_innov_test_lim;	///< preflight velocity innovation spike limit (m/sec)
-	const float _hgt_innov_spike_lim = 2.0f * _hgt_innov_test_lim;	///< preflight position innovation spike limit (m)
-
 	// set pose/velocity as invalid if standard deviation is bigger than max_std_dev
 	// TODO: the user should be allowed to set these values by a parameter
 	static constexpr float ep_max_std_dev = 100.0f;	///< Maximum permissible standard deviation for estimated position
@@ -549,8 +534,6 @@ Ekf2::Ekf2(bool replay_mode):
 	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
 	_replay_mode(replay_mode),
-	_cycle_perf(perf_alloc_once(PC_ELAPSED, MODULE_NAME": cycle time")),
-	_interval_perf(perf_alloc_once(PC_INTERVAL, MODULE_NAME": interval")),
 	_ekf_update_perf(perf_alloc_once(PC_ELAPSED, MODULE_NAME": update")),
 	_params(_ekf.getParamHandle()),
 	_param_ekf2_min_obs_dt(_params->sensor_interval_min_ms),
@@ -656,8 +639,6 @@ Ekf2::Ekf2(bool replay_mode):
 
 Ekf2::~Ekf2()
 {
-	perf_free(_cycle_perf);
-	perf_free(_interval_perf);
 	perf_free(_ekf_update_perf);
 }
 
@@ -679,8 +660,6 @@ int Ekf2::print_status()
 
 	PX4_INFO("time slip: %" PRId64 " us", _last_time_slip_us);
 
-	perf_print_counter(_cycle_perf);
-	perf_print_counter(_interval_perf);
 	perf_print_counter(_ekf_update_perf);
 
 	return 0;
@@ -728,9 +707,6 @@ void Ekf2::Run()
 		return;
 	}
 
-	perf_begin(_cycle_perf);
-	perf_count(_interval_perf);
-
 	sensor_combined_s sensors;
 
 	if (_sensors_sub.update(&sensors)) {
@@ -1314,10 +1290,10 @@ void Ekf2::Run()
 				lpos.az = vel_deriv[2];
 
 				// TODO: better status reporting
-				lpos.xy_valid = _ekf.local_position_is_valid() && !_preflt_horiz_fail;
+				lpos.xy_valid = _ekf.local_position_is_valid() && !_preflt_checker.hasHorizFailed();
-				lpos.z_valid = !_preflt_vert_fail;
+				lpos.z_valid = !_preflt_checker.hasVertFailed();
-				lpos.v_xy_valid = _ekf.local_position_is_valid() && !_preflt_horiz_fail;
+				lpos.v_xy_valid = _ekf.local_position_is_valid() && !_preflt_checker.hasHorizFailed();
-				lpos.v_z_valid = !_preflt_vert_fail;
+				lpos.v_z_valid = !_preflt_checker.hasVertFailed();
 
 				// Position of local NED origin in GPS / WGS84 frame
 				map_projection_reference_s ekf_origin;
@@ -1481,7 +1457,7 @@ void Ekf2::Run()
 					_vehicle_visual_odometry_aligned_pub.publish(aligned_ev_odom);
 				}
 
-				if (_ekf.global_position_is_valid() && !_preflt_fail) {
+				if (_ekf.global_position_is_valid() && !_preflt_checker.hasFailed()) {
 					// generate and publish global position data
 					vehicle_global_position_s &global_pos = _vehicle_global_position_pub.get();
 
@@ -1563,7 +1539,10 @@ void Ekf2::Run()
 			status.time_slip = _last_time_slip_us / 1e6f;
 			status.health_flags = 0.0f; // unused
 			status.timeout_flags = 0.0f; // unused
-			status.pre_flt_fail = _preflt_fail;
+			status.pre_flt_fail_innov_heading = _preflt_checker.hasHeadingFailed();
+			status.pre_flt_fail_innov_vel_horiz = _preflt_checker.hasHorizVelFailed();
+			status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed();
+			status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed();
 
 			_estimator_status_pub.publish(status);
 
@@ -1684,64 +1663,11 @@ void Ekf2::Run()
 
 				// calculate noise filtered velocity innovations which are used for pre-flight checking
 				if (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
-					// calculate coefficients for LPF applied to innovation sequences
+					float dt_seconds = sensors.accelerometer_integral_dt * 1e-6f;
-					float alpha = constrain(sensors.accelerometer_integral_dt / 1.e6f * _innov_lpf_tau_inv, 0.0f, 1.0f);
+					runPreFlightChecks(dt_seconds, control_status, _vehicle_status, innovations);
-					float beta = 1.0f - alpha;
-
-					// filter the velocity and innvovations
-					_vel_ne_innov_lpf(0) = beta * _vel_ne_innov_lpf(0) + alpha * constrain(innovations.vel_pos_innov[0],
-							       -_vel_innov_spike_lim, _vel_innov_spike_lim);
-					_vel_ne_innov_lpf(1) = beta * _vel_ne_innov_lpf(1) + alpha * constrain(innovations.vel_pos_innov[1],
-							       -_vel_innov_spike_lim, _vel_innov_spike_lim);
-					_vel_d_innov_lpf = beta * _vel_d_innov_lpf + alpha * constrain(innovations.vel_pos_innov[2],
-							   -_vel_innov_spike_lim, _vel_innov_spike_lim);
-
-					// set the max allowed yaw innovaton depending on whether we are not aiding navigation using
-					// observations in the NE reference frame.
-					bool doing_ne_aiding = control_status.flags.gps ||  control_status.flags.ev_pos;
-
-					float yaw_test_limit;
-
-					if (doing_ne_aiding && (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)) {
-						// use a smaller tolerance when doing NE inertial frame aiding as a rotary wing
-						// vehicle which cannot use GPS course to realign heading in flight
-						yaw_test_limit = _nav_yaw_innov_test_lim;
-
-					} else {
-						// use a larger tolerance when not doing NE inertial frame aiding or
-						// if a fixed wing vehicle which can realign heading using GPS course
-						yaw_test_limit = _yaw_innov_test_lim;
-					}
-
-					// filter the yaw innovations
-					_yaw_innov_magnitude_lpf = beta * _yaw_innov_magnitude_lpf + alpha * constrain(innovations.heading_innov,
-								   -2.0f * yaw_test_limit, 2.0f * yaw_test_limit);
-
-					_hgt_innov_lpf = beta * _hgt_innov_lpf + alpha * constrain(innovations.vel_pos_innov[5], -_hgt_innov_spike_lim,
-							 _hgt_innov_spike_lim);
-
-					// check the yaw and horizontal velocity innovations
-					float vel_ne_innov_length = sqrtf(innovations.vel_pos_innov[0] * innovations.vel_pos_innov[0] +
-									  innovations.vel_pos_innov[1] * innovations.vel_pos_innov[1]);
-					_preflt_horiz_fail = (_vel_ne_innov_lpf.norm() > _vel_innov_test_lim)
-							     || (vel_ne_innov_length > 2.0f * _vel_innov_test_lim)
-							     || (_yaw_innov_magnitude_lpf > yaw_test_limit);
-
-					// check the vertical velocity and position innovations
-					_preflt_vert_fail = (fabsf(_vel_d_innov_lpf) > _vel_innov_test_lim)
-							    || (fabsf(innovations.vel_pos_innov[2]) > 2.0f * _vel_innov_test_lim)
-							    || (fabsf(_hgt_innov_lpf) > _hgt_innov_test_lim);
-
-					// master pass-fail status
-					_preflt_fail = _preflt_horiz_fail || _preflt_vert_fail;
 
 				} else {
-					_vel_ne_innov_lpf.zero();
+					resetPreFlightChecks();
-					_vel_d_innov_lpf = 0.0f;
-					_hgt_innov_lpf = 0.0f;
-					_preflt_horiz_fail = false;
-					_preflt_vert_fail = false;
-					_preflt_fail = false;
 				}
 
 				_estimator_innovations_pub.publish(innovations);
@@ -1751,8 +1677,26 @@ void Ekf2::Run()
 		// publish ekf2_timestamps
 		_ekf2_timestamps_pub.publish(ekf2_timestamps);
 	}
+}
 
-	perf_end(_cycle_perf);
+void Ekf2::runPreFlightChecks(const float dt,
+			      const filter_control_status_u &control_status,
+			      const vehicle_status_s &vehicle_status,
+			      const ekf2_innovations_s &innov)
+{
+	const bool can_observe_heading_in_flight = (vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
+
+	_preflt_checker.setVehicleCanObserveHeadingInFlight(can_observe_heading_in_flight);
+	_preflt_checker.setUsingGpsAiding(control_status.flags.gps);
+	_preflt_checker.setUsingFlowAiding(control_status.flags.opt_flow);
+	_preflt_checker.setUsingEvPosAiding(control_status.flags.ev_pos);
+
+	_preflt_checker.update(dt, innov);
+}
+
+void Ekf2::resetPreFlightChecks()
+{
+	_preflt_checker.reset();
 }
 
 int Ekf2::getRangeSubIndex()

src/modules/logger/logger.cpp

@@ -530,6 +530,7 @@ void Logger::add_default_topics()
 	add_topic("position_setpoint_triplet", 200);
 	add_topic("radio_status");
 	add_topic("rate_ctrl_status", 200);
+	add_topic("safety", 1000);
 	add_topic("sensor_combined", 100);
 	add_topic("sensor_preflight", 200);
 	add_topic("system_power", 500);

src/modules/mavlink/mavlink_main.cpp

@@ -489,18 +489,6 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
 	}
 }
 
-int
-Mavlink::get_uart_fd(unsigned index)
-{
-	Mavlink *inst = get_instance(index);
-
-	if (inst) {
-		return inst->get_uart_fd();
-	}
-
-	return -1;
-}
-
 int
 Mavlink::mavlink_open_uart(const int baud, const char *uart_name, const bool force_flow_control)
 {
@@ -592,8 +580,7 @@ Mavlink::mavlink_open_uart(const int baud, const char *uart_name, const bool for
 	}
 
 	/* back off 1800 ms to avoid running into the USB setup timing */
-	while (_is_usb_uart &&
+	while (_is_usb_uart && hrt_absolute_time() < 1800U * 1000U) {
-	       hrt_absolute_time() < 1800U * 1000U) {
 		px4_usleep(50000);
 	}
 
@@ -606,7 +593,10 @@ Mavlink::mavlink_open_uart(const int baud, const char *uart_name, const bool for
 		uORB::SubscriptionData<actuator_armed_s> armed_sub{ORB_ID(actuator_armed)};
 
 		/* get the system arming state and abort on arming */
-		while (_uart_fd < 0) {
+		while (_uart_fd < 0 && !_task_should_exit) {
+
+			/* another task might have requested subscriptions: make sure we handle it */
+			check_requested_subscriptions();
 
 			/* abort if an arming topic is published and system is armed */
 			armed_sub.update();
@@ -1895,6 +1885,12 @@ Mavlink::task_main(int argc, char *argv[])
 		case 'd':
 			_device_name = myoptarg;
 			set_protocol(Protocol::SERIAL);
+
+			if (access(_device_name, F_OK) == -1) {
+				PX4_ERR("Device %s does not exist", _device_name);
+				err_flag = true;
+			}
+
 			break;
 
 		case 'n':
@@ -2062,7 +2058,7 @@ Mavlink::task_main(int argc, char *argv[])
 		return PX4_ERROR;
 	}
 
-	/* USB serial is indicated by /dev/ttyACM0*/
+	/* USB serial is indicated by /dev/ttyACMx */
 	if (strcmp(_device_name, "/dev/ttyACM0") == OK || strcmp(_device_name, "/dev/ttyACM1") == OK) {
 		if (_datarate == 0) {
 			_datarate = 800000;
@@ -2100,19 +2096,6 @@ Mavlink::task_main(int argc, char *argv[])
 
 		/* flush stdout in case MAVLink is about to take it over */
 		fflush(stdout);
-
-		/* default values for arguments */
-		_uart_fd = mavlink_open_uart(_baudrate, _device_name, _force_flow_control);
-
-		if (_uart_fd < 0 && !_is_usb_uart) {
-			PX4_ERR("could not open %s", _device_name);
-			return PX4_ERROR;
-
-		} else if (_uart_fd < 0 && _is_usb_uart) {
-			/* the config link is optional */
-			return OK;
-		}
-
 	}
 
 #if defined(MAVLINK_UDP)
@@ -2212,6 +2195,20 @@ Mavlink::task_main(int argc, char *argv[])
 	/* now the instance is fully initialized and we can bump the instance count */
 	LL_APPEND(_mavlink_instances, this);
 
+	/* open the UART device after setting the instance, as it might block */
+	if (get_protocol() == Protocol::SERIAL) {
+		_uart_fd = mavlink_open_uart(_baudrate, _device_name, _force_flow_control);
+
+		if (_uart_fd < 0 && !_is_usb_uart) {
+			PX4_ERR("could not open %s", _device_name);
+			return PX4_ERROR;
+
+		} else if (_uart_fd < 0 && _is_usb_uart) {
+			/* the config link is optional */
+			return PX4_OK;
+		}
+	}
+
 #if defined(MAVLINK_UDP)
 
 	/* init socket if necessary */

src/modules/mavlink/mavlink_main.h

@@ -164,8 +164,6 @@ public:
 
 	static void		forward_message(const mavlink_message_t *msg, Mavlink *self);
 
-	static int		get_uart_fd(unsigned index);
-
 	int			get_uart_fd() const { return _uart_fd; }
 
 	/**

src/modules/mavlink/mavlink_orb_subscription.cpp

@@ -42,7 +42,7 @@
 bool
 MavlinkOrbSubscription::is_published()
 {
-	const bool published = _sub.published();
+	const bool published = _sub.advertised();
 
 	if (published) {
 		return true;

src/modules/mavlink/mavlink_receiver.cpp

@@ -2632,8 +2632,8 @@ MavlinkReceiver::Run()
 				}
 			}
 
-			// only start accepting messages once we're sure who we talk to
+			// only start accepting messages on UDP once we're sure who we talk to
-			if (_mavlink->get_client_source_initialized()) {
+			if (_mavlink->get_protocol() != Protocol::UDP || _mavlink->get_client_source_initialized()) {
 #endif // MAVLINK_UDP
 
 				/* if read failed, this loop won't execute */

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -195,7 +195,6 @@ private:
 	WeatherVane *_wv_controller{nullptr};
 
 	perf_counter_t _cycle_perf;
-	perf_counter_t _interval_perf;
 
 	/**
 	 * Update our local parameter cache.
@@ -289,8 +288,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_vel_y_deriv(this, "VELD"),
 	_vel_z_deriv(this, "VELD"),
 	_control(this),
-	_cycle_perf(perf_alloc_once(PC_ELAPSED, MODULE_NAME": cycle time")),
+	_cycle_perf(perf_alloc_once(PC_ELAPSED, MODULE_NAME": cycle time"))
-	_interval_perf(perf_alloc_once(PC_INTERVAL, MODULE_NAME": interval"))
 {
 	// fetch initial parameter values
 	parameters_update(true);
@@ -306,7 +304,6 @@ MulticopterPositionControl::~MulticopterPositionControl()
 	}
 
 	perf_free(_cycle_perf);
-	perf_free(_interval_perf);
 }
 
 bool
@@ -515,7 +512,6 @@ MulticopterPositionControl::print_status()
 	}
 
 	perf_print_counter(_cycle_perf);
-	perf_print_counter(_interval_perf);
 
 	return 0;
 }
@@ -530,7 +526,6 @@ MulticopterPositionControl::Run()
 	}
 
 	perf_begin(_cycle_perf);
-	perf_count(_interval_perf);
 
 	if (_local_pos_sub.update(&_local_pos)) {
 
@@ -736,6 +731,11 @@ MulticopterPositionControl::Run()
 			q_sp.copyTo(_att_sp.q_d);
 			_att_sp.q_d_valid = true;
 			_att_sp.thrust_body[2] = 0.0f;
+
+			// reset the numerical derivatives to not generate d term spikes when coming from non-position controlled operation
+			_vel_x_deriv.reset();
+			_vel_y_deriv.reset();
+			_vel_z_deriv.reset();
 		}
 	}
 

src/modules/navigator/mission.cpp

@@ -174,18 +174,31 @@ Mission::on_activation()
 void
 Mission::on_active()
 {
+	if (_work_item_type == WORK_ITEM_TYPE_PRECISION_LAND) {
+		// switch out of precision land once landed
+		if (_navigator->get_land_detected()->landed) {
+			_navigator->get_precland()->on_inactivation();
+			_work_item_type = WORK_ITEM_TYPE_DEFAULT;
+
+		} else {
+			_navigator->get_precland()->on_active();
+		}
+	}
+
 	check_mission_valid(false);
 
 	/* check if anything has changed */
 	bool mission_sub_updated = _mission_sub.updated();
 
 	if (mission_sub_updated) {
+		_navigator->reset_triplets();
 		update_mission();
 	}
 
 	/* reset the current mission if needed */
 	if (need_to_reset_mission(true)) {
 		reset_mission(_mission);
+		_navigator->reset_triplets();
 		update_mission();
 		_navigator->reset_cruising_speed();
 		mission_sub_updated = true;
@@ -263,17 +276,6 @@ Mission::on_active()
 
 		do_abort_landing();
 	}
-
-	if (_work_item_type == WORK_ITEM_TYPE_PRECISION_LAND) {
-		// switch out of precision land once landed
-		if (_navigator->get_land_detected()->landed) {
-			_navigator->get_precland()->on_inactivation();
-			_work_item_type = WORK_ITEM_TYPE_DEFAULT;
-
-		} else {
-			_navigator->get_precland()->on_active();
-		}
-	}
 }
 
 bool
@@ -442,9 +444,6 @@ Mission::update_mission()
 
 	bool failed = true;
 
-	/* reset triplets */
-	_navigator->reset_triplets();
-
 	/* Reset vehicle_roi
 	 * Missions that do not explicitly configure ROI would not override
 	 * an existing ROI setting from previous missions */
@@ -805,7 +804,7 @@ Mission::set_mission_items()
 
 				/* move to land wp as fixed wing */
 				if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
-				    && _work_item_type == WORK_ITEM_TYPE_DEFAULT
+				    && (_work_item_type == WORK_ITEM_TYPE_DEFAULT || _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF)
 				    && new_work_item_type == WORK_ITEM_TYPE_DEFAULT
 				    && !_navigator->get_land_detected()->landed) {
 

src/modules/navigator/navigator_main.cpp

@@ -620,7 +620,6 @@ Navigator::run()
 		case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
 		case vehicle_status_s::NAVIGATION_STATE_STAB:
 		default:
-			_pos_sp_triplet_published_invalid_once = false;
 			navigation_mode_new = nullptr;
 			_can_loiter_at_sp = false;
 			break;
@@ -720,16 +719,8 @@ Navigator::print_status()
 void
 Navigator::publish_position_setpoint_triplet()
 {
-	// do not publish an invalid setpoint
-	if (!_pos_sp_triplet.current.valid) {
-		return;
-	}
-
 	_pos_sp_triplet.timestamp = hrt_absolute_time();
-
-	/* lazily publish the position setpoint triplet only once available */
 	_pos_sp_triplet_pub.publish(_pos_sp_triplet);
-
 	_pos_sp_triplet_updated = false;
 }
 

src/modules/sensors/parameters.cpp

@@ -159,6 +159,9 @@ void initialize_parameter_handles(ParameterHandles &parameter_handles)
 	// We do a param_find here to force them into the list.
 	(void)param_find("RC_CHAN_CNT");
 
+	(void)param_find("BAT_V_DIV");
+	(void)param_find("BAT_A_PER_V");
+
 	(void)param_find("CAL_ACC0_ID");
 	(void)param_find("CAL_GYRO0_ID");
 

src/modules/sensors/sensor_params_battery.c

@@ -0,0 +1,57 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Battery voltage divider (V divider)
+ *
+ * This is the divider from battery voltage to 3.3V ADC voltage.
+ * If using e.g. Mauch power modules the value from the datasheet
+ * can be applied straight here. A value of -1 means to use
+ * the board default.
+ *
+ * @group Battery Calibration
+ * @decimal 8
+ */
+PARAM_DEFINE_FLOAT(BAT_V_DIV, -1.0);
+
+/**
+ * Battery current per volt (A/V)
+ *
+ * The voltage seen by the 3.3V ADC multiplied by this factor
+ * will determine the battery current. A value of -1 means to use
+ * the board default.
+ *
+ * @group Battery Calibration
+ * @decimal 8
+ */
+PARAM_DEFINE_FLOAT(BAT_A_PER_V, -1.0);

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp

@@ -40,18 +40,13 @@ using namespace time_literals;
 
 VehicleAcceleration::VehicleAcceleration() :
 	ModuleParams(nullptr),
-	WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
+	WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl)
-	_cycle_perf(perf_alloc(PC_ELAPSED, "vehicle_acceleration: cycle time")),
-	_sensor_latency_perf(perf_alloc(PC_ELAPSED, "vehicle_acceleration: sensor latency"))
 {
 }
 
 VehicleAcceleration::~VehicleAcceleration()
 {
 	Stop();
-
-	perf_free(_cycle_perf);
-	perf_free(_sensor_latency_perf);
 }
 
 bool
@@ -175,16 +170,12 @@ VehicleAcceleration::ParametersUpdate(bool force)
 void
 VehicleAcceleration::Run()
 {
-	perf_begin(_cycle_perf);
-
 	// update corrections first to set _selected_sensor
 	SensorCorrectionsUpdate();
 
 	sensor_accel_s sensor_data;
 
 	if (_sensor_sub[_selected_sensor].update(&sensor_data)) {
-		perf_set_elapsed(_sensor_latency_perf, hrt_elapsed_time(&sensor_data.timestamp));
-
 		ParametersUpdate();
 		SensorBiasUpdate();
 
@@ -207,15 +198,10 @@ VehicleAcceleration::Run()
 
 		_vehicle_acceleration_pub.publish(out);
 	}
-
-	perf_end(_cycle_perf);
 }
 
 void
 VehicleAcceleration::PrintStatus()
 {
 	PX4_INFO("selected sensor: %d", _selected_sensor);
-
-	perf_print_counter(_cycle_perf);
-	perf_print_counter(_sensor_latency_perf);
 }

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp

@@ -36,7 +36,6 @@
 #include <lib/conversion/rotation.h>
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
-#include <lib/perf/perf_counter.h>
 #include <px4_config.h>
 #include <px4_log.h>
 #include <px4_module_params.h>
@@ -101,9 +100,6 @@ private:
 	matrix::Vector3f			_scale;
 	matrix::Vector3f			_bias;
 
-	perf_counter_t				_cycle_perf;
-	perf_counter_t				_sensor_latency_perf;
-
 	uint8_t					_selected_sensor{0};
 
 };

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -40,18 +40,13 @@ using namespace time_literals;
 
 VehicleAngularVelocity::VehicleAngularVelocity() :
 	ModuleParams(nullptr),
-	WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
+	WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
-	_cycle_perf(perf_alloc(PC_ELAPSED, "vehicle_angular_velocity: cycle time")),
-	_sensor_latency_perf(perf_alloc(PC_ELAPSED, "vehicle_angular_velocity: sensor latency"))
 {
 }
 
 VehicleAngularVelocity::~VehicleAngularVelocity()
 {
 	Stop();
-
-	perf_free(_cycle_perf);
-	perf_free(_sensor_latency_perf);
 }
 
 bool
@@ -214,8 +209,6 @@ VehicleAngularVelocity::ParametersUpdate(bool force)
 void
 VehicleAngularVelocity::Run()
 {
-	perf_begin(_cycle_perf);
-
 	// update corrections first to set _selected_sensor
 	SensorCorrectionsUpdate();
 
@@ -224,8 +217,6 @@ VehicleAngularVelocity::Run()
 		sensor_gyro_control_s sensor_data;
 
 		if (_sensor_control_sub[_selected_sensor].update(&sensor_data)) {
-			perf_set_elapsed(_sensor_latency_perf, hrt_elapsed_time(&sensor_data.timestamp));
-
 			ParametersUpdate();
 			SensorBiasUpdate();
 
@@ -251,8 +242,6 @@ VehicleAngularVelocity::Run()
 		sensor_gyro_s sensor_data;
 
 		if (_sensor_sub[_selected_sensor].update(&sensor_data)) {
-			perf_set_elapsed(_sensor_latency_perf, hrt_elapsed_time(&sensor_data.timestamp));
-
 			ParametersUpdate();
 			SensorBiasUpdate();
 
@@ -276,8 +265,6 @@ VehicleAngularVelocity::Run()
 			_vehicle_angular_velocity_pub.publish(angular_velocity);
 		}
 	}
-
-	perf_end(_cycle_perf);
 }
 
 void
@@ -291,7 +278,4 @@ VehicleAngularVelocity::PrintStatus()
 	} else {
 		PX4_WARN("sensor_gyro_control unavailable for selected sensor: %d (%d)", _selected_sensor_device_id,  _selected_sensor);
 	}
-
-	perf_print_counter(_cycle_perf);
-	perf_print_counter(_sensor_latency_perf);
 }

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -36,7 +36,6 @@
 #include <lib/conversion/rotation.h>
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
-#include <lib/perf/perf_counter.h>
 #include <px4_config.h>
 #include <px4_log.h>
 #include <px4_module_params.h>
@@ -109,9 +108,6 @@ private:
 	matrix::Vector3f			_scale;
 	matrix::Vector3f			_bias;
 
-	perf_counter_t				_cycle_perf;
-	perf_counter_t				_sensor_latency_perf;
-
 	uint32_t				_selected_sensor_device_id{0};
 	uint8_t					_selected_sensor{0};
 	uint8_t					_selected_sensor_control{0};

src/modules/uORB/Subscription.cpp

@@ -116,7 +116,7 @@ Subscription::init()
 bool
 Subscription::update(uint64_t *time, void *dst)
 {
-	if ((time != nullptr) && (dst != nullptr) && published()) {
+	if ((time != nullptr) && (dst != nullptr) && advertised()) {
 		// always copy data to dst regardless of update
 		const uint64_t t = _node->copy_and_get_timestamp(dst, _last_generation);
 

src/modules/uORB/Subscription.hpp

@@ -75,17 +75,17 @@ public:
 	void unsubscribe();
 
 	bool valid() const { return _node != nullptr; }
-	bool published()
+	bool advertised()
 	{
 		if (valid()) {
-			return _node->is_published();
+			return _node->is_advertised();
 		}
 
 		// try to initialize
 		if (init()) {
 			// check again if valid
 			if (valid()) {
-				return _node->is_published();
+				return _node->is_advertised();
 			}
 		}
 
@@ -95,7 +95,7 @@ public:
 	/**
 	 * Check if there is a new update.
 	 * */
-	bool updated() { return published() ? (_node->published_message_count() != _last_generation) : false; }
+	bool updated() { return advertised() ? (_node->published_message_count() != _last_generation) : false; }
 
 	/**
 	 * Update the struct
@@ -118,7 +118,7 @@ public:
 	 * Copy the struct
 	 * @param data The uORB message struct we are updating.
 	 */
-	bool copy(void *dst) { return published() ? _node->copy(dst, _last_generation) : false; }
+	bool copy(void *dst) { return advertised() ? _node->copy(dst, _last_generation) : false; }
 
 	uint8_t		get_instance() const { return _instance; }
 	orb_id_t	get_topic() const { return _meta; }

src/modules/uORB/SubscriptionInterval.hpp

@@ -73,14 +73,14 @@ public:
 
 	bool subscribe() { return _subscription.subscribe(); }
 
-	bool published() { return _subscription.published(); }
+	bool advertised() { return _subscription.advertised(); }
 
 	/**
 	 * Check if there is a new update.
 	 * */
 	bool updated()
 	{
-		if (published() && (hrt_elapsed_time(&_last_update) >= _interval_us)) {
+		if (advertised() && (hrt_elapsed_time(&_last_update) >= _interval_us)) {
 			return _subscription.updated();
 		}
 

src/modules/uORB/uORBDeviceMaster.cpp

@@ -55,7 +55,7 @@ uORB::DeviceMaster::~DeviceMaster()
 }
 
 int
-uORB::DeviceMaster::advertise(const struct orb_metadata *meta, int *instance, int priority)
+uORB::DeviceMaster::advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance, int priority)
 {
 	int ret = PX4_ERROR;
 
@@ -119,22 +119,45 @@ uORB::DeviceMaster::advertise(const struct orb_metadata *meta, int *instance, in
 			delete node;
 
 			if (ret == -EEXIST) {
-				/* if the node exists already, get the existing one and check if
+				/* if the node exists already, get the existing one and check if it's advertised. */
-				 * something has been published yet. */
 				uORB::DeviceNode *existing_node = getDeviceNodeLocked(meta, group_tries);
 
-				if ((existing_node != nullptr) && !(existing_node->is_published())) {
+				/*
-					/* nothing has been published yet, lets claim it */
+				 * We can claim an existing node in these cases:
-					existing_node->set_priority(priority);
+				 * - The node is not advertised (yet). It means there is already one or more subscribers or it was
+				 *   unadvertised.
+				 * - We are a single-instance advertiser requesting the first instance.
+				 *   (Usually we don't end up here, but we might in case of a race condition between 2
+				 *   advertisers).
+				 * - We are a subscriber requesting a certain instance.
+				 *   (Also we usually don't end up in that case, but we might in case of a race condtion
+				 *   between an advertiser and subscriber).
+				 */
+				bool is_single_instance_advertiser = is_advertiser && !instance;
+
+				if (existing_node != nullptr &&
+				    (!existing_node->is_advertised() || is_single_instance_advertiser || !is_advertiser)) {
+					if (is_advertiser) {
+						existing_node->set_priority(priority);
+						/* Set as advertised to avoid race conditions (otherwise 2 multi-instance advertisers
+						 * could get the same instance).
+						 */
+						existing_node->mark_as_advertised();
+					}
+
 					ret = PX4_OK;
 
 				} else {
-					/* otherwise: data has already been published, keep looking */
+					/* otherwise: already advertised, keep looking */
 				}
 			}
 
 		} else {
-			// add to the node map;.
+			if (is_advertiser) {
+				node->mark_as_advertised();
+			}
+
+			// add to the node map.
 			_node_list.add(node);
 		}
 

src/modules/uORB/uORBDeviceMaster.hpp

@@ -60,7 +60,7 @@ class uORB::DeviceMaster
 {
 public:
 
-	int advertise(const struct orb_metadata *meta, int *instance, int priority);
+	int advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance, int priority);
 
 	/**
 	 * Public interface for getDeviceNodeLocked(). Takes care of synchronization.

src/modules/uORB/uORBDeviceNode.cpp

@@ -76,35 +76,15 @@ uORB::DeviceNode::~DeviceNode()
 int
 uORB::DeviceNode::open(cdev::file_t *filp)
 {
-	int ret;
-
 	/* is this a publisher? */
 	if (filp->f_oflags == PX4_F_WRONLY) {
 
-		/* become the publisher if we can */
 		lock();
-
+		mark_as_advertised();
-		if (_publisher == 0) {
-			_publisher = px4_getpid();
-			ret = PX4_OK;
-
-		} else {
-			ret = -EBUSY;
-		}
-
 		unlock();
 
 		/* now complete the open */
-		if (ret == PX4_OK) {
+		return CDev::open(filp);
-			ret = CDev::open(filp);
-
-			/* open failed - not the publisher anymore */
-			if (ret != PX4_OK) {
-				_publisher = 0;
-			}
-		}
-
-		return ret;
 	}
 
 	/* is this a new subscriber? */
@@ -123,7 +103,7 @@ uORB::DeviceNode::open(cdev::file_t *filp)
 
 		filp->f_priv = (void *)sd;
 
-		ret = CDev::open(filp);
+		int ret = CDev::open(filp);
 
 		add_internal_subscriber();
 
@@ -146,11 +126,7 @@ uORB::DeviceNode::open(cdev::file_t *filp)
 int
 uORB::DeviceNode::close(cdev::file_t *filp)
 {
-	/* is this the publisher closing? */
+	if (filp->f_oflags == PX4_F_RDONLY) { /* subscriber */
-	if (px4_getpid() == _publisher) {
-		_publisher = 0;
-
-	} else {
 		SubscriberData *sd = filp_to_sd(filp);
 
 		if (sd != nullptr) {
@@ -170,14 +146,15 @@ uORB::DeviceNode::copy_locked(void *dst, unsigned &generation)
 	bool updated = false;
 
 	if ((dst != nullptr) && (_data != nullptr)) {
+		unsigned current_generation = _generation.load();
 
-		if (_generation > generation + _queue_size) {
+		if (current_generation > generation + _queue_size) {
 			// Reader is too far behind: some messages are lost
-			_lost_messages += _generation - (generation + _queue_size);
+			_lost_messages += current_generation - (generation + _queue_size);
-			generation = _generation - _queue_size;
+			generation = current_generation - _queue_size;
 		}
 
-		if ((_generation == generation) && (generation > 0)) {
+		if ((current_generation == generation) && (generation > 0)) {
 			/* The subscriber already read the latest message, but nothing new was published yet.
 			 * Return the previous message
 			 */
@@ -186,7 +163,7 @@ uORB::DeviceNode::copy_locked(void *dst, unsigned &generation)
 
 		memcpy(dst, _data + (_meta->o_size * (generation % _queue_size)), _meta->o_size);
 
-		if (generation < _generation) {
+		if (generation < current_generation) {
 			++generation;
 		}
 
@@ -299,14 +276,14 @@ uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen)
 
 	/* Perform an atomic copy. */
 	ATOMIC_ENTER;
-	memcpy(_data + (_meta->o_size * (_generation % _queue_size)), buffer, _meta->o_size);
+	/* wrap-around happens after ~49 days, assuming a publisher rate of 1 kHz */
+	unsigned generation = _generation.fetch_add(1);
+
+	memcpy(_data + (_meta->o_size * (generation % _queue_size)), buffer, _meta->o_size);
 
 	/* update the timestamp and generation count */
 	_last_update = hrt_absolute_time();
-	/* wrap-around happens after ~49 days, assuming a publisher rate of 1 kHz */
-	_generation++;
 
-	_published = true;
 
 	// callbacks
 	for (auto item : _callbacks) {
@@ -379,10 +356,12 @@ uORB::DeviceNode::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
 		*(int *)arg = get_priority();
 		return PX4_OK;
 
-	case ORBIOCSETQUEUESIZE:
+	case ORBIOCSETQUEUESIZE: {
-		//no need for locking here, since this is used only during the advertisement call,
+			lock();
-		//and only one advertiser is allowed to open the DeviceNode at the same time.
+			int ret = update_queue_size(arg);
-		return update_queue_size(arg);
+			unlock();
+			return ret;
+		}
 
 	case ORBIOCGETINTERVAL:
 		if (sd->update_interval) {
@@ -394,8 +373,8 @@ uORB::DeviceNode::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
 
 		return OK;
 
-	case ORBIOCISPUBLISHED:
+	case ORBIOCISADVERTISED:
-		*(unsigned long *)arg = _published;
+		*(unsigned long *)arg = _advertised;
 
 		return OK;
 
@@ -473,7 +452,7 @@ int uORB::DeviceNode::unadvertise(orb_advert_t handle)
 	 * of subscribers and publishers. But we also do not have a leak since future
 	 * publishers reuse the same DeviceNode object.
 	 */
-	devnode->_published = false;
+	devnode->_advertised = false;
 
 	return PX4_OK;
 }

src/modules/uORB/uORBDeviceNode.hpp

@@ -39,6 +39,7 @@
 #include <lib/cdev/CDev.hpp>
 
 #include <containers/List.hpp>
+#include <px4_atomic.h>
 
 namespace uORB
 {
@@ -157,11 +158,13 @@ public:
 	void remove_internal_subscriber();
 
 	/**
-	 * Return true if this topic has been published.
+	 * Return true if this topic has been advertised.
 	 *
 	 * This is used in the case of multi_pub/sub to check if it's valid to advertise
 	 * and publish to this node or if another node should be tried. */
-	bool is_published() const { return _published; }
+	bool is_advertised() const { return _advertised; }
+
+	void mark_as_advertised() { _advertised = true; }
 
 	/**
 	 * Try to change the size of the queue. This can only be done as long as nobody published yet.
@@ -185,7 +188,7 @@ public:
 
 	uint32_t lost_message_count() const { return _lost_messages; }
 
-	unsigned published_message_count() const { return _generation; }
+	unsigned published_message_count() const { return _generation.load(); }
 
 	const orb_metadata *get_meta() const { return _meta; }
 
@@ -267,16 +270,13 @@ private:
 	const uint8_t _instance; /**< orb multi instance identifier */
 	uint8_t     *_data{nullptr};   /**< allocated object buffer */
 	hrt_abstime   _last_update{0}; /**< time the object was last updated */
-	volatile unsigned   _generation{0};  /**< object generation count */
+	px4::atomic<unsigned>  _generation{0};  /**< object generation count */
 	List<uORB::SubscriptionCallback *>	_callbacks;
 	uint8_t   _priority;  /**< priority of the topic */
-	bool _published{false};  /**< has ever data been published */
+	bool _advertised{false};  /**< has ever been advertised (not necessarily published data yet) */
 	uint8_t _queue_size; /**< maximum number of elements in the queue */
 	int8_t _subscriber_count{0};
 
-	px4_task_t _publisher{0}; /**< if nonzero, current publisher. Only used inside the advertise call.
-						We allow one publisher to have an open file descriptor at the same time. */
-
 	// statistics
 	uint32_t _lost_messages = 0; /**< nr of lost messages for all subscribers. If two subscribers lose the same
 					message, it is counted as two. */

src/modules/uORB/uORBManager.cpp

@@ -116,7 +116,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
 		uORB::DeviceNode *node = _device_master->getDeviceNode(meta, instance);
 
 		if (node != nullptr) {
-			if (node->is_published()) {
+			if (node->is_advertised()) {
 				return PX4_OK;
 			}
 		}
@@ -150,10 +150,10 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
 		int fd = px4_open(path, 0);
 
 		if (fd >= 0) {
-			unsigned long is_published;
+			unsigned long is_advertised;
 
-			if (px4_ioctl(fd, ORBIOCISPUBLISHED, (unsigned long)&is_published) == 0) {
+			if (px4_ioctl(fd, ORBIOCISADVERTISED, (unsigned long)&is_advertised) == 0) {
-				if (!is_published) {
+				if (!is_advertised) {
 					ret = PX4_ERROR;
 				}
 			}
@@ -327,14 +327,12 @@ int uORB::Manager::orb_get_interval(int handle, unsigned *interval)
 	return ret;
 }
 
-int uORB::Manager::node_advertise(const struct orb_metadata *meta, int *instance, int priority)
+int uORB::Manager::node_advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance, int priority)
 {
 	int ret = PX4_ERROR;
 
-	/* fill advertiser data */
-
 	if (get_device_master()) {
-		ret = _device_master->advertise(meta, instance, priority);
+		ret = _device_master->advertise(meta, is_advertiser, instance, priority);
 	}
 
 	/* it's PX4_OK if it already exists */
@@ -384,7 +382,7 @@ int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, i
 	if (fd < 0) {
 
 		/* try to create the node */
-		ret = node_advertise(meta, instance, priority);
+		ret = node_advertise(meta, advertiser, instance, priority);
 
 		if (ret == PX4_OK) {
 			/* update the path, as it might have been updated during the node_advertise call */
@@ -396,7 +394,7 @@ int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, i
 			}
 		}
 
-		/* on success, try the open again */
+		/* on success, try to open again */
 		if (ret == PX4_OK) {
 			fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY);
 		}

src/modules/uORB/uORBManager.hpp

@@ -392,11 +392,9 @@ private: // class methods
 	/**
 	 * Advertise a node; don't consider it an error if the node has
 	 * already been advertised.
-	 *
-	 * @todo verify that the existing node is the same as the one
-	 *       we tried to advertise.
 	 */
-	int node_advertise(const struct orb_metadata *meta, int *instance = nullptr, int priority = ORB_PRIO_DEFAULT);
+	int node_advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance = nullptr,
+			   int priority = ORB_PRIO_DEFAULT);
 
 	/**
 	 * Common implementation for orb_advertise and orb_subscribe.

src/modules/vmount/output.cpp

@@ -99,11 +99,6 @@ void OutputBase::publish()
 		mount_orientation.attitude_euler_angle[i] = _angle_outputs[i];
 	}
 
-	//PX4_INFO("roll: %.2f, pitch: %.2f, yaw: %.2f",
-	//		(double)_angle_outputs[0],
-	//		(double)_angle_outputs[1],
-	//		(double)_angle_outputs[2]);
-
 	orb_publish_auto(ORB_ID(mount_orientation), &_mount_orientation_pub, &mount_orientation, &instance, ORB_PRIO_DEFAULT);
 }
 

src/modules/vmount/output_mavlink.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2016-2019 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -91,6 +91,7 @@ int OutputMavlink::update(const ControlData *control_data)
 	vehicle_command.param1 = (_angle_outputs[1] + _config.pitch_offset) * M_RAD_TO_DEG_F;
 	vehicle_command.param2 = (_angle_outputs[0] + _config.roll_offset) * M_RAD_TO_DEG_F;
 	vehicle_command.param3 = (_angle_outputs[2] + _config.yaw_offset) * M_RAD_TO_DEG_F;
+	vehicle_command.param7 = 2.0f; // MAV_MOUNT_MODE_MAVLINK_TARGETING;
 
 	_vehicle_command_pub.publish(vehicle_command);
 

src/modules/vmount/vmount.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -431,12 +431,21 @@ int vmount_main(int argc, char *argv[])
 		return -1;
 	}
 
-	if (!strcmp(argv[1], "start") || !strcmp(argv[1], "test")) {
+	const bool found_start = !strcmp(argv[1], "start");
+	const bool found_test = !strcmp(argv[1], "test");
+
+	if (found_start || found_test) {
 
 		/* this is not an error */
 		if (thread_running) {
-			PX4_WARN("mount driver already running");
+			if (found_start) {
-			return 0;
+				PX4_WARN("mount driver already running");
+				return 0;
+
+			} else {
+				PX4_WARN("mount driver already running, run vmount stop before 'vmount test'");
+				return 1;
+			}
 		}
 
 		thread_should_exit = false;

src/systemcmds/topic_listener/generate_listener.py

@@ -119,9 +119,9 @@ print("""
 
 for index, (m, t) in enumerate(zip(messages, topics)):
 	if index == 0:
-		print("\tif (strncmp(topic_name,\"%s\", %d) == 0) {" % (t, len(t)))
+		print("\tif (strcmp(topic_name,\"%s\") == 0) {" % (t))
 	else:
-		print("\t} else if (strncmp(topic_name,\"%s\", %d) == 0) {" % (t, len(t)))
+		print("\t} else if (strcmp(topic_name,\"%s\") == 0) {" % (t))
 	print("\t\tlistener(listener_print_topic<%s_s>, ORB_ID(%s), num_msgs, topic_instance, topic_interval);" % (m, t))
 
 print("\t} else {")