navigator: wrote unit tests for Takeoff navigation mode

Signed-off-by: RomanBapst <bapstroman@gmail.com>

.ci/Jenkinsfile-compile

@@ -9,10 +9,10 @@ pipeline {
         script {
           def build_nodes = [:]
           def docker_images = [
-            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",
+            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",
             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 'px4io/px4-dev-base-focal:2021-02-04' }
+    //     docker { image 'px4io/px4-dev-base-focal:2021-04-29' }
     //   }
     //   options {
     //         skipDefaultCheckout()

.ci/Jenkinsfile-hardware

@@ -12,7 +12,7 @@ pipeline {
             stage("build cubepilot_cubeorange_test") {
               agent {
                 docker {
-                  image 'px4io/px4-dev-nuttx-focal:2020-09-14'
+                  image 'px4io/px4-dev-nuttx-focal:2021-04-29'
                   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:2020-09-14'
+                  image 'px4io/px4-dev-nuttx-focal:2021-04-29'
                   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:2020-09-14'
+                  image 'px4io/px4-dev-nuttx-focal:2021-04-29'
                   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:2020-09-14'
+                  image 'px4io/px4-dev-nuttx-focal:2021-04-29'
                   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:2020-09-14'
+                  image 'px4io/px4-dev-nuttx-focal:2021-04-29'
                   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:2020-09-14'
+                  image 'px4io/px4-dev-nuttx-focal:2021-04-29'
                   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:2020-09-14'
+                  image 'px4io/px4-dev-nuttx-focal:2021-04-29'
                   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:2020-09-14'
+                  image 'px4io/px4-dev-nuttx-focal:2021-04-29'
                   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:2020-09-14'
+                  image 'px4io/px4-dev-nuttx-focal:2021-04-29'
                   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:2020-09-14'
+                  image 'px4io/px4-dev-nuttx-focal:2021-04-29'
                   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:2020-09-14'
+                  image 'px4io/px4-dev-nuttx-focal:2021-04-29'
                   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:2020-09-14'
+                  image 'px4io/px4-dev-nuttx-focal:2021-04-29'
                   args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                 }
               }

.devcontainer/devcontainer.json

@@ -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-02-04",
+	"image": "px4io/px4-dev-nuttx-focal:2021-04-29",
 
 	"runArgs": [ "--cap-add=SYS_PTRACE", "--security-opt", "seccomp=unconfined" ],
 

.github/workflows/checks.yml

@@ -28,7 +28,7 @@ jobs:
           "parameters_metadata",
         ]
     container:
-      image: px4io/px4-dev-nuttx-focal:2021-02-04
+      image: px4io/px4-dev-nuttx-focal:2021-04-29
       options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
     steps:
     - uses: actions/checkout@v1

.github/workflows/clang-tidy.yml

@@ -11,7 +11,7 @@ on:
 jobs:
   build:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-clang:2021-02-04
+    container: px4io/px4-dev-clang:2021-04-29
     steps:
     - uses: actions/checkout@v1
       with:

.github/workflows/compile_linux.yml

@@ -11,7 +11,7 @@ on:
 jobs:
   build:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-armhf:2021-02-04
+    container: px4io/px4-dev-armhf:2021-04-29
     strategy:
       matrix:
         config: [

.github/workflows/compile_linux_arm64.yml

@@ -11,7 +11,7 @@ on:
 jobs:
   build:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-aarch64:2021-02-04
+    container: px4io/px4-dev-aarch64:2021-04-29
     strategy:
       matrix:
         config: [

.github/workflows/compile_nuttx.yml

@@ -11,7 +11,7 @@ on:
 jobs:
   build:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-nuttx-focal:2021-02-04
+    container: px4io/px4-dev-nuttx-focal:2021-04-29
     strategy:
       matrix:
         config: [
@@ -123,6 +123,8 @@ 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

.github/workflows/compile_nuttx_cannode.yml

@@ -11,7 +11,7 @@ on:
 jobs:
   build:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-nuttx-focal:2021-02-04
+    container: px4io/px4-dev-nuttx-focal:2021-04-29
     strategy:
       matrix:
         config: [

.github/workflows/deploy_all.yml

@@ -23,7 +23,7 @@ jobs:
     needs: enumerate_targets
     strategy:
       matrix: ${{fromJson(needs.enumerate_targets.outputs.matrix)}}
-    container: px4io/px4-dev-${{ matrix.container }}:2021-02-04
+    container: px4io/px4-dev-${{ matrix.container }}:2021-04-29
     steps:
     - uses: actions/checkout@v1
       with:

.github/workflows/mavros_avoidance_tests.yml

@@ -1,131 +0,0 @@
-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

.github/workflows/mavros_mission_tests.yml

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

.github/workflows/mavros_offboard_tests.yml

@@ -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-02-04
+      image: px4io/px4-dev-ros-melodic:2021-04-29
       options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
     steps:
     - uses: actions/checkout@v1

.github/workflows/metadata.yml

@@ -11,7 +11,7 @@ jobs:
 
   airframe:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-02-04
+    container: px4io/px4-dev-base-focal:2021-04-29
     steps:
     - uses: actions/checkout@v1
       with:
@@ -26,7 +26,7 @@ jobs:
 
   module:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-02-04
+    container: px4io/px4-dev-base-focal:2021-04-29
     steps:
     - uses: actions/checkout@v1
       with:
@@ -41,7 +41,7 @@ jobs:
 
   parameter:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-02-04
+    container: px4io/px4-dev-base-focal:2021-04-29
     steps:
     - uses: actions/checkout@v1
       with:
@@ -65,7 +65,7 @@ jobs:
 
   uorb_graph:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-nuttx-focal:2021-02-04
+    container: px4io/px4-dev-nuttx-focal:2021-04-29
     steps:
     - uses: actions/checkout@v1
       with:
@@ -80,7 +80,7 @@ jobs:
 
   micrortps_agent:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-02-04
+    container: px4io/px4-dev-base-focal:2021-04-29
     steps:
     - uses: actions/checkout@v1
       with:
@@ -94,7 +94,7 @@ jobs:
 
   ROS_msgs:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-02-04
+    container: px4io/px4-dev-base-focal:2021-04-29
     steps:
     - uses: actions/checkout@v1
       with:
@@ -107,7 +107,7 @@ jobs:
 
   ROS2_bridge:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-02-04
+    container: px4io/px4-dev-base-focal:2021-04-29
     steps:
     - uses: actions/checkout@v1
       with:

.github/workflows/sitl_tests.yml

@@ -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-02-04
+      image: px4io/px4-dev-simulation-focal:2021-04-29
       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/v0.38.0/mavsdk_0.38.0_ubuntu20.04_amd64.deb
+      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"
     - name: Install MAVSDK
-      run: dpkg -i mavsdk_0.38.0_ubuntu20.04_amd64.deb
+      run: dpkg -i "mavsdk_$(cat test/mavsdk_tests/MAVSDK_VERSION)_ubuntu20.04_amd64.deb"
 
     - name: Prepare ccache timestamp
       id: ccache_cache_timestamp

.gitmodules

@@ -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 = px4
+	branch = master
 [submodule "platforms/nuttx/NuttX/nuttx"]
 	path = platforms/nuttx/NuttX/nuttx
 	url = https://github.com/PX4/NuttX.git
@@ -66,3 +66,4 @@
 [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

CMakeLists.txt

@@ -185,6 +185,8 @@ 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)

Jenkinsfile

@@ -15,7 +15,7 @@ pipeline {
         // stage('Catkin build on ROS workspace') {
         //   agent {
         //     docker {
-        //       image 'px4io/px4-dev-ros-melodic:2021-02-04'
+        //       image 'px4io/px4-dev-ros-melodic:2021-04-29'
         //       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-02-04'
+              image 'px4io/px4-dev-ros2-foxy:2021-04-29'
               args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE'
             }
           }
@@ -85,7 +85,7 @@ pipeline {
 
         stage('Airframe') {
           agent {
-            docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
+            docker { image 'px4io/px4-dev-base-focal:2021-04-29' }
           }
           steps {
             sh 'make distclean'
@@ -105,7 +105,7 @@ pipeline {
 
         stage('Parameter') {
           agent {
-            docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
+            docker { image 'px4io/px4-dev-base-focal:2021-04-29' }
           }
           steps {
             sh 'make distclean'
@@ -125,7 +125,7 @@ pipeline {
 
         stage('Module') {
           agent {
-            docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
+            docker { image 'px4io/px4-dev-base-focal:2021-04-29' }
           }
           steps {
             sh 'make distclean'
@@ -146,7 +146,7 @@ pipeline {
         stage('uORB graphs') {
           agent {
             docker {
-              image 'px4io/px4-dev-nuttx-focal:2021-02-04'
+              image 'px4io/px4-dev-nuttx-focal:2021-04-29'
               args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
             }
           }
@@ -176,7 +176,7 @@ pipeline {
 
         stage('Userguide') {
           agent {
-            docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
+            docker { image 'px4io/px4-dev-base-focal:2021-04-29' }
           }
           steps {
             sh('export')
@@ -206,7 +206,7 @@ pipeline {
 
         stage('QGroundControl') {
           agent {
-            docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
+            docker { image 'px4io/px4-dev-base-focal:2021-04-29' }
           }
           steps {
             sh('export')
@@ -234,7 +234,7 @@ pipeline {
 
         stage('microRTPS agent') {
           agent {
-            docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
+            docker { image 'px4io/px4-dev-base-focal:2021-04-29' }
           }
           steps {
             sh('export')
@@ -264,7 +264,7 @@ pipeline {
 
         stage('PX4 ROS msgs') {
           agent {
-            docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
+            docker { image 'px4io/px4-dev-base-focal:2021-04-29' }
           }
           steps {
             sh('export')
@@ -293,7 +293,7 @@ pipeline {
 
         stage('PX4 ROS2 bridge') {
           agent {
-            docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
+            docker { image 'px4io/px4-dev-base-focal:2021-04-29' }
           }
           steps {
             sh('export')
@@ -336,7 +336,7 @@ pipeline {
 
         stage('S3') {
           agent {
-            docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
+            docker { image 'px4io/px4-dev-base-focal:2021-04-29' }
           }
           steps {
             sh('export')

Makefile

@@ -468,25 +468,27 @@ validate_module_configs:
 .PHONY: clean submodulesclean submodulesupdate gazeboclean distclean
 
 clean:
-	@rm -rf "$(SRC_DIR)"/build
+	@find "$(SRC_DIR)/build" -mindepth 1 -maxdepth 1 -type d -exec sh -c "echo {}; cmake --build {} -- clean || rm -rf {}" \; # use generated build system to clean, wipe build directory if it fails
+	@git submodule foreach git clean -dX --force # some submodules generate build artifacts in source
 
 submodulesclean:
 	@git submodule foreach --quiet --recursive git clean -ff -x -d
 	@git submodule update --quiet --init --recursive --force || true
 	@git submodule sync --recursive
-	@git submodule update --init --recursive --force
+	@git submodule update --init --recursive --force --jobs 4
 
 submodulesupdate:
-	@git submodule update --quiet --init --recursive || true
+	@git submodule update --quiet --init --recursive --jobs 4 || true
 	@git submodule sync --recursive
-	@git submodule update --init --recursive
+	@git submodule update --init --recursive --jobs 4
 
 gazeboclean:
 	@rm -rf ~/.gazebo/*
 
 distclean: gazeboclean
-	@git submodule deinit -f .
-	@git clean -ff -x -d -e ".project" -e ".cproject" -e ".idea" -e ".settings" -e ".vscode"
+	@git submodule deinit --force $(SRC_DIR)
+	@rm -rf "$(SRC_DIR)/build"
+	@git clean --force -X "$(SRC_DIR)/msg/" "$(SRC_DIR)/platforms/" "$(SRC_DIR)/posix-configs/" "$(SRC_DIR)/ROMFS/" "$(SRC_DIR)/src/" "$(SRC_DIR)/test/" "$(SRC_DIR)/Tools/"
 
 # Help / Error
 # --------------------------------------------------------------------

ROMFS/CMakeLists.txt

@@ -115,6 +115,7 @@ add_custom_command(
 set(romfs_extract_stamp ${CMAKE_CURRENT_BINARY_DIR}/romfs_extract.stamp)
 add_custom_command(
 	OUTPUT ${romfs_extract_stamp}
+	COMMAND ${CMAKE_COMMAND} -E remove_directory ${romfs_gen_root_dir}/*
 	COMMAND ${CMAKE_COMMAND} -E tar xf ${romfs_tar_file}
 	COMMAND ${CMAKE_COMMAND} -E touch ${romfs_extract_stamp}
 	WORKING_DIRECTORY ${romfs_gen_root_dir}

ROMFS/px4fmu_common/init.d-posix/airframes/17001_tf-g1

@@ -14,7 +14,8 @@
 
 param set-default EKF2_ARSP_THR 8
 param set-default EKF2_FUSE_BETA 1
-param set-default ASPD_STALL 10.0
+
+param set-default FW_AIRSPD_STALL 8
 
 param set-default FW_P_RMAX_NEG 20.0
 param set-default FW_P_RMAX_POS 60.0

ROMFS/px4fmu_common/init.d/rc.vtol_defaults

@@ -23,6 +23,7 @@ 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
 

ROMFS/px4fmu_common/init.d/rcS

@@ -30,7 +30,7 @@ set FRC /fs/microsd/etc/rc.txt
 set IOFW "/etc/extras/px4_io-v2_default.bin"
 set IO_PRESENT no
 set LOGGER_ARGS ""
-set LOGGER_BUF  14
+set LOGGER_BUF 8
 set MAV_TYPE none
 set MIXER none
 set MIXER_AUX none

Tools/bloaty_static_ram.bloaty

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

Tools/docker_run.sh

@@ -4,7 +4,7 @@ if [ -z ${PX4_DOCKER_REPO+x} ]; then
 	echo "guessing PX4_DOCKER_REPO based on input";
 	if [[ $@ =~ .*px4_fmu.* ]]; then
 		# nuttx-px4fmu-v{1,2,3,4,5}
-		PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2021-02-04"
+		PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2021-04-29"
 	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"
@@ -30,7 +30,7 @@ fi
 
 # otherwise default to nuttx
 if [ -z ${PX4_DOCKER_REPO+x} ]; then
-	PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2020-09-14"
+	PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2021-04-29"
 fi
 
 # docker hygiene

Tools/jMAVSim

@@ -1 +1 @@
-Subproject commit 358b6cca4093646eb96e0cb075e45efa8f4a0c48
+Subproject commit 2b610caab81726ab79019de0f2fa8cff5e341bd5

Tools/process_sensor_caldata.py

@@ -1,4 +1,4 @@
-#! /usr/bin/env python
+#! /usr/bin/env python3
 
 from __future__ import print_function
 
@@ -7,6 +7,7 @@ import os
 import math
 import matplotlib.pyplot as plt
 import numpy as np
+import scipy as sp
 
 from pyulog import *
 
@@ -63,6 +64,9 @@ 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',
@@ -184,12 +188,16 @@ 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]
@@ -200,10 +208,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]
@@ -214,9 +222,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]
@@ -292,6 +300,10 @@ 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)
@@ -400,6 +412,10 @@ 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)
@@ -416,10 +432,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]
@@ -430,10 +446,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]
@@ -508,8 +524,12 @@ 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]
@@ -518,7 +538,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]
@@ -527,7 +547,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]
@@ -540,8 +560,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)')
@@ -601,13 +621,17 @@ 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]
@@ -617,12 +641,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]
@@ -632,12 +656,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]
@@ -712,13 +736,17 @@ 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]
@@ -728,7 +756,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:
@@ -743,12 +771,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]
@@ -824,13 +852,17 @@ 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]
@@ -840,7 +872,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:
@@ -855,7 +887,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:
@@ -935,9 +967,13 @@ 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]
@@ -946,8 +982,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]
@@ -956,8 +992,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]
@@ -1024,8 +1060,10 @@ 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:
@@ -1081,8 +1119,10 @@ 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:
@@ -1139,8 +1179,10 @@ 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:
@@ -1197,6 +1239,8 @@ 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

Tools/setup/arch.sh

@@ -155,8 +155,7 @@ if [[ $INSTALL_SIM == "true" ]]; then
 			# fix VMWare 3D graphics acceleration for gazebo
 			exportline="export SVGA_VGPU10=0"
 
-			if grep -Fxq "$exportline" $HOME/.profile; then
-			else
+			if !grep -Fxq "$exportline" $HOME/.profile; then
 				echo $exportline >> $HOME/.profile;
 			fi
 		fi

Tools/setup/ubuntu.sh

@@ -115,18 +115,32 @@ if [[ $INSTALL_NUTTX == "true" ]]; then
 	echo "Installing NuttX dependencies"
 
 	sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
-		autoconf \
 		automake \
+		binutils-dev \
 		bison \
-		bzip2 \
-		file \
+		build-essential \
 		flex \
+		g++-multilib \
+		gcc-multilib \
 		gdb-multiarch \
+		genromfs \
+		gettext \
 		gperf \
-		libncurses-dev \
+		kconfig-frontends \
+		libelf-dev \
+		libexpat-dev \
+		libgmp-dev \
+		libisl-dev \
+		libmpc-dev \
+		libmpfr-dev \
+		libncurses5-dev \
+		libncursesw5-dev \
 		libtool \
 		pkg-config \
 		screen \
+		texinfo \
+		u-boot-tools \
+		util-linux \
 		vim-common \
 		;
 

Tools/sitl_gazebo

@@ -1 +1 @@
-Subproject commit 05e2cd9c03ffa77f0c0d5bd3e788d33e06480b25
+Subproject commit 2496b85dd22d2f9c1c1281d5ef77dfb86ef604a3

boards/av/x-v1/src/init.c

@@ -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(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
 	}
 
 	if (status >= 0) {

boards/cuav/nora/nuttx-config/nsh/defconfig

@@ -116,6 +116,7 @@ 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

boards/cuav/nora/src/init.c

@@ -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(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
 	}
 
 	if (status >= 0) {

boards/cuav/x7pro/nuttx-config/nsh/defconfig

@@ -116,6 +116,7 @@ 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

boards/cuav/x7pro/src/init.c

@@ -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(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
 	}
 
 	if (status >= 0) {

boards/cubepilot/cubeorange/src/init.c

@@ -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(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
 	}
 
 	if (status >= 0) {

boards/cubepilot/cubeyellow/src/init.c

@@ -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(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
 	}
 
 	if (status >= 0) {

boards/holybro/durandal-v1/nuttx-config/nsh/defconfig

@@ -117,6 +117,7 @@ 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

boards/holybro/durandal-v1/src/init.c

@@ -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(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
 	}
 
 	if (status >= 0) {

boards/holybro/kakutef7/src/init.c

@@ -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(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
 	}
 
 	if (status >= 0) {

boards/holybro/pix32v5/nuttx-config/nsh/defconfig

@@ -116,6 +116,7 @@ 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

boards/holybro/pix32v5/src/init.c

@@ -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(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
 	}
 
 	if (status >= 0) {

boards/modalai/fc-v1/nuttx-config/nsh/defconfig

@@ -115,6 +115,7 @@ 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

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

@@ -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(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
 	}
 
 	if (status >= 0) {

boards/mro/ctrl-zero-f7-oem/src/init.c

@@ -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(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
 	}
 
 	if (status >= 0) {

boards/mro/ctrl-zero-f7/src/init.c

@@ -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(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
 	}
 
 	if (status >= 0) {

boards/mro/ctrl-zero-h7-oem/src/init.c

@@ -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(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
 	}
 
 	if (status >= 0) {

boards/mro/ctrl-zero-h7/src/init.c

@@ -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(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
 	}
 
 	if (status >= 0) {

boards/mro/pixracerpro/src/board_config.h

@@ -119,7 +119,7 @@
 
 /* RC Serial port */
 #define RC_SERIAL_PORT          "/dev/ttyS4"
-#define RC_SERIAL_SINGLEWIRE
+#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT
 
 #define GPIO_RSSI_IN            /* PC1  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)
 

boards/mro/pixracerpro/src/init.c

@@ -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(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
 	}
 
 	if (status >= 0) {

boards/nxp/fmuk66-e/nuttx-config/socketcan/defconfig

@@ -26,6 +26,7 @@ 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,6 +124,7 @@ 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

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

@@ -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 5120
+#define BOARD_DMA_ALLOC_POOL_SIZE 2048
 
 /* This board provides the board_on_reset interface */
 

boards/nxp/fmuk66-e/src/init.c

@@ -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(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
 	}
 
 	if (status >= 0) {

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

@@ -25,6 +25,7 @@ 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
@@ -62,7 +63,7 @@ CONFIG_HAVE_CXXINITIALIZE=y
 CONFIG_I2C_RESET=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_IOB_NBUFFERS=48
-CONFIG_IOB_NCHAINS=16
+CONFIG_IOB_NCHAINS=18
 CONFIG_KINETIS_ADC0=y
 CONFIG_KINETIS_ADC1=y
 CONFIG_KINETIS_CRC=y
@@ -122,6 +123,7 @@ 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

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

@@ -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 5120
+#define BOARD_DMA_ALLOC_POOL_SIZE 2048
 
 /* This board provides the board_on_reset interface */
 

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

@@ -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(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
 	}
 
 	if (status >= 0) {

boards/nxp/ucans32k146/nuttx-config/include/board.h

@@ -90,6 +90,12 @@
 #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:
@@ -152,11 +158,9 @@
 /* CAN selections ***********************************************************/
 #define PIN_CAN0_TX      PIN_CAN0_TX_4      /* PTE5 */
 #define PIN_CAN0_RX      PIN_CAN0_RX_4      /* PTE4 */
-#define PIN_CAN0_ENABLE  (GPIO_OUTPUT | PIN_PORTE | PIN11 )
-#define CAN0_ENABLE_OUT  0
+#define PIN_CAN0_STB     (GPIO_OUTPUT | PIN_PORTE | PIN11 )
 #define PIN_CAN1_TX      PIN_CAN1_TX_1      /* PTA13 */
 #define PIN_CAN1_RX      PIN_CAN1_RX_1      /* PTA12 */
-#define PIN_CAN1_ENABLE  (GPIO_OUTPUT | PIN_PORTE | PIN10 )
-#define CAN1_ENABLE_OUT  0
+#define PIN_CAN1_STB     (GPIO_OUTPUT | PIN_PORTE | PIN10 )
 
 #endif  /* __BOARDS_ARM_RDDRONE_UAVCAN146_INCLUDE_BOARD_H */

boards/nxp/ucans32k146/src/bringup.c

@@ -151,5 +151,20 @@ 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;
 }

boards/nxp/ucans32k146/src/can_boot.c

@@ -71,7 +71,18 @@ __EXPORT void s32k1xx_board_initialize(void)
 	// Can GPIO
 	s32k1xx_pinconfig(PIN_CAN0_TX);
 	s32k1xx_pinconfig(PIN_CAN0_RX);
-	s32k1xx_pinconfig(PIN_CAN0_ENABLE  | GPIO_OUTPUT_ZERO);
+
+	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_gpiowrite
 #if defined(OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO)
 	s32k1xx_pinconfig(GPIO_GETNODEINFO_JUMPER);
@@ -101,7 +112,14 @@ void board_deinitialize(void)
 	} while ((regval & CAN_MCR_LPMACK) == 0);
 
 
-	s32k1xx_pinconfig(PIN_CAN0_ENABLE  | GPIO_OUTPUT_ONE);
+	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);
+	}
 }
 
 /****************************************************************************

boards/px4/fmu-v2/nuttx-config/nsh/defconfig

@@ -82,7 +82,7 @@ CONFIG_I2C_RESET=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
-CONFIG_LIBC_STRERROR=y
+CONFIG_LIBC_STRERROR=n
 CONFIG_LIBC_STRERROR_SHORT=y
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y

boards/px4/fmu-v5/nuttx-config/nsh/defconfig

@@ -116,6 +116,7 @@ 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

boards/px4/fmu-v5/optimized.cmake

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

boards/px4/fmu-v5/src/init.c

@@ -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(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
 	}
 
 	if (status >= 0) {

boards/px4/fmu-v5x/default.cmake

@@ -33,6 +33,7 @@ px4_add_board(
 		imu/bosch/bmi088
 		imu/invensense/icm20602
 		imu/invensense/icm20948 # required for ak09916 mag
+		imu/invensense/icm20649
 		imu/invensense/icm42688p
 		irlock
 		lights # all available light drivers

boards/px4/fmu-v5x/init/rc.board_sensors

@@ -8,13 +8,19 @@ board_adc start
 ina226 -X -b 1 -t 1 -k start
 ina226 -X -b 2 -t 2 -k start
 
-if ver hwtypecmp V5X90 V5X91 V5Xa0 V5Xa1
+if ver hwtypecmp V5X90 V5X91 V5X92 V5Xa0 V5Xa1 V5Xa2
 then
 	#SKYNODE base fmu board orientation
 
-	# Internal SPI BMI088
-	bmi088 -A -R 2 -s start
-	bmi088 -G -R 2 -s start
+	if ver hwtypecmp V5X90 V5X91 V5Xa0 V5Xa1
+	then
+		# Internal SPI BMI088
+		bmi088 -A -R 2 -s start
+		bmi088 -G -R 2 -s start
+	else
+		# Internal SPI bus ICM20649
+		icm20649 -s -R 4 start
+	fi
 
 	# Internal SPI bus ICM42688p
 	icm42688p -R 4 -s start
@@ -28,9 +34,15 @@ then
 else
 	#FMUv5Xbase board orientation
 
-	# Internal SPI BMI088
-	bmi088 -A -R 4 -s start
-	bmi088 -G -R 4 -s start
+	if ver hwtypecmp V5X00 V5X01
+	then
+		# Internal SPI BMI088
+		bmi088 -A -R 4 -s start
+		bmi088 -G -R 4 -s start
+	else
+		# Internal SPI bus ICM20649
+		icm20649 -s -R 6 start
+	fi
 
 	# Internal SPI bus ICM42688p
 	icm42688p -R 6 -s start

boards/px4/fmu-v5x/nuttx-config/nsh/defconfig

@@ -146,6 +146,7 @@ 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

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

@@ -185,6 +185,18 @@
 #define HW_INFO_INIT           {'V','5','X','x', 'x',0}
 #define HW_INFO_INIT_VER       3 /* Offset in above string of the VER */
 #define HW_INFO_INIT_REV       4 /* Offset in above string of the REV */
+#define BOARD_NUM_SPI_CFG_HW_VERSIONS 3
+// Base                   FMUM
+#define V5X00   HW_VER_REV(0x0,0x0) // FMUV5X,                 Rev 0
+#define V5X10   HW_VER_REV(0x1,0x0) // NO PX4IO,               Rev 0
+#define V5X01   HW_VER_REV(0x0,0x1) // FMUV5X I2C2 BMP388,     Rev 1
+#define V5X02   HW_VER_REV(0x0,0x2) // FMUV5X,                 Rev 2
+#define V5X90   HW_VER_REV(0x9,0x0) // NO USB,                 Rev 0
+#define V5X91   HW_VER_REV(0x9,0x1) // NO USB I2C2 BMP388,     Rev 1
+#define V5X92   HW_VER_REV(0x9,0x2) // NO USB I2C2 BMP388,     Rev 2
+#define V5Xa0   HW_VER_REV(0xa,0x0) // NO USB (Q),             Rev 0
+#define V5Xa1   HW_VER_REV(0xa,0x1) // NO USB (Q) I2C2 BMP388, Rev 1
+#define V5Xa2   HW_VER_REV(0xa,0x2) // NO USB (Q) I2C2 BMP388, Rev 2
 
 /* HEATER
  * PWM in future

boards/px4/fmu-v5x/src/init.cpp

@@ -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(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
 	}
 
 	if (status >= 0) {
@@ -175,12 +175,6 @@ stm32_boardinitialize(void)
 	const uint32_t gpio[] = PX4_GPIO_INIT_LIST;
 	px4_gpio_init(gpio, arraySize(gpio));
 
-	/* configure SPI interfaces (we can do this here as long as we only have a single SPI hw config version -
-	 * otherwise we need to move this after board_determine_hw_info()) */
-	static_assert(BOARD_NUM_SPI_CFG_HW_VERSIONS == 1, "Need to move the SPI initialization for multi-version support");
-
-	stm32_spiinitialize();
-
 	/* configure USB interfaces */
 
 	stm32_usbinitialize();
@@ -220,7 +214,6 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 	VDD_3V3_SD_CARD_EN(true);
 	VDD_5V_PERIPH_EN(true);
 	VDD_5V_HIPOWER_EN(true);
-	board_spi_reset(10, 0xffff);
 	VDD_3V3_SENSORS4_EN(true);
 	VDD_3V3_SPEKTRUM_POWER_EN(true);
 
@@ -237,6 +230,9 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 		syslog(LOG_ERR, "[boot] Failed to read HW revision and version\n");
 	}
 
+	stm32_spiinitialize();
+
+	board_spi_reset(10, 0xffff);
 
 	/* configure the DMA allocator */
 

boards/px4/fmu-v5x/src/manifest.c

@@ -111,16 +111,19 @@ static const px4_hw_mft_item_t hw_mft_list_v0509[] = {
 };
 
 static px4_hw_mft_list_entry_t mft_lists[] = {
-	{0x0000, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)},
-	{0x0001, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)},
-	{0x0100, hw_mft_list_v0510, arraySize(hw_mft_list_v0510)},
-	{0x0900, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)},
-	{0x0901, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)},
-	{0x0a00, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)},
-	{0x0a01, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)},
+//  ver_rev
+	{V5X00, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X,                 Rev 0
+	{V5X01, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X,                 Rev 1
+	{V5X02, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X,                 Rev 2
+	{V5X10, hw_mft_list_v0510, arraySize(hw_mft_list_v0510)}, // NO PX4IO,               Rev 0
+	{V5X90, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB,                 Rev 0
+	{V5X91, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB I2C2 BMP388,     Rev 1
+	{V5X92, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB I2C2 BMP388,     Rev 2
+	{V5Xa0, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB (Q),             Rev 0
+	{V5Xa1, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB (Q) I2C2 BMP388, Rev 1
+	{V5Xa2, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB (Q) I2C2 BMP388, Rev 2
 };
 
-
 /************************************************************************************
  * Name: board_query_manifest
  *

boards/px4/fmu-v5x/src/spi.cpp

@@ -35,28 +35,77 @@
 #include <drivers/drv_sensor.h>
 #include <nuttx/spi/spi.h>
 
-constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
-	initSPIBus(SPI::Bus::SPI1, {
-		initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
-	}, {GPIO::PortI, GPIO::Pin11}),
-	initSPIBus(SPI::Bus::SPI2, {
-		initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortH, GPIO::Pin12}),
-	}, {GPIO::PortD, GPIO::Pin15}),
-	initSPIBus(SPI::Bus::SPI3, {
-		initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}),
-		initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}),
-	}, {GPIO::PortE, GPIO::Pin7}),
-//	initSPIBus(SPI::Bus::SPI4, {
-//		// no devices
-// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
-//	}, {GPIO::PortG, GPIO::Pin8}),
-	initSPIBus(SPI::Bus::SPI5, {
-		initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
+constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = {
+	initSPIHWVersion(HW_VER_REV(0, 0), {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
+		}, {GPIO::PortI, GPIO::Pin11}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortH, GPIO::Pin12}),
+		}, {GPIO::PortD, GPIO::Pin15}),
+		initSPIBus(SPI::Bus::SPI3, {
+			initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}),
+			initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}),
+		}, {GPIO::PortE, GPIO::Pin7}),
+		//	initSPIBus(SPI::Bus::SPI4, {
+		//		// no devices
+		// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
+		//	}, {GPIO::PortG, GPIO::Pin8}),
+		initSPIBus(SPI::Bus::SPI5, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
+		}),
+		initSPIBusExternal(SPI::Bus::SPI6, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
+		}),
 	}),
-	initSPIBusExternal(SPI::Bus::SPI6, {
-		initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
-		initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
+
+	initSPIHWVersion(HW_VER_REV(0, 1), {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
+		}, {GPIO::PortI, GPIO::Pin11}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortH, GPIO::Pin12}),
+		}, {GPIO::PortD, GPIO::Pin15}),
+		initSPIBus(SPI::Bus::SPI3, {
+			initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}),
+			initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}),
+		}, {GPIO::PortE, GPIO::Pin7}),
+		//  initSPIBus(SPI::Bus::SPI4, {
+		//    // no devices
+		// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
+		//  }, {GPIO::PortG, GPIO::Pin8}),
+		initSPIBus(SPI::Bus::SPI5, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
+		}),
+		initSPIBusExternal(SPI::Bus::SPI6, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
+		}),
+	}),
+
+	initSPIHWVersion(HW_VER_REV(0, 2), {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
+		}, {GPIO::PortI, GPIO::Pin11}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortH, GPIO::Pin12}),
+		}, {GPIO::PortD, GPIO::Pin15}),
+		initSPIBus(SPI::Bus::SPI3, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}),
+		}, {GPIO::PortE, GPIO::Pin7}),
+		//  initSPIBus(SPI::Bus::SPI4, {
+		//    // no devices
+		// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
+		//  }, {GPIO::PortG, GPIO::Pin8}),
+		initSPIBus(SPI::Bus::SPI5, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
+		}),
+		initSPIBusExternal(SPI::Bus::SPI6, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
+		}),
 	}),
 };
 
-static constexpr bool unused = validateSPIConfig(px4_spi_buses);
+static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw);

boards/px4/fmu-v6u/src/init.c

@@ -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(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
 	}
 
 	if (status >= 0) {

boards/px4/fmu-v6x/nuttx-config/nsh/defconfig

@@ -128,7 +128,6 @@ 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

boards/px4/fmu-v6x/src/init.c

@@ -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(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
 	}
 
 	if (status >= 0) {

boards/spracing/h7extreme/src/init.c

@@ -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(io_timer_channel_get_as_pwm_input(i)));
+		px4_arch_configgpio(PX4_MAKE_GPIO_INPUT_PULL_DOWN(io_timer_channel_get_as_pwm_input(i)));
 	}
 
 	if (status >= 0) {

cmake/bloaty.cmake

@@ -71,6 +71,13 @@ 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

mavlink/include/mavlink/v2.0

@@ -1 +1 @@
-Subproject commit a1c60151a4bda7626b14d51e5af30ee4d974e066
+Subproject commit 3a3aea8fe57c9f14d215b8769f9b77bca8b7c750

msg/telemetry_status.msg

@@ -28,7 +28,6 @@ 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

msg/vehicle_land_detected.msg

@@ -5,3 +5,8 @@ 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

msg/vehicle_status.msg

@@ -89,6 +89,7 @@ bool high_latency_data_link_lost 			# Set to true if the high latency data link
 
 bool engine_failure				# Set to true if an engine failure is detected
 bool mission_failure				# Set to true if mission could not continue/finish
+bool geofence_violated
 
 uint8 failure_detector_status			# Bitmask containing FailureDetector status [0, 0, 0, 0, 0, FAILURE_ALT, FAILURE_PITCH, FAILURE_ROLL]
 

platforms/common/include/px4_platform_common/board_common.h

@@ -262,6 +262,7 @@
 
 #if defined(BOARD_HAS_HW_VERSIONING)
 #  define BOARD_HAS_VERSIONING 1
+#  define HW_VER_REV(v,r)       ((uint32_t)((v) & 0xff) << 8) | ((uint32_t)(r) & 0xff)
 #endif
 
 /* Default LED logical to color mapping */

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

@@ -48,7 +48,7 @@ struct wq_config_t {
 
 namespace wq_configurations
 {
-static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1920, 0}; // PX4 inner loop highest priority
+static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1952, 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};

platforms/common/include/px4_platform_common/spi.h

@@ -71,7 +71,7 @@ struct px4_spi_bus_t {
 
 struct px4_spi_bus_all_hw_t {
 	px4_spi_bus_t buses[SPI_BUS_MAX_BUS_ITEMS];
-	int board_hw_version{-1}; ///< 0=default, >0 for a specific revision (see board_get_hw_version), -1=unused
+	int board_hw_version_revision{-1}; ///< 0=default, >0 for a specific revision (see board_get_hw_version & board_get_hw_revision), -1=unused
 };
 
 #if BOARD_NUM_SPI_CFG_HW_VERSIONS > 1

platforms/common/spi.cpp

@@ -39,14 +39,19 @@
 #if BOARD_NUM_SPI_CFG_HW_VERSIONS > 1
 void px4_set_spi_buses_from_hw_version()
 {
-	int hw_version = board_get_hw_version();
+#if defined(BOARD_HAS_SIMPLE_HW_VERSIONING)
+	int hw_version_revision = board_get_hw_version();
+#else
+	int hw_version_revision = board_get_hw_revision();
+#endif
+
 
 	for (int i = 0; i < BOARD_NUM_SPI_CFG_HW_VERSIONS; ++i) {
-		if (!px4_spi_buses && px4_spi_buses_all_hw[i].board_hw_version == 0) {
+		if (!px4_spi_buses && px4_spi_buses_all_hw[i].board_hw_version_revision == 0) {
 			px4_spi_buses = px4_spi_buses_all_hw[i].buses;
 		}
 
-		if (px4_spi_buses_all_hw[i].board_hw_version == hw_version) {
+		if (px4_spi_buses_all_hw[i].board_hw_version_revision == hw_version_revision) {
 			px4_spi_buses = px4_spi_buses_all_hw[i].buses;
 		}
 	}

platforms/nuttx/NuttX/nuttx

@@ -1 +1 @@
-Subproject commit 405cfa2571ab9eee2fd8cdf28726f8ccfeb3e5e6
+Subproject commit 20376b65d1a8cb2c02bbfe6a494add6238b29051

platforms/nuttx/init/kinetis/rc.board_arch_defaults

@@ -9,7 +9,7 @@ param set-default SENS_IMU_MODE 1
 param set-default EKF2_MULTI_MAG 0
 param set-default SENS_MAG_MODE 1
 
-set LOGGER_BUF 12
+set LOGGER_BUF 8
 
 if param greater -s UAVCAN_ENABLE 1
 then

platforms/nuttx/init/s32k1xx/rc.board_arch_defaults

@@ -3,7 +3,7 @@
 # S32K1XX specific defaults
 #------------------------------------------------------------------------------
 
-set LOGGER_BUF 12
+set LOGGER_BUF 8
 
 if param greater -s UAVCAN_ENABLE 1
 then

platforms/nuttx/init/stm32/rc.board_arch_defaults

@@ -9,7 +9,7 @@ param set-default SENS_IMU_MODE 1
 param set-default EKF2_MULTI_MAG 0
 param set-default SENS_MAG_MODE 1
 
-set LOGGER_BUF 12
+set LOGGER_BUF 8
 
 if param greater -s UAVCAN_ENABLE 1
 then

platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_servo.c

@@ -104,10 +104,10 @@ int up_pwm_servo_init(uint32_t channel_mask)
 	return OK;
 }
 
-void up_pwm_servo_deinit(void)
+void up_pwm_servo_deinit(uint32_t channel_mask)
 {
 	/* disable the timers */
-	up_pwm_servo_arm(false);
+	up_pwm_servo_arm(false, channel_mask);
 }
 
 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)
+up_pwm_servo_arm(bool armed, uint32_t 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);
+	io_timer_set_enable(armed, IOTimerChanMode_OneShot, channel_mask);
+	io_timer_set_enable(armed, IOTimerChanMode_PWMOut, channel_mask);
 }

platforms/nuttx/src/px4/nxp/k66/include/px4_arch/micro_hal.h

@@ -112,7 +112,8 @@ 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_OUTPUT(gpio) _PX4_MAKE_GPIO(gpio, GPIO_HIGHDRIVE)
+#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)
 
 __END_DECLS

platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_servo.c

@@ -105,10 +105,10 @@ int up_pwm_servo_init(uint32_t channel_mask)
 	return OK;
 }
 
-void up_pwm_servo_deinit(void)
+void up_pwm_servo_deinit(uint32_t channel_mask)
 {
 	/* disable the timers */
-	up_pwm_servo_arm(false);
+	up_pwm_servo_arm(false, channel_mask);
 }
 
 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)
+up_pwm_servo_arm(bool armed, uint32_t 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);
+	io_timer_set_enable(armed, IOTimerChanMode_OneShot, channel_mask);
+	io_timer_set_enable(armed, IOTimerChanMode_PWMOut, channel_mask);
 }

platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/micro_hal.h

@@ -105,6 +105,7 @@ 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

platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_servo.c

@@ -102,10 +102,10 @@ int up_pwm_servo_init(uint32_t channel_mask)
 	return OK;
 }
 
-void up_pwm_servo_deinit(void)
+void up_pwm_servo_deinit(uint32_t channel_mask)
 {
 	/* disable the timers */
-	up_pwm_servo_arm(false);
+	up_pwm_servo_arm(false, channel_mask);
 }
 
 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)
+up_pwm_servo_arm(bool armed, uint32_t 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);
+	io_timer_set_enable(armed, IOTimerChanMode_OneShot, channel_mask);
+	io_timer_set_enable(armed, IOTimerChanMode_PWMOut, channel_mask);
 }

platforms/nuttx/src/px4/stm/stm32_common/board_reset/board_reset.cpp

@@ -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   */  0xb0070001,
+	/* BOARD_RESET_MODE_BOOT_TO_BL           0   n   */  0xb007b007,
 	/* 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

platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/micro_hal.h

@@ -103,6 +103,7 @@ __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))

platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/spi_hw_description.h

@@ -129,7 +129,8 @@ static inline constexpr SPI::bus_device_external_cfg_t initSPIConfigExternal(SPI
 struct px4_spi_bus_array_t {
 	px4_spi_bus_t item[SPI_BUS_MAX_BUS_ITEMS];
 };
-static inline constexpr px4_spi_bus_all_hw_t initSPIHWVersion(int hw_version, const px4_spi_bus_array_t &bus_items)
+static inline constexpr px4_spi_bus_all_hw_t initSPIHWVersion(int hw_version_revision,
+		const px4_spi_bus_array_t &bus_items)
 {
 	px4_spi_bus_all_hw_t ret{};
 
@@ -137,7 +138,7 @@ static inline constexpr px4_spi_bus_all_hw_t initSPIHWVersion(int hw_version, co
 		ret.buses[i] = bus_items.item[i];
 	}
 
-	ret.board_hw_version = hw_version;
+	ret.board_hw_version_revision = hw_version_revision;
 	return ret;
 }
 constexpr bool validateSPIConfig(const px4_spi_bus_t spi_buses_conf[SPI_BUS_MAX_BUS_ITEMS]);

platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c

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

platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c

@@ -109,10 +109,10 @@ int up_pwm_servo_init(uint32_t channel_mask)
 	return OK;
 }
 
-void up_pwm_servo_deinit(void)
+void up_pwm_servo_deinit(uint32_t channel_mask)
 {
 	/* disable the timers */
-	up_pwm_servo_arm(false);
+	up_pwm_servo_arm(false, channel_mask);
 }
 
 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)
+up_pwm_servo_arm(bool armed, uint32_t 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);
+	io_timer_set_enable(armed, IOTimerChanMode_OneShot, channel_mask);
+	io_timer_set_enable(armed, IOTimerChanMode_PWMOut, channel_mask);
 }

src/drivers/distance_sensor/CMakeLists.txt

@@ -46,3 +46,4 @@ add_subdirectory(tfmini)
 add_subdirectory(ulanding_radar)
 add_subdirectory(vl53l0x)
 add_subdirectory(vl53l1x)
+add_subdirectory(gy_us42)

src/drivers/distance_sensor/gy_us42/CMakeLists.txt

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   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
+	)

src/drivers/distance_sensor/gy_us42/GY_US42.cpp

@@ -0,0 +1,133 @@
+/****************************************************************************
+ *
+ *   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);
+}