StickAccelerationXY: make brake filter configurable

StickAccelerationXY: make brake boost factor configurable
2026-08-12 10:00:36 +08:00 · 2021-05-04 11:08:48 +02:00 · 2021-05-04 10:53:30 +02:00
155 changed files with 1064 additions and 1629 deletions
@@ -9,10 +9,10 @@ pipeline {
        script {
          def build_nodes = [:]
          def docker_images = [
-            armhf: "px4io/px4-dev-armhf:2021-04-29",
-            arm64: "px4io/px4-dev-aarch64:2021-04-29",
-            base: "px4io/px4-dev-base-bionic:2021-04-29",
-            nuttx: "px4io/px4-dev-nuttx-focal:2021-04-29",
+            armhf: "px4io/px4-dev-armhf:2021-02-04",
+            arm64: "px4io/px4-dev-aarch64:2021-02-04",
+            base: "px4io/px4-dev-base-bionic:2021-02-04",
+            nuttx: "px4io/px4-dev-nuttx-focal:2021-02-04",
            snapdragon: "lorenzmeier/px4-dev-snapdragon:2020-04-01"
          ]

@@ -130,7 +130,7 @@ pipeline {
    // TODO: actually upload artifacts to S3
    // stage('S3 Upload') {
    //   agent {
-    //     docker { image 'ghcr.io/px4/px4-dev:2021-05-06' }
+    //     docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
    //   }
    //   options {
    //         skipDefaultCheckout()
@@ -12,7 +12,7 @@ pipeline {
            stage("build cubepilot_cubeorange_test") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-04-29'
+                  image 'px4io/px4-dev-nuttx-focal:2020-09-14'
                  args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -89,7 +89,7 @@ pipeline {
            stage("build cuav_x7pro_test") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-04-29'
+                  image 'px4io/px4-dev-nuttx-focal:2020-09-14'
                  args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -165,7 +165,7 @@ pipeline {
            stage("build px4_fmu-v3_test") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-04-29'
+                  image 'px4io/px4-dev-nuttx-focal:2020-09-14'
                  args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -241,7 +241,7 @@ pipeline {
            stage("build px4_fmu-v4_test") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-04-29'
+                  image 'px4io/px4-dev-nuttx-focal:2020-09-14'
                  args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -317,7 +317,7 @@ pipeline {
            stage("build px4_fmu-v4pro_test") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-04-29'
+                  image 'px4io/px4-dev-nuttx-focal:2020-09-14'
                  args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -393,7 +393,7 @@ pipeline {
            stage("build px4_fmu-v5_test") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-04-29'
+                  image 'px4io/px4-dev-nuttx-focal:2020-09-14'
                  args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -469,7 +469,7 @@ pipeline {
            stage("build px4_fmu-v5_debug") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-04-29'
+                  image 'px4io/px4-dev-nuttx-focal:2020-09-14'
                  args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -549,7 +549,7 @@ pipeline {
            stage("build px4_fmu-v5_optimized") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-04-29'
+                  image 'px4io/px4-dev-nuttx-focal:2020-09-14'
                  args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -625,7 +625,7 @@ pipeline {
            stage("build px4_fmu-v5_stackcheck") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-04-29'
+                  image 'px4io/px4-dev-nuttx-focal:2020-09-14'
                  args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -705,7 +705,7 @@ pipeline {
            stage("build modalai_fc-v1_test") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-04-29'
+                  image 'px4io/px4-dev-nuttx-focal:2020-09-14'
                  args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -781,7 +781,7 @@ pipeline {
            stage("build holybro_durandal-v1_test") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-04-29'
+                  image 'px4io/px4-dev-nuttx-focal:2020-09-14'
                  args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -858,7 +858,7 @@ pipeline {
            stage("build nxp_fmuk66-v3_test") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-04-29'
+                  image 'px4io/px4-dev-nuttx-focal:2020-09-14'
                  args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -2,7 +2,7 @@
 // https://github.com/microsoft/vscode-dev-containers/tree/v0.134.0/containers/cpp
 {
 	"name": "px4-dev-nuttx",
-	"image": "px4io/px4-dev-nuttx-focal:2021-04-29",
+	"image": "px4io/px4-dev-nuttx-focal:2021-02-04",

 	"runArgs": [ "--cap-add=SYS_PTRACE", "--security-opt", "seccomp=unconfined" ],

@@ -28,7 +28,7 @@ jobs:
          "parameters_metadata",
        ]
    container:
-      image: ghcr.io/px4/px4-dev:2021-05-06
+      image: px4io/px4-dev-nuttx-focal:2021-02-04
      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
    steps:
    - uses: actions/checkout@v1
@@ -11,7 +11,7 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-clang:2021-04-29
+    container: px4io/px4-dev-clang:2021-02-04
    steps:
    - uses: actions/checkout@v1
      with:
@@ -11,7 +11,7 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-armhf:2021-04-29
+    container: px4io/px4-dev-armhf:2021-02-04
    strategy:
      matrix:
        config: [
@@ -11,7 +11,7 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-aarch64:2021-04-29
+    container: px4io/px4-dev-aarch64:2021-02-04
    strategy:
      matrix:
        config: [
@@ -4,7 +4,6 @@ on:
  push:
    branches:
    - 'master'
-    - 'github-actions'
  pull_request:
    branches:
    - '*'
@@ -12,7 +11,7 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    container: ghcr.io/px4/px4-dev:2021-05-06
+    container: px4io/px4-dev-nuttx-focal:2021-02-04
    strategy:
      matrix:
        config: [
@@ -124,8 +123,6 @@ jobs:
      run: make ${{matrix.config}} bloaty_symbols || true
    - name: make ${{matrix.config}} bloaty_templates
      run: make ${{matrix.config}} bloaty_templates || true
-    - name: make ${{matrix.config}} bloaty_ram
-      run: make ${{matrix.config}} bloaty_ram || true
    - name: make ${{matrix.config}} bloaty_compare_master
      run: make ${{matrix.config}} bloaty_compare_master || true
    - name: ccache post-run
@@ -11,7 +11,7 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    container: ghcr.io/px4/px4-dev:2021-05-06
+    container: px4io/px4-dev-nuttx-focal:2021-02-04
    strategy:
      matrix:
        config: [
@@ -1,31 +0,0 @@
-name: Build and publish Docker image
-
-on:
-  push:
-    branches:
-    - 'master'
-    - 'github-actions'
-
-jobs:
-  docker:
-    runs-on: ubuntu-latest
-    steps:
-      - name: Login to GitHub Container Registry
-        uses: docker/login-action@v1
-        with:
-          registry: ghcr.io
-          username: ${{ github.repository_owner }}
-          password: ${{ secrets.GITHUB_TOKEN }}
-
-      - name: Build and push
-        uses: docker/build-push-action@v2
-        with:
-          context: .
-          platforms: linux/amd64
-          push: true
-          tags: |
-            px4/px4-dev:latest
-            ghcr.io/px4/px4-dev:latest
-
-      - name: Image digest
-        run: echo ${{ steps.docker_build.outputs.digest }}
@@ -23,7 +23,7 @@ jobs:
    needs: enumerate_targets
    strategy:
      matrix: ${{fromJson(needs.enumerate_targets.outputs.matrix)}}
-    container: px4io/px4-dev-${{ matrix.container }}:2021-04-29
+    container: px4io/px4-dev-${{ matrix.container }}:2021-02-04
    steps:
    - uses: actions/checkout@v1
      with:
@@ -0,0 +1,131 @@
+name: MAVROS Avoidance Tests
+
+on:
+  push:
+    branches:
+    - 'master'
+  pull_request:
+    branches:
+    - '*'
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    strategy:
+      fail-fast: false
+      matrix:
+        config:
+          - {test_file: "mavros_posix_test_avoidance.test",    vehicle: "iris_obs_avoid", mission: "avoidance",       build_type: "RelWithDebInfo"}
+          - {test_file: "mavros_posix_test_safe_landing.test", vehicle: "iris_obs_avoid", mission: "MC_safe_landing", build_type: "RelWithDebInfo"}
+
+    container:
+      image: px4io/px4-dev-ros-melodic:2021-02-04
+      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
+    steps:
+    - uses: actions/checkout@v1
+      with:
+        token: ${{ secrets.ACCESS_TOKEN }}
+
+    - name: Prepare ccache timestamp
+      id: ccache_cache_timestamp
+      shell: cmake -P {0}
+      run: |
+        string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC)
+        message("::set-output name=timestamp::${current_date}")
+    - name: ccache cache files
+      uses: actions/cache@v2
+      with:
+        path: ~/.ccache
+        key: sitl_tests-${{matrix.config.build_type}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}}
+        restore-keys: sitl_tests-${{matrix.config.build_type}}-ccache-
+    - name: setup ccache
+      run: |
+          mkdir -p ~/.ccache
+          echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf
+          echo "compression = true" >> ~/.ccache/ccache.conf
+          echo "compression_level = 5" >> ~/.ccache/ccache.conf
+          echo "max_size = 100M" >> ~/.ccache/ccache.conf
+          ccache -s
+          ccache -z
+
+    - name: check environment
+      run: |
+          export
+          ulimit -a
+    - name: Build PX4 and sitl_gazebo
+      env:
+        PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}}
+      run: |
+        ccache -z
+        make px4_sitl_default
+        make px4_sitl_default sitl_gazebo
+        ccache -s
+
+    - name: Core dump settings
+      run: |
+          ulimit -c unlimited
+          echo "`pwd`/%e.core" > /proc/sys/kernel/core_pattern
+
+    - name: Run SITL tests
+      env:
+        PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}}
+      run: |
+        export
+        ./test/rostest_avoidance_run.sh ${{matrix.config.test_file}} mission:=${{matrix.config.mission}} vehicle:=${{matrix.config.vehicle}} || true
+
+    - name: Look at core files
+      if: failure()
+      run: gdb build/px4_sitl_default/bin/px4 px4.core -ex "thread apply all bt" -ex "quit"
+    - name: Upload px4 coredump
+      if: failure()
+      uses: actions/upload-artifact@v2-preview
+      with:
+        name: coredump
+        path: px4.core
+
+    # - name: ecl EKF analysis
+    #   if: always()
+    #   run: ./Tools/ecl_ekf/process_logdata_ekf.py ~/.ros/log/*/*.ulg
+
+    - name: Upload logs to flight review
+      if: always()
+      run: ./Tools/upload_log.py -q --description "${GITHUB_WORKFLOW} ${GITHUB_RUN_ID}" --feedback "${GITHUB_WORKFLOW} ${GITHUB_RUN_ID} ${GITHUB_REPOSITORY} ${GITHUB_REF}" --source CI ~/.ros/log/*/*.ulg
+
+    - name: Upload px4 binary
+      if: failure()
+      uses: actions/upload-artifact@v2
+      with:
+        name: binary
+        path: build/px4_sitl_default/bin/px4
+
+    - name: Store PX4 log
+      if: failure()
+      uses: actions/upload-artifact@v2
+      with:
+        name: px4_log
+        path: ~/.ros/log/*/*.ulg
+
+    - name: Store ROS log
+      if: failure()
+      uses: actions/upload-artifact@v2
+      with:
+        name: ros_log
+        path: ~/.ros/**/rostest-*.log
+
+    # Report test coverage
+    - name: Upload coverage
+      if: contains(matrix.config.build_type, 'Coverage')
+      run: |
+          git config --global credential.helper "" # disable the keychain credential helper
+          git config --global --add credential.helper store # enable the local store credential helper
+          echo "https://x-access-token:${{ secrets.ACCESS_TOKEN }}@github.com" >> ~/.git-credentials # add credential
+          git config --global url."https://github.com/".insteadof git@github.com: # credentials add credential
+          mkdir -p coverage
+          lcov --directory build/px4_sitl_default --base-directory build/px4_sitl_default --gcov-tool gcov --capture -o coverage/lcov.info
+    - name: Upload coverage information to Codecov
+      if: contains(matrix.config.build_type, 'Coverage')
+      uses: codecov/codecov-action@v1
+      with:
+        token: ${{ secrets.CODECOV_TOKEN }}
+        flags: mavros_avoidance
+        file: coverage/lcov.info
@@ -25,7 +25,7 @@ jobs:
          #- {vehicle: "tiltrotor",     mission: "VTOL_mission_1",  build_type: "RelWithDebInfo"}

    container:
-      image: px4io/px4-dev-ros-melodic:2021-04-29
+      image: px4io/px4-dev-ros-melodic:2021-02-04
      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
    steps:
    - uses: actions/checkout@v1
@@ -20,7 +20,7 @@ jobs:
          #- {test_file: "mavros_posix_tests_offboard_rpyrt_ctl.test", vehicle: "iris", build_type: "RelWithDebInfo"}

    container:
-      image: px4io/px4-dev-ros-melodic:2021-04-29
+      image: px4io/px4-dev-ros-melodic:2021-02-04
      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
    steps:
    - uses: actions/checkout@v1
@@ -11,7 +11,7 @@ jobs:

  airframe:
    runs-on: ubuntu-latest
-    container: ghcr.io/px4/px4-dev:2021-05-06
+    container: px4io/px4-dev-base-focal:2021-02-04
    steps:
    - uses: actions/checkout@v1
      with:
@@ -26,7 +26,7 @@ jobs:

  module:
    runs-on: ubuntu-latest
-    container: ghcr.io/px4/px4-dev:2021-05-06
+    container: px4io/px4-dev-base-focal:2021-02-04
    steps:
    - uses: actions/checkout@v1
      with:
@@ -41,7 +41,7 @@ jobs:

  parameter:
    runs-on: ubuntu-latest
-    container: ghcr.io/px4/px4-dev:2021-05-06
+    container: px4io/px4-dev-base-focal:2021-02-04
    steps:
    - uses: actions/checkout@v1
      with:
@@ -65,7 +65,7 @@ jobs:

  uorb_graph:
    runs-on: ubuntu-latest
-    container: ghcr.io/px4/px4-dev:2021-05-06
+    container: px4io/px4-dev-nuttx-focal:2021-02-04
    steps:
    - uses: actions/checkout@v1
      with:
@@ -80,7 +80,7 @@ jobs:

  micrortps_agent:
    runs-on: ubuntu-latest
-    container: ghcr.io/px4/px4-dev:2021-05-06
+    container: px4io/px4-dev-base-focal:2021-02-04
    steps:
    - uses: actions/checkout@v1
      with:
@@ -94,7 +94,7 @@ jobs:

  ROS_msgs:
    runs-on: ubuntu-latest
-    container: ghcr.io/px4/px4-dev:2021-05-06
+    container: px4io/px4-dev-base-focal:2021-02-04
    steps:
    - uses: actions/checkout@v1
      with:
@@ -107,7 +107,7 @@ jobs:

  ROS2_bridge:
    runs-on: ubuntu-latest
-    container: ghcr.io/px4/px4-dev:2021-05-06
+    container: px4io/px4-dev-base-focal:2021-02-04
    steps:
    - uses: actions/checkout@v1
      with:
@@ -20,7 +20,7 @@ jobs:
          - {latitude:  "29.660316", longitude:  "-82.316658", altitude:  "30", build_type: "RelWithDebInfo", model: "tailsitter"    } # Florida
          - {latitude:  "47.397742", longitude:    "8.545594", altitude: "488", build_type: "Coverage",       model: "standard_vtol" } # Zurich
    container:
-      image: px4io/px4-dev-simulation-focal:2021-04-29
+      image: px4io/px4-dev-simulation-focal:2021-02-04
      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
    steps:
    - uses: actions/checkout@v1
@@ -28,9 +28,9 @@ jobs:
        token: ${{ secrets.ACCESS_TOKEN }}

    - name: Download MAVSDK
-      run: wget "https://github.com/mavlink/MAVSDK/releases/download/v$(cat test/mavsdk_tests/MAVSDK_VERSION)/mavsdk_$(cat test/mavsdk_tests/MAVSDK_VERSION)_ubuntu20.04_amd64.deb"
+      run: wget https://github.com/mavlink/MAVSDK/releases/download/v0.38.0/mavsdk_0.38.0_ubuntu20.04_amd64.deb
    - name: Install MAVSDK
-      run: dpkg -i "mavsdk_$(cat test/mavsdk_tests/MAVSDK_VERSION)_ubuntu20.04_amd64.deb"
+      run: dpkg -i mavsdk_0.38.0_ubuntu20.04_amd64.deb

    - name: Prepare ccache timestamp
      id: ccache_cache_timestamp
@@ -33,7 +33,7 @@
 [submodule "src/modules/micrortps_bridge/micro-CDR"]
 	path = src/modules/micrortps_bridge/micro-CDR
 	url = https://github.com/PX4/Micro-CDR.git
-	branch = master
+	branch = px4
 [submodule "platforms/nuttx/NuttX/nuttx"]
 	path = platforms/nuttx/NuttX/nuttx
 	url = https://github.com/PX4/NuttX.git
@@ -66,4 +66,3 @@
 [submodule "src/drivers/uavcan_v1/legacy_data_types"]
 	path = src/drivers/uavcan_v1/legacy_data_types
 	url = https://github.com/px4/public_regulated_data_types/
-	branch = legacy
@@ -185,8 +185,6 @@ if((CMAKE_BUILD_TYPE STREQUAL "Debug") OR (CMAKE_BUILD_TYPE STREQUAL "Coverage")
 	set(MAX_CUSTOM_OPT_LEVEL -O0)
 elseif(CMAKE_BUILD_TYPE MATCHES "Sanitizer")
 	set(MAX_CUSTOM_OPT_LEVEL -O1)
-elseif(CMAKE_BUILD_TYPE MATCHES "Release")
-	set(MAX_CUSTOM_OPT_LEVEL -O3)
 else()
 	if(px4_constrained_flash_build)
 		set(MAX_CUSTOM_OPT_LEVEL -Os)
@@ -1,29 +0,0 @@
-#
-# PX4 development environment
-#
-
-FROM ubuntu:20.04
-LABEL maintainer="Daniel Agar <daniel@agar.ca>"
-
-COPY Tools/setup/ubuntu.sh /tmp/ubuntu.sh
-COPY Tools/setup/requirements.txt /tmp/requirements.txt
-RUN DEBIAN_FRONTEND=noninteractive /tmp/ubuntu.sh --no-sim-tools \
-	&& apt-get -y autoremove \
-	&& apt-get clean autoclean \
-	&& rm -rf /var/lib/apt/lists/{apt,dpkg,cache,log} /tmp/* /var/tmp/*
-
-# create user with id 1001 (jenkins docker workflow default)
-RUN useradd --shell /bin/bash -u 1001 -c "" -m user && usermod -a -G dialout user
-
-ENV CCACHE_UMASK=000
-ENV PATH="/usr/lib/ccache:$PATH"
-
-# SITL UDP PORTS
-EXPOSE 14556/udp
-EXPOSE 14557/udp
-
-# create and start as LOCAL_USER_ID
-COPY Tools/setup/entrypoint.sh /usr/local/bin/entrypoint.sh
-ENTRYPOINT ["/usr/local/bin/entrypoint.sh"]
-
-CMD ["/bin/bash"]
@@ -15,7 +15,7 @@ pipeline {
        // stage('Catkin build on ROS workspace') {
        //   agent {
        //     docker {
-        //       image 'px4io/px4-dev-ros-melodic:2021-04-29'
+        //       image 'px4io/px4-dev-ros-melodic:2021-02-04'
        //       args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE'
        //     }
        //   }
@@ -56,7 +56,7 @@ pipeline {
        stage('Colcon build on ROS2 workspace') {
          agent {
            docker {
-              image 'px4io/px4-dev-ros2-foxy:2021-04-29'
+              image 'px4io/px4-dev-ros2-foxy:2021-02-04'
              args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE'
            }
          }
@@ -85,7 +85,7 @@ pipeline {

        stage('Airframe') {
          agent {
-            docker { image 'ghcr.io/px4/px4-dev:2021-05-06' }
+            docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
          }
          steps {
            sh 'make distclean'
@@ -105,7 +105,7 @@ pipeline {

        stage('Parameter') {
          agent {
-            docker { image 'ghcr.io/px4/px4-dev:2021-05-06' }
+            docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
          }
          steps {
            sh 'make distclean'
@@ -125,7 +125,7 @@ pipeline {

        stage('Module') {
          agent {
-            docker { image 'ghcr.io/px4/px4-dev:2021-05-06' }
+            docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
          }
          steps {
            sh 'make distclean'
@@ -146,7 +146,7 @@ pipeline {
        stage('uORB graphs') {
          agent {
            docker {
-              image 'px4io/px4-dev-nuttx-focal:2021-04-29'
+              image 'px4io/px4-dev-nuttx-focal:2021-02-04'
              args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
            }
          }
@@ -176,7 +176,7 @@ pipeline {

        stage('Userguide') {
          agent {
-            docker { image 'ghcr.io/px4/px4-dev:2021-05-06' }
+            docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
          }
          steps {
            sh('export')
@@ -206,7 +206,7 @@ pipeline {

        stage('QGroundControl') {
          agent {
-            docker { image 'ghcr.io/px4/px4-dev:2021-05-06' }
+            docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
          }
          steps {
            sh('export')
@@ -234,7 +234,7 @@ pipeline {

        stage('microRTPS agent') {
          agent {
-            docker { image 'ghcr.io/px4/px4-dev:2021-05-06' }
+            docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
          }
          steps {
            sh('export')
@@ -264,7 +264,7 @@ pipeline {

        stage('PX4 ROS msgs') {
          agent {
-            docker { image 'ghcr.io/px4/px4-dev:2021-05-06' }
+            docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
          }
          steps {
            sh('export')
@@ -293,7 +293,7 @@ pipeline {

        stage('PX4 ROS2 bridge') {
          agent {
-            docker { image 'ghcr.io/px4/px4-dev:2021-05-06' }
+            docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
          }
          steps {
            sh('export')
@@ -336,7 +336,7 @@ pipeline {

        stage('S3') {
          agent {
-            docker { image 'ghcr.io/px4/px4-dev:2021-05-06' }
+            docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
          }
          steps {
            sh('export')
@@ -469,7 +469,6 @@ validate_module_configs:

 clean:
 	@rm -rf "$(SRC_DIR)"/build
-	@git submodule foreach git clean -df

 submodulesclean:
 	@git submodule foreach --quiet --recursive git clean -ff -x -d
@@ -487,7 +486,7 @@ gazeboclean:

 distclean: gazeboclean
 	@git submodule deinit -f .
-	@git clean -ff -x -d -e ".cproject" -e ".idea" -e ".project" -e ".settings" -e ".vscode"
+	@git clean -ff -x -d -e ".project" -e ".cproject" -e ".idea" -e ".settings" -e ".vscode"

 # Help / Error
 # --------------------------------------------------------------------
@@ -23,7 +23,6 @@ param set-default MPC_XY_ERR_MAX 5
 param set-default MPC_XY_VEL_MAX 4
 param set-default MPC_Z_VEL_MAX_DN 1.5
 param set-default MPC_JERK_MAX 4.5
-param set-default MPC_YAW_MODE 4

 param set-default NAV_ACC_RAD 3

@@ -1,13 +0,0 @@
-custom_data_source: {
-  name: "bloaty_static_ram"
-  base_data_source: "sections"
-
-  rewrite: {
-    pattern: "^\\.bss"
-    replacement: "ram"
-  }
-  rewrite: {
-    pattern: "^\\.data"
-    replacement: "ram"
-  }
-}
@@ -2,7 +2,10 @@

 if [ -z ${PX4_DOCKER_REPO+x} ]; then
 	echo "guessing PX4_DOCKER_REPO based on input";
-	if [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*beaglebone.* ]] || [[ $@ =~ .*pilotpi.default ]]; then
+	if [[ $@ =~ .*px4_fmu.* ]]; then
+		# nuttx-px4fmu-v{1,2,3,4,5}
+		PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2021-02-04"
+	elif [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*beaglebone.* ]] || [[ $@ =~ .*pilotpi.default ]]; then
 		# beaglebone_blue_default, emlid_navio2_default, px4_raspberrypi_default, scumaker_pilotpi_default
 		PX4_DOCKER_REPO="px4io/px4-dev-armhf:2021-02-04"
 	elif [[ $@ =~ .*pilotpi.arm64 ]]; then
@@ -27,7 +30,7 @@ fi

 # otherwise default to nuttx
 if [ -z ${PX4_DOCKER_REPO+x} ]; then
-	PX4_DOCKER_REPO="ghcr.io/px4/px4-dev:2021-05-06"
+	PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2020-09-14"
 fi

 # docker hygiene
@@ -64,5 +67,4 @@ docker run -it --rm -w "${SRC_DIR}" \
 	--publish 14556:14556/udp \
 	--volume=${CCACHE_DIR}:${CCACHE_DIR}:rw \
 	--volume=${SRC_DIR}:${SRC_DIR}:rw \
-	-v /tmp/.X11-unix:/tmp/.X11-unix -e DISPLAY=$DISPLAY -h $HOSTNAME
 	${PX4_DOCKER_REPO} /bin/bash -c "$1 $2 $3"
@@ -1,4 +1,4 @@
-#! /usr/bin/env python3
+#! /usr/bin/env python

 from __future__ import print_function

@@ -7,7 +7,6 @@ import os
 import math
 import matplotlib.pyplot as plt
 import numpy as np
-import scipy as sp

 from pyulog import *

@@ -64,9 +63,6 @@ def resampleWithDeltaX(x,y):

    return resampledX,resampledY

-def median_filter(data):
-    return sp.signal.medfilt(data, 31)
-
 parser = argparse.ArgumentParser(description='Reads in IMU data from a static thermal calibration test and performs a curve fit of gyro, accel and baro bias vs temperature')
 parser.add_argument('filename', metavar='file.ulg', help='ULog input file')
 parser.add_argument('--no_resample', dest='noResample', action='store_const',
@@ -188,16 +184,12 @@ if num_gyros >= 1 and not math.isnan(sensor_gyro_0['temperature'][0]):
    temp_rel_resample = np.linspace(gyro_0_params['TC_G0_TMIN']-gyro_0_params['TC_G0_TREF'], gyro_0_params['TC_G0_TMAX']-gyro_0_params['TC_G0_TREF'], 100)
    temp_resample = temp_rel_resample + gyro_0_params['TC_G0_TREF']

-    sensor_gyro_0['x'] = median_filter(sensor_gyro_0['x'])
-    sensor_gyro_0['y'] = median_filter(sensor_gyro_0['y'])
-    sensor_gyro_0['z'] = median_filter(sensor_gyro_0['z'])
-
    # fit X axis
    if noResample:
-        coef_gyro_0_x = np.polyfit(temp_rel, sensor_gyro_0['x'], 3)
+        coef_gyro_0_x = np.polyfit(temp_rel,sensor_gyro_0['x'],3)
    else:
-        temp, sens = resampleWithDeltaX(temp_rel, sensor_gyro_0['x'])
-        coef_gyro_0_x = np.polyfit(temp, sens, 3)
+        temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_0['x'])
+        coef_gyro_0_x = np.polyfit(temp, sens ,3)

    gyro_0_params['TC_G0_X3_0'] = coef_gyro_0_x[0]
    gyro_0_params['TC_G0_X2_0'] = coef_gyro_0_x[1]
@@ -208,10 +200,10 @@ if num_gyros >= 1 and not math.isnan(sensor_gyro_0['temperature'][0]):

    # fit Y axis
    if noResample:
-        coef_gyro_0_y = np.polyfit(temp_rel, sensor_gyro_0['y'], 3)
+        coef_gyro_0_y = np.polyfit(temp_rel,sensor_gyro_0['y'],3)
    else:
        temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_0['y'])
-        coef_gyro_0_y = np.polyfit(temp, sens, 3)
+        coef_gyro_0_y = np.polyfit(temp, sens ,3)

    gyro_0_params['TC_G0_X3_1'] = coef_gyro_0_y[0]
    gyro_0_params['TC_G0_X2_1'] = coef_gyro_0_y[1]
@@ -222,9 +214,9 @@ if num_gyros >= 1 and not math.isnan(sensor_gyro_0['temperature'][0]):

    # fit Z axis
    if noResample:
-        coef_gyro_0_z = np.polyfit(temp_rel, sensor_gyro_0['z'],3)
+        coef_gyro_0_z = np.polyfit(temp_rel,sensor_gyro_0['z'],3)
    else:
-        temp, sens = resampleWithDeltaX(temp_rel, sensor_gyro_0['z'])
+        temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_0['z'])
        coef_gyro_0_z = np.polyfit(temp, sens ,3)

    gyro_0_params['TC_G0_X3_2'] = coef_gyro_0_z[0]
@@ -300,10 +292,6 @@ if num_gyros >= 2 and not math.isnan(sensor_gyro_1['temperature'][0]):
    temp_rel_resample = np.linspace(gyro_1_params['TC_G1_TMIN']-gyro_1_params['TC_G1_TREF'], gyro_1_params['TC_G1_TMAX']-gyro_1_params['TC_G1_TREF'], 100)
    temp_resample = temp_rel_resample + gyro_1_params['TC_G1_TREF']

-    sensor_gyro_1['x'] = median_filter(sensor_gyro_1['x'])
-    sensor_gyro_1['y'] = median_filter(sensor_gyro_1['y'])
-    sensor_gyro_1['z'] = median_filter(sensor_gyro_1['z'])
-
    # fit X axis
    if noResample:
        coef_gyro_1_x = np.polyfit(temp_rel,sensor_gyro_1['x'],3)
@@ -412,10 +400,6 @@ if num_gyros >= 3 and not math.isnan(sensor_gyro_2['temperature'][0]):
    temp_rel_resample = np.linspace(gyro_2_params['TC_G2_TMIN']-gyro_2_params['TC_G2_TREF'], gyro_2_params['TC_G2_TMAX']-gyro_2_params['TC_G2_TREF'], 100)
    temp_resample = temp_rel_resample + gyro_2_params['TC_G2_TREF']

-    sensor_gyro_2['x'] = median_filter(sensor_gyro_2['x'])
-    sensor_gyro_2['y'] = median_filter(sensor_gyro_2['y'])
-    sensor_gyro_2['z'] = median_filter(sensor_gyro_2['z'])
-
    # fit X axis
    if noResample:
        coef_gyro_2_x = np.polyfit(temp_rel,sensor_gyro_2['x'],3)
@@ -432,10 +416,10 @@ if num_gyros >= 3 and not math.isnan(sensor_gyro_2['temperature'][0]):

    # fit Y axis
    if noResample:
-        coef_gyro_2_y = np.polyfit(temp_rel, sensor_gyro_2['y'], 3)
+        coef_gyro_2_y = np.polyfit(temp_rel,sensor_gyro_2['y'],3)
    else:
-        temp, sens = resampleWithDeltaX(temp_rel, sensor_gyro_2['y'])
-        coef_gyro_2_y = np.polyfit(temp, sens, 3)
+        temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_2['y'])
+        coef_gyro_2_y = np.polyfit(temp, sens ,3)

    gyro_2_params['TC_G2_X3_1'] = coef_gyro_2_y[0]
    gyro_2_params['TC_G2_X2_1'] = coef_gyro_2_y[1]
@@ -446,10 +430,10 @@ if num_gyros >= 3 and not math.isnan(sensor_gyro_2['temperature'][0]):

    # fit Z axis
    if noResample:
-        coef_gyro_2_z = np.polyfit(temp_rel,sensor_gyro_2['z'], 3)
+        coef_gyro_2_z = np.polyfit(temp_rel,sensor_gyro_2['z'],3)
    else:
        temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_2['z'])
-        coef_gyro_2_z = np.polyfit(temp, sens, 3)
+        coef_gyro_2_z = np.polyfit(temp, sens ,3)

    gyro_2_params['TC_G2_X3_2'] = coef_gyro_2_z[0]
    gyro_2_params['TC_G2_X2_2'] = coef_gyro_2_z[1]
@@ -524,12 +508,8 @@ if num_gyros >= 4 and not math.isnan(sensor_gyro_3['temperature'][0]):
    temp_rel_resample = np.linspace(gyro_3_params['TC_G3_TMIN']-gyro_3_params['TC_G3_TREF'], gyro_3_params['TC_G3_TMAX']-gyro_3_params['TC_G3_TREF'], 100)
    temp_resample = temp_rel_resample + gyro_3_params['TC_G3_TREF']

-    sensor_gyro_3['x'] = median_filter(sensor_gyro_3['x'])
-    sensor_gyro_3['y'] = median_filter(sensor_gyro_3['y'])
-    sensor_gyro_3['z'] = median_filter(sensor_gyro_3['z'])
-
    # fit X axis
-    coef_gyro_3_x = np.polyfit(temp_rel,sensor_gyro_3['x'], 3)
+    coef_gyro_3_x = np.polyfit(temp_rel,sensor_gyro_3['x'],3)
    gyro_3_params['TC_G3_X3_0'] = coef_gyro_3_x[0]
    gyro_3_params['TC_G3_X2_0'] = coef_gyro_3_x[1]
    gyro_3_params['TC_G3_X1_0'] = coef_gyro_3_x[2]
@@ -538,7 +518,7 @@ if num_gyros >= 4 and not math.isnan(sensor_gyro_3['temperature'][0]):
    gyro_3_x_resample = fit_coef_gyro_3_x(temp_rel_resample)

    # fit Y axis
-    coef_gyro_3_y = np.polyfit(temp_rel,sensor_gyro_3['y'], 3)
+    coef_gyro_3_y = np.polyfit(temp_rel,sensor_gyro_3['y'],3)
    gyro_3_params['TC_G3_X3_1'] = coef_gyro_3_y[0]
    gyro_3_params['TC_G3_X2_1'] = coef_gyro_3_y[1]
    gyro_3_params['TC_G3_X1_1'] = coef_gyro_3_y[2]
@@ -547,7 +527,7 @@ if num_gyros >= 4 and not math.isnan(sensor_gyro_3['temperature'][0]):
    gyro_3_y_resample = fit_coef_gyro_3_y(temp_rel_resample)

    # fit Z axis
-    coef_gyro_3_z = np.polyfit(temp_rel,sensor_gyro_3['z'], 3)
+    coef_gyro_3_z = np.polyfit(temp_rel,sensor_gyro_3['z'],3)
    gyro_3_params['TC_G3_X3_2'] = coef_gyro_3_z[0]
    gyro_3_params['TC_G3_X2_2'] = coef_gyro_3_z[1]
    gyro_3_params['TC_G3_X1_2'] = coef_gyro_3_z[2]
@@ -560,8 +540,8 @@ if num_gyros >= 4 and not math.isnan(sensor_gyro_3['temperature'][0]):

    # draw plots
    plt.subplot(3,1,1)
-    plt.plot(sensor_gyro_3['temperature'],sensor_gyro_3['x'], 'b')
-    plt.plot(temp_resample,gyro_3_x_resample, 'r')
+    plt.plot(sensor_gyro_3['temperature'],sensor_gyro_3['x'],'b')
+    plt.plot(temp_resample,gyro_3_x_resample,'r')
    plt.title('Gyro 2 ({}) Bias vs Temperature'.format(gyro_3_params['TC_G3_ID']))
    plt.ylabel('X bias (rad/s)')
    plt.xlabel('temperature (degC)')
@@ -621,17 +601,13 @@ if num_accels >= 1 and not math.isnan(sensor_accel_0['temperature'][0]):
    temp_rel_resample = np.linspace(accel_0_params['TC_A0_TMIN']-accel_0_params['TC_A0_TREF'], accel_0_params['TC_A0_TMAX']-accel_0_params['TC_A0_TREF'], 100)
    temp_resample = temp_rel_resample + accel_0_params['TC_A0_TREF']

-    sensor_accel_0['x'] = median_filter(sensor_accel_0['x'])
-    sensor_accel_0['y'] = median_filter(sensor_accel_0['y'])
-    sensor_accel_0['z'] = median_filter(sensor_accel_0['z'])
-
    # fit X axis
    correction_x = sensor_accel_0['x'] - np.median(sensor_accel_0['x'])
    if noResample:
-        coef_accel_0_x = np.polyfit(temp_rel,correction_x, 3)
+        coef_accel_0_x = np.polyfit(temp_rel,correction_x,3)
    else:
        temp, sens = resampleWithDeltaX(temp_rel,correction_x)
-        coef_accel_0_x = np.polyfit(temp, sens, 3)
+        coef_accel_0_x = np.polyfit(temp, sens ,3)

    accel_0_params['TC_A0_X3_0'] = coef_accel_0_x[0]
    accel_0_params['TC_A0_X2_0'] = coef_accel_0_x[1]
@@ -641,12 +617,12 @@ if num_accels >= 1 and not math.isnan(sensor_accel_0['temperature'][0]):
    correction_x_resample = fit_coef_accel_0_x(temp_rel_resample)

    # fit Y axis
-    correction_y = sensor_accel_0['y'] - np.median(sensor_accel_0['y'])
+    correction_y = sensor_accel_0['y']-np.median(sensor_accel_0['y'])
    if noResample:
-        coef_accel_0_y = np.polyfit(temp_rel, correction_y, 3)
+        coef_accel_0_y = np.polyfit(temp_rel,correction_y,3)
    else:
        temp, sens = resampleWithDeltaX(temp_rel,correction_y)
-        coef_accel_0_y = np.polyfit(temp, sens, 3)
+        coef_accel_0_y = np.polyfit(temp, sens ,3)

    accel_0_params['TC_A0_X3_1'] = coef_accel_0_y[0]
    accel_0_params['TC_A0_X2_1'] = coef_accel_0_y[1]
@@ -656,12 +632,12 @@ if num_accels >= 1 and not math.isnan(sensor_accel_0['temperature'][0]):
    correction_y_resample = fit_coef_accel_0_y(temp_rel_resample)

    # fit Z axis
-    correction_z = sensor_accel_0['z'] - np.median(sensor_accel_0['z'])
+    correction_z = sensor_accel_0['z']-np.median(sensor_accel_0['z'])
    if noResample:
-        coef_accel_0_z = np.polyfit(temp_rel,correction_z, 3)
+        coef_accel_0_z = np.polyfit(temp_rel,correction_z,3)
    else:
        temp, sens = resampleWithDeltaX(temp_rel,correction_z)
-        coef_accel_0_z = np.polyfit(temp, sens, 3)
+        coef_accel_0_z = np.polyfit(temp, sens ,3)

    accel_0_params['TC_A0_X3_2'] = coef_accel_0_z[0]
    accel_0_params['TC_A0_X2_2'] = coef_accel_0_z[1]
@@ -736,17 +712,13 @@ if num_accels >= 2 and not math.isnan(sensor_accel_1['temperature'][0]):
    temp_rel_resample = np.linspace(accel_1_params['TC_A1_TMIN']-accel_1_params['TC_A1_TREF'], accel_1_params['TC_A1_TMAX']-accel_1_params['TC_A1_TREF'], 100)
    temp_resample = temp_rel_resample + accel_1_params['TC_A1_TREF']

-    sensor_accel_1['x'] = median_filter(sensor_accel_1['x'])
-    sensor_accel_1['y'] = median_filter(sensor_accel_1['y'])
-    sensor_accel_1['z'] = median_filter(sensor_accel_1['z'])
-
    # fit X axis
-    correction_x = sensor_accel_1['x'] - np.median(sensor_accel_1['x'])
+    correction_x = sensor_accel_1['x']-np.median(sensor_accel_1['x'])
    if noResample:
-        coef_accel_1_x = np.polyfit(temp_rel, correction_x, 3)
+        coef_accel_1_x = np.polyfit(temp_rel,correction_x,3)
    else:
-        temp, sens = resampleWithDeltaX(temp_rel, correction_x)
-        coef_accel_1_x = np.polyfit(temp, sens, 3)
+        temp, sens = resampleWithDeltaX(temp_rel,correction_x)
+        coef_accel_1_x = np.polyfit(temp, sens ,3)

    accel_1_params['TC_A1_X3_0'] = coef_accel_1_x[0]
    accel_1_params['TC_A1_X2_0'] = coef_accel_1_x[1]
@@ -756,7 +728,7 @@ if num_accels >= 2 and not math.isnan(sensor_accel_1['temperature'][0]):
    correction_x_resample = fit_coef_accel_1_x(temp_rel_resample)

    # fit Y axis
-    correction_y = sensor_accel_1['y'] - np.median(sensor_accel_1['y'])
+    correction_y = sensor_accel_1['y']-np.median(sensor_accel_1['y'])
    if noResample:
        coef_accel_1_y = np.polyfit(temp_rel,correction_y,3)
    else:
@@ -771,12 +743,12 @@ if num_accels >= 2 and not math.isnan(sensor_accel_1['temperature'][0]):
    correction_y_resample = fit_coef_accel_1_y(temp_rel_resample)

    # fit Z axis
-    correction_z = sensor_accel_1['z'] - np.median(sensor_accel_1['z'])
+    correction_z = (sensor_accel_1['z'])-np.median(sensor_accel_1['z'])
    if noResample:
-        coef_accel_1_z = np.polyfit(temp_rel,correction_z, 3)
+        coef_accel_1_z = np.polyfit(temp_rel,correction_z,3)
    else:
        temp, sens = resampleWithDeltaX(temp_rel,correction_z)
-        coef_accel_1_z = np.polyfit(temp, sens, 3)
+        coef_accel_1_z = np.polyfit(temp, sens ,3)

    accel_1_params['TC_A1_X3_2'] = coef_accel_1_z[0]
    accel_1_params['TC_A1_X2_2'] = coef_accel_1_z[1]
@@ -852,17 +824,13 @@ if num_accels >= 3 and not math.isnan(sensor_accel_2['temperature'][0]):
    temp_rel_resample = np.linspace(accel_2_params['TC_A2_TMIN']-accel_2_params['TC_A2_TREF'], accel_2_params['TC_A2_TMAX']-accel_2_params['TC_A2_TREF'], 100)
    temp_resample = temp_rel_resample + accel_2_params['TC_A2_TREF']

-    sensor_accel_2['x'] = median_filter(sensor_accel_2['x'])
-    sensor_accel_2['y'] = median_filter(sensor_accel_2['y'])
-    sensor_accel_2['z'] = median_filter(sensor_accel_2['z'])
-
    # fit X axis
-    correction_x = sensor_accel_2['x'] - np.median(sensor_accel_2['x'])
+    correction_x = sensor_accel_2['x']-np.median(sensor_accel_2['x'])
    if noResample:
-        coef_accel_2_x = np.polyfit(temp_rel,correction_x, 3)
+        coef_accel_2_x = np.polyfit(temp_rel,correction_x,3)
    else:
-        temp, sens = resampleWithDeltaX(temp_rel, correction_x)
-        coef_accel_2_x = np.polyfit(temp, sens, 3)
+        temp, sens = resampleWithDeltaX(temp_rel,correction_x)
+        coef_accel_2_x = np.polyfit(temp, sens ,3)

    accel_2_params['TC_A2_X3_0'] = coef_accel_2_x[0]
    accel_2_params['TC_A2_X2_0'] = coef_accel_2_x[1]
@@ -872,7 +840,7 @@ if num_accels >= 3 and not math.isnan(sensor_accel_2['temperature'][0]):
    correction_x_resample = fit_coef_accel_2_x(temp_rel_resample)

    # fit Y axis
-    correction_y = sensor_accel_2['y'] - np.median(sensor_accel_2['y'])
+    correction_y = sensor_accel_2['y']-np.median(sensor_accel_2['y'])
    if noResample:
        coef_accel_2_y = np.polyfit(temp_rel,correction_y,3)
    else:
@@ -887,7 +855,7 @@ if num_accels >= 3 and not math.isnan(sensor_accel_2['temperature'][0]):
    correction_y_resample = fit_coef_accel_2_y(temp_rel_resample)

    # fit Z axis
-    correction_z = sensor_accel_2['z'] - np.median(sensor_accel_2['z'])
+    correction_z = sensor_accel_2['z']-np.median(sensor_accel_2['z'])
    if noResample:
        coef_accel_2_z = np.polyfit(temp_rel,correction_z,3)
    else:
@@ -967,13 +935,9 @@ if num_accels >= 4 and not math.isnan(sensor_accel_3['temperature'][0]):
    temp_rel_resample = np.linspace(accel_3_params['TC_A3_TMIN']-accel_3_params['TC_A3_TREF'], accel_3_params['TC_A3_TMAX']-accel_3_params['TC_A3_TREF'], 100)
    temp_resample = temp_rel_resample + accel_3_params['TC_A3_TREF']

-    sensor_accel_3['x'] = median_filter(sensor_accel_3['x'])
-    sensor_accel_3['y'] = median_filter(sensor_accel_3['y'])
-    sensor_accel_3['z'] = median_filter(sensor_accel_3['z'])
-
    # fit X axis
-    correction_x = sensor_accel_3['x'] - np.median(sensor_accel_3['x'])
-    coef_accel_3_x = np.polyfit(temp_rel, correction_x, 3)
+    correction_x = sensor_accel_3['x']-np.median(sensor_accel_3['x'])
+    coef_accel_3_x = np.polyfit(temp_rel,correction_x,3)
    accel_3_params['TC_A3_X3_0'] = coef_accel_3_x[0]
    accel_3_params['TC_A3_X2_0'] = coef_accel_3_x[1]
    accel_3_params['TC_A3_X1_0'] = coef_accel_3_x[2]
@@ -982,8 +946,8 @@ if num_accels >= 4 and not math.isnan(sensor_accel_3['temperature'][0]):
    correction_x_resample = fit_coef_accel_3_x(temp_rel_resample)

    # fit Y axis
-    correction_y = sensor_accel_3['y'] - np.median(sensor_accel_3['y'])
-    coef_accel_3_y = np.polyfit(temp_rel, correction_y, 3)
+    correction_y = sensor_accel_3['y']-np.median(sensor_accel_3['y'])
+    coef_accel_3_y = np.polyfit(temp_rel,correction_y,3)
    accel_3_params['TC_A3_X3_1'] = coef_accel_3_y[0]
    accel_3_params['TC_A3_X2_1'] = coef_accel_3_y[1]
    accel_3_params['TC_A3_X1_1'] = coef_accel_3_y[2]
@@ -992,8 +956,8 @@ if num_accels >= 4 and not math.isnan(sensor_accel_3['temperature'][0]):
    correction_y_resample = fit_coef_accel_3_y(temp_rel_resample)

    # fit Z axis
-    correction_z = sensor_accel_3['z'] - np.median(sensor_accel_3['z'])
-    coef_accel_3_z = np.polyfit(temp_rel, correction_z, 3)
+    correction_z = sensor_accel_3['z']-np.median(sensor_accel_3['z'])
+    coef_accel_3_z = np.polyfit(temp_rel,correction_z,3)
    accel_3_params['TC_A3_X3_2'] = coef_accel_3_z[0]
    accel_3_params['TC_A3_X2_2'] = coef_accel_3_z[1]
    accel_3_params['TC_A3_X1_2'] = coef_accel_3_z[2]
@@ -1060,10 +1024,8 @@ temp_rel = sensor_baro_0['temperature'] - baro_0_params['TC_B0_TREF']
 temp_rel_resample = np.linspace(baro_0_params['TC_B0_TMIN']-baro_0_params['TC_B0_TREF'], baro_0_params['TC_B0_TMAX']-baro_0_params['TC_B0_TREF'], 100)
 temp_resample = temp_rel_resample + baro_0_params['TC_B0_TREF']

-sensor_baro_0['pressure'] = median_filter(sensor_baro_0['pressure'])
-
 # fit data
-median_pressure = np.median(sensor_baro_0['pressure'])
+median_pressure = np.median(sensor_baro_0['pressure']);
 if noResample:
    coef_baro_0_x = np.polyfit(temp_rel,100*(sensor_baro_0['pressure']-median_pressure),5) # convert from hPa to Pa
 else:
@@ -1119,10 +1081,8 @@ if num_baros >= 2:
    temp_rel_resample = np.linspace(baro_1_params['TC_B1_TMIN']-baro_1_params['TC_B1_TREF'], baro_1_params['TC_B1_TMAX']-baro_1_params['TC_B1_TREF'], 100)
    temp_resample = temp_rel_resample + baro_1_params['TC_B1_TREF']

-    sensor_baro_1['pressure'] = median_filter(sensor_baro_1['pressure'])
-
    # fit data
-    median_pressure = np.median(sensor_baro_1['pressure'])
+    median_pressure = np.median(sensor_baro_1['pressure']);
    if noResample:
        coef_baro_1_x = np.polyfit(temp_rel,100*(sensor_baro_1['pressure']-median_pressure),5) # convert from hPa to Pa
    else:
@@ -1179,10 +1139,8 @@ if num_baros >= 3:
    temp_rel_resample = np.linspace(baro_2_params['TC_B2_TMIN']-baro_2_params['TC_B2_TREF'], baro_2_params['TC_B2_TMAX']-baro_2_params['TC_B2_TREF'], 100)
    temp_resample = temp_rel_resample + baro_2_params['TC_B2_TREF']

-    sensor_baro_2['pressure'] = median_filter(sensor_baro_2['pressure'])
-
    # fit data
-    median_pressure = np.median(sensor_baro_2['pressure'])
+    median_pressure = np.median(sensor_baro_2['pressure']);
    if noResample:
        coef_baro_2_x = np.polyfit(temp_rel,100*(sensor_baro_2['pressure']-median_pressure),5) # convert from hPa to Pa
    else:
@@ -1239,8 +1197,6 @@ if num_baros >= 4:
    temp_rel_resample = np.linspace(baro_3_params['TC_B3_TMIN']-baro_3_params['TC_B3_TREF'], baro_3_params['TC_B3_TMAX']-baro_3_params['TC_B3_TREF'], 100)
    temp_resample = temp_rel_resample + baro_3_params['TC_B3_TREF']

-    sensor_baro_3['pressure'] = median_filter(sensor_baro_3['pressure'])
-
    # fit data
    median_pressure = np.median(sensor_baro_3['pressure'])
    coef_baro_3_x = np.polyfit(temp_rel,100*(sensor_baro_3['pressure']-median_pressure),5) # convert from hPa to Pa
@@ -1,11 +0,0 @@
-.PHONY: docker_build docker_push all
-
-DATE := $(shell date +%Y-%m-%d)
-
-docker_build:
-	docker build -f Dockerfile -t px4-dev:${DATE} .
-
-docker_push: build
-        docker push px4io/px4-dev:${DATE}
-
-all: docker_build
@@ -155,7 +155,8 @@ if [[ $INSTALL_SIM == "true" ]]; then
 			# fix VMWare 3D graphics acceleration for gazebo
 			exportline="export SVGA_VGPU10=0"

-			if !grep -Fxq "$exportline" $HOME/.profile; then
+			if grep -Fxq "$exportline" $HOME/.profile; then
+			else
 				echo $exportline >> $HOME/.profile;
 			fi
 		fi
@@ -1,27 +0,0 @@
-#!/bin/bash
-
-# Start virtual X server in the background
-# - DISPLAY default is :99, set in dockerfile
-# - Users can override with `-e DISPLAY=` in `docker run` command to avoid
-#   running Xvfb and attach their screen
-if [[ -x "$(command -v Xvfb)" && "$DISPLAY" == ":99" ]]; then
-	echo "Starting Xvfb"
-	Xvfb :99 -screen 0 1600x1200x24+32 &
-fi
-
-# Check if the ROS_DISTRO is passed and use it
-# to source the ROS environment
-if [ -n "${ROS_DISTRO}" ]; then
-	source "/opt/ros/$ROS_DISTRO/setup.bash"
-fi
-
-# Use the LOCAL_USER_ID if passed in at runtime
-if [ -n "${LOCAL_USER_ID}" ]; then
-	echo "Starting with UID : $LOCAL_USER_ID"
-	# modify existing user's id
-	usermod -u $LOCAL_USER_ID user
-	# run as user
-	exec gosu user "$@"
-else
-	exec "$@"
-fi
@@ -1,26 +1,24 @@
 argcomplete
-argparse
+argparse>=1.2
 cerberus
 coverage
 empy>=3.3
 jinja2>=2.8
-jsonschema
-kconfiglib
 matplotlib>=3.0.*
 numpy>=1.13
 nunavut>=1.1.0
 packaging
+pandas>=0.21
 pkgconfig
 psutil
 pygments
+wheel>=0.31.1
 pymavlink
 pyros-genmsg
 pyserial>=3.0
 pyulog>=0.5.0
 pyyaml
 requests
-serial
 setuptools>=39.2.0
 six>=1.12.0
 toml>=0.9
-wheel>=0.31.1
@@ -31,9 +31,10 @@ done
 # detect if running in docker
 if [ -f /.dockerenv ]; then
 	echo "Running within docker, installing initial dependencies";
-	apt-get -qq update && DEBIAN_FRONTEND=noninteractive apt-get -q -y --no-install-recommends install \
+	apt-get --quiet -y update && DEBIAN_FRONTEND=noninteractive apt-get --quiet -y install \
 		ca-certificates \
-		gosu \
+		gnupg \
+		lsb-core \
 		sudo \
 		wget \
 		;
@@ -52,7 +53,7 @@ fi

 # check ubuntu version
 # otherwise warn and point to docker?
-UBUNTU_RELEASE=$(cat /etc/os-release | grep VERSION_ID | cut -d "\"" -f 2)
+UBUNTU_RELEASE="`lsb_release -rs`"

 if [[ "${UBUNTU_RELEASE}" == "14.04" ]]; then
 	echo "Ubuntu 14.04 is no longer supported"
@@ -62,18 +63,16 @@ elif [[ "${UBUNTU_RELEASE}" == "16.04" ]]; then
 	exit 1
 elif [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then
 	echo "Ubuntu 18.04"
-
 elif [[ "${UBUNTU_RELEASE}" == "20.04" ]]; then
 	echo "Ubuntu 20.04"
-
 fi


 echo
 echo "Installing PX4 general dependencies"

-sudo apt-get -qq update
-sudo DEBIAN_FRONTEND=noninteractive apt-get -q -y --no-install-recommends install \
+sudo apt-get update -y --quiet
+sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
 	astyle \
 	build-essential \
 	ccache \
@@ -94,23 +93,20 @@ sudo DEBIAN_FRONTEND=noninteractive apt-get -q -y --no-install-recommends instal
 	python3-wheel \
 	rsync \
 	shellcheck \
+	unzip \
+	zip \
 	;

 if [[ "${UBUNTU_RELEASE}" == "16.04" ]]; then
 	echo "Installing Ubuntu 16.04 PX4-compatible ccache version"
-	wget -q -O /tmp/ccache_3.4.1-1_amd64.deb http://launchpadlibrarian.net/356662933/ccache_3.4.1-1_amd64.deb
+	wget -O /tmp/ccache_3.4.1-1_amd64.deb http://launchpadlibrarian.net/356662933/ccache_3.4.1-1_amd64.deb
 	sudo dpkg -i /tmp/ccache_3.4.1-1_amd64.deb
 fi

 # Python3 dependencies
 echo
 echo "Installing PX4 Python3 dependencies"
-if [ -f /.dockerenv ]; then
-	# system wide for docker
-	python3 -m pip install -r ${DIR}/requirements.txt
-else
-	python3 -m pip install --user --quiet -r ${DIR}/requirements.txt
-fi
+python3 -m pip install --user -r ${DIR}/requirements.txt

 # NuttX toolchain (arm-none-eabi-gcc)
 if [[ $INSTALL_NUTTX == "true" ]]; then
@@ -118,11 +114,17 @@ if [[ $INSTALL_NUTTX == "true" ]]; then
 	echo
 	echo "Installing NuttX dependencies"

-	sudo DEBIAN_FRONTEND=noninteractive apt-get -q -y --no-install-recommends install \
-		g++-multilib \
-		gcc-multilib \
+	sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
+		autoconf \
+		automake \
+		bison \
+		bzip2 \
+		file \
+		flex \
 		gdb-multiarch \
-		genromfs \
+		gperf \
+		libncurses-dev \
+		libtool \
 		pkg-config \
 		screen \
 		vim-common \
@@ -148,7 +150,7 @@ if [[ $INSTALL_NUTTX == "true" ]]; then

 	else
 		echo "Installing arm-none-eabi-gcc-${NUTTX_GCC_VERSION}";
-		wget -q -O /tmp/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}-linux.tar.bz2 https://armkeil.blob.core.windows.net/developer/Files/downloads/gnu-rm/${NUTTX_GCC_VERSION_SHORT}/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}-${INSTALL_ARCH}-linux.tar.bz2 && \
+		wget -O /tmp/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}-linux.tar.bz2 https://armkeil.blob.core.windows.net/developer/Files/downloads/gnu-rm/${NUTTX_GCC_VERSION_SHORT}/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}-${INSTALL_ARCH}-linux.tar.bz2 && \
 			sudo tar -jxf /tmp/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}-linux.tar.bz2 -C /opt/;

 		# add arm-none-eabi-gcc to user's PATH
@@ -169,36 +171,37 @@ if [[ $INSTALL_SIM == "true" ]]; then
 	echo "Installing PX4 simulation dependencies"

 	# General simulation dependencies
-	sudo DEBIAN_FRONTEND=noninteractive apt-get -q -y --no-install-recommends install \
+	sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
 		bc \
 		;

 	if [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then
+		java_version=11
 		gazebo_version=9
-		MAVSDK_VERSION=0.39.0
-		wget -q "https://github.com/mavlink/MAVSDK/releases/download/v${MAVSDK_VERSION}/mavsdk_{MAVSDK_VERSION})_ubuntu18.04_amd64.deb" -O /tmp/mavsdk.deb && sudo dpkg -i /tmp/mavsdk.deb
 	elif [[ "${UBUNTU_RELEASE}" == "20.04" ]]; then
+		java_version=14
 		gazebo_version=11
-		MAVSDK_VERSION=0.39.0
-		wget -q "https://github.com/mavlink/MAVSDK/releases/download/v{MAVSDK_VERSION}/mavsdk_{MAVSDK_VERSION}_ubuntu20.04_amd64.deb" -O /tmp/mavsdk.deb && sudo dpkg -i /tmp/mavsdk.deb
 	else
+		java_version=14
 		gazebo_version=11
 	fi
-
 	# Java (jmavsim or fastrtps)
-	sudo DEBIAN_FRONTEND=noninteractive apt-get -q -y --no-install-recommends install \
+	sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
 		ant \
-		default-jre-headless \
-		default-jdk-headless \
+		openjdk-$java_version-jre \
+		openjdk-$java_version-jdk \
 		libvecmath-java \
 		;

+	# Set Java 11 as default
+	sudo update-alternatives --set java $(update-alternatives --list java | grep "java-$java_version")
+
 	# Gazebo
 	sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
-	wget -q http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
+	wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
 	# Update list, since new gazebo-stable.list has been added
-	sudo apt-get update -qq
-	sudo DEBIAN_FRONTEND=noninteractive apt-get -q -y --no-install-recommends install \
+	sudo apt-get update -y --quiet
+	sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
 		dmidecode \
 		gazebo$gazebo_version \
 		gstreamer1.0-plugins-bad \
@@ -90,7 +90,7 @@ static int configure_switch(void);
 __EXPORT void board_on_reset(int status)
 {
 	for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) {
-		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i)));
 	}

 	if (status >= 0) {
@@ -116,7 +116,6 @@ CONFIG_NSH_ROMFSETC=y
 CONFIG_NSH_ROMFSSECTSIZE=128
 CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
-CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
@@ -106,7 +106,7 @@ __EXPORT void board_peripheral_reset(int ms)
 __EXPORT void board_on_reset(int status)
 {
 	for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) {
-		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i)));
 	}

 	if (status >= 0) {
@@ -116,7 +116,6 @@ CONFIG_NSH_ROMFSETC=y
 CONFIG_NSH_ROMFSSECTSIZE=128
 CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
-CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
@@ -106,7 +106,7 @@ __EXPORT void board_peripheral_reset(int ms)
 __EXPORT void board_on_reset(int status)
 {
 	for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) {
-		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i)));
 	}

 	if (status >= 0) {
@@ -97,7 +97,7 @@ __EXPORT void board_peripheral_reset(int ms)
 __EXPORT void board_on_reset(int status)
 {
 	for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) {
-		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i)));
 	}

 	if (status >= 0) {
@@ -97,7 +97,7 @@ __EXPORT void board_peripheral_reset(int ms)
 __EXPORT void board_on_reset(int status)
 {
 	for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) {
-		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i)));
 	}

 	if (status >= 0) {
@@ -117,7 +117,6 @@ CONFIG_NSH_ROMFSETC=y
 CONFIG_NSH_ROMFSSECTSIZE=128
 CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
-CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
@@ -138,7 +138,7 @@ __EXPORT void board_peripheral_reset(int ms)
 __EXPORT void board_on_reset(int status)
 {
 	for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) {
-		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i)));
 	}

 	if (status >= 0) {
@@ -120,7 +120,7 @@ __EXPORT void board_peripheral_reset(int ms)
 __EXPORT void board_on_reset(int status)
 {
 	for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) {
-		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i)));
 	}

 	if (status >= 0) {
@@ -116,7 +116,6 @@ CONFIG_NSH_ROMFSETC=y
 CONFIG_NSH_ROMFSSECTSIZE=128
 CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
-CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
@@ -139,7 +139,7 @@ __EXPORT void board_peripheral_reset(int ms)
 __EXPORT void board_on_reset(int status)
 {
 	for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) {
-		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i)));
 	}

 	if (status >= 0) {
@@ -115,7 +115,6 @@ CONFIG_NSH_ROMFSETC=y
 CONFIG_NSH_ROMFSSECTSIZE=128
 CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
-CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
@@ -137,7 +137,7 @@ __EXPORT void board_peripheral_reset(int ms)
 __EXPORT void board_on_reset(int status)
 {
 	for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) {
-		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i)));
 	}

 	if (status >= 0) {
@@ -128,7 +128,7 @@ __EXPORT void board_peripheral_reset(int ms)
 __EXPORT void board_on_reset(int status)
 {
 	for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) {
-		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i)));
 	}

 	if (status >= 0) {
@@ -128,7 +128,7 @@ __EXPORT void board_peripheral_reset(int ms)
 __EXPORT void board_on_reset(int status)
 {
 	for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) {
-		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i)));
 	}

 	if (status >= 0) {
@@ -100,7 +100,7 @@ __EXPORT void board_peripheral_reset(int ms)
 __EXPORT void board_on_reset(int status)
 {
 	for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) {
-		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i)));
 	}

 	if (status >= 0) {
@@ -100,7 +100,7 @@ __EXPORT void board_peripheral_reset(int ms)
 __EXPORT void board_on_reset(int status)
 {
 	for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) {
-		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i)));
 	}

 	if (status >= 0) {
@@ -119,7 +119,7 @@

 /* RC Serial port */
 #define RC_SERIAL_PORT          "/dev/ttyS4"
-#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT
+#define RC_SERIAL_SINGLEWIRE

 #define GPIO_RSSI_IN            /* PC1  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)

@@ -100,7 +100,7 @@ __EXPORT void board_peripheral_reset(int ms)
 __EXPORT void board_on_reset(int status)
 {
 	for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) {
-		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i)));
 	}

 	if (status >= 0) {
@@ -26,7 +26,6 @@ CONFIG_BOARD_LOOPSPERMSEC=15175
 CONFIG_BOARD_RESET_ON_ASSERT=2
 CONFIG_BUILTIN=y
 CONFIG_C99_BOOL8=y
-CONFIG_CAN_CONNS=1
 CONFIG_CDCACM=y
 CONFIG_CDCACM_BULKIN_REQLEN=96
 CONFIG_CDCACM_PRODUCTID=0x001c
@@ -124,7 +123,6 @@ CONFIG_NETUTILS_TELNETD=y
 CONFIG_NET_BROADCAST=y
 CONFIG_NET_CAN=y
 CONFIG_NET_CAN_NOTIFIER=y
-CONFIG_NET_CAN_RAW_FILTER_MAX=0
 CONFIG_NET_CAN_RAW_TX_DEADLINE=y
 CONFIG_NET_CAN_SOCK_OPTS=y
 CONFIG_NET_ICMP=y
@@ -339,7 +339,7 @@ __END_DECLS
 #define LED_TIM3_CH4OUT   /* PTC8  RGB_B */ PIN_FTM3_CH4_1

 /* This board provides a DMA pool and APIs */
-#define BOARD_DMA_ALLOC_POOL_SIZE 2048
+#define BOARD_DMA_ALLOC_POOL_SIZE 5120

 /* This board provides the board_on_reset interface */

@@ -117,7 +117,7 @@ __END_DECLS
 void board_on_reset(int status)
 {
 	for (int i = 0; i < 6; ++i) {
-		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i)));
 	}

 	if (status >= 0) {
@@ -25,7 +25,6 @@ CONFIG_BOARD_LOOPSPERMSEC=15175
 CONFIG_BOARD_RESET_ON_ASSERT=2
 CONFIG_BUILTIN=y
 CONFIG_C99_BOOL8=y
-CONFIG_CAN_CONNS=1
 CONFIG_CDCACM=y
 CONFIG_CDCACM_BULKIN_REQLEN=96
 CONFIG_CDCACM_PRODUCTID=0x001c
@@ -123,7 +122,6 @@ CONFIG_NETUTILS_TELNETD=y
 CONFIG_NET_BROADCAST=y
 CONFIG_NET_CAN=y
 CONFIG_NET_CAN_NOTIFIER=y
-CONFIG_NET_CAN_RAW_FILTER_MAX=0
 CONFIG_NET_CAN_RAW_TX_DEADLINE=y
 CONFIG_NET_CAN_SOCK_OPTS=y
 CONFIG_NET_ICMP=y
@@ -343,7 +343,7 @@ __END_DECLS
 #define LED_TIM3_CH4OUT   /* PTC8  RGB_B */ PIN_FTM3_CH4_1

 /* This board provides a DMA pool and APIs */
-#define BOARD_DMA_ALLOC_POOL_SIZE 2048
+#define BOARD_DMA_ALLOC_POOL_SIZE 5120

 /* This board provides the board_on_reset interface */

@@ -117,7 +117,7 @@ __END_DECLS
 void board_on_reset(int status)
 {
 	for (int i = 0; i < 6; ++i) {
-		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i)));
 	}

 	if (status >= 0) {
@@ -90,12 +90,6 @@
 #define BOARD_LED_G_BIT   (1 << BOARD_LED_G)
 #define BOARD_LED_B_BIT   (1 << BOARD_LED_B)

-/* Board revision detection pin
- * 0 equals UCANS32K146-01
- * 1 equals UCANS32K146B
- */
-#define BOARD_REVISION_DETECT_PIN  (GPIO_INPUT | PIN_PORTA | PIN10 )
-
 /* If CONFIG_ARCH_LEDs is defined, then NuttX will control the LEDs on board
 * the UCANS32K146.  The following definitions describe how NuttX
 * controls the LEDs:
@@ -158,9 +152,11 @@
 /* CAN selections ***********************************************************/
 #define PIN_CAN0_TX      PIN_CAN0_TX_4      /* PTE5 */
 #define PIN_CAN0_RX      PIN_CAN0_RX_4      /* PTE4 */
-#define PIN_CAN0_STB     (GPIO_OUTPUT | PIN_PORTE | PIN11 )
+#define PIN_CAN0_ENABLE  (GPIO_OUTPUT | PIN_PORTE | PIN11 )
+#define CAN0_ENABLE_OUT  0
 #define PIN_CAN1_TX      PIN_CAN1_TX_1      /* PTA13 */
 #define PIN_CAN1_RX      PIN_CAN1_RX_1      /* PTA12 */
-#define PIN_CAN1_STB     (GPIO_OUTPUT | PIN_PORTE | PIN10 )
+#define PIN_CAN1_ENABLE  (GPIO_OUTPUT | PIN_PORTE | PIN10 )
+#define CAN1_ENABLE_OUT  0

 #endif  /* __BOARDS_ARM_RDDRONE_UAVCAN146_INCLUDE_BOARD_H */
@@ -151,20 +151,5 @@ int s32k1xx_bringup(void)

 #endif
 #endif
-
-#ifdef CONFIG_S32K1XX_FLEXCAN
-	s32k1xx_pinconfig(BOARD_REVISION_DETECT_PIN);
-
-	if (s32k1xx_gpioread(BOARD_REVISION_DETECT_PIN)) {
-		/* STB high -> active CAN phy */
-		s32k1xx_pinconfig(PIN_CAN0_STB  | GPIO_OUTPUT_ONE);
-
-	} else {
-		/* STB low -> active CAN phy */
-		s32k1xx_pinconfig(PIN_CAN0_STB  | GPIO_OUTPUT_ZERO);
-	}
-
-#endif
-
 	return ret;
 }
@@ -71,18 +71,7 @@ __EXPORT void s32k1xx_board_initialize(void)
 	// Can GPIO
 	s32k1xx_pinconfig(PIN_CAN0_TX);
 	s32k1xx_pinconfig(PIN_CAN0_RX);
-
-	s32k1xx_pinconfig(BOARD_REVISION_DETECT_PIN);
-
-	if (s32k1xx_gpioread(BOARD_REVISION_DETECT_PIN)) {
-		/* STB high -> active CAN phy */
-		s32k1xx_pinconfig(PIN_CAN0_STB  | GPIO_OUTPUT_ONE);
-
-	} else {
-		/* STB low -> active CAN phy */
-		s32k1xx_pinconfig(PIN_CAN0_STB  | GPIO_OUTPUT_ZERO);
-	}
-
+	s32k1xx_pinconfig(PIN_CAN0_ENABLE  | GPIO_OUTPUT_ZERO);
 	//s32k1xx_gpiowrite
 #if defined(OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO)
 	s32k1xx_pinconfig(GPIO_GETNODEINFO_JUMPER);
@@ -112,14 +101,7 @@ void board_deinitialize(void)
 	} while ((regval & CAN_MCR_LPMACK) == 0);


-	if (s32k1xx_gpioread(BOARD_REVISION_DETECT_PIN)) {
-		/* STB high -> standby CAN phy */
-		s32k1xx_pinconfig(PIN_CAN0_STB  | GPIO_OUTPUT_ZERO);
-
-	} else {
-		/* STB low -> standby CAN phy */
-		s32k1xx_pinconfig(PIN_CAN0_STB  | GPIO_OUTPUT_ONE);
-	}
+	s32k1xx_pinconfig(PIN_CAN0_ENABLE  | GPIO_OUTPUT_ONE);
 }

 /****************************************************************************
@@ -116,7 +116,6 @@ CONFIG_NSH_ROMFSETC=y
 CONFIG_NSH_ROMFSSECTSIZE=128
 CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
-CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
@@ -37,7 +37,7 @@ px4_add_board(
 		imu/invensense/icm20602
 		imu/invensense/icm20689
 		imu/invensense/icm20948 # required for ak09916 mag
-		#irlock
+		irlock
 		lights # all available light drivers
 		#lights/rgbled_pwm
 		#magnetometer # all available magnetometer drivers
@@ -91,7 +91,7 @@ px4_add_board(
 		#rover_pos_control
 		sensors
 		#sih
-		#temperature_compensation
+		temperature_compensation
 		#uuv_att_control
 		#uuv_pos_control
 		#vmount
@@ -139,7 +139,7 @@ __EXPORT void board_peripheral_reset(int ms)
 __EXPORT void board_on_reset(int status)
 {
 	for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) {
-		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i)));
 	}

 	if (status >= 0) {
@@ -146,7 +146,6 @@ CONFIG_NSH_STRERROR=y
 CONFIG_NSH_TELNET=y
 CONFIG_NSH_TELNET_LOGIN=y
 CONFIG_NSH_VARS=y
-CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
@@ -143,7 +143,7 @@ __EXPORT void board_peripheral_reset(int ms)
 __EXPORT void board_on_reset(int status)
 {
 	for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) {
-		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i)));
 	}

 	if (status >= 0) {
@@ -140,7 +140,7 @@ __EXPORT void board_peripheral_reset(int ms)
 __EXPORT void board_on_reset(int status)
 {
 	for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) {
-		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i)));
 	}

 	if (status >= 0) {
@@ -128,6 +128,7 @@ CONFIG_NET_TCP_WRITE_BUFFERS=y
 CONFIG_NET_UDP=y
 CONFIG_NET_UDP_CHECKSUMS=y
 CONFIG_NET_UDP_WRITE_BUFFERS=y
+CONFIG_NFILE_DESCRIPTORS=12
 CONFIG_NSH_ARCHINIT=y
 CONFIG_NSH_ARCHROMFS=y
 CONFIG_NSH_ARGCAT=y
@@ -140,7 +140,7 @@ __EXPORT void board_peripheral_reset(int ms)
 __EXPORT void board_on_reset(int status)
 {
 	for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) {
-		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i)));
 	}

 	if (status >= 0) {
@@ -121,7 +121,7 @@ __EXPORT void board_peripheral_reset(int ms)
 __EXPORT void board_on_reset(int status)
 {
 	for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) {
-		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i)));
 	}

 	if (status >= 0) {
@@ -71,13 +71,6 @@ if (BLOATY_PROGRAM)
 		USES_TERMINAL
 		)

-	# bloaty statically allocated RAM
-	add_custom_target(bloaty_ram
-		COMMAND ${BLOATY_PROGRAM} -c ${PX4_SOURCE_DIR}/Tools/bloaty_static_ram.bloaty -d bloaty_static_ram,compileunits --source-filter ^ram$ ${BLOATY_OPTS} $<TARGET_FILE:px4>
-		DEPENDS px4
-		USES_TERMINAL
-		)
-
 	# bloaty compare with last master build
 	add_custom_target(bloaty_compare_master
 		COMMAND wget -c -N --no-verbose https://s3.amazonaws.com/px4-travis/Firmware/master/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}.elf -O master.elf
@@ -28,6 +28,7 @@ uint32 tx_buffer_overruns               # number of TX buffer overruns

 float32 rx_rate_avg                     # transmit rate average (Bytes/s)
 uint32 rx_message_count                 # count of total messages received
+uint32 rx_message_count_supported       # count of total messages received from supported systems and components (for loss statistics)
 uint32 rx_message_lost_count
 uint32 rx_buffer_overruns               # number of RX buffer overruns
 uint32 rx_parse_errors                  # number of parse errors
@@ -5,8 +5,3 @@ bool ground_contact	# true if vehicle has ground contact but is not landed (1. s
 bool maybe_landed	# true if the vehicle might have landed (2. stage)
 bool landed		# true if vehicle is currently landed on the ground (3. stage)
 bool in_ground_effect # indicates if from the perspective of the landing detector the vehicle might be in ground effect (baro). This flag will become true if the vehicle is not moving horizontally and is descending (crude assumption that user is landing).
-bool in_descend
-bool has_low_throttle
-bool vertical_movement
-bool horizontal_movement
-bool close_to_ground_or_skipped_check
@@ -48,7 +48,7 @@ struct wq_config_t {

 namespace wq_configurations
 {
-static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1952, 0}; // PX4 inner loop highest priority
+static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1920, 0}; // PX4 inner loop highest priority
 static constexpr wq_config_t ctrl_alloc{"wq:ctrl_alloc", 9500, 0}; // PX4 control allocation, same priority as rate_ctrl

 static constexpr wq_config_t SPI0{"wq:SPI0", 2336, -1};
@@ -104,10 +104,10 @@ int up_pwm_servo_init(uint32_t channel_mask)
 	return OK;
 }

-void up_pwm_servo_deinit(uint32_t channel_mask)
+void up_pwm_servo_deinit(void)
 {
 	/* disable the timers */
-	up_pwm_servo_arm(false, channel_mask);
+	up_pwm_servo_arm(false);
 }

 int up_pwm_servo_set_rate_group_update(unsigned channel, unsigned rate)
@@ -154,8 +154,8 @@ uint32_t up_pwm_servo_get_rate_group(unsigned group)
 }

 void
-up_pwm_servo_arm(bool armed, uint32_t channel_mask)
+up_pwm_servo_arm(bool armed)
 {
-	io_timer_set_enable(armed, IOTimerChanMode_OneShot, channel_mask);
-	io_timer_set_enable(armed, IOTimerChanMode_PWMOut, channel_mask);
+	io_timer_set_enable(armed, IOTimerChanMode_OneShot, IO_TIMER_ALL_MODES_CHANNELS);
+	io_timer_set_enable(armed, IOTimerChanMode_PWMOut, IO_TIMER_ALL_MODES_CHANNELS);
 }
@@ -112,8 +112,7 @@ int kinetis_gpiosetevent(uint32_t pinset, bool risingedge, bool fallingedge, boo
 #define px4_arch_gpiosetevent(pinset,r,f,e,fp,a)  kinetis_gpiosetevent(pinset,r,f,e,fp,a)

 #define _PX4_MAKE_GPIO(pin_ftmx, io)    ((((uint32_t)(pin_ftmx)) & ~(_PIN_MODE_MASK | _PIN_OPTIONS_MASK)) |(io))
-#define PX4_MAKE_GPIO_INPUT(gpio) _PX4_MAKE_GPIO((gpio), GPIO_PULLUP)
-#define PX4_MAKE_GPIO_INPUT_PULL_DOWN(gpio) _PX4_MAKE_GPIO((gpio), GPIO_PULLDOWN)
-#define PX4_MAKE_GPIO_OUTPUT(gpio) _PX4_MAKE_GPIO((gpio), GPIO_HIGHDRIVE)
+#define PX4_MAKE_GPIO_INPUT(gpio) _PX4_MAKE_GPIO(gpio, GPIO_PULLUP)
+#define PX4_MAKE_GPIO_OUTPUT(gpio) _PX4_MAKE_GPIO(gpio, GPIO_HIGHDRIVE)

 __END_DECLS
@@ -105,10 +105,10 @@ int up_pwm_servo_init(uint32_t channel_mask)
 	return OK;
 }

-void up_pwm_servo_deinit(uint32_t channel_mask)
+void up_pwm_servo_deinit(void)
 {
 	/* disable the timers */
-	up_pwm_servo_arm(false, channel_mask);
+	up_pwm_servo_arm(false);
 }

 int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
@@ -155,8 +155,8 @@ uint32_t up_pwm_servo_get_rate_group(unsigned group)
 }

 void
-up_pwm_servo_arm(bool armed, uint32_t channel_mask)
+up_pwm_servo_arm(bool armed)
 {
-	io_timer_set_enable(armed, IOTimerChanMode_OneShot, channel_mask);
-	io_timer_set_enable(armed, IOTimerChanMode_PWMOut, channel_mask);
+	io_timer_set_enable(armed, IOTimerChanMode_OneShot, IO_TIMER_ALL_MODES_CHANNELS);
+	io_timer_set_enable(armed, IOTimerChanMode_PWMOut, IO_TIMER_ALL_MODES_CHANNELS);
 }
@@ -105,7 +105,6 @@ int imxrt_gpiosetevent(uint32_t pinset, bool risingedge, bool fallingedge, bool
 #define px4_arch_gpiosetevent(pinset,r,f,e,fp,a)  imxrt_gpiosetevent(pinset,r,f,e,fp,a)

 #define PX4_MAKE_GPIO_INPUT(gpio) (((gpio) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT | IOMUX_SCHMITT_TRIGGER | IOMUX_PULL_UP_47K | IOMUX_DRIVE_HIZ))
-#define PX4_MAKE_GPIO_INPUT_PULL_DOWN(gpio) (((gpio) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT | IOMUX_SCHMITT_TRIGGER | IOMUX_PULL_DOWN_100K | IOMUX_DRIVE_HIZ))
 #define PX4_MAKE_GPIO_OUTPUT(gpio) (((gpio) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_OUTPUT | GPIO_OUTPUT_ZERO | IOMUX_CMOS_OUTPUT | IOMUX_PULL_KEEP | IOMUX_DRIVE_33OHM  | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST))

 __END_DECLS
@@ -102,10 +102,10 @@ int up_pwm_servo_init(uint32_t channel_mask)
 	return OK;
 }

-void up_pwm_servo_deinit(uint32_t channel_mask)
+void up_pwm_servo_deinit(void)
 {
 	/* disable the timers */
-	up_pwm_servo_arm(false, channel_mask);
+	up_pwm_servo_arm(false);
 }

 int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
@@ -152,8 +152,8 @@ uint32_t up_pwm_servo_get_rate_group(unsigned group)
 }

 void
-up_pwm_servo_arm(bool armed, uint32_t channel_mask)
+up_pwm_servo_arm(bool armed)
 {
-	io_timer_set_enable(armed, IOTimerChanMode_OneShot, channel_mask);
-	io_timer_set_enable(armed, IOTimerChanMode_PWMOut, channel_mask);
+	io_timer_set_enable(armed, IOTimerChanMode_OneShot, IO_TIMER_ALL_MODES_CHANNELS);
+	io_timer_set_enable(armed, IOTimerChanMode_PWMOut, IO_TIMER_ALL_MODES_CHANNELS);
 }
@@ -65,7 +65,7 @@
 static const uint32_t modes[] = {
 	/*                                      to  tb   */
 	/* BOARD_RESET_MODE_CLEAR                5   y   */  0,
-	/* BOARD_RESET_MODE_BOOT_TO_BL           0   n   */  0xb007b007,
+	/* BOARD_RESET_MODE_BOOT_TO_BL           0   n   */  0xb0070001,
 	/* BOARD_RESET_MODE_BOOT_TO_VALID_APP    0   y   */  0xb0070002,
 	/* BOARD_RESET_MODE_CAN_BL               10  n   */  0xb0080000,
 	/* BOARD_RESET_MODE_RTC_BOOT_FWOK        0   n   */  0xb0093a26
@@ -103,7 +103,6 @@ __BEGIN_DECLS
 #define px4_arch_gpiosetevent(pinset,r,f,e,fp,a)  stm32_gpiosetevent(pinset,r,f,e,fp,a)

 #define PX4_MAKE_GPIO_INPUT(gpio) (((gpio) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_PULLUP))
-#define PX4_MAKE_GPIO_INPUT_PULL_DOWN(gpio) (((gpio) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_PULLDOWN))
 #define PX4_MAKE_GPIO_OUTPUT(gpio) (((gpio) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR))

 #define PX4_GPIO_PIN_OFF(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_2MHz))
@@ -769,7 +769,7 @@ int io_timer_set_rate(unsigned timer, unsigned rate)
 			int changeOneShot = reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut);
 			int changeDshot = reallocate_channel_resources(channels, IOTimerChanMode_Dshot, IOTimerChanMode_PWMOut);

-			if (changeOneShot || changeDshot) {
+			if (changeOneShot && changeDshot) {
 				io_timer_set_PWM_mode(timer);
 			}

@@ -109,10 +109,10 @@ int up_pwm_servo_init(uint32_t channel_mask)
 	return OK;
 }

-void up_pwm_servo_deinit(uint32_t channel_mask)
+void up_pwm_servo_deinit(void)
 {
 	/* disable the timers */
-	up_pwm_servo_arm(false, channel_mask);
+	up_pwm_servo_arm(false);
 }

 int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
@@ -158,8 +158,8 @@ uint32_t up_pwm_servo_get_rate_group(unsigned group)
 }

 void
-up_pwm_servo_arm(bool armed, uint32_t channel_mask)
+up_pwm_servo_arm(bool armed)
 {
-	io_timer_set_enable(armed, IOTimerChanMode_OneShot, channel_mask);
-	io_timer_set_enable(armed, IOTimerChanMode_PWMOut, channel_mask);
+	io_timer_set_enable(armed, IOTimerChanMode_OneShot, IO_TIMER_ALL_MODES_CHANNELS);
+	io_timer_set_enable(armed, IOTimerChanMode_PWMOut, IO_TIMER_ALL_MODES_CHANNELS);
 }
@@ -46,4 +46,3 @@ add_subdirectory(tfmini)
 add_subdirectory(ulanding_radar)
 add_subdirectory(vl53l0x)
 add_subdirectory(vl53l1x)
-add_subdirectory(gy_us42)
@@ -1,43 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2021 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-px4_add_module(
-	MODULE drivers__distance_sensor__gy_us42
-	MAIN gy_us42
-	SRCS
-		GY_US42.cpp
-		GY_US42.hpp
-		GY_US42_main.cpp
-	DEPENDS
-		drivers_rangefinder
-		px4_work_queue
-	)
@@ -1,133 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "GY_US42.hpp"
-
-GY_US42::GY_US42(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency, int address) :
-	I2C(DRV_DIST_DEVTYPE_GY_US42, MODULE_NAME, bus, address, bus_frequency),
-	I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
-	_px4_rangefinder(get_device_id(), rotation)
-{
-	_px4_rangefinder.set_max_distance(GY_US42_MAX_DISTANCE);
-	_px4_rangefinder.set_min_distance(GY_US42_MIN_DISTANCE);
-}
-
-GY_US42::~GY_US42()
-{
-	perf_free(_sample_perf);
-	perf_free(_comms_errors);
-}
-
-int GY_US42::init()
-{
-	// I2C init (and probe) first.
-	if (I2C::init() != OK) {
-		return PX4_ERROR;
-	}
-
-	_state = STATE::POWERON_WAIT;
-	ScheduleDelayed(2000);
-	return OK;
-}
-
-int GY_US42::collect()
-{
-	// Read from the sensor.
-	uint8_t val[2] = {};
-	perf_begin(_sample_perf);
-
-	const hrt_abstime timestamp_sample = hrt_absolute_time();
-	int ret = transfer(nullptr, 0, val, 2);
-
-	if (ret < 0) {
-		PX4_DEBUG("error reading from sensor: %d", ret);
-		perf_count(_comms_errors);
-		perf_end(_sample_perf);
-		return ret;
-	}
-
-	uint16_t distance_cm = val[0] << 8 | val[1];
-	float distance_m = float(distance_cm) * 1e-2f;
-
-	_px4_rangefinder.update(timestamp_sample, distance_m);
-
-	perf_end(_sample_perf);
-	return PX4_OK;
-}
-
-int GY_US42::measure()
-{
-	uint8_t cmd[1] = {GY_US42_TAKE_RANGE_REG};
-
-	// Send the command to begin a measurement.
-	int ret = transfer(cmd, 1, nullptr, 0);
-
-	if (ret != PX4_OK) {
-		perf_count(_comms_errors);
-		PX4_DEBUG("i2c::transfer returned %d", ret);
-		return ret;
-	}
-
-	return PX4_OK;
-}
-
-void GY_US42::RunImpl()
-{
-	switch (_state) {
-	case STATE::INIT:
-		// do nothing
-		break;
-
-	case STATE::POWERON_WAIT:
-		measure();
-		_state = STATE::MEASURE_WAIT;
-		ScheduleOnInterval(GY_US42_CONVERSION_INTERVAL, GY_US42_CONVERSION_INTERVAL);
-		break;
-
-	case STATE::MEASURE_WAIT:
-		collect();
-		measure();
-		// forever loop
-		break;
-
-	case STATE::MODIFYADDR_WAIT:
-		break;
-	}
-}
-
-void GY_US42::print_status()
-{
-	I2CSPIDriverBase::print_status();
-	perf_print_counter(_sample_perf);
-	perf_print_counter(_comms_errors);
-}
@@ -1,103 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file GY_US42.cpp
- *
- * Driver for the GY-US42 sonar range finder on I2C.
- */
-
-#pragma once
-
-#include <px4_platform_common/px4_config.h>
-#include <px4_platform_common/i2c_spi_buses.h>
-#include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
-#include <drivers/device/i2c.h>
-#include <drivers/drv_hrt.h>
-#include <lib/perf/perf_counter.h>
-
-/* Configuration Constants */
-#define GY_US42_BASEADDR 				0x70 	// 7-bit address. 8-bit address is 0xE0.
-
-/* GY_US42 Registers addresses */
-#define GY_US42_TAKE_RANGE_REG			0x51	// Measure range Register.
-#define GY_US42_SET_ADDRESS_CMD1			0xAA	// Change address 1 cmd.
-#define GY_US42_SET_ADDRESS_CMD2			0xA5	// Change address 2 cmd.
-
-/* Device limits */
-#define GY_US42_MIN_DISTANCE 			(0.20f)
-#define GY_US42_MAX_DISTANCE 			(7.2f)
-
-#define GY_US42_CONVERSION_INTERVAL 		50000	// 50ms for one sonar.
-
-class GY_US42 : public device::I2C, public I2CSPIDriver<GY_US42>
-{
-public:
-	GY_US42(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency,
-		int address = GY_US42_BASEADDR);
-	~GY_US42() override;
-
-	static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
-					     int runtime_instance);
-	static void print_usage();
-
-	int init() override;
-	void print_status() override;
-
-	void RunImpl();
-
-private:
-
-	enum class STATE : uint8_t {
-		INIT,
-		POWERON_WAIT,
-		MEASURE_WAIT,
-		MODIFYADDR_WAIT
-	};
-	STATE _state{STATE::INIT};
-
-	int collect();
-	int measure();
-
-	/**
-	 * Test whether the device supported by the driver is present at a
-	 * specific address.
-	 * @param address The I2C bus address to probe.
-	 * @return True if the device is present.
-	 */
-
-	PX4Rangefinder _px4_rangefinder;
-
-	perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")};
-	perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED,  MODULE_NAME": read")};
-};
@@ -1,107 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "GY_US42.hpp"
-
-#include <px4_platform_common/getopt.h>
-#include <px4_platform_common/module.h>
-
-void
-GY_US42::print_usage()
-{
-	PRINT_MODULE_USAGE_NAME("gy_us42", "driver");
-	PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
-	PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - downward facing by default", true);
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-}
-
-I2CSPIDriverBase *GY_US42::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
-				       int runtime_instance)
-{
-	GY_US42 *instance = new GY_US42(iterator.configuredBusOption(), iterator.bus(), cli.orientation, cli.bus_frequency);
-
-	if (instance == nullptr) {
-		PX4_ERR("alloc failed");
-		return nullptr;
-	}
-
-	if (instance->init() != PX4_OK) {
-		delete instance;
-		return nullptr;
-	}
-
-	return instance;
-}
-
-extern "C" __EXPORT int gy_us42_main(int argc, char *argv[])
-{
-	int ch;
-	using ThisDriver = GY_US42;
-	BusCLIArguments cli{true, false};
-	cli.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
-	cli.default_i2c_frequency = 100000;
-
-	while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
-		switch (ch) {
-		case 'R':
-			cli.orientation = atoi(cli.optarg());
-			break;
-		}
-	}
-
-	const char *verb = cli.optarg();
-
-	if (!verb) {
-		ThisDriver::print_usage();
-		return -1;
-	}
-
-	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIST_DEVTYPE_GY_US42);
-
-	if (!strcmp(verb, "start")) {
-		return ThisDriver::module_start(cli, iterator);
-	}
-
-	if (!strcmp(verb, "stop")) {
-		return ThisDriver::module_stop(iterator);
-	}
-
-	if (!strcmp(verb, "status")) {
-		return ThisDriver::module_status(iterator);
-	}
-
-	ThisDriver::print_usage();
-	return -1;
-}
@@ -317,14 +317,8 @@ __EXPORT extern int	up_pwm_servo_init(uint32_t channel_mask);

 /**
 * De-initialise the PWM servo outputs.
- *
- * @param channel_mask  Bitmask of channels (LSB = channel 0) to enable.
- *      This allows some of the channels to remain configured
- *      as GPIOs or as another function.
- *      A value of 0 is ALL channels
- *
 */
