Reposition landing gear check so that it is not overwritten by setpoint operations. (#13412 )

smbus: fix invalid memory access in read_word()
read_word() expected 3 bytes (uint16_t + PEC byte), but was passed an address to an uint16_t value. write_word() is changed to be type-safe as well.
2026-05-26 20:30:05 +08:00 · 2019-11-07 14:35:05 -05:00 · 2019-11-07 10:02:42 -05:00 · 2019-11-06 11:41:42 +01:00 · 2019-11-05 12:48:29 -05:00 · 2019-11-05 09:07:19 -05:00
64 changed files with 877 additions and 362 deletions
@@ -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
@@ -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
@@ -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')
@@ -9,10 +9,10 @@ pipeline {
        script {
          def build_nodes = [:]
          def docker_images = [
-            armhf: "px4io/px4-dev-armhf:2019-10-04",
-            base: "px4io/px4-dev-base-bionic:2019-10-04",
-            nuttx: "px4io/px4-dev-nuttx:2019-10-04",
-            rpi: "px4io/px4-dev-raspi:2019-10-04",
+            armhf: "px4io/px4-dev-armhf:2019-10-24",
+            base: "px4io/px4-dev-base-bionic:2019-10-24",
+            nuttx: "px4io/px4-dev-nuttx:2019-10-24",
+            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()
@@ -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'
            }
          }
@@ -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:
@@ -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
@@ -51,11 +51,11 @@ CONFIG:
      buildType: MinSizeRel
      settings:
        CONFIG: intel_aerofc-v1_default
-    modalai_fc1_default:
-      short: modalai_fc1_default
+    modalai_fc-v1_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
@@ -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')
@@ -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

@@ -15,7 +15,7 @@ ekf2 start
 #
 fw_att_control start
 fw_pos_control_l1 start
-airspeed_selector start
+# airspeed_selector start
 #
 # Start Land Detector.
 #
@@ -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
@@ -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
-			param reset_nostart RC* COM_FLTMODE* LND_FLIGHT_T_* TC_* CAL_ACC* CAL_GYRO*
+			# 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* COM_FLIGHT_UUID
 		fi

 		set AUTOCNF yes
@@ -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
@@ -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
@@ -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
@@ -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

 /****************************************************************************************************
@@ -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,
@@ -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)
@@ -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) {
@@ -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
@@ -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 tmp = 0;
+	uint16_t serial_num;
+	result = _interface->read_word(BATT_SMBUS_SERIAL_NUMBER, serial_num);

-	result = _interface->read_word(BATT_SMBUS_SERIAL_NUMBER, &tmp);
-	uint16_t serial_num = tmp;
+	uint16_t remaining_cap;
+	result |= _interface->read_word(BATT_SMBUS_REMAINING_CAPACITY, remaining_cap);

-	result |= _interface->read_word(BATT_SMBUS_REMAINING_CAPACITY, &tmp);
-	uint16_t remaining_cap = tmp;
+	uint16_t cycle_count;
+	result |= _interface->read_word(BATT_SMBUS_CYCLE_COUNT, cycle_count);

-	result |= _interface->read_word(BATT_SMBUS_CYCLE_COUNT, &tmp);
-	uint16_t cycle_count = tmp;
-
-	result |= _interface->read_word(BATT_SMBUS_FULL_CHARGE_CAPACITY, &tmp);
-	uint16_t full_cap = tmp;
+	uint16_t full_cap;
+	result |= _interface->read_word(BATT_SMBUS_FULL_CHARGE_CAPACITY, full_cap);

 	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;
-	uint8_t code = BATT_SMBUS_MANUFACTURE_DATE;
-
-	int result = _interface->read_word(code, &date);
+	uint16_t date;
+	int result = _interface->read_word(BATT_SMBUS_MANUFACTURE_DATE, 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;
 }
@@ -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;

@@ -39,4 +39,6 @@ px4_add_module(
 		cm8jl65_main.cpp
 	MODULE_CONFIG
 		module.yaml
+	DEPENDS
+		drivers_rangefinder
 	)
@@ -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",
@@ -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_interval_perf(perf_alloc(PC_INTERVAL, "dshot: cycle interval"))
+	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
 {
 	_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);
@@ -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_interval_perf(perf_alloc(PC_INTERVAL, "px4fmu: cycle interval"))
+	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
 {
 	_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;
@@ -40,6 +40,7 @@

 #include <px4_config.h>
 #include <px4_tasks.h>
+#include <px4_sem.hpp>

 #include <sys/types.h>
 #include <stdint.h>
@@ -2333,6 +2334,7 @@ PX4IO::print_status(bool extended_status)
 int
 PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 {
+	SmartLock lock_guard(_lock);
 	int ret = OK;

 	/* regular ioctl? */
@@ -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;
@@ -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];

@@ -80,7 +80,7 @@ public:
 		List<T>::clear();
 	}

