mc_att_control: copy sensor_correction topic once initially

Signed-off-by: Roman <bapstroman@gmail.com>
tap_esc increase stack 1100 -> 1180 bytes
2026-06-19 08:40:34 +08:00 · 2018-11-12 21:00:28 -05:00 · 2018-10-19 20:11:14 -04:00 · 2018-10-19 20:11:14 -04:00 · 2018-10-19 20:11:14 -04:00 · 2018-10-19 20:11:14 -04:00
311 changed files with 3116 additions and 3387 deletions
@@ -5,7 +5,7 @@ about: Create a report to help us improve
 ---

 **Describe the bug**
-A clear and concise description of what the bug is.
+A clear and concise description of the bug.

 **To Reproduce**
 Steps to reproduce the behavior:
@@ -18,13 +18,15 @@ Steps to reproduce the behavior:
 A clear and concise description of what you expected to happen.

 **Log Files and Screenshots**
-Always provide the flight log file, add screenshots to help explain your problem.
- - Download the flight log file from the vehicle ([tutorial](https://docs.px4.io/en/flight-reporting.html))
- - Share the link to a log showing the problem on [PX4 Flight Review](http://logs.px4.io/)
+*Always* provide a link to the flight log file:
+- Download the flight log file from the vehicle ([tutorial](https://docs.px4.io/en/getting_started/flight_reporting.html)).
+- Share the link to a log showing the problem on [PX4 Flight Review](http://logs.px4.io/).
+
+Add screenshots to help explain your problem.

 **Drone (please complete the following information):**
- - Description of the type of drone
- - Photo of the IMU / autopilot setup if possible
+- Describe the type of drone.
+- Photo of the IMU / autopilot setup if possible.

 **Additional context**
 Add any other context about the problem here.
@@ -4,16 +4,16 @@ about: Suggest an idea for this project

 ---

-For general questions please use [PX4 Discuss](http://discuss.px4.io/) or [Slack](slack.px4.io).
+For general questions please use [PX4 Discuss](http://discuss.px4.io/) or [Slack](http://slack.px4.io/).

-**Is your feature request related to a problem? Please describe.**
-A clear and concise description of what the problem is. Ex. I'm always frustrated when [...]
+**Describe problem solved by the proposed feature**
+A clear and concise description of the problem, if any, this feature will solve. E.g. I'm always frustrated when ...

-**Describe the solution you'd like**
+**Describe your preferred solution**
 A clear and concise description of what you want to happen.

-**Describe alternatives you've considered**
-A clear and concise description of any alternative solutions or features you've considered.
+**Describe possible alternatives**
+A clear and concise description of alternative solutions or features you've considered.

 **Additional context**
-Add any other context or screenshots about the feature request here.
+Add any other context or screenshots for the feature request here.
@@ -1,9 +0,0 @@
-**Bug Report** or **Feature Request**
-
-For general questions please use [PX4 Discuss](http://discuss.px4.io/) or [Slack](slack.px4.io). Issue reports need to contain the items below:
-
- - Download the flight log file from the vehicle ([tutorial](https://docs.px4.io/en/flight-reporting.html))
- - Share the link to a log showing the problem on [PX4 Flight Review](http://logs.px4.io/)
- - Expected behavior and actual behavior.
- - Steps to reproduce the problem.
- - Specifications like the version of the project, operating system, or hardware.
@@ -466,7 +466,7 @@ if ("${CMAKE_SYSTEM}" MATCHES "Linux")
 	if (EXISTS ${DPKG_PROGRAM})
 		list (APPEND CPACK_GENERATOR "DEB")
 	endif()
-else()	
+else()
 	set(CPACK_GENERATOR "ZIP")
 endif()

@@ -44,7 +44,7 @@ pipeline {
                    sh "make nuttx_px4fmu-v3_rtps"
                    sh "make sizes"
                    sh "ccache -s"
-                    archiveArtifacts(allowEmptyArchive: true, artifacts: 'build/**/*.px4, build/**/*.elf', fingerprint: true, onlyIfSuccessful: true)
+                    archiveArtifacts(allowEmptyArchive: false, artifacts: 'build/**/*.px4, build/**/*.elf', fingerprint: true, onlyIfSuccessful: true)
                    sh "make distclean"
                  }
                }
@@ -55,7 +55,7 @@ pipeline {
          // nuttx default targets that are archived and uploaded to s3
          for (def option in ["px4fmu-v4", "px4fmu-v4pro", "px4fmu-v5", "aerofc-v1", "aerocore2", "auav-x21", "crazyflie", "mindpx-v2", "nxphlite-v3", "tap-v1", "omnibus-f4sd"]) {
            def node_name = "${option}"
-            builds[node_name] = createBuildNode(docker_nuttx, "${node_name}_default")
+            builds[node_name] = createBuildNodeArchive(docker_nuttx, "${node_name}_default")
          }

          // other nuttx default targets
@@ -64,7 +64,6 @@ pipeline {
            builds[node_name] = createBuildNode(docker_nuttx, "${node_name}_default")
          }

-          builds["sitl"] = createBuildNode(docker_base, 'posix_sitl_default')
          builds["sitl_rtps"] = createBuildNode(docker_base, 'posix_sitl_rtps')
          builds["sitl (GCC 7)"] = createBuildNode(docker_arch, 'posix_sitl_default')

@@ -264,24 +263,51 @@ pipeline {
          }
        }

-        stage('tests (address sanitizer)') {
-          agent {
-            docker {
-              image 'px4io/px4-dev-base:2018-03-30'
-              args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
-            }
-          }
-          environment {
-              PX4_ASAN = 1
-              ASAN_OPTIONS = "color=always:check_initialization_order=1:detect_stack_use_after_return=1"
-          }
-          steps {
-            sh 'export'
-            sh 'make distclean'
-            sh 'make tests'
-            sh 'make distclean'
-          }
-        }
+        // TODO: PX4 requires clean shutdown first
+        // stage('tests (address sanitizer)') {
+        //   agent {
+        //     docker {
+        //       image 'px4io/px4-dev-base:2018-03-30'
+        //       args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
+        //     }
+        //   }
+        //   environment {
+        //       PX4_ASAN = 1
+        //       ASAN_OPTIONS = "color=always:check_initialization_order=1:detect_stack_use_after_return=1"
+        //   }
+        //   steps {
+        //     sh 'export'
+        //     sh 'make distclean'
+        //     sh 'make tests'
+        //     sh 'make distclean'
+        //   }
+        // }
+
+        // TODO: test and re-enable once GDB is available in px4-dev-ros
+        // stage('tests (code coverage)') {
+        //   agent {
+        //     docker {
+        //       image 'px4io/px4-dev-ros:2018-03-30'
+        //       args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
+        //     }
+        //   }
+        //   steps {
+        //     sh 'export'
+        //     sh 'make distclean'
+        //     sh 'ulimit -c unlimited; make tests_coverage'
+        //     sh 'ls'
+        //     withCredentials([string(credentialsId: 'FIRMWARE_CODECOV_TOKEN', variable: 'CODECOV_TOKEN')]) {
+        //       sh 'curl -s https://codecov.io/bash | bash -s'
+        //     }
+        //     sh 'make distclean'
+        //   }
+        //   post {
+        //     failure {
+        //       sh('find . -name core')
+        //       sh('gdb --batch --quiet -ex "thread apply all bt full" -ex "quit" build/posix_sitl_default/px4 core')
+        //     }
+        //   }
+        // }

        stage('check stack') {
          agent {
@@ -310,9 +336,9 @@ pipeline {
          }
          steps {
            sh 'export'
-            sh 'rm -rf .ros; rm -rf .gazebo'
+            sh 'rm -rf build; rm -rf px4-posix_sitl_default*; rm -rf .ros; rm -rf .gazebo'
            unstash 'px4_sitl_package'
-            sh 'tar -xjpf build/posix_sitl_default/px4-posix_sitl_default*.bz2'
+            sh 'tar -xjpvf build/posix_sitl_default/px4-posix_sitl_default*.bz2'
            sh 'px4-posix_sitl_default*/px4/test/rostest_px4_run.sh mavros_posix_test_mission.test mission:=vtol_new_1 vehicle:=standard_vtol'
            sh 'px4-posix_sitl_default*/px4/Tools/ecl_ekf/process_logdata_ekf.py `find . -name *.ulg -print -quit`'
          }
@@ -321,8 +347,10 @@ pipeline {
              sh 'px4-posix_sitl_default*/px4/Tools/upload_log.py -q --description "${JOB_NAME}: ${STAGE_NAME}" --feedback "${JOB_NAME} ${CHANGE_TITLE} ${CHANGE_URL}" --source CI .ros/rootfs/fs/microsd/log/*/*.ulg'
              archiveArtifacts '.ros/**/*.pdf'
              archiveArtifacts '.ros/**/*.csv'
+              deleteDir()
            }
            failure {
+              sh 'ls -a'
              archiveArtifacts '.ros/**/*.ulg'
              archiveArtifacts '.ros/**/rosunit-*.xml'
              archiveArtifacts '.ros/**/rostest-*.log'
@@ -342,9 +370,9 @@ pipeline {
          }
          steps {
            sh 'export'
-            sh 'rm -rf .ros; rm -rf .gazebo'
+            sh 'rm -rf build; rm -rf px4-posix_sitl_default*; rm -rf .ros; rm -rf .gazebo'
            unstash 'px4_sitl_package'
-            sh 'tar -xjpf build/posix_sitl_default/px4-posix_sitl_default*.bz2'
+            sh 'tar -xjpvf build/posix_sitl_default/px4-posix_sitl_default*.bz2'
            sh 'px4-posix_sitl_default*/px4/test/rostest_px4_run.sh mavros_posix_test_mission.test mission:=vtol_new_1 vehicle:=tailsitter'
            sh 'px4-posix_sitl_default*/px4/Tools/ecl_ekf/process_logdata_ekf.py `find . -name *.ulg -print -quit`'
          }
@@ -353,8 +381,10 @@ pipeline {
              sh 'px4-posix_sitl_default*/px4/Tools/upload_log.py -q --description "${JOB_NAME}: ${STAGE_NAME}" --feedback "${JOB_NAME} ${CHANGE_TITLE} ${CHANGE_URL}" --source CI .ros/rootfs/fs/microsd/log/*/*.ulg'
              archiveArtifacts '.ros/**/*.pdf'
              archiveArtifacts '.ros/**/*.csv'
+              deleteDir()
            }
            failure {
+              sh 'ls -a'
              archiveArtifacts '.ros/**/*.ulg'
              archiveArtifacts '.ros/**/rosunit-*.xml'
              archiveArtifacts '.ros/**/rostest-*.log'
@@ -374,9 +404,9 @@ pipeline {
          }
          steps {
            sh 'export'
-            sh 'rm -rf .ros; rm -rf .gazebo'
+            sh 'rm -rf build; rm -rf px4-posix_sitl_default*; rm -rf .ros; rm -rf .gazebo'
            unstash 'px4_sitl_package'
-            sh 'tar -xjpf build/posix_sitl_default/px4-posix_sitl_default*.bz2'
+            sh 'tar -xjpvf build/posix_sitl_default/px4-posix_sitl_default*.bz2'
            sh 'px4-posix_sitl_default*/px4/test/rostest_px4_run.sh mavros_posix_test_mission.test mission:=vtol_new_1 vehicle:=tiltrotor'
            sh 'px4-posix_sitl_default*/px4/Tools/ecl_ekf/process_logdata_ekf.py `find . -name *.ulg -print -quit`'
          }
@@ -385,8 +415,10 @@ pipeline {
              sh 'px4-posix_sitl_default*/px4/Tools/upload_log.py -q --description "${JOB_NAME}: ${STAGE_NAME}" --feedback "${JOB_NAME} ${CHANGE_TITLE} ${CHANGE_URL}" --source CI .ros/rootfs/fs/microsd/log/*/*.ulg'
              archiveArtifacts '.ros/**/*.pdf'
              archiveArtifacts '.ros/**/*.csv'
+              deleteDir()
            }
            failure {
+              sh 'ls -a'
              archiveArtifacts '.ros/**/*.ulg'
              archiveArtifacts '.ros/**/rosunit-*.xml'
              archiveArtifacts '.ros/**/rostest-*.log'
@@ -406,9 +438,9 @@ pipeline {
          }
          steps {
            sh 'export'
-            sh 'rm -rf .ros; rm -rf .gazebo'
+            sh 'rm -rf build; rm -rf px4-posix_sitl_default*; rm -rf .ros; rm -rf .gazebo'
            unstash 'px4_sitl_package'
-            sh 'tar -xjpf build/posix_sitl_default/px4-posix_sitl_default*.bz2'
+            sh 'tar -xjpvf build/posix_sitl_default/px4-posix_sitl_default*.bz2'
            sh 'px4-posix_sitl_default*/px4/test/rostest_px4_run.sh mavros_posix_test_mission.test mission:=vtol_new_2 vehicle:=standard_vtol'
            sh 'px4-posix_sitl_default*/px4/Tools/ecl_ekf/process_logdata_ekf.py `find . -name *.ulg -print -quit`'
          }
@@ -417,8 +449,10 @@ pipeline {
              sh 'px4-posix_sitl_default*/px4/Tools/upload_log.py -q --description "${JOB_NAME}: ${STAGE_NAME}" --feedback "${JOB_NAME} ${CHANGE_TITLE} ${CHANGE_URL}" --source CI .ros/rootfs/fs/microsd/log/*/*.ulg'
              archiveArtifacts '.ros/**/*.pdf'
              archiveArtifacts '.ros/**/*.csv'
+              deleteDir()
            }
            failure {
+              sh 'ls -a'
              archiveArtifacts '.ros/**/*.ulg'
              archiveArtifacts '.ros/**/rosunit-*.xml'
              archiveArtifacts '.ros/**/rostest-*.log'
@@ -438,9 +472,9 @@ pipeline {
          }
          steps {
            sh 'export'
-            sh 'rm -rf .ros; rm -rf .gazebo'
+            sh 'rm -rf build; rm -rf px4-posix_sitl_default*; rm -rf .ros; rm -rf .gazebo'
            unstash 'px4_sitl_package'
-            sh 'tar -xjpf build/posix_sitl_default/px4-posix_sitl_default*.bz2'
+            sh 'tar -xjpvf build/posix_sitl_default/px4-posix_sitl_default*.bz2'
            sh 'px4-posix_sitl_default*/px4/test/rostest_px4_run.sh mavros_posix_test_mission.test mission:=vtol_old_1 vehicle:=standard_vtol'
            sh 'px4-posix_sitl_default*/px4/Tools/ecl_ekf/process_logdata_ekf.py `find . -name *.ulg -print -quit`'
          }
@@ -449,8 +483,10 @@ pipeline {
              sh 'px4-posix_sitl_default*/px4/Tools/upload_log.py -q --description "${JOB_NAME}: ${STAGE_NAME}" --feedback "${JOB_NAME} ${CHANGE_TITLE} ${CHANGE_URL}" --source CI .ros/rootfs/fs/microsd/log/*/*.ulg'
              archiveArtifacts '.ros/**/*.pdf'
              archiveArtifacts '.ros/**/*.csv'
+              deleteDir()
            }
            failure {
+              sh 'ls -a'
              archiveArtifacts '.ros/**/*.ulg'
              archiveArtifacts '.ros/**/rosunit-*.xml'
              archiveArtifacts '.ros/**/rostest-*.log'
@@ -470,9 +506,9 @@ pipeline {
          }
          steps {
            sh 'export'
-            sh 'rm -rf .ros; rm -rf .gazebo'
+            sh 'rm -rf build; rm -rf px4-posix_sitl_default*; rm -rf .ros; rm -rf .gazebo'
            unstash 'px4_sitl_package'
-            sh 'tar -xjpf build/posix_sitl_default/px4-posix_sitl_default*.bz2'
+            sh 'tar -xjpvf build/posix_sitl_default/px4-posix_sitl_default*.bz2'
            sh 'px4-posix_sitl_default*/px4/test/rostest_px4_run.sh mavros_posix_test_mission.test mission:=vtol_old_2 vehicle:=standard_vtol'
            sh 'px4-posix_sitl_default*/px4/Tools/ecl_ekf/process_logdata_ekf.py `find . -name *.ulg -print -quit`'
          }
@@ -481,8 +517,10 @@ pipeline {
              sh 'px4-posix_sitl_default*/px4/Tools/upload_log.py -q --description "${JOB_NAME}: ${STAGE_NAME}" --feedback "${JOB_NAME} ${CHANGE_TITLE} ${CHANGE_URL}" --source CI .ros/rootfs/fs/microsd/log/*/*.ulg'
              archiveArtifacts '.ros/**/*.pdf'
              archiveArtifacts '.ros/**/*.csv'
+              deleteDir()
            }
            failure {
+              sh 'ls -a'
              archiveArtifacts '.ros/**/*.ulg'
              archiveArtifacts '.ros/**/rosunit-*.xml'
              archiveArtifacts '.ros/**/rostest-*.log'
@@ -502,9 +540,9 @@ pipeline {
          }
          steps {
            sh 'export'
-            sh 'rm -rf .ros; rm -rf .gazebo'
+            sh 'rm -rf build; rm -rf px4-posix_sitl_default*; rm -rf .ros; rm -rf .gazebo'
            unstash 'px4_sitl_package'
-            sh 'tar -xjpf build/posix_sitl_default/px4-posix_sitl_default*.bz2'
+            sh 'tar -xjpvf build/posix_sitl_default/px4-posix_sitl_default*.bz2'
            sh 'px4-posix_sitl_default*/px4/test/rostest_px4_run.sh mavros_posix_test_mission.test mission:=multirotor_box vehicle:=iris'
            sh 'px4-posix_sitl_default*/px4/Tools/ecl_ekf/process_logdata_ekf.py `find . -name *.ulg -print -quit`'
          }
@@ -513,8 +551,10 @@ pipeline {
              sh 'px4-posix_sitl_default*/px4/Tools/upload_log.py -q --description "${JOB_NAME}: ${STAGE_NAME}" --feedback "${JOB_NAME} ${CHANGE_TITLE} ${CHANGE_URL}" --source CI .ros/rootfs/fs/microsd/log/*/*.ulg'
              archiveArtifacts '.ros/**/*.pdf'
              archiveArtifacts '.ros/**/*.csv'
+              deleteDir()
            }
            failure {
+              sh 'ls -a'
              archiveArtifacts '.ros/**/*.ulg'
              archiveArtifacts '.ros/**/rosunit-*.xml'
              archiveArtifacts '.ros/**/rostest-*.log'
@@ -534,9 +574,9 @@ pipeline {
          }
          steps {
            sh 'export'
-            sh 'rm -rf .ros; rm -rf .gazebo'
+            sh 'rm -rf build; rm -rf px4-posix_sitl_default*; rm -rf .ros; rm -rf .gazebo'
            unstash 'px4_sitl_package'
-            sh 'tar -xjpf build/posix_sitl_default/px4-posix_sitl_default*.bz2'
+            sh 'tar -xjpvf build/posix_sitl_default/px4-posix_sitl_default*.bz2'
            sh 'px4-posix_sitl_default*/px4/test/rostest_px4_run.sh mavros_posix_tests_offboard_attctl.test'
            sh 'px4-posix_sitl_default*/px4/Tools/ecl_ekf/process_logdata_ekf.py `find . -name *.ulg -print -quit`'
          }
@@ -545,8 +585,10 @@ pipeline {
              sh 'px4-posix_sitl_default*/px4/Tools/upload_log.py -q --description "${JOB_NAME}: ${STAGE_NAME}" --feedback "${JOB_NAME} ${CHANGE_TITLE} ${CHANGE_URL}" --source CI .ros/rootfs/fs/microsd/log/*/*.ulg'
              archiveArtifacts '.ros/**/*.pdf'
              archiveArtifacts '.ros/**/*.csv'
+              deleteDir()
            }
            failure {
+              sh 'ls -a'
              archiveArtifacts '.ros/**/*.ulg'
              archiveArtifacts '.ros/**/rosunit-*.xml'
              archiveArtifacts '.ros/**/rostest-*.log'
@@ -563,9 +605,9 @@ pipeline {
          }
          steps {
            sh 'export'
-            sh 'rm -rf .ros; rm -rf .gazebo'
+            sh 'rm -rf build; rm -rf px4-posix_sitl_default*; rm -rf .ros; rm -rf .gazebo'
            unstash 'px4_sitl_package'
-            sh 'tar -xjpf build/posix_sitl_default/px4-posix_sitl_default*.bz2'
+            sh 'tar -xjpvf build/posix_sitl_default/px4-posix_sitl_default*.bz2'
            sh 'px4-posix_sitl_default*/px4/test/rostest_px4_run.sh mavros_posix_tests_offboard_posctl.test'
            sh 'px4-posix_sitl_default*/px4/Tools/ecl_ekf/process_logdata_ekf.py `find . -name *.ulg -print -quit`'
          }
@@ -574,8 +616,10 @@ pipeline {
              sh 'px4-posix_sitl_default*/px4/Tools/upload_log.py -q --description "${JOB_NAME}: ${STAGE_NAME}" --feedback "${JOB_NAME} ${CHANGE_TITLE} ${CHANGE_URL}" --source CI .ros/rootfs/fs/microsd/log/*/*.ulg'
              archiveArtifacts '.ros/**/*.pdf'
              archiveArtifacts '.ros/**/*.csv'
+              deleteDir()
            }
            failure {
+              sh 'ls -a'
              archiveArtifacts '.ros/**/*.ulg'
              archiveArtifacts '.ros/**/rosunit-*.xml'
              archiveArtifacts '.ros/**/rostest-*.log'
@@ -644,11 +688,14 @@ pipeline {
      }
    }

+    // TODO: actually upload artifacts to S3
    stage('S3 Upload') {
      agent {
        docker { image 'px4io/px4-dev-base:2018-03-30' }
      }
-
+      options {
+            skipDefaultCheckout()
+      }
      when {
        anyOf {
          branch 'master'
@@ -656,7 +703,6 @@ pipeline {
          branch 'stable'
        }
      }
-
      steps {
        sh 'echo "uploading to S3"'
      }
@@ -686,7 +732,27 @@ def createBuildNode(String docker_repo, String target) {
          sh('make ' + target)
          sh('ccache -s')
          sh('make sizes')
-          archiveArtifacts(allowEmptyArchive: true, artifacts: 'build/**/*.px4, build/**/*.elf', fingerprint: true, onlyIfSuccessful: true)
+          sh('make distclean')
+        }
+      }
+    }
+  }
+}
+
+def createBuildNodeArchive(String docker_repo, String target) {
+  return {
+    node {
+      docker.image(docker_repo).inside('-e CCACHE_BASEDIR=${WORKSPACE} -v ${CCACHE_DIR}:${CCACHE_DIR}:rw') {
+        stage(target) {
+          sh('export')
+          checkout scm
+          sh('make distclean')
+          sh('git fetch --tags')
+          sh('ccache -z')
+          sh('make ' + target)
+          sh('ccache -s')
+          sh('make sizes')
+          archiveArtifacts(allowEmptyArchive: false, artifacts: 'build/**/*.px4, build/**/*.elf, build/**/*.bin', fingerprint: true, onlyIfSuccessful: true)
          sh('make distclean')
        }
      }
@@ -708,7 +774,6 @@ def createBuildNodeDockerLogin(String docker_repo, String docker_credentials, St
            sh('make ' + target)
            sh('ccache -s')
            sh('make sizes')
-            archiveArtifacts(allowEmptyArchive: true, artifacts: 'build/**/*.px4, build/**/*.elf', fingerprint: true, onlyIfSuccessful: true)
            sh('make distclean')
          }
        }
@@ -1,6 +1,6 @@
 BSD 3-Clause License

-Copyright (c) 2012 - 2017, PX4 Development Team
+Copyright (c) 2012 - 2018, PX4 Development Team
 All rights reserved.

 Redistribution and use in source and binary forms, with or without
@@ -120,10 +120,11 @@ endif
 # --------------------------------------------------------------------
 # describe how to build a cmake config
 define cmake-build
-+@$(eval BUILD_DIR = $(SRC_DIR)/build/$@$(BUILD_DIR_SUFFIX))
+@$(eval PX4_CONFIG = $(1))
+@$(eval BUILD_DIR = $(SRC_DIR)/build/$(PX4_CONFIG)$(BUILD_DIR_SUFFIX))
 +@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e $(BUILD_DIR)/Makefile ]; then rm -rf $(BUILD_DIR); fi
-+@if [ ! -e $(BUILD_DIR)/CMakeCache.txt ]; then mkdir -p $(BUILD_DIR) && cd $(BUILD_DIR) && cmake $(2) -G"$(PX4_CMAKE_GENERATOR)" $(CMAKE_ARGS) -DCONFIG=$(1) || (rm -rf $(BUILD_DIR)); fi
-+@(cd $(BUILD_DIR) && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS))
+@if [ ! -e $(BUILD_DIR)/CMakeCache.txt ]; then mkdir -p $(BUILD_DIR) && cd $(BUILD_DIR) && cmake $(SRC_DIR) -G"$(PX4_CMAKE_GENERATOR)" $(CMAKE_ARGS) -DCONFIG=$(PX4_CONFIG) || (rm -rf $(BUILD_DIR)); fi
+@$(PX4_MAKE) -C $(BUILD_DIR) $(PX4_MAKE_ARGS) $(ARGS)
 endef

 COLOR_BLUE = \033[0;94m
@@ -144,13 +145,13 @@ NUTTX_CONFIG_TARGETS := $(patsubst nuttx_%,%,$(filter nuttx_%,$(ALL_CONFIG_TARGE

 # All targets.
 $(ALL_CONFIG_TARGETS):
-	$(call cmake-build,$@,$(SRC_DIR))
+	$(call cmake-build,$@)

 # Abbreviated config targets.

 # nuttx_ is left off by default; provide a rule to allow that.
 $(NUTTX_CONFIG_TARGETS):
-	$(call cmake-build,nuttx_$@,$(SRC_DIR))
+	$(call cmake-build,nuttx_$@)

 all_nuttx_targets: $(NUTTX_CONFIG_TARGETS)

@@ -288,11 +289,6 @@ tests:

 tests_coverage:
 	@$(MAKE) clean
-	@$(MAKE) --no-print-directory posix_sitl_default PX4_CMAKE_BUILD_TYPE=Coverage
-	@$(MAKE) --no-print-directory posix_sitl_default sitl_gazebo PX4_CMAKE_BUILD_TYPE=Coverage
-	@$(SRC_DIR)/test/rostest_px4_run.sh mavros_posix_tests_missions.test
-	@$(SRC_DIR)/test/rostest_px4_run.sh mavros_posix_tests_offboard_attctl.test
-	@$(SRC_DIR)/test/rostest_px4_run.sh mavros_posix_tests_offboard_posctl.test
 	@$(MAKE) --no-print-directory posix_sitl_default test_coverage_genhtml PX4_CMAKE_BUILD_TYPE=Coverage
 	@echo "Open $(SRC_DIR)/build/posix_sitl_default/coverage-html/index.html to see coverage"

@@ -35,7 +35,7 @@ See the [forum and chat](https://dev.px4.io/en/#support) if you need help!

 The PX4 Dev Team syncs up on a [weekly dev call](https://dev.px4.io/en/contribute/#dev_call).

-> **Note** The dev call is open to all interested developers (not just the core dev team). This is a great opportunity to meet the team and contribute to the ongoing development of the platform. It includes a QA session for newcomers.
+> **Note** The dev call is open to all interested developers (not just the core dev team). This is a great opportunity to meet the team and contribute to the ongoing development of the platform. It includes a QA session for newcomers. All regular calls are listed in the [Dronecode calendar](https://www.dronecode.org/calendar/).


 ## Maintenance Team
@@ -13,32 +13,38 @@ sh /etc/init.d/4001_quad_x
 if [ $AUTOCNF == yes ]
 then
 	param set MC_ROLL_P 8.0
-	param set MC_ROLLRATE_P 0.19
-	param set MC_ROLLRATE_I 0.1
-	param set MC_ROLLRATE_D 0.0055
+	param set MC_ROLLRATE_P 0.08
+	param set MC_ROLLRATE_I 0.25
+	param set MC_ROLLRATE_D 0.001
 	param set MC_PITCH_P 8.0
-	param set MC_PITCHRATE_P 0.19
-	param set MC_PITCHRATE_I 0.1
-	param set MC_PITCHRATE_D 0.0055
+	param set MC_PITCHRATE_P 0.08
+	param set MC_PITCHRATE_I 0.25
+	param set MC_PITCHRATE_D 0.001
 	param set MC_YAW_P 4.0
 	param set MC_YAWRATE_P 0.2
 	param set MC_YAWRATE_I 0.1
 	param set MC_YAWRATE_D 0.0
 	param set MC_YAW_FF 0.5
-	param set MC_ROLLRATE_MAX 720.0
-	param set MC_PITCHRATE_MAX 720.0
-	param set MC_YAWRATE_MAX 400.0
-	param set MC_ACRO_R_MAX 360.0
-	param set MC_ACRO_P_MAX 360.0
-	param set MC_TPA_BREAK_P 0.5
-	param set MC_TPA_RATE_P 0.5
+
+	param set MC_ROLLRATE_MAX 1600.0
+	param set MC_PITCHRATE_MAX 1600.0
+	param set MC_YAWRATE_MAX 1000.0
+
+	param set MPC_MANTHR_MIN 0.0
+	param set MPC_MAN_TILT_MAX 60
+
+	# use thrust curve factor (instead of TPA)
+	param set THR_MDL_FAC 0.3

 	param set PWM_MIN 1075
+	# enable one-shot
+	param set PWM_RATE 0

-	param set MPC_THR_MIN 0.06
-	param set MPC_MANTHR_MIN 0.06
+	# enable high-rate logging profile (helps with tuning)
+	param set SDLOG_PROFILE 19

-	param set ATT_BIAS_MAX 0.0
+	# disable RC filtering
+	param set RC_FLT_CUTOFF 0.00000

 	param set CBRK_IO_SAFETY 22027
 fi
@@ -125,6 +125,11 @@ then
 	set MIXER_AUX none
 fi

+if [ $USE_IO == no ]
+then
+	set MIXER_AUX none
+fi
+
 if [ $MIXER_AUX != none -a $AUX_MODE != none ]
 then
 	#
@@ -280,6 +280,8 @@ then
 	hmc5883 -X start

 	bmp280 start
+
+	adc start
 fi

 if ver hwcmp PX4FMU_V4PRO
@@ -341,11 +343,7 @@ if [ ${VEHICLE_TYPE} == fw -o ${VEHICLE_TYPE} == vtol ]
 then
 	if param compare CBRK_AIRSPD_CHK 0
 	then
-		if sdp3x_airspeed start
-		then
-		else
-			sdp3x_airspeed start -b 2
-		fi
+		sdp3x_airspeed start -a

 		# Pixhawk 2.1 has a MS5611 on I2C which gets wrongly
 		# detected as MS5525 because the chip manufacturer was so
@@ -355,20 +353,13 @@ then
 		then
 			ms5525_airspeed start -b 2
 		else
-			ms5525_airspeed start
+			ms5525_airspeed start -a
 		fi

-		if ms4525_airspeed start
-		then
-		else
-			ms4525_airspeed start -b 2
-		fi
+		ms4525_airspeed start -a
+
+		ets_airspeed start -a

-		if ets_airspeed start
-		then
-		else
-			ets_airspeed start -b 1
-		fi
 	fi
 fi

@@ -321,60 +321,57 @@ then

 	set IO_PRESENT no

-	if [ $USE_IO == yes ]
+	#
+	# Check if PX4IO present and update firmware if needed
+	#
+	if [ -f /etc/extras/px4io-v2.bin ]
 	then
-		#
-		# Check if PX4IO present and update firmware if needed
-		#
-		if [ -f /etc/extras/px4io-v2.bin ]
+		set IO_FILE /etc/extras/px4io-v2.bin
+
+		if px4io checkcrc ${IO_FILE}
 		then
-			set IO_FILE /etc/extras/px4io-v2.bin
+			set IO_PRESENT yes
+		else
+			tone_alarm MLL32CP8MB

-			if px4io checkcrc ${IO_FILE}
+			if px4io start
 			then
-				set IO_PRESENT yes
-			else
-				tone_alarm MLL32CP8MB
-
-				if px4io start
+				# try to safe px4 io so motor outputs dont go crazy
+				if px4io safety_on
 				then
-					# try to safe px4 io so motor outputs dont go crazy
-					if px4io safety_on
-					then
-						# success! no-op
-					else
-						# px4io did not respond to the safety command
-						px4io stop
-					fi
-				fi
-
-				if px4io forceupdate 14662 ${IO_FILE}
-				then
-					usleep 10000
-					if px4io checkcrc ${IO_FILE}
-					then
-						echo "PX4IO CRC OK after updating" >> $LOG_FILE
-						tone_alarm MLL8CDE
-
-						set IO_PRESENT yes
-					else
-						echo "PX4IO update failed" >> $LOG_FILE
-						tone_alarm ${TUNE_ERR}
-					fi
+					# success! no-op
 				else
-					echo "PX4IO update failed" >> $LOG_FILE
-					tune_control play -m ${TUNE_ERR}
+					# px4io did not respond to the safety command
+					px4io stop
 				fi
 			fi
-		fi
-		unset IO_FILE

-		if [ $IO_PRESENT == no ]
-		then
-			echo "PX4IO not found" >> $LOG_FILE
-			tune_control play -m ${TUNE_ERR}
+			if px4io forceupdate 14662 ${IO_FILE}
+			then
+				usleep 10000
+				if px4io checkcrc ${IO_FILE}
+				then
+					echo "PX4IO CRC OK after updating" >> $LOG_FILE
+					tone_alarm MLL8CDE
+
+					set IO_PRESENT yes
+				else
+					echo "PX4IO update failed" >> $LOG_FILE
+					tone_alarm ${TUNE_ERR}
+				fi
+			else
+				echo "PX4IO update failed" >> $LOG_FILE
+				tune_control play -m ${TUNE_ERR}
+			fi
 		fi
 	fi
+	unset IO_FILE
+
+	if [ $USE_IO == yes -a $IO_PRESENT == no ]
+	then
+		echo "PX4IO not found" >> $LOG_FILE
+		tune_control play -m ${TUNE_ERR}
+	fi

 	#
 	# Set default output if not set
@@ -8,7 +8,7 @@ import matplotlib.pyplot as plt
 from matplotlib.backends.backend_pdf import PdfPages

 def analyse_ekf(estimator_status, ekf2_innovations, sensor_preflight, check_levels,
-                plot=False, output_plot_filename=None):
+                plot=False, output_plot_filename=None, late_start_early_ending=True):

    if plot:
        # create summary plots
@@ -502,6 +502,11 @@ def analyse_ekf(estimator_status, ekf2_innovations, sensor_preflight, check_leve
    using_evpos = ((2 ** 12 & estimator_status['control_mode_flags']) > 0) * 1
    using_evyaw = ((2 ** 13 & estimator_status['control_mode_flags']) > 0) * 1
    using_evhgt = ((2 ** 14 & estimator_status['control_mode_flags']) > 0) * 1
+
+    # define flags for starting and finishing in air
+    b_starts_in_air = False
+    b_finishes_in_air = False
+
    # calculate in-air transition time
    if (np.amin(airborne) < 0.5) and (np.amax(airborne) > 0.5):
        in_air_transtion_time_arg = np.argmax(np.diff(airborne))
@@ -509,6 +514,7 @@ def analyse_ekf(estimator_status, ekf2_innovations, sensor_preflight, check_leve
    elif (np.amax(airborne) > 0.5):
        in_air_transition_time = np.amin(status_time)
        print('log starts while in-air at ' + str(round(in_air_transition_time, 1)) + ' sec')
+        b_starts_in_air = True
    else:
        in_air_transition_time = float('NaN')
        print('always on ground')
@@ -519,6 +525,7 @@ def analyse_ekf(estimator_status, ekf2_innovations, sensor_preflight, check_leve
    elif (np.amax(airborne) > 0.5):
        on_ground_transition_time = np.amax(status_time)
        print('log finishes while in-air at ' + str(round(on_ground_transition_time, 1)) + ' sec')
+        b_finishes_in_air = True
    else:
        on_ground_transition_time = float('NaN')
        print('always on ground')
@@ -710,9 +717,10 @@ def analyse_ekf(estimator_status, ekf2_innovations, sensor_preflight, check_leve
    # 5 - true if the Z magnetometer observation has been rejected
    # 6 - true if the yaw observation has been rejected
    # 7 - true if the airspeed observation has been rejected
-    # 8 - true if the height above ground observation has been rejected
-    # 9 - true if the X optical flow observation has been rejected
-    # 10 - true if the Y optical flow observation has been rejected
+    # 8 - true if synthetic sideslip observation has been rejected
+    # 9 - true if the height above ground observation has been rejected
+    # 10 - true if the X optical flow observation has been rejected
+    # 11 - true if the Y optical flow observation has been rejected
    vel_innov_fail = ((2 ** 0 & estimator_status['innovation_check_flags']) > 0) * 1
    posh_innov_fail = ((2 ** 1 & estimator_status['innovation_check_flags']) > 0) * 1
    posv_innov_fail = ((2 ** 2 & estimator_status['innovation_check_flags']) > 0) * 1
@@ -721,14 +729,15 @@ def analyse_ekf(estimator_status, ekf2_innovations, sensor_preflight, check_leve
    magz_innov_fail = ((2 ** 5 & estimator_status['innovation_check_flags']) > 0) * 1
    yaw_innov_fail = ((2 ** 6 & estimator_status['innovation_check_flags']) > 0) * 1
    tas_innov_fail = ((2 ** 7 & estimator_status['innovation_check_flags']) > 0) * 1
-    hagl_innov_fail = ((2 ** 8 & estimator_status['innovation_check_flags']) > 0) * 1
-    ofx_innov_fail = ((2 ** 9 & estimator_status['innovation_check_flags']) > 0) * 1
-    ofy_innov_fail = ((2 ** 10 & estimator_status['innovation_check_flags']) > 0) * 1
+    sli_innov_fail = ((2 ** 8 & estimator_status['innovation_check_flags']) > 0) * 1
+    hagl_innov_fail = ((2 ** 9 & estimator_status['innovation_check_flags']) > 0) * 1
+    ofx_innov_fail = ((2 ** 10 & estimator_status['innovation_check_flags']) > 0) * 1
+    ofy_innov_fail = ((2 ** 11 & estimator_status['innovation_check_flags']) > 0) * 1

    if plot:
        # plot innovation_check_flags summary
        plt.figure(11, figsize=(20, 13))
-        plt.subplot(5, 1, 1)
+        plt.subplot(6, 1, 1)
        plt.title('EKF Innovation Test Fails')
        plt.plot(status_time, vel_innov_fail, 'b', label='vel NED')
        plt.plot(status_time, posh_innov_fail, 'r', label='pos NE')
@@ -736,14 +745,14 @@ def analyse_ekf(estimator_status, ekf2_innovations, sensor_preflight, check_leve
        plt.ylabel('failed')
        plt.legend(loc='upper left')
        plt.grid()
-        plt.subplot(5, 1, 2)
+        plt.subplot(6, 1, 2)
        plt.plot(status_time, posv_innov_fail, 'b', label='hgt absolute')
        plt.plot(status_time, hagl_innov_fail, 'r', label='hgt above ground')
        plt.ylim(-0.1, 1.1)
        plt.ylabel('failed')
        plt.legend(loc='upper left')
        plt.grid()
-        plt.subplot(5, 1, 3)
+        plt.subplot(6, 1, 3)
        plt.plot(status_time, magx_innov_fail, 'b', label='mag_x')
        plt.plot(status_time, magy_innov_fail, 'r', label='mag_y')
        plt.plot(status_time, magz_innov_fail, 'g', label='mag_z')
@@ -752,13 +761,19 @@ def analyse_ekf(estimator_status, ekf2_innovations, sensor_preflight, check_leve
        plt.ylim(-0.1, 1.1)
        plt.ylabel('failed')
        plt.grid()
-        plt.subplot(5, 1, 4)
+        plt.subplot(6, 1, 4)
        plt.plot(status_time, tas_innov_fail, 'b', label='airspeed')
        plt.ylim(-0.1, 1.1)
        plt.ylabel('failed')
        plt.legend(loc='upper left')
        plt.grid()
-        plt.subplot(5, 1, 5)
+        plt.subplot(6, 1, 5)
+        plt.plot(status_time, sli_innov_fail, 'b', label='sideslip')
+        plt.ylim(-0.1, 1.1)
+        plt.ylabel('failed')
+        plt.legend(loc='upper left')
+        plt.grid()
+        plt.subplot(6, 1, 6)
        plt.plot(status_time, ofx_innov_fail, 'b', label='flow X')
        plt.plot(status_time, ofy_innov_fail, 'r', label='flow Y')
        plt.ylim(-0.1, 1.1)
@@ -770,26 +785,29 @@ def analyse_ekf(estimator_status, ekf2_innovations, sensor_preflight, check_leve
        plt.close(11)
        # gps_check_fail_flags summary
        plt.figure(12, figsize=(20, 13))
-        # 0 : minimum required sat count fail
-        # 1 : minimum required GDoP fail
-        # 2 : maximum allowed horizontal position error fail
-        # 3 : maximum allowed vertical position error fail
-        # 4 : maximum allowed speed error fail
-        # 5 : maximum allowed horizontal position drift fail
-        # 6 : maximum allowed vertical position drift fail
-        # 7 : maximum allowed horizontal speed fail
-        # 8 : maximum allowed vertical velocity discrepancy fail
-        nsat_fail = ((2 ** 0 & estimator_status['gps_check_fail_flags']) > 0) * 1
-        gdop_fail = ((2 ** 1 & estimator_status['gps_check_fail_flags']) > 0) * 1
-        herr_fail = ((2 ** 2 & estimator_status['gps_check_fail_flags']) > 0) * 1
-        verr_fail = ((2 ** 3 & estimator_status['gps_check_fail_flags']) > 0) * 1
-        serr_fail = ((2 ** 4 & estimator_status['gps_check_fail_flags']) > 0) * 1
-        hdrift_fail = ((2 ** 5 & estimator_status['gps_check_fail_flags']) > 0) * 1
-        vdrift_fail = ((2 ** 6 & estimator_status['gps_check_fail_flags']) > 0) * 1
-        hspd_fail = ((2 ** 7 & estimator_status['gps_check_fail_flags']) > 0) * 1
-        veld_diff_fail = ((2 ** 8 & estimator_status['gps_check_fail_flags']) > 0) * 1
+        # 0 : insufficient fix type (no 3D solution)
+        # 1 : minimum required sat count fail
+        # 2 : minimum required GDoP fail
+        # 3 : maximum allowed horizontal position error fail
+        # 4 : maximum allowed vertical position error fail
+        # 5 : maximum allowed speed error fail
+        # 6 : maximum allowed horizontal position drift fail
+        # 7 : maximum allowed vertical position drift fail
+        # 8 : maximum allowed horizontal speed fail
+        # 9 : maximum allowed vertical velocity discrepancy fail
+        gfix_fail = ((2 ** 0 & estimator_status['gps_check_fail_flags']) > 0) * 1
+        nsat_fail = ((2 ** 1 & estimator_status['gps_check_fail_flags']) > 0) * 1
+        gdop_fail = ((2 ** 2 & estimator_status['gps_check_fail_flags']) > 0) * 1
+        herr_fail = ((2 ** 3 & estimator_status['gps_check_fail_flags']) > 0) * 1
+        verr_fail = ((2 ** 4 & estimator_status['gps_check_fail_flags']) > 0) * 1
+        serr_fail = ((2 ** 5 & estimator_status['gps_check_fail_flags']) > 0) * 1
+        hdrift_fail = ((2 ** 6 & estimator_status['gps_check_fail_flags']) > 0) * 1
+        vdrift_fail = ((2 ** 7 & estimator_status['gps_check_fail_flags']) > 0) * 1
+        hspd_fail = ((2 ** 8 & estimator_status['gps_check_fail_flags']) > 0) * 1
+        veld_diff_fail = ((2 ** 9 & estimator_status['gps_check_fail_flags']) > 0) * 1
        plt.subplot(2, 1, 1)
        plt.title('GPS Direct Output Check Failures')
+        plt.plot(status_time, gfix_fail, 'k', label='fix type')
        plt.plot(status_time, nsat_fail, 'b', label='N sats')
        plt.plot(status_time, gdop_fail, 'r', label='GDOP')
        plt.plot(status_time, herr_fail, 'g', label='horiz pos error')
@@ -997,22 +1015,27 @@ def analyse_ekf(estimator_status, ekf2_innovations, sensor_preflight, check_leve
        plt.close("all")

    # Do some automated analysis of the status data
-    # find a late/early index range from 5 sec after in_air_transtion_time to 5 sec before on-ground transition time for mag and optical flow checks to avoid false positives
-    # this can be used to prevent false positives for sensors adversely affected by close proximity to the ground
-    late_start_index = np.amin(np.where(status_time > (in_air_transition_time + 5.0)))
-    early_end_index = np.amax(np.where(status_time < (on_ground_transition_time - 5.0)))
-    num_valid_values_trimmed = (early_end_index - late_start_index + 1)
    # normal index range is defined by the flight duration
    start_index = np.amin(np.where(status_time > in_air_transition_time))
-    end_index = np.amax(np.where(status_time < on_ground_transition_time))
+    end_index = np.amax(np.where(status_time <= on_ground_transition_time))
    num_valid_values = (end_index - start_index + 1)
+    # find a late/early index range from 5 sec after in_air_transtion_time to 5 sec before on-ground transition time for mag and optical flow checks to avoid false positives
+    # this can be used to prevent false positives for sensors adversely affected by close proximity to the ground
+    # don't do this if the log starts or finishes in air or if it is shut off by flag
+    late_start_index = np.amin(np.where(status_time > (in_air_transition_time + 5.0)))\
+        if (late_start_early_ending and not b_starts_in_air) else start_index
+    early_end_index = np.amax(np.where(status_time <= (on_ground_transition_time - 5.0))) \
+        if (late_start_early_ending and not b_finishes_in_air) else end_index
+    num_valid_values_trimmed = (early_end_index - late_start_index + 1)
    # also find the start and finish indexes for the innovation data
-    innov_late_start_index = np.amin(np.where(innov_time > (in_air_transition_time + 5.0)))
-    innov_early_end_index = np.amax(np.where(innov_time < (on_ground_transition_time - 5.0)))
-    innov_num_valid_values_trimmed = (innov_early_end_index - innov_late_start_index + 1)
    innov_start_index = np.amin(np.where(innov_time > in_air_transition_time))
-    innov_end_index = np.amax(np.where(innov_time < on_ground_transition_time))
+    innov_end_index = np.amax(np.where(innov_time <= on_ground_transition_time))
    innov_num_valid_values = (innov_end_index - innov_start_index + 1)
+    innov_late_start_index = np.amin(np.where(innov_time > (in_air_transition_time + 5.0))) \
+        if (late_start_early_ending and not b_starts_in_air) else innov_start_index
+    innov_early_end_index = np.amax(np.where(innov_time <= (on_ground_transition_time - 5.0))) \
+        if (late_start_early_ending and not b_finishes_in_air) else innov_end_index
+    innov_num_valid_values_trimmed = (innov_early_end_index - innov_late_start_index + 1)
    # define dictionary of test results and descriptions
    test_results = {
        'master_status': ['Pass',
@@ -1033,8 +1056,16 @@ def analyse_ekf(estimator_status, ekf2_innovations, sensor_preflight, check_leve
                              'Airspeed sensor check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required'],
        'imu_sensor_status': ['Pass',
                              'IMU sensor check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required'],
+        'imu_vibration_check': ['Pass',
+                              'IMU vibration check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required'],
+        'imu_bias_check': ['Pass',
+                              'IMU bias check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required'],
+        'imu_output_predictor_check': ['Pass',
+                              'IMU output predictor check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required'],
        'flow_sensor_status': ['Pass',
                               'Optical Flow sensor check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required'],
+        'filter_fault_status': ['Pass',
+                               'Internal Filter check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required'],
        'mag_percentage_red': [float('NaN'),
                               'The percentage of in-flight consolidated magnetic field sensor innovation consistency test values > 1.0.'],
        'mag_percentage_amber': [float('NaN'),
@@ -1138,105 +1169,105 @@ def analyse_ekf(estimator_status, ekf2_innovations, sensor_preflight, check_leve
    if (innov_early_end_index > (innov_late_start_index + 100)):
        # Output Observer Tracking Errors
        test_results['output_obs_ang_err_median'][0] = np.median(
-            ekf2_innovations['output_tracking_error[0]'][innov_late_start_index:innov_early_end_index])
+            ekf2_innovations['output_tracking_error[0]'][innov_late_start_index:innov_early_end_index + 1])
        test_results['output_obs_vel_err_median'][0] = np.median(
-            ekf2_innovations['output_tracking_error[1]'][innov_late_start_index:innov_early_end_index])
+            ekf2_innovations['output_tracking_error[1]'][innov_late_start_index:innov_early_end_index + 1])
        test_results['output_obs_pos_err_median'][0] = np.median(
-            ekf2_innovations['output_tracking_error[2]'][innov_late_start_index:innov_early_end_index])
+            ekf2_innovations['output_tracking_error[2]'][innov_late_start_index:innov_early_end_index + 1])
    # reduction of status message data
    if (early_end_index > (late_start_index + 100)):
        # IMU vibration checks
        temp = np.amax(estimator_status['vibe[0]'][late_start_index:early_end_index])
        if (temp > 0.0):
            test_results['imu_coning_peak'][0] = temp
-            test_results['imu_coning_mean'][0] = np.mean(estimator_status['vibe[0]'][late_start_index:early_end_index])
+            test_results['imu_coning_mean'][0] = np.mean(estimator_status['vibe[0]'][late_start_index:early_end_index + 1])
        temp = np.amax(estimator_status['vibe[1]'][late_start_index:early_end_index])
        if (temp > 0.0):
            test_results['imu_hfdang_peak'][0] = temp
-            test_results['imu_hfdang_mean'][0] = np.mean(estimator_status['vibe[1]'][late_start_index:early_end_index])
+            test_results['imu_hfdang_mean'][0] = np.mean(estimator_status['vibe[1]'][late_start_index:early_end_index + 1])
        temp = np.amax(estimator_status['vibe[2]'][late_start_index:early_end_index])
        if (temp > 0.0):
            test_results['imu_hfdvel_peak'][0] = temp
-            test_results['imu_hfdvel_mean'][0] = np.mean(estimator_status['vibe[2]'][late_start_index:early_end_index])
+            test_results['imu_hfdvel_mean'][0] = np.mean(estimator_status['vibe[2]'][late_start_index:early_end_index + 1])

        # Magnetometer Sensor Checks
        if (np.amax(yaw_aligned) > 0.5):
-            mag_num_red = (estimator_status['mag_test_ratio'][start_index:end_index] > 1.0).sum()
-            mag_num_amber = (estimator_status['mag_test_ratio'][start_index:end_index] > 0.5).sum() - mag_num_red
+            mag_num_red = (estimator_status['mag_test_ratio'][start_index:end_index + 1] > 1.0).sum()
+            mag_num_amber = (estimator_status['mag_test_ratio'][start_index:end_index + 1] > 0.5).sum() - mag_num_red
            test_results['mag_percentage_red'][0] = 100.0 * mag_num_red / num_valid_values_trimmed
            test_results['mag_percentage_amber'][0] = 100.0 * mag_num_amber / num_valid_values_trimmed
            test_results['mag_test_max'][0] = np.amax(
-                estimator_status['mag_test_ratio'][late_start_index:early_end_index])
+                estimator_status['mag_test_ratio'][late_start_index:early_end_index + 1])
            test_results['mag_test_mean'][0] = np.mean(estimator_status['mag_test_ratio'][start_index:end_index])
            test_results['magx_fail_percentage'][0] = 100.0 * (
-                    magx_innov_fail[late_start_index:early_end_index] > 0.5).sum() / num_valid_values_trimmed
+                    magx_innov_fail[late_start_index:early_end_index + 1] > 0.5).sum() / num_valid_values_trimmed
            test_results['magy_fail_percentage'][0] = 100.0 * (
-                    magy_innov_fail[late_start_index:early_end_index] > 0.5).sum() / num_valid_values_trimmed
+                    magy_innov_fail[late_start_index:early_end_index + 1] > 0.5).sum() / num_valid_values_trimmed
            test_results['magz_fail_percentage'][0] = 100.0 * (
-                    magz_innov_fail[late_start_index:early_end_index] > 0.5).sum() / num_valid_values_trimmed
+                    magz_innov_fail[late_start_index:early_end_index + 1] > 0.5).sum() / num_valid_values_trimmed
            test_results['yaw_fail_percentage'][0] = 100.0 * (
-                    yaw_innov_fail[late_start_index:early_end_index] > 0.5).sum() / num_valid_values_trimmed
+                    yaw_innov_fail[late_start_index:early_end_index + 1] > 0.5).sum() / num_valid_values_trimmed

        # Velocity Sensor Checks
        if (np.amax(using_gps) > 0.5):
-            vel_num_red = (estimator_status['vel_test_ratio'][start_index:end_index] > 1.0).sum()
-            vel_num_amber = (estimator_status['vel_test_ratio'][start_index:end_index] > 0.5).sum() - vel_num_red
+            vel_num_red = (estimator_status['vel_test_ratio'][start_index:end_index + 1] > 1.0).sum()
+            vel_num_amber = (estimator_status['vel_test_ratio'][start_index:end_index + 1] > 0.5).sum() - vel_num_red
            test_results['vel_percentage_red'][0] = 100.0 * vel_num_red / num_valid_values
            test_results['vel_percentage_amber'][0] = 100.0 * vel_num_amber / num_valid_values
-            test_results['vel_test_max'][0] = np.amax(estimator_status['vel_test_ratio'][start_index:end_index])
-            test_results['vel_test_mean'][0] = np.mean(estimator_status['vel_test_ratio'][start_index:end_index])
+            test_results['vel_test_max'][0] = np.amax(estimator_status['vel_test_ratio'][start_index:end_index + 1])
+            test_results['vel_test_mean'][0] = np.mean(estimator_status['vel_test_ratio'][start_index:end_index + 1])
            test_results['vel_fail_percentage'][0] = 100.0 * (
-                    vel_innov_fail[start_index:end_index] > 0.5).sum() / num_valid_values
+                    vel_innov_fail[start_index:end_index + 1] > 0.5).sum() / num_valid_values

        # Position Sensor Checks
        if ((np.amax(using_gps) > 0.5) or (np.amax(using_evpos) > 0.5)):
-            pos_num_red = (estimator_status['pos_test_ratio'][start_index:end_index] > 1.0).sum()
-            pos_num_amber = (estimator_status['pos_test_ratio'][start_index:end_index] > 0.5).sum() - pos_num_red
+            pos_num_red = (estimator_status['pos_test_ratio'][start_index:end_index + 1] > 1.0).sum()
+            pos_num_amber = (estimator_status['pos_test_ratio'][start_index:end_index + 1] > 0.5).sum() - pos_num_red
            test_results['pos_percentage_red'][0] = 100.0 * pos_num_red / num_valid_values
            test_results['pos_percentage_amber'][0] = 100.0 * pos_num_amber / num_valid_values
-            test_results['pos_test_max'][0] = np.amax(estimator_status['pos_test_ratio'][start_index:end_index])
-            test_results['pos_test_mean'][0] = np.mean(estimator_status['pos_test_ratio'][start_index:end_index])
+            test_results['pos_test_max'][0] = np.amax(estimator_status['pos_test_ratio'][start_index:end_index + 1])
+            test_results['pos_test_mean'][0] = np.mean(estimator_status['pos_test_ratio'][start_index:end_index + 1])
            test_results['pos_fail_percentage'][0] = 100.0 * (
-                    posh_innov_fail[start_index:end_index] > 0.5).sum() / num_valid_values
+                    posh_innov_fail[start_index:end_index + 1] > 0.5).sum() / num_valid_values

        # Height Sensor Checks
-        hgt_num_red = (estimator_status['hgt_test_ratio'][late_start_index:early_end_index] > 1.0).sum()
-        hgt_num_amber = (estimator_status['hgt_test_ratio'][late_start_index:early_end_index] > 0.5).sum() - hgt_num_red
+        hgt_num_red = (estimator_status['hgt_test_ratio'][late_start_index:early_end_index + 1] > 1.0).sum()
+        hgt_num_amber = (estimator_status['hgt_test_ratio'][late_start_index:early_end_index + 1] > 0.5).sum() - hgt_num_red
        test_results['hgt_percentage_red'][0] = 100.0 * hgt_num_red / num_valid_values_trimmed
        test_results['hgt_percentage_amber'][0] = 100.0 * hgt_num_amber / num_valid_values_trimmed
-        test_results['hgt_test_max'][0] = np.amax(estimator_status['hgt_test_ratio'][late_start_index:early_end_index])
-        test_results['hgt_test_mean'][0] = np.mean(estimator_status['hgt_test_ratio'][late_start_index:early_end_index])
+        test_results['hgt_test_max'][0] = np.amax(estimator_status['hgt_test_ratio'][late_start_index:early_end_index + 1])
+        test_results['hgt_test_mean'][0] = np.mean(estimator_status['hgt_test_ratio'][late_start_index:early_end_index + 1])
        test_results['hgt_fail_percentage'][0] = 100.0 * (
-                posv_innov_fail[late_start_index:early_end_index] > 0.5).sum() / num_valid_values_trimmed
+                posv_innov_fail[late_start_index:early_end_index + 1] > 0.5).sum() / num_valid_values_trimmed

        # Airspeed Sensor Checks
        if (tas_test_max > 0.0):
-            tas_num_red = (estimator_status['tas_test_ratio'][start_index:end_index] > 1.0).sum()
-            tas_num_amber = (estimator_status['tas_test_ratio'][start_index:end_index] > 0.5).sum() - tas_num_red
+            tas_num_red = (estimator_status['tas_test_ratio'][start_index:end_index + 1] > 1.0).sum()
+            tas_num_amber = (estimator_status['tas_test_ratio'][start_index:end_index + 1] > 0.5).sum() - tas_num_red
            test_results['tas_percentage_red'][0] = 100.0 * tas_num_red / num_valid_values
            test_results['tas_percentage_amber'][0] = 100.0 * tas_num_amber / num_valid_values
-            test_results['tas_test_max'][0] = np.amax(estimator_status['tas_test_ratio'][start_index:end_index])
-            test_results['tas_test_mean'][0] = np.mean(estimator_status['tas_test_ratio'][start_index:end_index])
+            test_results['tas_test_max'][0] = np.amax(estimator_status['tas_test_ratio'][start_index:end_index + 1])
+            test_results['tas_test_mean'][0] = np.mean(estimator_status['tas_test_ratio'][start_index:end_index + 1])
            test_results['tas_fail_percentage'][0] = 100.0 * (
-                    tas_innov_fail[start_index:end_index] > 0.5).sum() / num_valid_values
+                    tas_innov_fail[start_index:end_index + 1] > 0.5).sum() / num_valid_values

        # HAGL Sensor Checks
        if (hagl_test_max > 0.0):
-            hagl_num_red = (estimator_status['hagl_test_ratio'][start_index:end_index] > 1.0).sum()
-            hagl_num_amber = (estimator_status['hagl_test_ratio'][start_index:end_index] > 0.5).sum() - hagl_num_red
+            hagl_num_red = (estimator_status['hagl_test_ratio'][start_index:end_index + 1] > 1.0).sum()
+            hagl_num_amber = (estimator_status['hagl_test_ratio'][start_index:end_index + 1] > 0.5).sum() - hagl_num_red
            test_results['hagl_percentage_red'][0] = 100.0 * hagl_num_red / num_valid_values
            test_results['hagl_percentage_amber'][0] = 100.0 * hagl_num_amber / num_valid_values
-            test_results['hagl_test_max'][0] = np.amax(estimator_status['hagl_test_ratio'][start_index:end_index])
-            test_results['hagl_test_mean'][0] = np.mean(estimator_status['hagl_test_ratio'][start_index:end_index])
+            test_results['hagl_test_max'][0] = np.amax(estimator_status['hagl_test_ratio'][start_index:end_index + 1])
+            test_results['hagl_test_mean'][0] = np.mean(estimator_status['hagl_test_ratio'][start_index:end_index + 1])
            test_results['hagl_fail_percentage'][0] = 100.0 * (
-                    hagl_innov_fail[start_index:end_index] > 0.5).sum() / num_valid_values
+                    hagl_innov_fail[start_index:end_index + 1] > 0.5).sum() / num_valid_values

        # optical flow sensor checks
        if (np.amax(using_optflow) > 0.5):
            test_results['ofx_fail_percentage'][0] = 100.0 * (
-                    ofx_innov_fail[late_start_index:early_end_index] > 0.5).sum() / num_valid_values_trimmed
+                    ofx_innov_fail[late_start_index:early_end_index + 1] > 0.5).sum() / num_valid_values_trimmed
            test_results['ofy_fail_percentage'][0] = 100.0 * (
-                    ofy_innov_fail[late_start_index:early_end_index] > 0.5).sum() / num_valid_values_trimmed
+                    ofy_innov_fail[late_start_index:early_end_index + 1] > 0.5).sum() / num_valid_values_trimmed

        # IMU bias checks
        test_results['imu_dang_bias_median'][0] = (np.median(estimator_status['states[10]']) ** 2 + np.median(
@@ -1278,16 +1309,19 @@ def analyse_ekf(estimator_status, ekf2_innovations, sensor_preflight, check_leve
            (test_results.get('imu_hfdvel_peak')[0] > check_levels.get('imu_hfdvel_peak_warn')) or
            (test_results.get('imu_hfdvel_mean')[0] > check_levels.get('imu_hfdvel_mean_warn'))):
        test_results['master_status'][0] = 'Warning'
-        test_results['imu_sensor_status'][0] = 'Warning - Vibration'
+        test_results['imu_sensor_status'][0] = 'Warning'
+        test_results['imu_vibration_check'][0] = 'Warning'
    if ((test_results.get('imu_dang_bias_median')[0] > check_levels.get('imu_dang_bias_median_warn')) or
            (test_results.get('imu_dvel_bias_median')[0] > check_levels.get('imu_dvel_bias_median_warn'))):
        test_results['master_status'][0] = 'Warning'
-        test_results['imu_sensor_status'][0] = 'Warning - Bias'
+        test_results['imu_sensor_status'][0] = 'Warning'
+        test_results['imu_bias_check'][0] = 'Warning'
    if ((test_results.get('output_obs_ang_err_median')[0] > check_levels.get('obs_ang_err_median_warn')) or
            (test_results.get('output_obs_vel_err_median')[0] > check_levels.get('obs_vel_err_median_warn')) or
            (test_results.get('output_obs_pos_err_median')[0] > check_levels.get('obs_pos_err_median_warn'))):
        test_results['master_status'][0] = 'Warning'
-        test_results['imu_sensor_status'][0] = 'Warning - Output Predictor'
+        test_results['imu_sensor_status'][0] = 'Warning'
+        test_results['imu_output_predictor_check'][0] = 'Warning'
    # check for failures
    if ((test_results.get('magx_fail_percentage')[0] > check_levels.get('mag_fail_pct')) or
            (test_results.get('magy_fail_percentage')[0] > check_levels.get('mag_fail_pct')) or
@@ -27,7 +27,7 @@ add_definitions(
 )

 px4_nuttx_make_uavcan_bootloadable(BOARD ${BOARD}
-	BIN ${PX4_BINARY_DIR}/platforms/nuttx/esc35-v1.bin
+	BIN ${PX4_BINARY_DIR}/esc35-v1.bin
 	HWNAME ${uavcanblid_name}
 	HW_MAJOR ${uavcanblid_hw_version_major}
 	HW_MINOR ${uavcanblid_hw_version_minor}
@@ -18,19 +18,12 @@ set(config_module_list
 	#drivers/blinkm
 	#drivers/camera_trigger
 	drivers/gps
-	#drivers/irlock
-	#drivers/mkblctrl
-	#drivers/oreoled
-	#drivers/pca9685
-	#drivers/pwm_input
 	drivers/px4flow
 	drivers/px4fmu
 	drivers/rgbled
 	drivers/stm32
-	#drivers/stm32/adc
+	drivers/stm32/adc
 	#drivers/stm32/tone_alarm
-	#drivers/tap_esc
-	#drivers/vmount
 	modules/sensors

 	#
@@ -38,8 +31,7 @@ set(config_module_list
 	#
 	systemcmds/bl_update
 	systemcmds/config
-	#systemcmds/dumpfile
-	#systemcmds/esc_calib
+	systemcmds/esc_calib
 	systemcmds/hardfault_log
 	systemcmds/led_control
 	systemcmds/mixer
@@ -50,7 +42,7 @@ set(config_module_list
 	systemcmds/perf
 	systemcmds/pwm
 	systemcmds/reboot
-	#systemcmds/sd_bench
+	systemcmds/sd_bench
 	systemcmds/top
 	systemcmds/topic_listener
 	systemcmds/tune_control
@@ -80,7 +72,6 @@ set(config_module_list
 	modules/load_mon
 	modules/mavlink
 	modules/navigator
-	#modules/uavcan

 	#
 	# Estimation modules
@@ -89,7 +80,6 @@ set(config_module_list
 	modules/ekf2
 	modules/landing_target_estimator
 	modules/local_position_estimator
-	#modules/position_estimator_inav
 	modules/wind_estimator

 	#
@@ -107,7 +97,6 @@ set(config_module_list
 	# Logging
 	#
 	modules/logger
-	modules/sdlog2

 	#
 	# Library modules
@@ -25,7 +25,7 @@ add_definitions(
 )

 px4_nuttx_make_uavcan_bootloadable(BOARD ${BOARD}
-	BIN ${PX4_BINARY_DIR}/platforms/nuttx/px4cannode-v1.bin
+	BIN ${PX4_BINARY_DIR}/px4cannode-v1.bin
 	HWNAME ${uavcanblid_name}
 	HW_MAJOR ${uavcanblid_hw_version_major}
 	HW_MINOR ${uavcanblid_hw_version_minor}
@@ -27,7 +27,7 @@ add_definitions(
 )

 px4_nuttx_make_uavcan_bootloadable(BOARD ${BOARD}
-	BIN ${PX4_BINARY_DIR}/platforms/nuttx/px4esc-v1.bin
+	BIN ${PX4_BINARY_DIR}/px4esc-v1.bin
 	HWNAME ${uavcanblid_name}
 	HW_MAJOR ${uavcanblid_hw_version_major}
 	HW_MINOR ${uavcanblid_hw_version_minor}
@@ -31,7 +31,8 @@ set(config_module_list
 	drivers/px4fmu
 	drivers/px4io
 	drivers/rgbled
-	drivers/rgbled_pwm
+	# Enable the line below to put the three leds into PWM RGB mode
+	#drivers/rgbled_pwm
 	drivers/stm32
 	drivers/stm32/adc
 	drivers/stm32/tone_alarm
@@ -21,7 +21,7 @@ include(configs/uavcan_board_ident/s2740vc-v1)

 # N.B. this would be uncommented when there is an APP
 #px4_nuttx_make_uavcan_bootloadable(BOARD ${BOARD}
-# BIN ${PX4_BINARY_DIR}/platforms/nuttx/s2740vc-v1.bin
+# BIN ${PX4_BINARY_DIR}/s2740vc-v1.bin
 # HWNAME ${uavcanblid_name}
 # HW_MAJOR ${uavcanblid_hw_version_major}
 # HW_MINOR ${uavcanblid_hw_version_minor}
@@ -24,6 +24,8 @@
    <!-- MAVROS configs -->
    <arg name="fcu_url" default="udp://:14540@localhost:14557"/>
    <arg name="respawn_mavros" default="false"/>
+    <!-- PX4 configs -->
+    <arg name="interactive" default="true"/>
    <!-- PX4 SITL and Gazebo -->
    <include file="$(find px4)/launch/posix_sitl.launch">
        <arg name="x" value="$(arg x)"/>
@@ -37,6 +39,7 @@
        <arg name="sdf" value="$(arg sdf)"/>
        <arg name="rcS" value="$(arg rcS)"/>
        <arg name="gui" value="$(arg gui)"/>
+        <arg name="interactive" value="$(arg interactive)"/>
        <arg name="debug" value="$(arg debug)"/>
        <arg name="verbose" value="$(arg verbose)"/>
        <arg name="paused" value="$(arg paused)"/>
@@ -21,8 +21,12 @@
    <arg name="verbose" default="false"/>
    <arg name="paused" default="false"/>
    <arg name="respawn_gazebo" default="false"/>
+    <!-- PX4 configs -->
+    <arg name="interactive" default="true"/>
    <!-- PX4 SITL -->
-    <node name="sitl" pkg="px4" type="px4" output="screen" args="$(find px4) $(arg rcS)"/>
+    <arg unless="$(arg interactive)" name="px4_command_arg1" value="-d"/>
+    <arg     if="$(arg interactive)" name="px4_command_arg1" value=""/>
+    <node name="sitl" pkg="px4" type="px4" output="screen" args="$(find px4) $(arg rcS) $(arg px4_command_arg1)" required="true"/>
    <!-- Gazebo sim -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="gui" value="$(arg gui)"/>
@@ -15,11 +15,15 @@
    <arg name="ID" default="1"/>
    <arg name="rcS" default="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_$(arg ID)"/>
    <arg name="mavlink_udp_port" default="14560"/>
+    <!-- PX4 configs -->
+    <arg name="interactive" default="true"/>
    <!-- generate urdf vehicle model -->
    <arg name="cmd" default="$(find xacro)/xacro $(find px4)/Tools/sitl_gazebo/models/rotors_description/urdf/$(arg vehicle)_base.xacro rotors_description_dir:=$(find px4)/Tools/sitl_gazebo/models/rotors_description mavlink_udp_port:=$(arg mavlink_udp_port) --inorder"/>
    <param command="$(arg cmd)" name="rotors_description"/>
    <!-- PX4 SITL -->
-    <node name="sitl_$(arg ID)" pkg="px4" type="px4" output="screen" args="$(find px4) $(arg rcS)">
+    <arg unless="$(arg interactive)" name="px4_command_arg1" value=""/>
+    <arg     if="$(arg interactive)" name="px4_command_arg1" value="-d"/>
+    <node name="sitl_$(arg ID)" pkg="px4" type="px4" output="screen" args="$(find px4) $(arg rcS) $(arg px4_command_arg1)">
    </node>
    <!-- spawn vehicle -->
    <node name="$(arg vehicle)_$(arg ID)_spawn" output="screen" pkg="gazebo_ros" type="spawn_model" args="-urdf -param rotors_description -model $(arg vehicle)_$(arg ID) -package_to_model -x $(arg x) -y $(arg y) -z $(arg z) -R $(arg R) -P $(arg P) -Y $(arg Y)"/>
@@ -13,38 +13,40 @@ float32[24] covariances	# Diagonal Elements of Covariance Matrix

 uint16 gps_check_fail_flags     # Bitmask to indicate status of GPS checks - see definition below
 # bits are true when corresponding test has failed
-uint16 GPS_CHECK_FAIL_GPS_FIX = 0				# 0 : insufficient fix type (no 3D solution)
-uint16 GPS_CHECK_FAIL_MIN_SAT_COUNT = 1			# 1 : minimum required sat count fail
-uint16 GPS_CHECK_FAIL_MIN_GDOP = 2				# 2 : minimum required GDoP fail
-uint16 GPS_CHECK_FAIL_MAX_HORZ_ERR = 3			# 3 : maximum allowed horizontal position error fail
-uint16 GPS_CHECK_FAIL_MAX_VERT_ERR = 4			# 4 : maximum allowed vertical position error fail
-uint16 GPS_CHECK_FAIL_MAX_SPD_ERR = 5			# 5 : maximum allowed speed error fail
-uint16 GPS_CHECK_FAIL_MAX_HORZ_DRIFT = 6		# 6 : maximum allowed horizontal position drift fail - requires stationary vehicle
-uint16 GPS_CHECK_FAIL_MAX_VERT_DRIFT = 7		# 7 : maximum allowed vertical position drift fail - requires stationary vehicle
-uint16 GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR = 8		# 8 : maximum allowed horizontal speed fail - requires stationary vehicle
-uint16 GPS_CHECK_FAIL_MAX_VERT_SPD_ERR = 9		# 9 : maximum allowed vertical velocity discrepancy fail
+uint8 GPS_CHECK_FAIL_GPS_FIX = 0		# 0 : insufficient fix type (no 3D solution)
+uint8 GPS_CHECK_FAIL_MIN_SAT_COUNT = 1		# 1 : minimum required sat count fail
+uint8 GPS_CHECK_FAIL_MIN_GDOP = 2		# 2 : minimum required GDoP fail
+uint8 GPS_CHECK_FAIL_MAX_HORZ_ERR = 3		# 3 : maximum allowed horizontal position error fail
+uint8 GPS_CHECK_FAIL_MAX_VERT_ERR = 4		# 4 : maximum allowed vertical position error fail
+uint8 GPS_CHECK_FAIL_MAX_SPD_ERR = 5		# 5 : maximum allowed speed error fail
+uint8 GPS_CHECK_FAIL_MAX_HORZ_DRIFT = 6		# 6 : maximum allowed horizontal position drift fail - requires stationary vehicle
+uint8 GPS_CHECK_FAIL_MAX_VERT_DRIFT = 7		# 7 : maximum allowed vertical position drift fail - requires stationary vehicle
+uint8 GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR = 8	# 8 : maximum allowed horizontal speed fail - requires stationary vehicle
+uint8 GPS_CHECK_FAIL_MAX_VERT_SPD_ERR = 9	# 9 : maximum allowed vertical velocity discrepancy fail

 uint32 control_mode_flags	# Bitmask to indicate EKF logic state
-# 0 - true if the filter tilt alignment is complete
-# 1 - true if the filter yaw alignment is complete
-# 2 - true if GPS measurements are being fused
-# 3 - true if optical flow measurements are being fused
-# 4 - true if a simple magnetic yaw heading is being fused
-# 5 - true if 3-axis magnetometer measurement are being fused
-# 6 - true if synthetic magnetic declination measurements are being fused
-# 7 - true when the vehicle is airborne
-# 8 - true when wind velocity is being estimated
-# 9 - true when baro height is being fused as a primary height reference
-# 10 - true when range finder height is being fused as a primary height reference
-# 11 - true when GPS height is being fused as a primary height reference
-# 12 - true when local position data from external vision is being fused
-# 13 - true when yaw data from external vision measurements is being fused
-# 14 - true when height data from external vision measurements is being fused
-# 15 - true when synthetic sideslip measurements are being fused
-# 16 - true when only the magnetic field states are updated by the magnetometer
-# 17 - true when the vehicle is operating as a fixed wing vehicle
-# 18 - true when the magnetomer has been declared faulty and is no longer being used
-# 19 - true when airspeed measurements are being fused
+uint8 CS_TILT_ALIGN = 0		# 0 - true if the filter tilt alignment is complete
+uint8 CS_YAW_ALIGN = 1		# 1 - true if the filter yaw alignment is complete
+uint8 CS_GPS = 2		# 2 - true if GPS measurements are being fused
+uint8 CS_OPT_FLOW = 3		# 3 - true if optical flow measurements are being fused
+uint8 CS_MAG_HDG = 4		# 4 - true if a simple magnetic yaw heading is being fused
+uint8 CS_MAG_3D = 5		# 5 - true if 3-axis magnetometer measurement are being fused
+uint8 CS_MAG_DEC = 6		# 6 - true if synthetic magnetic declination measurements are being fused
+uint8 CS_IN_AIR = 7		# 7 - true when thought to be airborne
+uint8 CS_WIND = 8		# 8 - true when wind velocity is being estimated
+uint8 CS_BARO_HGT = 9		# 9 - true when baro height is being fused as a primary height reference
+uint8 CS_RNG_HGT = 10		# 10 - true when range finder height is being fused as a primary height reference
+uint8 CS_GPS_HGT = 11		# 11 - true when GPS height is being fused as a primary height reference
+uint8 CS_EV_POS = 12		# 12 - true when local position data from external vision is being fused
+uint8 CS_EV_YAW = 13		# 13 - true when yaw data from external vision measurements is being fused
+uint8 CS_EV_HGT = 14		# 14 - true when height data from external vision measurements is being fused
+uint8 CS_BETA = 15		# 15 - true when synthetic sideslip measurements are being fused
+uint8 CS_MAG_FIELD = 16		# 16 - true when only the magnetic field states are updated by the magnetometer
+uint8 CS_FIXED_WING = 17	# 17 - true when thought to be operating as a fixed wing vehicle with constrained sideslip
+uint8 CS_MAG_FAULT = 18		# 18 - true when the magnetomer has been declared faulty and is no longer being used
+uint8 CS_ASPD = 19		# 19 - true when airspeed measurements are being fused
+uint8 CS_GND_EFFECT = 20	# 20 - true when when protection from ground effect induced static pressure rise is active
+uint8 CS_RNG_STUCK = 21		# 21 - true when a stuck range finder sensor has been detected

 uint16 filter_fault_flags	# Bitmask to indicate EKF internal faults
 # 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error
@@ -13,3 +13,7 @@ uint32 time_since_last_sonar_update	# time since last sonar update in microsecon
 uint16 frame_count_since_last_readout	# number of accumulated frames in timespan
 int16 gyro_temperature	# Temperature * 100 in centi-degrees Celsius
 uint8 quality	# Average of quality of accumulated frames, 0: bad quality, 255: maximum quality
+
+float32 max_flow_rate # Magnitude of maximum angular which the optical flow sensor can measure reliably
+float32 min_ground_distance # Minimum distance from ground at which the optical flow sensor operates reliably
+float32 max_ground_distance # Maximum distance from ground at which the optical flow sensor operates reliably
@@ -11,12 +11,25 @@ uint64 SUBSYSTEM_TYPE_EXTERNALGROUNDTRUTH = 512
 uint64 SUBSYSTEM_TYPE_ANGULARRATECONTROL = 1024
 uint64 SUBSYSTEM_TYPE_ATTITUDESTABILIZATION = 2048
 uint64 SUBSYSTEM_TYPE_YAWPOSITION = 4096
-uint64 SUBSYSTEM_TYPE_ALTITUDECONTROL = 16384
-uint64 SUBSYSTEM_TYPE_POSITIONCONTROL = 32768
-uint64 SUBSYSTEM_TYPE_MOTORCONTROL = 65536
-uint64 SUBSYSTEM_TYPE_RANGEFINDER = 131072
+uint64 SUBSYSTEM_TYPE_ALTITUDECONTROL = 8192
+uint64 SUBSYSTEM_TYPE_POSITIONCONTROL = 16384
+uint64 SUBSYSTEM_TYPE_MOTORCONTROL = 32768
+uint64 SUBSYSTEM_TYPE_RCRECEIVER = 65536
+uint64 SUBSYSTEM_TYPE_GYRO2 = 131072
+uint64 SUBSYSTEM_TYPE_ACC2 = 262144
+uint64 SUBSYSTEM_TYPE_MAG2 = 524288
+uint64 SUBSYSTEM_TYPE_GEOFENCE = 1048576
+uint64 SUBSYSTEM_TYPE_AHRS = 2097152
+uint64 SUBSYSTEM_TYPE_TERRAIN = 4194304
+uint64 SUBSYSTEM_TYPE_REVERSEMOTOR = 8388608
+uint64 SUBSYSTEM_TYPE_LOGGING = 16777216
+uint64 SUBSYSTEM_TYPE_SENSORBATTERY = 33554432
+uint64 SUBSYSTEM_TYPE_SENSORPROXIMITY = 67108864
+uint64 SUBSYSTEM_TYPE_MISSION = 134217728

 bool present
 bool enabled
 bool ok
 uint64 subsystem_type
+
+uint32 ORB_QUEUE_LENGTH = 5
@@ -57,6 +57,7 @@ import gencpp
 from px_generate_uorb_topic_helper import * # this is in Tools/

 uorb_struct = '%s_s'%spec.short_name
+uorb_struct_upper = spec.short_name.upper()
 topic_name = spec.short_name
 }@

@@ -83,7 +84,7 @@ for field in spec.parsed_fields():
@# Constants c style
 #ifndef __cplusplus
@[for constant in spec.constants]@
-#define @(constant.name) @(int(constant.val))
+#define @(uorb_struct_upper)_@(constant.name) @(int(constant.val))
@[end for]
 #endif

@@ -48,7 +48,7 @@ topic = spec.short_name
 *
 ****************************************************************************/

-/*! 
+/*!
 * @@file @(topic)_Publisher.cpp
 * This file contains the implementation of the publisher functions.
 *
@@ -74,26 +74,26 @@ topic = spec.short_name
 bool @(topic)_Publisher::init()
 {
    // Create RTPSParticipant
-    
+
    ParticipantAttributes PParam;
    PParam.rtps.builtin.domainId = 0;
    PParam.rtps.builtin.leaseDuration = c_TimeInfinite;
-    PParam.rtps.setName("Participant_publisher");  //You can put here the name you want
+    PParam.rtps.setName("@(topic)_publisher");  //You can put here the name you want
    mp_participant = Domain::createParticipant(PParam);
    if(mp_participant == nullptr)
        return false;
-    
+
    //Register the type
-    
-    Domain::registerType(mp_participant,(TopicDataType*) &myType);
-    
+
+    Domain::registerType(mp_participant, (TopicDataType*) &myType);
+
    // Create Publisher
-    
+
    PublisherAttributes Wparam;
    Wparam.topic.topicKind = NO_KEY;
    Wparam.topic.topicDataType = myType.getName();  //This type MUST be registered
-    Wparam.topic.topicName = "@(topic)_PubSubTopic";
-    mp_publisher = Domain::createPublisher(mp_participant,Wparam,(PublisherListener*)&m_listener);
+    Wparam.topic.topicName = "@(topic)";
+    mp_publisher = Domain::createPublisher(mp_participant, Wparam, (PublisherListener*) &m_listener);
    if(mp_publisher == nullptr)
        return false;
    //std::cout << "Publisher created, waiting for Subscribers." << std::endl;
@@ -120,13 +120,13 @@ void @(topic)_Publisher::run()
    {
        eClock::my_sleep(250); // Sleep 250 ms
    }
-    
+
    // Publication code
-    
+
    @(topic)_ st;
-    
+
    /* Initialize your structure here */
-    
+
    int msgsent = 0;
    char ch = 'y';
    do
@@ -48,7 +48,7 @@ topic = spec.short_name
 *
 ****************************************************************************/

-/*! 
+/*!
 * @@file @(topic)_Publisher.h
 * This header file contains the declaration of the publisher functions.
 *
@@ -65,8 +65,9 @@ topic = spec.short_name
 #include "@(topic)_PubSubTypes.h"

 using namespace eprosima::fastrtps;
+using namespace eprosima::fastrtps::rtps;

-class @(topic)_Publisher 
+class @(topic)_Publisher
 {
 public:
    @(topic)_Publisher();
@@ -77,16 +78,16 @@ public:
 private:
    Participant *mp_participant;
    Publisher *mp_publisher;
-    
+
    class PubListener : public PublisherListener
    {
    public:
        PubListener() : n_matched(0){};
        ~PubListener(){};
-        void onPublicationMatched(Publisher* pub,MatchingInfo& info);
+        void onPublicationMatched(Publisher* pub, MatchingInfo& info);
        int n_matched;
    } m_listener;
    @(topic)_PubSubType myType;
 };

-#endif // _@(topic)__PUBLISHER_H_
+#endif // _@(topic)__PUBLISHER_H_
@@ -48,7 +48,7 @@ topic = spec.short_name
 *
 ****************************************************************************/

-/*! 
+/*!
 * @@file @(topic)_Subscriber.cpp
 * This file contains the implementation of the subscriber functions.
 *
@@ -72,26 +72,26 @@ topic = spec.short_name
 bool @(topic)_Subscriber::init()
 {
    // Create RTPSParticipant
-    
+
    ParticipantAttributes PParam;
    PParam.rtps.builtin.domainId = 0; //MUST BE THE SAME AS IN THE PUBLISHER
    PParam.rtps.builtin.leaseDuration = c_TimeInfinite;
-    PParam.rtps.setName("Participant_subscriber"); //You can put the name you want
+    PParam.rtps.setName("@(topic)_subscriber"); //You can put the name you want
    mp_participant = Domain::createParticipant(PParam);
    if(mp_participant == nullptr)
            return false;
-    
+
    //Register the type
-    
-    Domain::registerType(mp_participant,(TopicDataType*) &myType);
-            
+
+    Domain::registerType(mp_participant, (TopicDataType*) &myType);
+
    // Create Subscriber
-            
+
    SubscriberAttributes Rparam;
    Rparam.topic.topicKind = NO_KEY;
    Rparam.topic.topicDataType = myType.getName(); //Must be registered before the creation of the subscriber
-    Rparam.topic.topicName = "@(topic)_PubSubTopic";
-    mp_subscriber = Domain::createSubscriber(mp_participant,Rparam,(SubscriberListener*)&m_listener);
+    Rparam.topic.topicName = "@(topic)";
+    mp_subscriber = Domain::createSubscriber(mp_participant, Rparam, (SubscriberListener*) &m_listener);
    if(mp_subscriber == nullptr)
        return false;
    return true;
@@ -143,4 +143,4 @@ bool @(topic)_Subscriber::hasMsg()
 {
    m_listener.has_msg = false;
    return m_listener.msg;
-}
+}
@@ -48,7 +48,7 @@ topic = spec.short_name
 *
 ****************************************************************************/

-/*! 
+/*!
 * @@file @(topic)_Subscriber.h
 * This header file contains the declaration of the subscriber functions.
 *
@@ -65,8 +65,9 @@ topic = spec.short_name
 #include "@(topic)_PubSubTypes.h"

 using namespace eprosima::fastrtps;
+using namespace eprosima::fastrtps::rtps;

-class @(topic)_Subscriber 
+class @(topic)_Subscriber
 {
 public:
    @(topic)_Subscriber();
@@ -78,13 +79,13 @@ public:
 private:
    Participant *mp_participant;
    Subscriber *mp_subscriber;
-    
+
    class SubListener : public SubscriberListener
    {
    public:
-        SubListener() : n_matched(0),n_msg(0){};
+        SubListener() : n_matched(0), n_msg(0){};
        ~SubListener(){};
-        void onSubscriptionMatched(Subscriber* sub,MatchingInfo& info);
+        void onSubscriptionMatched(Subscriber* sub, MatchingInfo& info);
        void onNewDataMessage(Subscriber* sub);
        SampleInfo_t m_info;
        int n_matched;
@@ -96,4 +97,4 @@ private:
    @(topic)_PubSubType myType;
 };

-#endif // _@(topic)__SUBSCRIBER_H_
+#endif // _@(topic)__SUBSCRIBER_H_
@@ -194,7 +194,7 @@ void t_send(void *data)
        // Send subscribed topics over UART
        while (topics.hasMsg(&topic_ID))
        {
-            uint16_t header_length = get_header_length();
+            uint16_t header_length = transport_node->get_header_length();
            /* make room for the header to fill in later */
            eprosima::fastcdr::FastBuffer cdrbuffer(&data_buffer[header_length], sizeof(data_buffer)-header_length);
            eprosima::fastcdr::Cdr scdr(cdrbuffer);
@@ -56,7 +56,9 @@ float32 evh				# Standard deviation of horizontal velocity error, (metres/sec)
 float32 evv				# Standard deviation of horizontal velocity error, (metres/sec)

 # estimator specified vehicle limits
-float32 vxy_max				# maximum horizontal speed - set to 0 when not applicable (metres/sec)
-bool limit_hagl				# true when the height above ground needs to be limited to observe optical flow focus and range finder limits
+float32 vxy_max				# maximum horizontal speed - set to 0 when limiting not required (meters/sec)
+float32 vz_max				# maximum vertical speed - set to 0 when limiting not required (meters/sec)
+float32 hagl_min			# minimum height above ground level - set to 0 when limiting not required (meters)
+float32 hagl_max			# maximum height above ground level - set to 0 when limiting not required (meters)

 # TOPICS vehicle_local_position vehicle_local_position_groundtruth vehicle_vision_position
@@ -27,5 +27,6 @@ bool offboard_control_loss_timeout                # true if offboard is lost for

 bool rc_signal_found_once
 bool rc_input_blocked                                # set if RC input should be ignored temporarily
+bool rc_calibration_valid                            # set if RC calibration is valid
 bool vtol_transition_failure                        # Set to true if vtol transition failed
 bool usb_connected                                # status of the USB power supply
@@ -52,7 +52,6 @@ list(APPEND nuttx_libs
 	nuttx_apps
 	nuttx_arch
 	nuttx_binfmt
-	nuttx_binfmt
 	nuttx_c
 	nuttx_configs
 	nuttx_cxx
@@ -116,12 +115,15 @@ if (config_romfs_root)
 endif()

 # create px4 file (combined firmware and metadata)
-set(fw_file ${PX4_BINARY_DIR}/${FW_NAME})
-string(REPLACE ".elf" ".px4" fw_file ${fw_file})
-string(REPLACE "nuttx_" "" fw_file ${fw_file})
+# for historical reasons we name the final output binary without nuttx_
+set(fw_name_short)
+string(REPLACE "nuttx_" "" fw_name_short ${FW_NAME})

-add_custom_command(OUTPUT ${BOARD}.bin
-	COMMAND ${OBJCOPY} -O binary ${PX4_BINARY_DIR_REL}/${FW_NAME} ${BOARD}.bin
+set(fw_file ${PX4_BINARY_DIR}/${fw_name_short})
+string(REPLACE ".elf" ".px4" fw_file ${fw_file})
+
+add_custom_command(OUTPUT ${PX4_BINARY_DIR_REL}/${BOARD}.bin
+	COMMAND ${OBJCOPY} -O binary ${PX4_BINARY_DIR_REL}/${FW_NAME} ${PX4_BINARY_DIR_REL}/${BOARD}.bin
 	DEPENDS ${FW_NAME}
 	)

@@ -136,8 +138,8 @@ if (TARGET parameters_xml AND TARGET airframes_xml)
 			--git_identity ${PX4_SOURCE_DIR}
 			--parameter_xml ${PX4_BINARY_DIR}/parameters.xml
 			--airframe_xml ${PX4_BINARY_DIR}/airframes.xml
-			--image ${BOARD}.bin > ${fw_file}
-		DEPENDS ${BOARD}.bin parameters_xml airframes_xml
+			--image ${PX4_BINARY_DIR}/${BOARD}.bin > ${fw_file}
+		DEPENDS ${PX4_BINARY_DIR}/${BOARD}.bin parameters_xml airframes_xml
 		COMMENT "Creating ${fw_file}"
 		)

@@ -184,6 +186,7 @@ if (TARGET parameters_xml AND TARGET airframes_xml)
 				${PX4_SOURCE_DIR}/Tools/px_uploader.py --port ${serial_ports} ${fw_file}
 			DEPENDS ${fw_file}
 			COMMENT "uploading px4"
+			VERBATIM
 			USES_TERMINAL
 			)
 	endif()
@@ -81,7 +81,7 @@ class NX_register_set(object):
 			self.regs['LR']         = self.mon_reg_call('lr')
 			self.regs['R15']        = self.mon_reg_call('r15')
 			self.regs['PC']         = self.mon_reg_call('pc')
-			self.regs['XPSR']       = self.mon_reg_call('xPSR')
+			#self.regs['XPSR']       = self.mon_reg_call('xPSR')
 		else:
 			for key in self.v7em_regmap.keys():
 				self.regs[key] = int(xcpt_regs[self.v7em_regmap[key]])
@@ -91,11 +91,11 @@ class NX_register_set(object):
 		register is the register as a string e.g. 'pc'
 		return integer containing the value of the register
 		"""
-		str_to_eval = "mon reg "+register
+		str_to_eval = "info registers "+register
 		resp = gdb.execute(str_to_eval,to_string = True)
-		content = resp.split()[-1];
+		content = resp.split()[-1]
 		try:
-			return int(content,16)
+			return int(content)
 		except:
 			return 0

@@ -371,7 +371,7 @@ CONFIG_STM32_HAVE_TIM14=y
 CONFIG_STM32_HAVE_ADC2=y
 CONFIG_STM32_HAVE_ADC3=y
 # CONFIG_STM32_HAVE_ADC4 is not set
-# CONFIG_STM32_HAVE_ADC1_DMA is not set
+CONFIG_STM32_HAVE_ADC1_DMA=y
 # CONFIG_STM32_HAVE_ADC2_DMA is not set
 # CONFIG_STM32_HAVE_ADC3_DMA is not set
 # CONFIG_STM32_HAVE_ADC4_DMA is not set
@@ -408,7 +408,7 @@ CONFIG_STM32_HAVE_I2S3=y
 # CONFIG_STM32_HAVE_OPAMP2 is not set
 # CONFIG_STM32_HAVE_OPAMP3 is not set
 # CONFIG_STM32_HAVE_OPAMP4 is not set
-# CONFIG_STM32_ADC1 is not set
+CONFIG_STM32_ADC1=y
 # CONFIG_STM32_ADC2 is not set
 # CONFIG_STM32_ADC3 is not set
 CONFIG_STM32_BKPSRAM=y
@@ -460,6 +460,7 @@ CONFIG_STM32_UART4=y
 CONFIG_STM32_USART6=y
 # CONFIG_STM32_IWDG is not set
 CONFIG_STM32_WWDG=y
+CONFIG_STM32_ADC=y
 CONFIG_STM32_SPI=y
 CONFIG_STM32_I2C=y
 # CONFIG_STM32_NOEXT_VECTORS is not set
@@ -488,6 +489,9 @@ CONFIG_STM32_DMACAPABLE=y
 # CONFIG_STM32_TIM1_PWM is not set
 # CONFIG_STM32_TIM3_PWM is not set
 # CONFIG_STM32_TIM5_PWM is not set
+# CONFIG_STM32_TIM1_ADC is not set
+# CONFIG_STM32_TIM3_ADC is not set
+# CONFIG_STM32_TIM5_ADC is not set
 # CONFIG_STM32_TIM1_CAP is not set
 # CONFIG_STM32_TIM3_CAP is not set
 # CONFIG_STM32_TIM4_CAP is not set
@@ -499,6 +503,12 @@ CONFIG_STM32_DMACAPABLE=y
 # CONFIG_STM32_TIM12_CAP is not set
 # CONFIG_STM32_TIM13_CAP is not set
 # CONFIG_STM32_TIM14_CAP is not set
+
+#
+# ADC Configuration
+#
+# CONFIG_STM32_ADC_NO_STARTUP_CONV is not set
+# CONFIG_STM32_ADC1_DMA is not set
 CONFIG_STM32_USART=y
 CONFIG_STM32_SERIALDRIVER=y

@@ -47,7 +47,7 @@ ExternalProject_Add_Step(sitl_gazebo forceconfigure
 # create targets for each viewer/model/debugger combination
 set(viewers none jmavsim gazebo replay)
 set(debuggers none ide gdb lldb ddd valgrind callgrind)
-set(models none iris iris_opt_flow iris_rplidar iris_irlock standard_vtol plane solo tailsitter typhoon_h480 rover hippocampus tiltrotor)
+set(models none iris iris_opt_flow iris_vision iris_rplidar iris_irlock standard_vtol plane solo tailsitter typhoon_h480 rover hippocampus tiltrotor)
 set(all_posix_vmd_make_targets)
 foreach(viewer ${viewers})
 	foreach(debugger ${debuggers})
@@ -83,6 +83,7 @@ extern "C" {
 		cout.flush();
 		_ExitFlag = true;
 	}
+
 	void _SigFpeHandler(int sig_num);
 	void _SigFpeHandler(int sig_num)
 	{
@@ -91,6 +92,15 @@ extern "C" {
 		PX4_BACKTRACE();
 		cout.flush();
 	}
+
+	void _SigSegvHandler(int sig_num);
+	void _SigSegvHandler(int sig_num)
+	{
+		cout.flush();
+		cout << endl << "segmentation fault" << endl;
+		PX4_BACKTRACE();
+		cout.flush();
+	}
 }

 static inline bool fileExists(const string &name)
@@ -299,20 +309,24 @@ int main(int argc, char **argv)
 	tcgetattr(0, &orig_term);
 	atexit(restore_term);

-	struct sigaction sig_int;
-	memset(&sig_int, 0, sizeof(struct sigaction));
+	// SIGINT
+	struct sigaction sig_int {};
 	sig_int.sa_handler = _SigIntHandler;
 	sig_int.sa_flags = 0;// not SA_RESTART!;
+	sigaction(SIGINT, &sig_int, nullptr);

-	struct sigaction sig_fpe;
-	memset(&sig_fpe, 0, sizeof(struct sigaction));
+	// SIGFPE
+	struct sigaction sig_fpe {};
 	sig_fpe.sa_handler = _SigFpeHandler;
 	sig_fpe.sa_flags = 0;// not SA_RESTART!;
-
-	sigaction(SIGINT, &sig_int, nullptr);
-	//sigaction(SIGTERM, &sig_int, NULL);
 	sigaction(SIGFPE, &sig_fpe, nullptr);

+	// SIGSEGV
+	struct sigaction sig_segv {};
+	sig_segv.sa_handler = _SigSegvHandler;
+	sig_segv.sa_flags = SA_RESTART | SA_SIGINFO;
+	sigaction(SIGSEGV, &sig_segv, nullptr);
+
 	set_cpu_scaling();

 	int index = 1;
@@ -519,11 +533,15 @@ int main(int argc, char **argv)

 		while (!_ExitFlag) {

-			char c = getchar();
+			int c = getchar();
 			string add_string; // string to add at current cursor position
 			bool update_prompt = true;

 			switch (c) {
+			case EOF:
+				_ExitFlag = true;
+				break;
+
 			case 127:	// backslash
 				if (mystr.length() - cursor_position > 0) {
 					mystr.erase(mystr.length() - cursor_position - 1, 1);
@@ -620,7 +638,7 @@ int main(int argc, char **argv)

 			default:	// any other input
 				if (c > 3) {
-					add_string += c;
+					add_string += (char)c;

 				} else {
 					update_prompt = false;
@@ -0,0 +1,83 @@
+uorb start
+param load
+param set MAV_TYPE 2
+param set SYS_AUTOSTART 4010
+param set SYS_RESTART_TYPE 2
+param set SYS_MC_EST_GROUP 2
+dataman start
+param set BAT_N_CELLS 3
+param set CAL_GYRO0_ID 2293768
+param set CAL_ACC0_ID 1376264
+param set CAL_ACC1_ID 1310728
+param set CAL_MAG0_ID 196616
+param set CAL_GYRO0_XOFF 0.01
+param set CAL_ACC0_XOFF 0.01
+param set CAL_ACC0_YOFF -0.01
+param set CAL_ACC0_ZOFF 0.01
+param set CAL_ACC0_XSCALE 1.01
+param set CAL_ACC0_YSCALE 1.01
+param set CAL_ACC0_ZSCALE 1.01
+param set CAL_ACC1_XOFF 0.01
+param set CAL_MAG0_XOFF 0.01
+param set SENS_BOARD_ROT 0
+param set SENS_BOARD_X_OFF 0.000001
+param set COM_RC_IN_MODE 1
+param set NAV_DLL_ACT 2
+param set COM_DISARM_LAND 3
+param set NAV_ACC_RAD 2.0
+param set COM_OF_LOSS_T 5
+param set COM_OBL_ACT 2
+param set COM_OBL_RC_ACT 0
+param set RTL_RETURN_ALT 30.0
+param set RTL_DESCEND_ALT 5.0
+param set RTL_LAND_DELAY 5
+param set SDLOG_DIRS_MAX 7
+param set MIS_TAKEOFF_ALT 2.5
+param set MC_ROLLRATE_P 0.2
+param set MC_PITCHRATE_P 0.2
+param set MC_PITCH_P 6
+param set MC_ROLL_P 6
+param set MPC_HOLD_MAX_Z 2.0
+param set MPC_Z_VEL_P 0.6
+param set MPC_Z_VEL_I 0.15
+param set EKF2_GBIAS_INIT 0.01
+param set EKF2_ANGERR_INIT 0.01
+param set EKF2_MAG_TYPE 1
+param set EKF2_AID_MASK 8
+param set EKF2_HGT_MODE 0
+param set EKF2_EV_DELAY 5
+param set EKF2_EVP_NOISE 0.05
+param set EKF2_EVA_NOISE 0.05
+replay tryapplyparams
+simulator start -s
+tone_alarm start
+gyrosim start
+accelsim start
+barosim start
+gpssim start
+pwm_out_sim start
+sensors start
+commander start
+land_detector start multicopter
+navigator start
+ekf2 start
+mc_pos_control start
+mc_att_control start
+mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
+mavlink start -x -u 14556 -r 4000000
+mavlink start -x -u 14557 -r 4000000 -m onboard -o 14540
+mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556
+mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556
+mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 14556
+mavlink stream -r 50 -s ATTITUDE -u 14556
+mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 14556
+mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556
+mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14556
+mavlink stream -r 20 -s RC_CHANNELS -u 14556
+mavlink stream -r 250 -s HIGHRES_IMU -u 14556
+mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
+mavlink stream -r 10 -s DISTANCE_SENSOR -u 14556
+mavlink stream -r 10 -s VISION_POSITION_ESTIMATE -u 14556
+logger start -e -t
+mavlink boot_complete
+replay trystart
@@ -26,7 +26,7 @@ param set MAV_TYPE 13
 param set MC_PITCH_P 6
 param set MC_PITCHRATE_P 0.1
 param set MC_ROLL_P 6
-param set MC_ROLLRATE_P 0.1
+param set MC_ROLLRATE_P 0.05
 param set MIS_TAKEOFF_ALT 2.5
 param set MPC_HOLD_MAX_Z 2.0
 param set MPC_XY_VEL_I 0.2
@@ -50,7 +50,7 @@
 #include <stdio.h>
 #include <math.h>
 #include <unistd.h>
-#include <getopt.h>
+#include <px4_getopt.h>
 #include <px4_log.h>

 #include <nuttx/arch.h>
@@ -849,11 +849,12 @@ usage()
 int
 bmp280_main(int argc, char *argv[])
 {
-	enum BMP280_BUS busid = BMP280_BUS_ALL;
+	int myoptind = 1;
 	int ch;
+	const char *myoptarg = nullptr;
+	enum BMP280_BUS busid = BMP280_BUS_ALL;

-	/* jump over start/off/etc and look at options first */
-	while ((ch = getopt(argc, argv, "XISs")) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "XISs", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'X':
 			busid = BMP280_BUS_I2C_EXTERNAL;
@@ -861,8 +862,6 @@ bmp280_main(int argc, char *argv[])

 		case 'I':
 			busid = BMP280_BUS_I2C_INTERNAL;
-			//PX4_ERR("not supported yet");
-			//exit(1);
 			break;

 		case 'S':
@@ -875,11 +874,16 @@ bmp280_main(int argc, char *argv[])

 		default:
 			bmp280::usage();
-			exit(0);
+			return 0;
 		}
 	}

-	const char *verb = argv[optind];
+	if (myoptind >= argc) {
+		bmp280::usage();
+		return -1;
+	}
+
+	const char *verb = argv[myoptind];

 	/*
 	 * Start/load the driver.
@@ -910,5 +914,5 @@ bmp280_main(int argc, char *argv[])
 	}

 	PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'");
-	exit(1);
+	return -1;
 }
@@ -48,7 +48,6 @@
 #include <uORB/uORB.h>

 #include <float.h>
-#include <getopt.h>

 static constexpr uint8_t WHO_AM_I = 0x0F;
 static constexpr uint8_t LPS22HB_ID_WHO_AM_I = 0xB1;
@@ -33,6 +33,8 @@

 #include "LPS22HB.hpp"

+#include <px4_getopt.h>
+
 #include <cstring>

 extern "C" __EXPORT int lps22hb_main(int argc, char *argv[]);
@@ -58,6 +60,12 @@ struct lps22hb_bus_option {
 	LPS22HB	*dev;
 } bus_options[] = {
 	{ LPS22HB_BUS_I2C_EXTERNAL, "/dev/lps22hb_ext", &LPS22HB_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL },
+#ifdef PX4_I2C_BUS_EXPANSION1
+	{ LPS22HB_BUS_I2C_EXTERNAL, "/dev/lps22hb_ext1", &LPS22HB_I2C_interface, PX4_I2C_BUS_EXPANSION1, NULL },
+#endif
+#ifdef PX4_I2C_BUS_EXPANSION2
+	{ LPS22HB_BUS_I2C_EXTERNAL, "/dev/lps22hb_ext2", &LPS22HB_I2C_interface, PX4_I2C_BUS_EXPANSION2, NULL },
+#endif
 #ifdef PX4_I2C_BUS_ONBOARD
 	{ LPS22HB_BUS_I2C_INTERNAL, "/dev/lps22hb_int", &LPS22HB_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL },
 #endif
@@ -234,10 +242,13 @@ usage()
 int
 lps22hb_main(int argc, char *argv[])
 {
-	enum LPS22HB_BUS busid = LPS22HB_BUS_ALL;
+	int myoptind = 1;
 	int ch;
+	const char *myoptarg = nullptr;

-	while ((ch = getopt(argc, argv, "XIS:")) != EOF) {
+	enum LPS22HB_BUS busid = LPS22HB_BUS_ALL;
+
+	while ((ch = px4_getopt(argc, argv, "XIS:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 #if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC)

@@ -256,11 +267,16 @@ lps22hb_main(int argc, char *argv[])

 		default:
 			lps22hb::usage();
-			exit(0);
+			return 0;
 		}
 	}

-	const char *verb = argv[optind];
+	if (myoptind >= argc) {
+		lps22hb::usage();
+		return -1;
+	}
+
+	const char *verb = argv[myoptind];

 	/*
 	 * Start/load the driver.
@@ -789,6 +789,12 @@ struct lps25h_bus_option {
 	LPS25H	*dev;
 } bus_options[] = {
 	{ LPS25H_BUS_I2C_EXTERNAL, "/dev/lps25h_ext", &LPS25H_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL },
+#ifdef PX4_I2C_BUS_EXPANSION1
+	{ LPS25H_BUS_I2C_EXTERNAL, "/dev/lps25h_ext1", &LPS25H_I2C_interface, PX4_I2C_BUS_EXPANSION1, NULL },
+#endif
+#ifdef PX4_I2C_BUS_EXPANSION2
+	{ LPS25H_BUS_I2C_EXTERNAL, "/dev/lps25h_ext2", &LPS25H_I2C_interface, PX4_I2C_BUS_EXPANSION2, NULL },
+#endif
 #ifdef PX4_I2C_BUS_ONBOARD
 	{ LPS25H_BUS_I2C_INTERNAL, "/dev/lps25h_int", &LPS25H_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL },
 #endif
@@ -714,6 +714,12 @@ struct mpl3115a2_bus_option {
 #ifdef PX4_I2C_BUS_EXPANSION
 	{ MPL3115A2_BUS_I2C_EXTERNAL, "/dev/mpl3115a2_ext", &MPL3115A2_i2c_interface, PX4_I2C_BUS_EXPANSION, NULL },
 #endif
+#ifdef PX4_I2C_BUS_EXPANSION1
+	{ MPL3115A2_BUS_I2C_EXTERNAL, "/dev/mpl3115a2_ext1", &MPL3115A2_i2c_interface, PX4_I2C_BUS_EXPANSION1, NULL },
+#endif
+#ifdef PX4_I2C_BUS_EXPANSION2
+	{ MPL3115A2_BUS_I2C_EXTERNAL, "/dev/mpl3115a2_ext2", &MPL3115A2_i2c_interface, PX4_I2C_BUS_EXPANSION2, NULL },
+#endif
 };
 #define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))

@@ -50,7 +50,6 @@
 #include <stdio.h>
 #include <math.h>
 #include <unistd.h>
-#include <getopt.h>

 #include <nuttx/arch.h>
 #include <nuttx/wqueue.h>
@@ -870,6 +869,12 @@ struct ms5611_bus_option {
 #ifdef PX4_I2C_BUS_EXPANSION
 	{ MS5611_BUS_I2C_EXTERNAL, "/dev/ms5611_ext", &MS5611_i2c_interface, PX4_I2C_BUS_EXPANSION, NULL },
 #endif
+#ifdef PX4_I2C_BUS_EXPANSION1
+	{ MS5611_BUS_I2C_EXTERNAL, "/dev/ms5611_ext1", &MS5611_i2c_interface, PX4_I2C_BUS_EXPANSION1, NULL },
+#endif
+#ifdef PX4_I2C_BUS_EXPANSION2
+	{ MS5611_BUS_I2C_EXTERNAL, "/dev/ms5611_ext2", &MS5611_i2c_interface, PX4_I2C_BUS_EXPANSION2, NULL },
+#endif
 };
 #define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))

@@ -1198,10 +1203,15 @@ ms5611_main(int argc, char *argv[])

 		default:
 			ms5611::usage();
-			exit(0);
+			return 0;
 		}
 	}

+	if (myoptind >= argc) {
+		ms5611::usage();
+		return -1;
+	}
+
 	const char *verb = argv[myoptind];

 	/*
@@ -1232,5 +1242,6 @@ ms5611_main(int argc, char *argv[])
 		ms5611::info();
 	}

-	errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'");
+	PX4_ERR("unrecognised command, try 'start', 'test', 'reset' or 'info'");
+	return -1;
 }
@@ -53,7 +53,6 @@
 #include <px4_workqueue.h>
 #include <perf/perf_counter.h>
 #include <uORB/topics/battery_status.h>
-#include <uORB/topics/subsystem_info.h>
 #include <uORB/uORB.h>

 #define BATT_SMBUS_ADDR_MIN             0x00	///< lowest possible address
@@ -250,6 +249,7 @@ int serial_number();
 BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) :
 	I2C("batt_smbus", "/dev/batt_smbus0", bus, batt_smbus_addr, 100000),
 	_enabled(false),
+	_last_report{},
 	_batt_topic(nullptr),
 	_batt_orb_id(nullptr),
 	_start_time(0),
@@ -112,7 +112,6 @@

 #include <perf/perf_counter.h>
 #include <systemlib/err.h>
-#include <systemlib/systemlib.h>

 #include <board_config.h>

@@ -73,7 +73,6 @@
 #include <systemlib/cpuload.h>
 #include <systemlib/err.h>
 #include <systemlib/hardfault_log.h>
-#include <systemlib/systemlib.h>
 #include <parameters/param.h>

 # if defined(FLASH_BASED_PARAMS)
@@ -78,7 +78,6 @@

 #include <systemlib/hardfault_log.h>

-#include <systemlib/systemlib.h>
 #include <parameters/param.h>

 /****************************************************************************
@@ -71,7 +71,6 @@
 #include <systemlib/cpuload.h>
 #include <systemlib/err.h>
 #include <systemlib/hardfault_log.h>
-#include <systemlib/systemlib.h>
 #include <parameters/param.h>

 /****************************************************************************
@@ -72,7 +72,6 @@
 #include <parameters/param.h>

 #if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE)
-#include <systemlib/systemlib.h>
 #endif

 # if defined(FLASH_BASED_PARAMS)
@@ -78,7 +78,6 @@

 #include <systemlib/hardfault_log.h>

-#include <systemlib/systemlib.h>
 #include <parameters/param.h>

 /****************************************************************************
@@ -80,7 +80,6 @@
 #include <systemlib/hardfault_log.h>
 #endif

-#include <systemlib/systemlib.h>

 /****************************************************************************
 * Pre-Processor Definitions
@@ -69,26 +69,16 @@
 *
 * These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4 Firmware in the adc driver
 */
-// TODO: ADCs, eg. pixracer
-//#define ADC_CHANNELS (1 << 2) | (1 << 3) | (1 << 4) | (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14)
+#define ADC_CHANNELS (1 << 0) | (1 << 11) | (1 << 12)

-// Placeholder
-#define ADC_BATTERY_VOLTAGE_CHANNEL	((uint8_t)(-1))
-#define ADC_BATTERY_CURRENT_CHANNEL	((uint8_t)(-1))
-#define ADC_AIRSPEED_VOLTAGE_CHANNEL	((uint8_t)(-1))
-
-// TODO: ADCs
-//#define ADC_BATTERY_VOLTAGE_CHANNEL  2
-//#define ADC_BATTERY_CURRENT_CHANNEL  3
-//#define ADC_5V_RAIL_SENSE            4
-//#define ADC_RC_RSSI_CHANNEL          11
+#define ADC_BATTERY_VOLTAGE_CHANNEL  12
+#define ADC_BATTERY_CURRENT_CHANNEL  11
+#define ADC_RC_RSSI_CHANNEL          0

 /* Define Battery 1 Voltage Divider and A per V
 */
-
-// TODO:
-//#define BOARD_BATTERY1_V_DIV         (13.653333333f)
-//#define BOARD_BATTERY1_A_PER_V       (36.367515152f)
+#define BOARD_BATTERY1_V_DIV         (11.12f)
+#define BOARD_BATTERY1_A_PER_V       (31.f)

 /* User GPIOs
 *
@@ -78,7 +78,6 @@

 #include <systemlib/hardfault_log.h>

-#include <systemlib/systemlib.h>
 #include <parameters/param.h>

 /****************************************************************************
@@ -185,12 +184,10 @@ stm32_boardinitialize(void)
 	board_autoled_initialize();


-	//TODO: ADCs
-	///* configure ADC pins */
-	//stm32_configgpio(GPIO_ADC1_IN2);	/* BATT_VOLTAGE_SENS */
-	//stm32_configgpio(GPIO_ADC1_IN3);	/* BATT_CURRENT_SENS */
-	//stm32_configgpio(GPIO_ADC1_IN4);	/* VDD_5V_SENS */
-	//stm32_configgpio(GPIO_ADC1_IN11);	/* RSSI analog in */
+	/* configure ADC pins */
+	stm32_configgpio(GPIO_ADC1_IN12);	/* BATT_VOLTAGE_SENS */
+	stm32_configgpio(GPIO_ADC1_IN11);	/* BATT_CURRENT_SENS */
+	stm32_configgpio(GPIO_ADC1_IN0);	/* RSSI analog in */

 	// TODO: power peripherals
 	///* configure power supply control/sense pins */
@@ -105,4 +105,3 @@ __EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
 {
 	uinfo("resume: %d\n", resume);
 }
-
@@ -80,7 +80,6 @@

 #include <systemlib/hardfault_log.h>

-#include <systemlib/systemlib.h>
 #include <parameters/param.h>

 /****************************************************************************
@@ -72,7 +72,6 @@
 #include <parameters/param.h>

 #if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE)
-#include <systemlib/systemlib.h>
 #endif

 #include "board_config.h"
@@ -72,7 +72,6 @@
 #include <parameters/param.h>

 #if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE)
-#include <systemlib/systemlib.h>
 #endif

 # if defined(FLASH_BASED_PARAMS)
@@ -71,7 +71,6 @@
 #include <systemlib/cpuload.h>

 #if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE)
-#include <systemlib/systemlib.h>
 #endif

 #include "board_config.h"
@@ -78,7 +78,6 @@

 #include <systemlib/hardfault_log.h>

-#include <systemlib/systemlib.h>
 #include <parameters/param.h>

 /****************************************************************************
@@ -79,7 +79,6 @@

 #include <systemlib/hardfault_log.h>

-#include <systemlib/systemlib.h>
 #include <parameters/param.h>

 /****************************************************************************
@@ -79,7 +79,6 @@

 #include <systemlib/hardfault_log.h>

-#include <systemlib/systemlib.h>
 #include <parameters/param.h>

 /****************************************************************************
@@ -228,10 +228,9 @@

 #define PX4_I2C_BUS_EXPANSION	1
 #define PX4_I2C_BUS_EXPANSION1	2
-#define PX4_I2C_BUS_EXPANSION2	3
-#define PX4_I2C_BUS_EXPANSION3	4
+#define PX4_I2C_BUS_ONBOARD		3
+#define PX4_I2C_BUS_EXPANSION2	4
 #define PX4_I2C_BUS_LED			PX4_I2C_BUS_EXPANSION
-#define PX4_I2C_BUS_ONBOARD	PX4_I2C_BUS_EXPANSION2

 #define BOARD_NUMBER_I2C_BUSES  4
 #define BOARD_I2C_BUS_CLOCK_INIT {100000, 100000, 100000, 100000}
@@ -316,7 +315,7 @@
 /* Define Battery 1 Voltage Divider and A per V
 */

-#define BOARD_BATTERY1_V_DIV         (10.133333333f)
+#define BOARD_BATTERY1_V_DIV         (18.1f) 		/* measured with the provided PM board */
 #define BOARD_BATTERY1_A_PER_V       (36.367515152f)

 /* HW has to large of R termination on ADC todo:change when HW value is chosen */
@@ -549,7 +548,11 @@

 #define GPIO_RSSI_IN                       /* PB0  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0)
 #define GPIO_RSSI_IN_INIT                  /* PB0  */ 0 /* Leave as ADC RSSI_IN */
-#define GPIO_nSAFETY_SWITCH_LED_OUT        /* PE12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN12)
+/* Change GPIO_nSAFETY_SWITCH_LED_OUT to
+ * (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN12)
+ * to enable the safety switch led on FMU
+ */
+#define GPIO_nSAFETY_SWITCH_LED_OUT        /* PE12 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_PORTE|GPIO_PIN12)
 #define GPIO_nSAFETY_SWITCH_LED_OUT_INIT   /* PE12 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_PORTE|GPIO_PIN12)
 #define GPIO_SAFETY_SWITCH_IN              /* PE10 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN10)

@@ -81,7 +81,6 @@

 #include <systemlib/hardfault_log.h>

-#include <systemlib/systemlib.h>
 #include <parameters/param.h>

 #include "up_internal.h"
@@ -79,7 +79,6 @@

 #include <systemlib/hardfault_log.h>

-#include <systemlib/systemlib.h>
 #include <parameters/param.h>

 #include "up_internal.h"
@@ -72,7 +72,6 @@
 #include <parameters/param.h>

 #if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE)
-#include <systemlib/systemlib.h>
 #endif

 #include "board_config.h"
@@ -71,7 +71,6 @@
 #include <systemlib/cpuload.h>
 #include <systemlib/err.h>
 #include <systemlib/hardfault_log.h>
-#include <systemlib/systemlib.h>
 #include <parameters/param.h>

 # if defined(FLASH_BASED_PARAMS)
@@ -71,7 +71,6 @@
 #include <systemlib/cpuload.h>

 #if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE)
-#include <systemlib/systemlib.h>
 #endif

 #include "board_config.h"
@@ -51,7 +51,6 @@
 #include <mathlib/mathlib.h>
 #include <matrix/math.hpp>
 #include <px4_workqueue.h>
-#include <systemlib/systemlib.h>
 #include <systemlib/err.h>
 #include <parameters/param.h>
 #include <systemlib/mavlink_log.h>
@@ -115,6 +115,9 @@ PARAM_DEFINE_INT32(TRIG_MODE, 0);
 * Selects which pin is used, ranges from 1 to 6 (AUX1-AUX6 on px4fmu-v2 and the rail
 * pins on px4fmu-v4). The PWM interface takes two pins per camera, while relay
 * triggers on every pin individually. Example: Value 56 would trigger on pins 5 and 6.
+ * For GPIO mode Pin 6 will be triggered followed by 5. With a value of 65 pin 5 will
+ * be triggered followed by 6. Pins may be non contiguous. I.E. 16 or 61.
+ * In GPIO mode the delay pin to pin is < .2 uS.
 *
 * @min 1
 * @max 123456
@@ -27,7 +27,7 @@ public:
 	 * trigger the camera
 	 * @param enable
 	 */
-	virtual void trigger(bool enable) {}
+	virtual void trigger(bool trigger_on_true) {}

 	/**
 	 * send command to turn the camera on/off
@@ -1,15 +1,21 @@
 #ifdef __PX4_NUTTX

 #include "gpio.h"
+#include <cstring>

 constexpr uint32_t CameraInterfaceGPIO::_gpios[6];

 CameraInterfaceGPIO::CameraInterfaceGPIO():
 	CameraInterface(),
-	_polarity(0)
+	_trigger_invert(false),
+	_triggers{0}
 {
 	_p_polarity = param_find("TRIG_POLARITY");
-	param_get(_p_polarity, &_polarity);
+
+	// polarity of the trigger (0 = active low, 1 = active high )
+	int32_t polarity;
+	param_get(_p_polarity, &polarity);
+	_trigger_invert = (polarity == 0);

 	get_pins();
 	setup();
@@ -21,29 +27,28 @@ CameraInterfaceGPIO::~CameraInterfaceGPIO()

 void CameraInterfaceGPIO::setup()
 {
-	for (unsigned i = 0; i < arraySize(_pins); i++) {
-		px4_arch_configgpio(_gpios[_pins[i]]);
-		px4_arch_gpiowrite(_gpios[_pins[i]], !_polarity);
+	for (unsigned i = 0, t = 0; i < arraySize(_pins); i++) {
+
+		// Pin range is from 1 to 6, indexes are 0 to 5
+
+		if (_pins[i] >= 0 && _pins[i] < (int)arraySize(_gpios)) {
+			uint32_t gpio = _gpios[_pins[i]];
+			_triggers[t++] = gpio;
+			px4_arch_configgpio(gpio);
+			px4_arch_gpiowrite(gpio, false ^ _trigger_invert);
+		}
 	}
 }

-void CameraInterfaceGPIO::trigger(bool enable)
+void CameraInterfaceGPIO::trigger(bool trigger_on_true)
 {
-	if (enable) {
-		for (unsigned i = 0; i < arraySize(_pins); i++) {
-			if (_pins[i] >= 0) {
-				// ACTIVE_LOW == 1
-				px4_arch_gpiowrite(_gpios[_pins[i]], _polarity);
-			}
-		}
+	bool trigger_state = trigger_on_true ^ _trigger_invert;

-	} else {
-		for (unsigned i = 0; i < arraySize(_pins); i++) {
-			if (_pins[i] >= 0) {
-				// ACTIVE_LOW == 1
-				px4_arch_gpiowrite(_gpios[_pins[i]], !_polarity);
-			}
-		}
+	for (unsigned i = 0; i < arraySize(_triggers); i++) {
+
+		if (_triggers[i] == 0) { break; }
+
+		px4_arch_gpiowrite(_triggers[i], trigger_state);
 	}
 }

@@ -51,7 +56,7 @@ void CameraInterfaceGPIO::info()
 {
 	PX4_INFO("GPIO trigger mode, pins enabled : [%d][%d][%d][%d][%d][%d], polarity : %s",
 		 _pins[5], _pins[4], _pins[3], _pins[2], _pins[1], _pins[0],
-		 _polarity ? "ACTIVE_HIGH" : "ACTIVE_LOW");
+		 _trigger_invert ? "ACTIVE_LOW" : "ACTIVE_HIGH");
 }

 #endif /* ifdef __PX4_NUTTX */
@@ -18,7 +18,7 @@ public:
 	CameraInterfaceGPIO();
 	virtual ~CameraInterfaceGPIO();

-	void trigger(bool enable);
+	void trigger(bool trigger_on_true);

 	void info();

@@ -28,7 +28,7 @@ private:

 	param_t _p_polarity;

-	int _polarity;
+	bool _trigger_invert;

 	static constexpr uint32_t _gpios[6] = {
 		GPIO_GPIO0_OUTPUT,
@@ -39,6 +39,7 @@ private:
 		GPIO_GPIO5_OUTPUT
 	};

+	uint32_t _triggers[arraySize(_gpios)];
 };

 #endif /* ifdef __PX4_NUTTX */
@@ -46,12 +46,12 @@ void CameraInterfacePWM::setup()

 }

-void CameraInterfacePWM::trigger(bool enable)
+void CameraInterfacePWM::trigger(bool trigger_on_true)
 {
 	for (unsigned i = 0; i < arraySize(_pins); i++) {
 		if (_pins[i] >= 0) {
 			// Set all valid pins to shoot or neutral levels
-			up_pwm_trigger_set(_pins[i], math::constrain(enable ? PWM_CAMERA_SHOOT : PWM_CAMERA_NEUTRAL, 1000, 2000));
+			up_pwm_trigger_set(_pins[i], math::constrain(trigger_on_true ? PWM_CAMERA_SHOOT : PWM_CAMERA_NEUTRAL, 1000, 2000));
 		}
 	}
 }
@@ -20,7 +20,7 @@ public:
 	CameraInterfacePWM();
 	virtual ~CameraInterfacePWM();

-	void trigger(bool enable);
+	void trigger(bool trigger_on_true);

 	void info();

@@ -52,7 +52,7 @@ void CameraInterfaceSeagull::setup()
 	PX4_ERR("Bad pin configuration - Seagull MAP2 requires 2 consecutive pins for control.");
 }

-void CameraInterfaceSeagull::trigger(bool enable)
+void CameraInterfaceSeagull::trigger(bool trigger_on_true)
 {

 	if (!_camera_is_on) {
@@ -62,7 +62,7 @@ void CameraInterfaceSeagull::trigger(bool enable)
 	for (unsigned i = 0; i < arraySize(_pins); i = i + 2) {
 		if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
 			// Set channel 1 to shoot or neutral levels
-			up_pwm_trigger_set(_pins[i + 1], enable ? PWM_1_CAMERA_INSTANT_SHOOT : PWM_CAMERA_NEUTRAL);
+			up_pwm_trigger_set(_pins[i + 1], trigger_on_true ? PWM_1_CAMERA_INSTANT_SHOOT : PWM_CAMERA_NEUTRAL);
 		}
 	}
 }
@@ -18,7 +18,7 @@ public:
 	CameraInterfaceSeagull();
 	virtual ~CameraInterfaceSeagull();

-	void trigger(bool enable);
+	void trigger(bool trigger_on_true);

 	void send_keep_alive(bool enable);

@@ -38,14 +38,16 @@
 * Driver for the Eagle Tree Airspeed V3 connected via I2C.
 */

+#include <cfloat>
+
 #include <px4_config.h>

 #include <drivers/device/i2c.h>

-#include <systemlib/airspeed.h>
 #include <systemlib/err.h>
 #include <parameters/param.h>
 #include <perf/perf_counter.h>
+#include <px4_getopt.h>

 #include <drivers/drv_airspeed.h>
 #include <drivers/drv_hrt.h>
@@ -53,7 +55,6 @@

 #include <uORB/uORB.h>
 #include <uORB/topics/differential_pressure.h>
-#include <uORB/topics/subsystem_info.h>
 #include <drivers/airspeed/airspeed.h>

 /* I2C bus address */
@@ -238,19 +239,58 @@ namespace ets_airspeed

 ETSAirspeed	*g_dev;

-int start(int i2c_bus);
+int bus_options[] = {
+#ifdef PX4_I2C_BUS_EXPANSION
+	PX4_I2C_BUS_EXPANSION,
+#endif
+#ifdef PX4_I2C_BUS_EXPANSION1
+	PX4_I2C_BUS_EXPANSION1,
+#endif
+#ifdef PX4_I2C_BUS_EXPANSION2
+	PX4_I2C_BUS_EXPANSION2,
+#endif
+#ifdef PX4_I2C_BUS_ONBOARD
+	PX4_I2C_BUS_ONBOARD,
+#endif
+};
+
+#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
+
+int start();
+int start_bus(int i2c_bus);
 int stop();
 int reset();
 int info();

 /**
- * Start the driver.
+ * Attempt to start driver on all available I2C busses.
+ *
+ * This function will return as soon as the first sensor
+ * is detected on one of the available busses or if no
+ * sensors are detected.
+ *
+ */
+int
+start()
+{
+	for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
+		if (start_bus(bus_options[i]) == PX4_OK) {
+			return PX4_OK;
+		}
+	}
+
+	return PX4_ERROR;
+
+}
+
+/**
+ * Start the driver on a specific bus.
 *
 * This function only returns if the sensor is up and running
 * or could not be detected successfully.
 */
 int
-start(int i2c_bus)
+start_bus(int i2c_bus)
 {
 	int fd;

@@ -348,6 +388,7 @@ ets_airspeed_usage()
 	PX4_INFO("usage: ets_airspeed command [options]");
 	PX4_INFO("options:");
 	PX4_INFO("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
+	PX4_INFO("\t-a --all");
 	PX4_INFO("command:");
 	PX4_INFO("\tstart|stop|reset|info");
 }
@@ -357,38 +398,58 @@ ets_airspeed_main(int argc, char *argv[])
 {
 	int i2c_bus = PX4_I2C_BUS_DEFAULT;

-	int i;
+	int myoptind = 1;
+	int ch;
+	const char *myoptarg = nullptr;
+	bool start_all = false;

-	for (i = 1; i < argc; i++) {
-		if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) {
-			if (argc > i + 1) {
-				i2c_bus = atoi(argv[i + 1]);
-			}
+	while ((ch = px4_getopt(argc, argv, "ab:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'b':
+			i2c_bus = atoi(myoptarg);
+			break;
+
+		case 'a':
+			start_all = true;
+			break;
+
+		default:
+			ets_airspeed_usage();
+			return 0;
 		}
 	}

+	if (myoptind >= argc) {
+		ets_airspeed_usage();
+		return -1;
+	}
+
 	/*
 	 * Start/load the driver.
 	 */
-	if (!strcmp(argv[1], "start")) {
-		return ets_airspeed::start(i2c_bus);
+	if (!strcmp(argv[myoptind], "start")) {
+		if (start_all) {
+			return ets_airspeed::start();
+
+		} else {
+			return ets_airspeed::start_bus(i2c_bus);
+		}
 	}

 	/*
 	 * Stop the driver
 	 */
-	if (!strcmp(argv[1], "stop")) {
+	if (!strcmp(argv[myoptind], "stop")) {
 		return ets_airspeed::stop();
 	}

 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset")) {
+	if (!strcmp(argv[myoptind], "reset")) {
 		return ets_airspeed::reset();
 	}

 	ets_airspeed_usage();
-
-	return PX4_OK;
+	return 0;
 }
@@ -50,10 +50,10 @@
 */

 #include <px4_config.h>
+#include <px4_getopt.h>

 #include <drivers/device/i2c.h>

-#include <systemlib/airspeed.h>
 #include <systemlib/err.h>
 #include <parameters/param.h>
 #include <perf/perf_counter.h>
@@ -65,7 +65,6 @@

 #include <uORB/uORB.h>
 #include <uORB/topics/differential_pressure.h>
-#include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/system_power.h>

 #include <drivers/airspeed/airspeed.h>
@@ -82,6 +81,7 @@
 #define MEAS_DRIVER_FILTER_FREQ 1.2f
 #define CONVERSION_INTERVAL	(1000000 / MEAS_RATE)	/* microseconds */

+
 class MEASAirspeed : public Airspeed
 {
 public:
@@ -374,18 +374,57 @@ namespace meas_airspeed

 MEASAirspeed	*g_dev = nullptr;

-int start(int i2c_bus);
+int bus_options[] = {
+#ifdef PX4_I2C_BUS_EXPANSION
+	PX4_I2C_BUS_EXPANSION,
+#endif
+#ifdef PX4_I2C_BUS_EXPANSION1
+	PX4_I2C_BUS_EXPANSION1,
+#endif
+#ifdef PX4_I2C_BUS_EXPANSION2
+	PX4_I2C_BUS_EXPANSION2,
+#endif
+#ifdef PX4_I2C_BUS_ONBOARD
+	PX4_I2C_BUS_ONBOARD,
+#endif
+};
+
+#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
+
+int start();
+int start_bus(int i2c_bus);
 int stop();
 int reset();

 /**
- * Start the driver.
+* Attempt to start driver on all available I2C busses.
+*
+* This function will return as soon as the first sensor
+* is detected on one of the available busses or if no
+* sensors are detected.
+*
+*/
+int
+start()
+{
+	for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
+		if (start_bus(bus_options[i]) == PX4_OK) {
+			return PX4_OK;
+		}
+	}
+
+	return PX4_ERROR;
+
+}
+
+/**
+ * Start the driver on a specific bus.
 *
 * This function call only returns once the driver is up and running
 * or failed to detect the sensor.
 */
 int
-start(int i2c_bus)
+start_bus(int i2c_bus)
 {
 	int fd;

@@ -484,6 +523,7 @@ meas_airspeed_usage()
 	PX4_INFO("usage: meas_airspeed command [options]");
 	PX4_INFO("options:");
 	PX4_INFO("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
+	PX4_INFO("\t-a --all");
 	PX4_INFO("command:");
 	PX4_INFO("\tstart|stop|reset");
 }
@@ -493,38 +533,60 @@ ms4525_airspeed_main(int argc, char *argv[])
 {
 	int i2c_bus = PX4_I2C_BUS_DEFAULT;

-	int i;
+	int myoptind = 1;
+	int ch;
+	const char *myoptarg = nullptr;

-	for (i = 1; i < argc; i++) {
-		if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) {
-			if (argc > i + 1) {
-				i2c_bus = atoi(argv[i + 1]);
-			}
+	bool start_all = false;
+
+	while ((ch = px4_getopt(argc, argv, "ab:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'b':
+			i2c_bus = atoi(myoptarg);
+			break;
+
+		case 'a':
+			start_all = true;
+			break;
+
+		default:
+			meas_airspeed_usage();
+			return 0;
 		}
 	}

+	if (myoptind >= argc) {
+		meas_airspeed_usage();
+		return -1;
+	}
+
 	/*
 	 * Start/load the driver.
 	 */
-	if (!strcmp(argv[1], "start")) {
-		return meas_airspeed::start(i2c_bus);
+	if (!strcmp(argv[myoptind], "start")) {
+		if (start_all) {
+			return meas_airspeed::start();
+
+		} else {
+			return meas_airspeed::start_bus(i2c_bus);
+		}
+
 	}

 	/*
 	 * Stop the driver
 	 */
-	if (!strcmp(argv[1], "stop")) {
+	if (!strcmp(argv[myoptind], "stop")) {
 		return meas_airspeed::stop();
 	}

 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset")) {
+	if (!strcmp(argv[myoptind], "reset")) {
 		return meas_airspeed::reset();
 	}

 	meas_airspeed_usage();
-
-	return PX4_OK;
+	return 0;
 }
@@ -41,7 +41,6 @@
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
 #include <px4_config.h>
 #include <sys/types.h>
-#include <systemlib/airspeed.h>
 #include <perf/perf_counter.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/uORB.h>
@@ -33,6 +33,8 @@

 #include "MS5525.hpp"

+#include <px4_getopt.h>
+
 // Driver 'main' command.
 extern "C" __EXPORT int ms5525_airspeed_main(int argc, char *argv[]);

@@ -41,15 +43,56 @@ namespace ms5525_airspeed
 {
 MS5525 *g_dev = nullptr;

-int start(uint8_t i2c_bus);
+int bus_options[] = {
+#ifdef PX4_I2C_BUS_EXPANSION
+	PX4_I2C_BUS_EXPANSION,
+#endif
+#ifdef PX4_I2C_BUS_EXPANSION1
+	PX4_I2C_BUS_EXPANSION1,
+#endif
+#ifdef PX4_I2C_BUS_EXPANSION2
+	PX4_I2C_BUS_EXPANSION2,
+#endif
+#ifdef PX4_I2C_BUS_ONBOARD
+	PX4_I2C_BUS_ONBOARD,
+#endif
+};
+
+#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
+
+int start();
+int start_bus(uint8_t i2c_bus);
 int stop();
 int reset();

-// Start the driver.
-// This function call only returns once the driver is up and running
-// or failed to detect the sensor.
+/**
+* Attempt to start driver on all available I2C busses.
+*
+* This function will return as soon as the first sensor
+* is detected on one of the available busses or if no
+* sensors are detected.
+*
+*/
 int
-start(uint8_t i2c_bus)
+start()
+{
+	for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
+		if (start_bus(bus_options[i]) == PX4_OK) {
+			return PX4_OK;
+		}
+	}
+
+	return PX4_ERROR;
+}
+
+/**
+ * Start the driver on a specific bus.
+ *
+ * This function call only returns once the driver is up and running
+ * or failed to detect the sensor.
+ */
+int
+start_bus(uint8_t i2c_bus)
 {
 	int fd = -1;

@@ -139,11 +182,12 @@ int reset()
 static void
 ms5525_airspeed_usage()
 {
-	PX4_WARN("usage: ms5525_airspeed command [options]");
-	PX4_WARN("options:");
-	PX4_WARN("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
-	PX4_WARN("command:");
-	PX4_WARN("\tstart|stop|reset");
+	PX4_INFO("usage: ms5525_airspeed command [options]");
+	PX4_INFO("options:");
+	PX4_INFO("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
+	PX4_INFO("\t-a --all");
+	PX4_INFO("command:");
+	PX4_INFO("\tstart|stop|reset");
 }

 int
@@ -151,36 +195,58 @@ ms5525_airspeed_main(int argc, char *argv[])
 {
 	uint8_t i2c_bus = PX4_I2C_BUS_DEFAULT;

-	for (int i = 1; i < argc; i++) {
-		if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) {
-			if (argc > i + 1) {
-				i2c_bus = atoi(argv[i + 1]);
-			}
+	int myoptind = 1;
+	int ch;
+	const char *myoptarg = nullptr;
+	bool start_all = false;
+
+	while ((ch = px4_getopt(argc, argv, "ab:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'b':
+			i2c_bus = atoi(myoptarg);
+			break;
+
+		case 'a':
+			start_all = true;
+			break;
+
+		default:
+			ms5525_airspeed_usage();
+			return 0;
 		}
 	}

+	if (myoptind >= argc) {
+		ms5525_airspeed_usage();
+		return -1;
+	}
+
 	/*
 	 * Start/load the driver.
 	 */
-	if (!strcmp(argv[1], "start")) {
-		return ms5525_airspeed::start(i2c_bus);
+	if (!strcmp(argv[myoptind], "start")) {
+		if (start_all) {
+			return ms5525_airspeed::start();
+
+		} else {
+			return ms5525_airspeed::start_bus(i2c_bus);
+		}
 	}

 	/*
 	 * Stop the driver
 	 */
-	if (!strcmp(argv[1], "stop")) {
+	if (!strcmp(argv[myoptind], "stop")) {
 		return ms5525_airspeed::stop();
 	}

 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset")) {
+	if (!strcmp(argv[myoptind], "reset")) {
 		return ms5525_airspeed::reset();
 	}

 	ms5525_airspeed_usage();
-
-	return PX4_OK;
+	return 0;
 }
@@ -49,7 +49,6 @@
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
 #include <px4_config.h>
 #include <sys/types.h>
-#include <systemlib/airspeed.h>
 #include <perf/perf_counter.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/uORB.h>
@@ -32,6 +32,7 @@
 ****************************************************************************/

 #include "SDP3X.hpp"
+#include <px4_getopt.h>

 // Driver 'main' command.
 extern "C" __EXPORT int sdp3x_airspeed_main(int argc, char *argv[]);
@@ -41,15 +42,56 @@ namespace sdp3x_airspeed
 {
 SDP3X *g_dev = nullptr;

-int start(uint8_t i2c_bus);
+int bus_options[] = {
+#ifdef PX4_I2C_BUS_EXPANSION
+	PX4_I2C_BUS_EXPANSION,
+#endif
+#ifdef PX4_I2C_BUS_EXPANSION1
+	PX4_I2C_BUS_EXPANSION1,
+#endif
+#ifdef PX4_I2C_BUS_EXPANSION2
+	PX4_I2C_BUS_EXPANSION2,
+#endif
+#ifdef PX4_I2C_BUS_ONBOARD
+	PX4_I2C_BUS_ONBOARD,
+#endif
+};
+
+#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
+
+int start();
+int start_bus(uint8_t i2c_bus);
 int stop();
 int reset();

-// Start the driver.
-// This function call only returns once the driver is up and running
-// or failed to detect the sensor.
+/**
+ * Attempt to start driver on all available I2C busses.
+ *
+ * This function will return as soon as the first sensor
+ * is detected on one of the available busses or if no
+ * sensors are detected.
+ *
+ */
 int
-start(uint8_t i2c_bus)
+start()
+{
+	for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
+		if (start_bus(bus_options[i]) == PX4_OK) {
+			return PX4_OK;
+		}
+	}
+
+	return PX4_ERROR;
+}
+
+/**
+ * Start the driver on a specific bus.
+ *
+ * This function call only returns once the driver is up and running
+ * or failed to detect the sensor.
+ */
+int
+start_bus(uint8_t i2c_bus)
 {
 	int fd = -1;

@@ -152,11 +194,12 @@ int reset()
 static void
 sdp3x_airspeed_usage()
 {
-	PX4_WARN("usage: sdp3x_airspeed command [options]");
-	PX4_WARN("options:");
-	PX4_WARN("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
-	PX4_WARN("command:");
-	PX4_WARN("\tstart|stop|reset");
+	PX4_INFO("usage: sdp3x_airspeed command [options]");
+	PX4_INFO("options:");
+	PX4_INFO("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
+	PX4_INFO("\t-a --all");
+	PX4_INFO("command:");
+	PX4_INFO("\tstart|stop|reset");
 }

 int
@@ -164,36 +207,59 @@ sdp3x_airspeed_main(int argc, char *argv[])
 {
 	uint8_t i2c_bus = PX4_I2C_BUS_DEFAULT;

-	for (int i = 1; i < argc; i++) {
-		if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) {
-			if (argc > i + 1) {
-				i2c_bus = atoi(argv[i + 1]);
-			}
+	int myoptind = 1;
+	int ch;
+	const char *myoptarg = nullptr;
+	bool start_all = false;
+
+	while ((ch = px4_getopt(argc, argv, "ab:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'b':
+			i2c_bus = atoi(myoptarg);
+			break;
+
+		case 'a':
+			start_all = true;
+			break;
+
+		default:
+			sdp3x_airspeed_usage();
+			return 0;
 		}
 	}

+	if (myoptind >= argc) {
+		sdp3x_airspeed_usage();
+		return -1;
+	}
+
+
 	/*
 	 * Start/load the driver.
 	 */
-	if (!strcmp(argv[1], "start")) {
-		return sdp3x_airspeed::start(i2c_bus);
+	if (!strcmp(argv[myoptind], "start")) {
+		if (start_all) {
+			return sdp3x_airspeed::start();
+
+		} else {
+			return sdp3x_airspeed::start_bus(i2c_bus);
+		}
 	}

 	/*
 	 * Stop the driver
 	 */
-	if (!strcmp(argv[1], "stop")) {
+	if (!strcmp(argv[myoptind], "stop")) {
 		return sdp3x_airspeed::stop();
 	}

 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset")) {
+	if (!strcmp(argv[myoptind], "reset")) {
 		return sdp3x_airspeed::reset();
 	}

 	sdp3x_airspeed_usage();
-
-	return PX4_OK;
+	return 0;
 }
@@ -64,7 +64,6 @@
 #include <drivers/device/ringbuffer.h>

 #include <uORB/uORB.h>
-#include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/distance_sensor.h>

 #define SR04_MAX_RANGEFINDERS 6
@@ -73,7 +72,6 @@
 /* Configuration Constants */
 #define SR04_DEVICE_PATH	"/dev/hc_sr04"

-#define SUBSYSTEM_TYPE_RANGEFINDER 131072
 /* Device limits */
 #define SR04_MIN_DISTANCE 	(0.10f)
 #define SR04_MAX_DISTANCE 	(4.00f)
@@ -623,25 +621,6 @@ HC_SR04::start()
 		   (worker_t)&HC_SR04::cycle_trampoline,
 		   this,
 		   USEC2TICK(_cycling_rate));
-
-
-	/* notify about state change */
-	struct subsystem_info_s info = {};
-	info.present = true;
-	info.enabled = true;
-	info.ok = true;
-	info.subsystem_type = SUBSYSTEM_TYPE_RANGEFINDER;
-
-	static orb_advert_t pub = nullptr;
-
-	if (pub != nullptr) {
-		orb_publish(ORB_ID(subsystem_info), pub, &info);
-
-
-	} else {
-		pub = orb_advertise(ORB_ID(subsystem_info), &info);
-
-	}
 }

 void
@@ -75,6 +75,9 @@ static constexpr struct ll40ls_bus_option {
 #ifdef PX4_I2C_BUS_EXPANSION1
 	{ LL40LS_BUS_I2C_EXTERNAL, "/dev/ll40ls_ext1", PX4_I2C_BUS_EXPANSION1 },
 #endif
+#ifdef PX4_I2C_BUS_EXPANSION2
+	{ LL40LS_BUS_I2C_EXTERNAL, "/dev/ll40ls_ext2", PX4_I2C_BUS_EXPANSION2 },
+#endif
 #ifdef PX4_I2C_BUS_ONBOARD
 	{ LL40LS_BUS_I2C_INTERNAL, "/dev/ll40ls_int", PX4_I2C_BUS_ONBOARD },
 #endif
@@ -67,7 +67,6 @@
 #include <drivers/device/ringbuffer.h>

 #include <uORB/uORB.h>
-#include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/distance_sensor.h>

 #include <board_config.h>
@@ -588,24 +587,6 @@ MB12XX::start()

 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 5);
-
-	/* notify about state change */
-	struct subsystem_info_s info = {};
-	info.present = true;
-	info.enabled = true;
-	info.ok = true;
-	info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER;
-
-	static orb_advert_t pub = nullptr;
-
-	if (pub != nullptr) {
-		orb_publish(ORB_ID(subsystem_info), pub, &info);
-
-
-	} else {
-		pub = orb_advertise(ORB_ID(subsystem_info), &info);
-
-	}
 }

 void
@@ -879,25 +860,27 @@ info()
 int
 mb12xx_main(int argc, char *argv[])
 {
-	// check for optional arguments
 	int ch;
 	int myoptind = 1;
 	const char *myoptarg = nullptr;
 	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;

-
 	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'R':
 			rotation = (uint8_t)atoi(myoptarg);
-			PX4_INFO("Setting distance sensor orientation to %d", (int)rotation);
 			break;

 		default:
 			PX4_WARN("Unknown option!");
+			return -1;
 		}
 	}

+	if (myoptind >= argc) {
+		goto out_error;
+	}
+
 	/*
 	 * Start/load the driver.
 	 */
@@ -929,10 +912,11 @@ mb12xx_main(int argc, char *argv[])
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[myoptind], "info") || !strcmp(argv[1], "status")) {
+	if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
 		mb12xx::info();
 	}

+out_error:
 	PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'");
 	return PX4_ERROR;
 }
@@ -66,7 +66,6 @@
 #include <drivers/device/ringbuffer.h>

 #include <uORB/uORB.h>
-#include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/distance_sensor.h>

 #include "sf0x_parser.h"
@@ -644,21 +643,6 @@ SF0X::start()
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&SF0X::cycle_trampoline, this, 1);

-	// /* notify about state change */
-	// struct subsystem_info_s info = {
-	// 	true,
-	// 	true,
-	// 	true,
-	// 	SUBSYSTEM_TYPE_RANGEFINDER
-	// };
-	// static orb_advert_t pub = -1;
-
-	// if (pub > 0) {
-	// 	orb_publish(ORB_ID(subsystem_info), pub, &info);
-
-	// } else {
-	// 	pub = orb_advertise(ORB_ID(subsystem_info), &info);
-	// }
 }

 void
@@ -947,25 +931,27 @@ info()
 int
 sf0x_main(int argc, char *argv[])
 {
-	// check for optional arguments
 	int ch;
 	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
 	int myoptind = 1;
 	const char *myoptarg = nullptr;

-
 	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'R':
 			rotation = (uint8_t)atoi(myoptarg);
-			PX4_INFO("Setting distance sensor orientation to %d", (int)rotation);
 			break;

 		default:
 			PX4_WARN("Unknown option!");
+			return -1;
 		}
 	}

+	if (myoptind >= argc) {
+		goto out_error;
+	}
+
 	/*
 	 * Start/load the driver.
 	 */
@@ -1002,10 +988,11 @@ sf0x_main(int argc, char *argv[])
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[myoptind], "info") || !strcmp(argv[1], "status")) {
+	if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
 		sf0x::info();
 	}

+out_error:
 	PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'");
-	return PX4_ERROR;
+	return -1;
 }
@@ -816,25 +816,27 @@ info()
 int
 sf1xx_main(int argc, char *argv[])
 {
-	// check for optional arguments
 	int ch;
 	int myoptind = 1;
 	const char *myoptarg = nullptr;
 	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;

-
 	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'R':
 			rotation = (uint8_t)atoi(myoptarg);
-			PX4_INFO("Setting distance sensor orientation to %d", (int)rotation);
 			break;

 		default:
 			PX4_WARN("Unknown option!");
+			return -1;
 		}
 	}

+	if (myoptind >= argc) {
+		goto out_error;
+	}
+
 	/*
 	 * Start/load the driver.
 	 */
@@ -870,6 +872,7 @@ sf1xx_main(int argc, char *argv[])
 		sf1xx::info();
 	}

+out_error:
 	PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'");
-	return PX4_ERROR;
+	return -1;
 }
@@ -66,7 +66,6 @@
 #include <drivers/device/ringbuffer.h>

 #include <uORB/uORB.h>
-#include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/distance_sensor.h>

 #include <board_config.h>
@@ -591,24 +590,6 @@ SRF02::start()

 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&SRF02::cycle_trampoline, this, 5);
-
-	/* notify about state change */
-	struct subsystem_info_s info = {};
-	info.present = true;
-	info.enabled = true;
-	info.ok = true;
-	info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER;
-
-	static orb_advert_t pub = nullptr;
-
-	if (pub != nullptr) {
-		orb_publish(ORB_ID(subsystem_info), pub, &info);
-
-
-	} else {
-		pub = orb_advertise(ORB_ID(subsystem_info), &info);
-
-	}
 }

 void
@@ -719,7 +700,7 @@ void	info();
 void
 start(uint8_t rotation)
 {
-	int fd;
+	int fd = -1;

 	if (g_dev != nullptr) {
 		errx(1, "already started");
@@ -747,10 +728,15 @@ start(uint8_t rotation)
 		goto fail;
 	}

+	close(fd);
 	exit(0);

 fail:

+	if (fd >= 0) {
+		close(fd);
+	}
+
 	if (g_dev != nullptr) {
 		delete g_dev;
 		g_dev = nullptr;
@@ -882,25 +868,27 @@ info()
 int
 srf02_main(int argc, char *argv[])
 {
-	// check for optional arguments
 	int ch;
 	int myoptind = 1;
 	const char *myoptarg = nullptr;
 	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;

-
 	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'R':
 			rotation = (uint8_t)atoi(myoptarg);
-			PX4_INFO("Setting distance sensor orientation to %d", (int)rotation);
 			break;

 		default:
 			PX4_WARN("Unknown option!");
+			return -1;
 		}
 	}

+	if (myoptind >= argc) {
+		goto out_error;
+	}
+
 	/*
 	 * Start/load the driver.
 	 */
@@ -936,6 +924,7 @@ srf02_main(int argc, char *argv[])
 		srf02::info();
 	}

+out_error:
 	PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'");
 	return PX4_ERROR;
 }
@@ -66,7 +66,6 @@
 #include <drivers/device/ringbuffer.h>

 #include <uORB/uORB.h>
-#include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/distance_sensor.h>

 #include <board_config.h>
@@ -668,22 +667,6 @@ TERARANGER::start()

 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&TERARANGER::cycle_trampoline, this, 1);
-
-	/* notify about state change */
-	struct subsystem_info_s info = {};
-	info.present = true;
-	info.enabled = true;
-	info.ok = true;
-	info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER;
-
-	static orb_advert_t pub = nullptr;
-
-	if (pub != nullptr) {
-		orb_publish(ORB_ID(subsystem_info), pub, &info);
-
-	} else {
-		pub = orb_advertise(ORB_ID(subsystem_info), &info);
-	}
 }

 void
@@ -950,14 +933,18 @@ teraranger_main(int argc, char *argv[])
 		switch (ch) {
 		case 'R':
 			rotation = (uint8_t)atoi(myoptarg);
-			PX4_INFO("Setting sensor orientation to %d", (int)rotation);
 			break;

 		default:
 			PX4_WARN("Unknown option!");
+			return -1;
 		}
 	}

+	if (myoptind >= argc) {
+		goto out_error;
+	}
+
 	/*
 	 * Start/load the driver.
 	 */
@@ -989,10 +976,11 @@ teraranger_main(int argc, char *argv[])
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[myoptind], "info") || !strcmp(argv[1], "status")) {
+	if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
 		teraranger::info();
 	}

+out_error:
 	PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'");
-	return PX4_ERROR;
+	return -1;
 }
@@ -68,7 +68,6 @@
 #include <drivers/device/ringbuffer.h>

 #include <uORB/uORB.h>
-#include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/distance_sensor.h>

 #include <board_config.h>
@@ -607,24 +606,6 @@ TFMINI::start()

 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&TFMINI::cycle_trampoline, this, 1);
-
-	/* notify about state change */
-	struct subsystem_info_s info = {};
-	info.present = true;
-	info.enabled = true;
-	info.ok = true;
-	info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER;
-
-	static orb_advert_t pub = nullptr;
-
-	if (pub != nullptr) {
-		orb_publish(ORB_ID(subsystem_info), pub, &info);
-
-
-	} else {
-		pub = orb_advertise(ORB_ID(subsystem_info), &info);
-
-	}
 }

 void
@@ -922,31 +903,32 @@ usage()
 int
 tfmini_main(int argc, char *argv[])
 {
-	// check for optional arguments
 	int ch;
 	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
 	const char *device_path = "";
 	int myoptind = 1;
 	const char *myoptarg = nullptr;

-
 	while ((ch = px4_getopt(argc, argv, "R:d:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'R':
 			rotation = (uint8_t)atoi(myoptarg);
-			PX4_INFO("Setting distance sensor orientation to %d", (int)rotation);
 			break;

 		case 'd':
 			device_path = myoptarg;
-			PX4_INFO("Using device path '%s'", device_path);
 			break;

 		default:
 			PX4_WARN("Unknown option!");
+			return -1;
 		}
 	}

+	if (myoptind >= argc) {
+		goto out_error;
+	}
+
 	/*
 	 * Start/load the driver.
 	 */
@@ -957,7 +939,7 @@ tfmini_main(int argc, char *argv[])
 		} else {
 			PX4_WARN("Please specify device path!");
 			tfmini::usage();
-			return PX4_ERROR;
+			return -1;
 		}
 	}

@@ -985,10 +967,11 @@ tfmini_main(int argc, char *argv[])
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[myoptind], "info") || !strcmp(argv[1], "status")) {
+	if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
 		tfmini::info();
 	}

+out_error:
 	PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'");
-	return PX4_ERROR;
+	return -1;
 }
@@ -66,7 +66,6 @@
 #include <drivers/device/ringbuffer.h>

 #include <uORB/uORB.h>
-#include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/obstacle_distance.h>

@@ -149,7 +148,6 @@ private:
 	int _orb_class_instance;

 	orb_advert_t _distance_sensor_topic;
-	orb_advert_t _subsystem_pub;

 	perf_counter_t _sample_perf;
 	perf_counter_t _comms_errors;
@@ -214,9 +212,9 @@ VL53LXX::VL53LXX(uint8_t rotation, int bus, int address) :
 	_class_instance(-1),
 	_orb_class_instance(-1),
 	_distance_sensor_topic(nullptr),
-	_subsystem_pub(nullptr),
 	_sample_perf(perf_alloc(PC_ELAPSED, "vl53lxx_read")),
-	_comms_errors(perf_alloc(PC_COUNT, "vl53lxx_com_err"))
+	_comms_errors(perf_alloc(PC_COUNT, "vl53lxx_com_err")),
+	_stop_variable(0)
 {
 	// up the retries since the device misses the first measure attempts
 	I2C::_retries = 3;
@@ -720,22 +718,6 @@ VL53LXX::start()

 	/* schedule a cycle to start things */
 	work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, USEC2TICK(VL53LXX_US));
-
-	/* notify about state change */
-	struct subsystem_info_s info = {};
-
-	info.timestamp = hrt_absolute_time();
-	info.present = true;
-	info.enabled = true;
-	info.ok = true;
-	info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER;
-
-	if (_subsystem_pub != nullptr) {
-		orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &info);
-
-	} else {
-		_subsystem_pub = orb_advertise(ORB_ID(subsystem_info), &info);
-	}
 }


@@ -1008,7 +990,7 @@ void	info();
 void
 start(uint8_t rotation)
 {
-	int fd;
+	int fd = -1;

 	if (g_dev != nullptr) {
 		errx(1, "already started");
@@ -1041,6 +1023,10 @@ start(uint8_t rotation)

 fail:

+	if (fd >= 0) {
+		close(fd);
+	}
+
 	if (g_dev != nullptr) {
 		delete g_dev;
 		g_dev = nullptr;
@@ -1092,6 +1078,8 @@ test()

 	print_message(report);

+	close(fd);
+
 	errx(0, "PASS");
 }

@@ -70,7 +70,6 @@
 #include <arch/board/board.h>
 #include <drivers/drv_hrt.h>
 #include <mathlib/mathlib.h>
-#include <systemlib/systemlib.h>
 #include <systemlib/err.h>
 #include <parameters/param.h>
 #include <drivers/drv_gps.h>
@@ -890,9 +890,12 @@ info()
 int
 bma180_main(int argc, char *argv[])
 {
+	if (argc < 2) {
+		goto out_error;
+	}
+
 	/*
 	 * Start/load the driver.
-
 	 */
 	if (!strcmp(argv[1], "start")) {
 		bma180::start();
@@ -919,5 +922,6 @@ bma180_main(int argc, char *argv[])
 		bma180::info();
 	}

+out_error:
 	errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'");
 }
--- a/Show More
+++ b/Show More