-__EXPORT extern void	up_pwm_servo_deinit(uint32_t channel_mask);
+__EXPORT extern void	up_pwm_servo_deinit(void);

 /**
 * Arm or disarm servo outputs.
@@ -336,14 +330,8 @@ __EXPORT extern void	up_pwm_servo_deinit(uint32_t channel_mask);
 *
 * @param armed		If true, outputs are armed; if false they
 *			are disarmed.
- *
- * @param channel_mask  Bitmask of channels (LSB = channel 0) to enable.
- *      This allows some of the channels to remain configured
- *      as GPIOs or as another function.
- *      A value of 0 is ALL channels
- *
 */
-__EXPORT extern void	up_pwm_servo_arm(bool armed, uint32_t channel_mask);
+__EXPORT extern void	up_pwm_servo_arm(bool armed);

 /**
 * Set the servo update rate for all rate groups.
@@ -176,7 +176,6 @@

 #define DRV_DIST_DEVTYPE_SIM 0x9a
 #define DRV_DIST_DEVTYPE_SRF05 0x9b
-#define DRV_DIST_DEVTYPE_GY_US42 0x9c


 #define DRV_GPS_DEVTYPE_ASHTECH 0xA0
@@ -530,27 +530,11 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],

 	} else {
 		for (int i = 0; i < (int)num_outputs; i++) {
-
-			uint16_t output = outputs[i];
-
-			// DShot 3D splits the throttle ranges in two.
-			// This is in terms of DShot values, code below is in terms of actuator_output
-			// Direction 1) 48 is the slowest, 1047 is the fastest.
-			// Direction 2) 1049 is the slowest, 2047 is the fastest.
-			if (_param_dshot_3d_enable.get()) {
-				if (output >= _param_dshot_3d_dead_l.get() && output <= _param_dshot_3d_dead_h.get()) {
-					output = DSHOT_DISARM_VALUE;
-
-				} else if (output < 1000 && output > 0) { //Todo: allow actuator 0 or dshot 48 to be used
-					output = 999 - output;
-				}
-			}
-
-			if (output == DSHOT_DISARM_VALUE) {
+			if (outputs[i] == DSHOT_DISARM_VALUE) {
 				up_dshot_motor_command(i, DShot_cmd_motor_stop, i == requested_telemetry_index);

 			} else {
-				up_dshot_motor_data_set(i, math::min(output, static_cast<uint16_t>(DSHOT_MAX_THROTTLE)),
+				up_dshot_motor_data_set(i, math::min(outputs[i], static_cast<uint16_t>(DSHOT_MAX_THROTTLE)),
 							i == requested_telemetry_index);
 			}
 		}
@@ -231,9 +231,6 @@ private:
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::DSHOT_CONFIG>)   _param_dshot_config,
 		(ParamFloat<px4::params::DSHOT_MIN>)    _param_dshot_min,
-		(ParamBool<px4::params::DSHOT_3D_ENABLE>) _param_dshot_3d_enable,
-		(ParamInt<px4::params::DSHOT_3D_DEAD_H>) _param_dshot_3d_dead_h,
-		(ParamInt<px4::params::DSHOT_3D_DEAD_L>) _param_dshot_3d_dead_l,
 		(ParamInt<px4::params::MOT_POLE_COUNT>) _param_mot_pole_count
 	)
 };
@@ -14,7 +14,7 @@ parameters:
                long: |
                    This enables/disables DShot. The different modes define different
                    speeds, for example DShot150 = 150kb/s. Not all ESCs support all modes.
-
+                    
                    Note: this enables DShot on the FMU outputs. For boards with an IO it is the
                    AUX outputs.
            type: enum
@@ -40,37 +40,6 @@ parameters:
            decimal: 2
            increment: 0.01
            default: 0.055
-        DSHOT_3D_ENABLE:
-            description:
-                short: Allows for 3d mode when using DShot and suitable mixer
-                long: |
-                    WARNING: ESC must be configured for 3D mode, and DSHOT_MIN set to 0.
-                    This splits the throttle ranges in two.
-                    Direction 1) 48 is the slowest, 1047 is the fastest.
-                    Direction 2) 1049 is the slowest, 2047 is the fastest.
-                    When mixer outputs 1000 or value inside DSHOT 3D deadband, DShot 0 is sent.
-            type: boolean
-            default: 0
-        DSHOT_3D_DEAD_H:
-            description:
-                short: DSHOT 3D deadband high
-                long: |
-                    When the actuator_output is between DSHOT_3D_DEAD_L and DSHOT_3D_DEAD_H, motor will not spin.
-                    This value is with respect to the mixer_module range (0-1999), not the DSHOT values.
-            type: int32
-            min: 1000
-            max: 1999
-            default: 1000
-        DSHOT_3D_DEAD_L:
-            description:
-                short: DSHOT 3D deadband low
-                long: |
-                    When the actuator_output is between DSHOT_3D_DEAD_L and DSHOT_3D_DEAD_H, motor will not spin.
-                    This value is with respect to the mixer_module range (0-1999), not the DSHOT values.
-            type: int32
-            min: 0
-            max: 1000
-            default: 1000
        MOT_POLE_COUNT: # only used by dshot so far, so keep it under the dshot group
            description:
                short: Number of magnetic poles of the motors
@@ -82,3 +51,4 @@ parameters:
                    Typical motors for 5 inch props have 14 poles.
            type: int32
            default: 14
+
@@ -47,7 +47,6 @@ px4_add_module(
 		devices/src/ubx.cpp
 		devices/src/rtcm.cpp
 		devices/src/emlid_reach.cpp
-		devices/src/femtomes.cpp
 	MODULE_CONFIG
 		module.yaml
 	DEPENDS
@@ -67,7 +67,6 @@
 # include "devices/src/ashtech.h"
 # include "devices/src/emlid_reach.h"
 # include "devices/src/mtk.h"
-# include "devices/src/femtomes.h"
 #endif // CONSTRAINED_FLASH
 #include "devices/src/ubx.h"

@@ -83,8 +82,7 @@ typedef enum {
 	GPS_DRIVER_MODE_UBX,
 	GPS_DRIVER_MODE_MTK,
 	GPS_DRIVER_MODE_ASHTECH,
-	GPS_DRIVER_MODE_EMLIDREACH,
-	GPS_DRIVER_MODE_FEMTOMES
+	GPS_DRIVER_MODE_EMLIDREACH
 } gps_driver_mode_t;

 /* struct for dynamic allocation of satellite info data */