-	LockGuard getLockGuard() { return LockGuard{_mutex}; }
+	pthread_mutex_t &mutex() { return _mutex; }

 private:

@@ -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);
@@ -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(1) = _constrainAbs(vel_sp_xy(1), vel_min_xy(1), vel_max_xy(1));
+			vel_sp_constrained_xy(0) = _constrainAbsPrioritizeMin(vel_sp_xy(0), vel_min_xy(0), vel_max_xy(0));
+			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(.)
@@ -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 */
-	inline float _constrainAbs(float val, float min, float max); /**< Constrain absolute value of val between min and max */
+	static float _constrainOneSide(float val, float constraint); /**< Constrain val between INF and constraint */

-	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();
@@ -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;

@@ -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;

@@ -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
+	)
@@ -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];
-		full_data_packet[0] = addr | 0x00;
-		full_data_packet[1] = cmd_code;
-		full_data_packet[2] = addr | 0x01;
-		memcpy(&full_data_packet[3], data, 2);
+		buf[0] = addr | 0x00;
+		buf[1] = cmd_code;
+		buf[2] = addr | 0x01;

-		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[3] = ((uint8_t *)data)[1];
+	buf[2] = data & 0xff;
+	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;
-}
+}
@@ -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);

@@ -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;
 		}
@@ -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'
@@ -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
@@ -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.
 *
@@ -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;
@@ -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
 	)
@@ -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)
@@ -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
+};
@@ -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();
+}
@@ -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;
+};
@@ -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));
+	}
+}
@@ -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.z_valid = !_preflt_vert_fail;
-				lpos.v_xy_valid = _ekf.local_position_is_valid() && !_preflt_horiz_fail;
-				lpos.v_z_valid = !_preflt_vert_fail;
+				lpos.xy_valid = _ekf.local_position_is_valid() && !_preflt_checker.hasHorizFailed();
+				lpos.z_valid = !_preflt_checker.hasVertFailed();
+				lpos.v_xy_valid = _ekf.local_position_is_valid() && !_preflt_checker.hasHorizFailed();
+				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 alpha = constrain(sensors.accelerometer_integral_dt / 1.e6f * _innov_lpf_tau_inv, 0.0f, 1.0f);
-					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;
+					float dt_seconds = sensors.accelerometer_integral_dt * 1e-6f;
+					runPreFlightChecks(dt_seconds, control_status, _vehicle_status, innovations);

 				} else {
-					_vel_ne_innov_lpf.zero();
-					_vel_d_innov_lpf = 0.0f;
-					_hgt_innov_lpf = 0.0f;
-					_preflt_horiz_fail = false;
-					_preflt_vert_fail = false;
-					_preflt_fail = false;
+					resetPreFlightChecks();
 				}

 				_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()
@@ -2632,8 +2632,8 @@ MavlinkReceiver::Run()
 				}
 			}

-			// only start accepting messages once we're sure who we talk to
-			if (_mavlink->get_client_source_initialized()) {
+			// only start accepting messages on UDP once we're sure who we talk to
+			if (_mavlink->get_protocol() != Protocol::UDP || _mavlink->get_client_source_initialized()) {
 #endif // MAVLINK_UDP

 				/* if read failed, this loop won't execute */
@@ -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")),
-	_interval_perf(perf_alloc_once(PC_INTERVAL, MODULE_NAME": interval"))
+	_cycle_perf(perf_alloc_once(PC_ELAPSED, MODULE_NAME": cycle time"))
 {
 	// 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)) {

@@ -174,6 +174,17 @@ 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 */
@@ -263,17 +274,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
@@ -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");

@@ -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);
@@ -40,18 +40,13 @@ using namespace time_literals;

 VehicleAcceleration::VehicleAcceleration() :
 	ModuleParams(nullptr),
-	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"))
+	WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl)
 {
 }

 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);
 }
@@ -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};

 };
@@ -40,18 +40,13 @@ using namespace time_literals;

 VehicleAngularVelocity::VehicleAngularVelocity() :
 	ModuleParams(nullptr),
-	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"))
+	WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
 {
 }

 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);
 }