@@ -776,11 +774,6 @@ GPS::run()
 			_helper = new GPSDriverEmlidReach(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info);
 			set_device_type(DRV_GPS_DEVTYPE_EMLID_REACH);
 			break;
-
-		case GPS_DRIVER_MODE_FEMTOMES:
-			_helper = new GPSDriverFemto(&GPS::callback, this, &_report_gps_pos/*, _p_report_sat_info*/);
-			set_device_type(DRV_GPS_DEVTYPE_FEMTOMES);
-			break;
 #endif // CONSTRAINED_FLASH

 		default:
@@ -919,10 +912,6 @@ GPS::run()
 				break;

 			case GPS_DRIVER_MODE_EMLIDREACH:
-				_mode = GPS_DRIVER_MODE_FEMTOMES;
-				break;
-
-			case GPS_DRIVER_MODE_FEMTOMES:
 #endif // CONSTRAINED_FLASH
 				_mode = GPS_DRIVER_MODE_UBX;
 				px4_usleep(500000); // tried all possible drivers. Wait a bit before next round
@@ -980,10 +969,6 @@ GPS::print_status()
 	case GPS_DRIVER_MODE_EMLIDREACH:
 		PX4_INFO("protocol: EMLIDREACH");
 		break;
-
-	case GPS_DRIVER_MODE_FEMTOMES:
-		PX4_INFO("protocol: FEMTOMES");
-		break;
 #endif // CONSTRAINED_FLASH

 	default:
@@ -1150,7 +1135,7 @@ $ gps reset warm

 	PRINT_MODULE_USAGE_PARAM_STRING('i', "uart", "spi|uart", "GPS interface", true);
 	PRINT_MODULE_USAGE_PARAM_STRING('j', "uart", "spi|uart", "secondary GPS interface", true);
-	PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, "ubx|mtk|ash|eml|fem", "GPS Protocol (default=auto select)", true);
+	PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, "ubx|mtk|ash|eml", "GPS Protocol (default=auto select)", true);

 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 	PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Reset GPS device");
@@ -1291,9 +1276,6 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)

 			} else if (!strcmp(myoptarg, "eml")) {
 				mode = GPS_DRIVER_MODE_EMLIDREACH;
-
-			} else if (!strcmp(myoptarg, "fem")) {
-				mode = GPS_DRIVER_MODE_FEMTOMES;
 #endif // CONSTRAINED_FLASH
 			} else {
 				PX4_ERR("unknown protocol: %s", myoptarg);
@@ -115,13 +115,12 @@ PARAM_DEFINE_FLOAT(GPS_YAW_OFFSET, 0.f);
 * Auto-detection will probe all protocols, and thus is a bit slower.
 *
 * @min 0
- * @max 5
+ * @max 4
 * @value 0 Auto detect
 * @value 1 u-blox
 * @value 2 MTK
 * @value 3 Ashtech / Trimble
 * @value 4 Emlid Reach
- * @value 5 Femtomes
 *
 * @reboot_required true
 * @group GPS
@@ -136,13 +135,12 @@ PARAM_DEFINE_INT32(GPS_1_PROTOCOL, 1);
 * Auto-detection will probe all protocols, and thus is a bit slower.
 *
 * @min 0
- * @max 5
+ * @max 4
 * @value 0 Auto detect
 * @value 1 u-blox
 * @value 2 MTK
 * @value 3 Ashtech / Trimble
 * @value 4 Emlid Reach
- * @value 5 Femtomes
 *
 * @reboot_required true
 * @group GPS
@@ -84,6 +84,8 @@ int PAW3902::init()
 		return PX4_ERROR;
 	}