@@ -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};
@@ -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 {")
Author	SHA1	Message	Date
Matthias Grob	748b664ad0	Reposition landing gear check so that it is not overwritten by setpoint operations. (#13412 )	2019-11-07 14:35:05 -05:00
Beat Küng	37cc877c80	smbus: fix invalid memory access in read_word() read_word() expected 3 bytes (uint16_t + PEC byte), but was passed an address to an uint16_t value. write_word() is changed to be type-safe as well.	2019-11-07 10:02:42 -05:00
Silvan Fuhrer	a124664b80	disable airspeed selector module startup for 1.10 Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2019-11-06 11:41:42 +01:00
Julian Oes	c8886ee32f	mixer_module: fix poll error in SITL lockstep This fixes the case where the mixer_module would subscribe and use its own test_motor publication which was created only to make sure the topic is advertised and subsequent updates will work properly. This happened in SITL lockstep because the timestamp would be 0 at the very beginning, and hence elapsed time would be 0 as well. This lead to an actuator publication which would then get lockstep out of sync causing poll errors on the Gazebo side.	2019-11-05 12:48:29 -05:00
Julian Oes	3c8685742e	Removed lidar lite reset after intialization	2019-11-05 09:07:19 -05:00
Beat Küng	23334df1e5	BlockingList: fix unsafe getLockGuard() API getLockGuard relies on copy elision to work correctly, which the compiler is not required to do (only with C++17). If no copy elision happens, the mutex ends up being unlocked twice, and the CS is executed with the mutex unlocked. The patch also ensures that the same pattern cannot be used again.	2019-11-05 14:19:10 +01:00
David Sidrane	5c6d16ca27	NuttX/nuttx with SDIO fixes (#13311 ) * NuttX/nuttx with SDIO fixes	2019-10-31 13:42:22 -04:00
Daniel Agar	5bd5fa34fa	sensors: own BAT_V_DIV and BAT_A_PER_V params (#13299 ) - this is currently necessary for the QGC power setup gui, but should be reverted in the future - fixes #13292	2019-10-28 16:52:42 -04:00
Julian Oes	24143a9fcf	mavlink: only ignore messages on UDP before init We should still accept messages arriving over serial.	2019-10-28 16:52:42 -04:00
Julian Oes	469ab9dce6	mavlink: accept msgs without source initialized I don't understand why we should wait to parse incoming MAVLink traffic just because we don't have the source address initialized. We still check the source address before doing a sendto. This should fix serial MAVLink communication on FMU5x where both serial and UDP is available. There the serial connection previously did not work because nothing was connected over UDP.	2019-10-28 16:52:42 -04:00
Beat Küng	d5f003eed5	px4io: add missing lock()/unlock() in ioctl	2019-10-28 16:04:11 -04:00
bresch	6535d5123e	AutoLineSmoothVel: fix constrain priority for autocontinue. The constrainAbs function was not prioritizing the minimum value that produces the autocontinue behaviour. This caused zig-zag paths when the waypoints were almost -but not exactly- aligned.	2019-10-28 16:04:11 -04:00
PX4 BuildBot	0d895f2a0a	Update submodule sitl_gazebo to latest Fri Oct 25 12:40:00 UTC 2019 - sitl_gazebo in PX4/Firmware (`ffefd458be`): https://github.com/PX4/sitl_gazebo/commit/c0634b241eb730cee2ef91e5f5298427a4a328c8 - sitl_gazebo current upstream: https://github.com/PX4/sitl_gazebo/commit/169d48217df89922e9fae72ef34fa46ce2e209dd - Changes: https://github.com/PX4/sitl_gazebo/compare/c0634b241eb730cee2ef91e5f5298427a4a328c8...169d48217df89922e9fae72ef34fa46ce2e209dd 169d482 2019-10-22 Julian Oes - travis: let's not bother about macOS Sierra 78fef51 2019-10-22 Julian Oes - travis: install gstreamer using brew 764d1b7 2019-10-21 Julian Oes - models: fix reported validation errors 053622b 2019-10-11 TSC21 - Travis CI: Cleanup scripts and add more MacOSX build pipelines 0c8214c 2019-10-21 Julian Oes - cmake: fix Qt prefix path for macOS bb2c08c 2019-10-06 TSC21 - CMake: fix Qt5 find in MacOS 81f072b 2019-10-14 Julian Oes - travis: install GStreamer using brew 39d4399 2019-10-14 Julian Oes - cmake: link to glib-2.0 and gobject-2.0 fef3ff5 2019-10-14 Julian Oes - cmake: fix GStreamer and Qt deps for macOS 605da22 2019-10-22 Nuno Marques - Update README.md 2a8a54e 2019-10-20 Nuno Marques - README: update install instructions	2019-10-28 16:04:11 -04:00
Jacob Crabill	940ce5b2d6	UAVCAN: Optical Flow: Bug fix (missing integration_timespan field in optical_flow topic). (#13257 )	2019-10-28 16:04:11 -04:00
JaeyoungLim	9771bac8e8	Fix typhoon h480 parameter (#13263 )	2019-10-28 16:04:11 -04:00
Dusan Zivkovic	9e3775515e	mission: ensure precland::on_inactivation() is called once landed	2019-10-28 16:04:11 -04:00
Hamish Willee	8e9dc60eaa	Mavlink Submodule update	2019-10-28 16:04:11 -04:00
Hamish Willee	662795cb90	Fix incorrect default for parser	2019-10-28 16:04:11 -04:00
Hamish Willee	e65515cd9b	Parameter parser/markdown includes boolean flag	2019-10-28 16:04:11 -04:00
bresch	16d7db1e69	InnovationLpf: initialize state to zero	2019-10-28 16:04:11 -04:00
TSC21	471bc23a9f	Jenkins CI: Colcon build on ROS2 Dashing workspace	2019-10-28 16:04:11 -04:00
TSC21	12c0d198ae	bump container tags to 2019-10-24	2019-10-28 16:04:11 -04:00
TSC21	1ee8b67591	Jenkins CI: Tests: bump px4-dev-ros-melodic tag to 2019-10-23	2019-10-28 16:04:11 -04:00
TSC21	a0367b30b8	Jenkins CI: move SITL tests to Melodic container	2019-10-28 16:04:11 -04:00
bresch	6cab14fc5d	ekf2: Fix heading not stable issue. The prblem was that a large dt could drive the alpha filter crazy because of the small dt approximation during the computation of the coefficient. - Remove unused bitmask packing of the innovation checks	2019-10-28 16:04:11 -04:00
Matthias Grob	45bc2c5882	arch.sh: get rid of ignition-cmake workaround The AUR repository got fixed upstream after I reported the issue. I tested on a fresh machine so we can get rid of the workaround that was necessary. See https://aur.archlinux.org/packages/ignition-cmake/#comment-712301	2019-10-28 16:04:11 -04:00
Travis Bottalico	571c6f136d	modalai fc-v1: add dshot support	2019-10-28 16:04:11 -04:00
Travis Bottalico	e038f06778	Fix a missed refactor of board name in vscode file	2019-10-28 16:04:11 -04:00
mcsauder	71a7bf420e	Add rangefinder dependency to the CM8JL65 driver CMakeList and organize #includes.	2019-10-28 16:04:11 -04:00
Jaeyoung-Lim	4a76f608ac	Retune h480	2019-10-28 16:04:11 -04:00
RomanBapst	3e84def1e2	rcS: do not wipe next flight UUID parameter when resetting parameters Signed-off-by: RomanBapst <bapstroman@gmail.com>	2019-10-28 16:04:11 -04:00
Mathieu Bresciani	112f24c54f	ekf2_main - Add optical flow innovation pre-flight check (#13036 ) * ekf2: Add FirstOrderLpf and InnovationLpf classes for innovation lowpass filtering * ekf2: use InnovLpf filter class in preflight checks * ekf2: move selection of yaw test limit for pre-flight check in function * ekf2: Move pre-flight checks into separate function * ekf2: use static constexpr insetead of inline for sq (square) function * ekf2: Split pre-flight checks in separate functions Also use the same check for all the innovations: innov_lpf < test and innov < 2xtest * ekf2: Add optical flow pre-flight check * ekf2: Combine FirstOrderLpf and InnovationLpf in single class * ekf2: check vel_pos_innov when ev_pos is active as well * ekf2: transform InnovationLpf into a header only library and pass the spike limit during the update call to avoid storing it here * ekf2: Static and const cleanup - set spike_lim constants as static constexpr, set innovation - set checker helper functions as static - rename the mix of heading and yaw as heading to avoid confusion * ekf2: use ternary operator in selectHeadingTestLimit instead of if-else * ekf2: store intermediate redults in const bool flags. Those will be used for logging * ekf2: set variable const whenever possible * ekf2: create PreFlightChecker class that handle all the innovation pre-flight checks. Add simple unit testing Use bitmask instead of general flag to have more granularity * PreFlightChecker: use setter for the innovations to check instead of sending booleans in the update function This makes it more scalable as more checks will be added * ekf: Use booleans instead of bitmask for ekf preflt checks Rename "down" to "vert"	2019-10-28 16:04:11 -04:00
Daniel Agar	17d0073f95	generate_listener.py don't use message length	2019-10-28 16:04:11 -04:00
Daniel Agar	a59a0b64b8	perf counter cleanup (mostly intervals) Some of these perf counters were useful during initial development, but realistically aren't needed anymore, some are redundant when we can now see the average interval from `work_queue status` and some of them simply aren't worth the cost at higher rates.	2019-10-28 16:04:11 -04:00