+	Reset();
+
 	// force to low light mode (1) initially
 	ChangeMode(Mode::LowLight, true);

@@ -148,6 +150,24 @@ bool PAW3902::DataReadyInterruptDisable()
 	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
 }

+bool PAW3902::Reset()
+{
+	DataReadyInterruptDisable();
+
+	// Power on reset
+	RegisterWrite(Register::Power_Up_Reset, 0x5A);
+	usleep(1000);
+
+	// Read from registers 0x02, 0x03, 0x04, 0x05 and 0x06 one time regardless of the motion state
+	RegisterRead(Register::Motion);
+	RegisterRead(Register::Delta_X_L);
+	RegisterRead(Register::Delta_X_H);
+	RegisterRead(Register::Delta_Y_L);
+	RegisterRead(Register::Delta_Y_H);
+
+	return true;
+}
+
 void PAW3902::exit_and_cleanup()
 {
 	DataReadyInterruptDisable();
@@ -163,7 +183,6 @@ bool PAW3902::ChangeMode(Mode newMode, bool force)

 		// Issue a soft reset
 		RegisterWrite(Register::Power_Up_Reset, 0x5A);
-		px4_usleep(1000);

 		uint32_t interval_us = 0;

@@ -187,10 +206,6 @@ bool PAW3902::ChangeMode(Mode newMode, bool force)
 			break;
 		}

-		EnableLed();
-
-		_discard_reading = 3;
-
 		if (DataReadyInterruptConfigure()) {
 			// backup schedule as a watchdog timeout
 			ScheduleDelayed(500_ms);
@@ -199,6 +214,17 @@ bool PAW3902::ChangeMode(Mode newMode, bool force)
 			ScheduleOnInterval(interval_us);
 		}

+		// Discard the first three motion data.
+		for (int i = 0; i < 3; i++) {
+			RegisterRead(Register::Motion);
+			RegisterRead(Register::Delta_X_L);
+			RegisterRead(Register::Delta_X_H);
+			RegisterRead(Register::Delta_Y_L);
+			RegisterRead(Register::Delta_Y_H);
+		}
+
+		EnableLed();
+
 		_mode = newMode;
 	}

@@ -206,6 +232,11 @@ bool PAW3902::ChangeMode(Mode newMode, bool force)
 	_low_to_superlow_counter = 0;
 	_low_to_bright_counter = 0;
 	_superlow_to_low_counter = 0;
+	// Approximate Resolution = (Register Value + 1) * (50 / 8450) ≈ 0.6% of data point in Figure 19
+	// The maximum register value is 0xA8. The minimum register value is 0.
+	uint8_t resolution = RegisterRead(Register::Resolution);
+	PX4_DEBUG("Resolution: %X", resolution);
+	PX4_DEBUG("Resolution is approx: %.3f", (double)((resolution + 1.0f) * (50.0f / 8450.0f)));

 	return true;
 }
@@ -551,9 +582,9 @@ void PAW3902::ModeSuperLowLight()
 void PAW3902::EnableLed()
 {
 	// Enable LED_N controls
-	RegisterWrite(0x7F, 0x14);
-	RegisterWrite(0x6F, 0x1c);
-	RegisterWrite(0x7F, 0x00);
+	RegisterWriteVerified(0x7F, 0x14);
+	RegisterWriteVerified(0x6F, 0x1c);
+	RegisterWriteVerified(0x7F, 0x00);
 }

 uint8_t PAW3902::RegisterRead(uint8_t reg, int retries)
@@ -632,17 +663,11 @@ void PAW3902::RunImpl()
 	// update for next iteration
 	_previous_collect_timestamp = timestamp_sample;

-	if (_discard_reading > 0) {
-		_discard_reading--;
-		ResetAccumulatedData();
-		return;
-	}
-
 	// check SQUAL & Shutter values
 	// To suppress false motion reports, discard Delta X and Delta Y values if the SQUAL and Shutter values meet the condition
-	// Bright Mode,          SQUAL < 0x19, Shutter ≥ 0x1FF0
-	// Low Light Mode,       SQUAL < 0x46, Shutter ≥ 0x1FF0
-	// Super Low Light Mode, SQUAL < 0x55, Shutter ≥ 0x0BC0
+	// Bright Mode,			SQUAL < 0x19, Shutter ≥ 0x1FF0
+	// Low Light Mode,		SQUAL < 0x46, Shutter ≥ 0x1FF0
+	// Super Low Light Mode,	SQUAL < 0x55, Shutter ≥ 0x0BC0
 	const uint16_t shutter = (buf.data.Shutter_Upper << 8) | buf.data.Shutter_Lower;

 	bool data_valid = true;
@@ -652,6 +677,7 @@ void PAW3902::RunImpl()
 		if ((buf.data.SQUAL < 0x19) && (shutter >= 0x1FF0)) {
 			// false motion report, discarding
 			perf_count(_false_motion_perf);
+			ResetAccumulatedData();
 			data_valid = false;
 		}

@@ -673,6 +699,7 @@ void PAW3902::RunImpl()
 		if ((buf.data.SQUAL < 0x46) && (shutter >= 0x1FF0)) {
 			// false motion report, discarding
 			perf_count(_false_motion_perf);
+			ResetAccumulatedData();
 			data_valid = false;
 		}

@@ -705,14 +732,11 @@ void PAW3902::RunImpl()
 		if ((buf.data.SQUAL < 0x55) && (shutter >= 0x0BC0)) {
 			// false motion report, discarding
 			perf_count(_false_motion_perf);
+			ResetAccumulatedData();
 			data_valid = false;
 		}

-		if (shutter < 0x01F4) {
-			// should not operate with Shutter < 0x01F4 in Mode 2
-			ChangeMode(Mode::LowLight);
-
-		} else if (shutter < 0x03E8) {
+		if (shutter < 0x03E8) {
 			// SuperLowLight -> LowLight
 			_superlow_to_low_counter++;

--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Matthias Grob	fef8c5e42e	StickAccelerationXY: make brake filter configurable	2021-05-04 11:08:48 +02:00
Matthias Grob	b98c7e63c7	StickAccelerationXY: make brake boost factor configurable	2021-05-04 10:53:30 +02:00