sensors/vehicle_angular_velocity: use 3 coefficient backward finite difference for angular acceleration (on FIFO data)

This gives an error on some new compilers, e.g. riscv64-unknown-elf 10.2.0

Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>

.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

@@ -469,6 +469,7 @@ 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
@@ -486,7 +487,7 @@ gazeboclean:
 
 distclean: gazeboclean
 	@git submodule deinit -f .
-	@git clean -ff -x -d -e ".project" -e ".cproject" -e ".idea" -e ".settings" -e ".vscode"
+	@git clean -ff -x -d -e ".cproject" -e ".idea" -e ".project" -e ".settings" -e ".vscode"
 
 # Help / Error
 # --------------------------------------------------------------------

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
 

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 4c27fc7dd659c262257abeea2308e2f1c54e9029

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
@@ -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-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/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/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) {

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 826b4ba68b83fa4e7a30636d2d97a9d6f1105122

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

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/nuttx/NuttX/nuttx

@@ -1 +1 @@
-Subproject commit 405cfa2571ab9eee2fd8cdf28726f8ccfeb3e5e6
+Subproject commit bf06434168d1b60474e90b277ff117975e250e6a

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/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);
+}

src/drivers/distance_sensor/gy_us42/GY_US42.hpp

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

src/drivers/distance_sensor/gy_us42/GY_US42_main.cpp

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

src/drivers/drv_pwm_output.h

@@ -317,8 +317,14 @@ __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(void);
+__EXPORT extern void	up_pwm_servo_deinit(uint32_t channel_mask);
 
 /**
  * Arm or disarm servo outputs.
@@ -330,8 +336,14 @@ __EXPORT extern void	up_pwm_servo_deinit(void);
  *
  * @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);
+__EXPORT extern void	up_pwm_servo_arm(bool armed, uint32_t channel_mask);
 
 /**
  * Set the servo update rate for all rate groups.

src/drivers/drv_sensor.h

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

src/drivers/gps/CMakeLists.txt

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

src/drivers/gps/devices

@@ -1 +1 @@
-Subproject commit ac1b5ce8e05d104e0f0a0cd5915e95f332066463
+Subproject commit f82313f082f6a9e63f83040996172906622f01a7

src/drivers/gps/gps.cpp

@@ -67,6 +67,7 @@
 # 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"
 
@@ -82,7 +83,8 @@ typedef enum {
 	GPS_DRIVER_MODE_UBX,
 	GPS_DRIVER_MODE_MTK,
 	GPS_DRIVER_MODE_ASHTECH,
-	GPS_DRIVER_MODE_EMLIDREACH
+	GPS_DRIVER_MODE_EMLIDREACH,
+	GPS_DRIVER_MODE_FEMTOMES
 } gps_driver_mode_t;
 
 /* struct for dynamic allocation of satellite info data */
@@ -774,6 +776,11 @@ 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:
@@ -912,6 +919,10 @@ 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
@@ -969,6 +980,10 @@ 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:
@@ -1135,7 +1150,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", "GPS Protocol (default=auto select)", true);
+	PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, "ubx|mtk|ash|eml|fem", "GPS Protocol (default=auto select)", true);
 
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 	PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Reset GPS device");
@@ -1276,6 +1291,9 @@ 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);

src/drivers/gps/params.c

@@ -115,12 +115,13 @@ PARAM_DEFINE_FLOAT(GPS_YAW_OFFSET, 0.f);
  * Auto-detection will probe all protocols, and thus is a bit slower.
  *
  * @min 0
- * @max 4
+ * @max 5
  * @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
@@ -135,12 +136,13 @@ PARAM_DEFINE_INT32(GPS_1_PROTOCOL, 1);
  * Auto-detection will probe all protocols, and thus is a bit slower.
  *
  * @min 0
- * @max 4
+ * @max 5
  * @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

src/drivers/protocol_splitter/protocol_splitter.cpp

@@ -55,9 +55,38 @@ class ReadBuffer;
 
 extern "C" __EXPORT int protocol_splitter_main(int argc, char *argv[]);
 
+/*
+MessageType is in MSB of header[1]
+		|
+		v
+	Mavlink 0000 0000b
+	Rtps    1000 0000b
+*/
+enum MessageType {Mavlink = 0x00, Rtps = 0x01};
 
-const char *Sp2HeaderMagic = "SP2";
-const int   Sp2HeaderSize  = 8;
+const char  Sp2HeaderMagic = 'S';
+const int   Sp2HeaderSize  = 4;
+
+/*
+Header Structure:
+
+     bits:   1 2 3 4 5 6 7 8
+header[0] - |     Magic     |
+header[1] - |T|   LenH      |
+header[2] - |     LenL      |
+header[3] - |   Checksum    |
+*/
+typedef union __attribute__((packed))
+{
+	uint8_t bytes[4];
+	struct {
+		char magic;                // 'S'
+		uint8_t len_h:	7,         // Length MSB
+			 type:	1;         // 0=MAVLINK, 1=RTPS
+		uint8_t len_l;             // Length LSB
+		uint8_t checksum;          // XOR of two above bytes
+	} fields;
+} Sp2Header_t;
 
 struct StaticData {
 	Mavlink2Dev *mavlink2;
@@ -145,18 +174,7 @@ public:
 
 protected:
 
-	/*
-	struct Sp2Header {
-		char magic[3];
-		uint8_t type;
-		uint16_t payload_len;
-		uint16_t reserved (align)
-	}
-	*/
-
-	enum MessageType {Mavlink = 0, Rtps};
-
-	uint8_t _header[8] = {};
+	Sp2Header_t _header;
 
 	virtual pollevent_t poll_state(struct file *filp);
 
@@ -273,15 +291,18 @@ Mavlink2Dev::Mavlink2Dev(ReadBuffer *read_buffer)
 	: DevCommon("/dev/mavlink")
 	, _read_buffer{read_buffer}
 {
-	memcpy(_header, Sp2HeaderMagic, 3);
-	_header[3] = MessageType::Mavlink;
-	memset(&_header[4], 0, 4);
+	_header.fields.magic 		= Sp2HeaderMagic;
+	_header.fields.len_h 		= 0;
+	_header.fields.len_l 		= 0;
+	_header.fields.checksum		= 0;
+	_header.fields.type		= MessageType::Mavlink;
 }
 
 ssize_t Mavlink2Dev::read(struct file *filp, char *buffer, size_t buflen)
 {
 	int i, ret;
 	uint16_t packet_len, payload_len;
+	Sp2Header_t *header;
 
 	/* last reading was partial (i.e., buffer didn't fit whole message),
 	 * so now we'll just send remaining bytes */
@@ -324,10 +345,11 @@ ssize_t Mavlink2Dev::read(struct file *filp, char *buffer, size_t buflen)
 	i = 0;
 
 	while ((unsigned)i < (_read_buffer->buf_size - Sp2HeaderSize) &&
-	       (_read_buffer->buffer[i] != 'S'
-		|| _read_buffer->buffer[i + 1] != 'P'
-		|| _read_buffer->buffer[i + 2] != '2'
-		|| _read_buffer->buffer[i + 3] != (uint8_t) MessageType::Mavlink)) {
+	       (((Sp2Header_t *) &_read_buffer->buffer[i])->fields.magic != Sp2HeaderMagic
+		|| ((Sp2Header_t *) &_read_buffer->buffer[i])->fields.type != (uint8_t)MessageType::Mavlink
+		|| ((Sp2Header_t *) &_read_buffer->buffer[i])->fields.checksum !=
+		(_read_buffer->buffer[i + 1] ^ _read_buffer->buffer[i + 2])
+	       )) {
 		i++;
 	}
 
@@ -336,7 +358,8 @@ ssize_t Mavlink2Dev::read(struct file *filp, char *buffer, size_t buflen)
 		goto end;
 	}
 
-	payload_len = ((uint16_t)_read_buffer->buffer[i + 4] << 8) | _read_buffer->buffer[i + 5];
+	header = (Sp2Header_t *)&_read_buffer->buffer[i];
+	payload_len = ((uint16_t)header->fields.len_h << 8) | header->fields.len_l;
 	packet_len = payload_len + Sp2HeaderSize;
 
 	// packet is bigger than what we've read, better luck next time
@@ -417,9 +440,10 @@ ssize_t Mavlink2Dev::write(struct file *filp, const char *buffer, size_t buflen)
 				ret = -1;
 
 			} else {
-				_header[4] = (uint8_t)((buflen >> 8) & 0xff);
-				_header[5] = (uint8_t)(buflen & 0xff);
-				::write(_fd, _header, 8);
+				_header.fields.len_h = (buflen >> 8) & 0x7f;
+				_header.fields.len_l = buflen & 0xff;
+				_header.fields.checksum = _header.bytes[1] ^ _header.bytes[2];
+				::write(_fd, _header.bytes, 4);
 				ret = ::write(_fd, buffer, buflen);
 			}
 
@@ -454,16 +478,18 @@ RtpsDev::RtpsDev(ReadBuffer *read_buffer)
 	: DevCommon("/dev/rtps")
 	, _read_buffer{read_buffer}
 {
-	memcpy(_header, Sp2HeaderMagic, 3);
-	_header[3] = MessageType::Rtps;
-	memset(&_header[4], 0, 4);
-
+	_header.fields.magic		= Sp2HeaderMagic;
+	_header.fields.len_h		= 0;
+	_header.fields.len_l		= 0;
+	_header.fields.checksum		= 0;
+	_header.fields.type		= MessageType::Rtps;
 }
 
 ssize_t RtpsDev::read(struct file *filp, char *buffer, size_t buflen)
 {
 	int i, ret;
 	uint16_t packet_len, payload_len;
+	Sp2Header_t *header;
 
 	if (!_had_data) {
 		return 0;
@@ -486,10 +512,11 @@ ssize_t RtpsDev::read(struct file *filp, char *buffer, size_t buflen)
 	i = 0;
 
 	while ((unsigned)i < (_read_buffer->buf_size - Sp2HeaderSize) &&
-	       (_read_buffer->buffer[i] != 'S'
-		|| _read_buffer->buffer[i + 1] != 'P'
-		|| _read_buffer->buffer[i + 2] != '2'
-		|| _read_buffer->buffer[i + 3] != (uint8_t) MessageType::Rtps)) {
+	       (((Sp2Header_t *) &_read_buffer->buffer[i])->fields.magic != Sp2HeaderMagic
+		|| ((Sp2Header_t *) &_read_buffer->buffer[i])->fields.type != (uint8_t)MessageType::Rtps
+		|| ((Sp2Header_t *) &_read_buffer->buffer[i])->fields.checksum !=
+		(_read_buffer->buffer[i + 1] ^ _read_buffer->buffer[i + 2])
+	       )) {
 		i++;
 	}
 
@@ -498,7 +525,8 @@ ssize_t RtpsDev::read(struct file *filp, char *buffer, size_t buflen)
 		goto end;
 	}
 
-	payload_len = ((uint16_t)_read_buffer->buffer[i + 4] << 8) | _read_buffer->buffer[i + 5];
+	header = (Sp2Header_t *)&_read_buffer->buffer[i];
+	payload_len = ((uint16_t)header->fields.len_h << 8) | header->fields.len_l;
 	packet_len = payload_len + Sp2HeaderSize;
 
 	// packet is bigger than what we've read, better luck next time
@@ -561,9 +589,10 @@ ssize_t RtpsDev::write(struct file *filp, const char *buffer, size_t buflen)
 				ret = -1;
 
 			} else {
-				_header[4] = (uint8_t)((buflen >> 8) & 0xff);
-				_header[5] = (uint8_t)(buflen & 0xff);
-				::write(_fd, _header, 8);
+				_header.fields.len_h = (buflen >> 8) & 0x7f;
+				_header.fields.len_l = buflen & 0xff;
+				_header.fields.checksum = _header.bytes[1] ^ _header.bytes[2];
+				::write(_fd, _header.bytes, 4);
 				ret = ::write(_fd, buffer, buflen);
 			}
 

src/drivers/pwm_out/PWMOut.cpp

@@ -35,6 +35,7 @@
 
 pthread_mutex_t pwm_out_module_mutex = PTHREAD_MUTEX_INITIALIZER;
 static px4::atomic<PWMOut *> _objects[PWM_OUT_MAX_INSTANCES] {};
+static px4::atomic<int> _all_instances_ready {0};
 
 static bool is_running()
 {
@@ -52,17 +53,24 @@ PWMOut::PWMOut(int instance, uint8_t output_base) :
 	OutputModuleInterface((instance == 0) ? MODULE_NAME"0" : MODULE_NAME"1", px4::wq_configurations::hp_default),
 	_instance(instance),
 	_output_base(output_base),
+	_output_mask(0),
 	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
 	_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": interval"))
 {
 	_mixing_output.setAllMinValues(PWM_DEFAULT_MIN);
 	_mixing_output.setAllMaxValues(PWM_DEFAULT_MAX);
+
+	for (int i = 0; i < MAX_PER_INSTANCE; i++) {
+		_output_mask |= 1 << i;
+	}
+
+	_output_mask <<= _output_base;
 }
 
 PWMOut::~PWMOut()
 {
 	/* make sure servos are off */
-	up_pwm_servo_deinit(); // TODO: review for multi
+	up_pwm_servo_deinit(_pwm_mask);
 
 	/* clean up the alternate device node */
 	unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance);
@@ -107,6 +115,7 @@ int PWMOut::init()
 int PWMOut::set_mode(Mode mode)
 {
 	unsigned old_mask = _pwm_mask;
+	bool old_pwm_initialized = _pwm_initialized;
 
 	/*
 	 * Configure for PWM output.
@@ -324,18 +333,24 @@ int PWMOut::set_mode(Mode mode)
 		_num_outputs = 0;
 		_mixing_output.setMaxNumOutputs(_num_outputs);
 		update_params();
-
-		if (old_mask != _pwm_mask) {
-			/* disable servo outputs - no need to set rates */
-			up_pwm_servo_deinit(); // TODO: review for multi
-		}
-
 		break;
 
 	default:
 		return -EINVAL;
 	}
 
+	if (old_mask != _pwm_mask) {
+		/* disable servo outputs - no need to set rates */
+		if (old_mask != 0) {
+			up_pwm_servo_deinit(old_mask);
+			_pwm_on = false;
+		}
+
+		if (old_pwm_initialized != _pwm_initialized) {
+			_all_instances_ready.fetch_sub(1);
+		}
+	}
+
 	_mode = mode;
 	return OK;
 }
@@ -382,14 +397,14 @@ int PWMOut::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_
 		 * common settings and can not be independent in terms of count frequency
 		 * (granularity of pulse width) and rate (period of repetition).
 		 *
-		 * To say it another way, all channels in a group moust have the same
+		 * To say it another way, all channels in a group must have the same
 		 * rate and mode. (See rates above.)
 		 */
 
 		for (unsigned group = 0; group < FMU_MAX_ACTUATORS; group++) {
 
 			// get the channel mask for this rate group
-			uint32_t mask = up_pwm_servo_get_rate_group(group);
+			uint32_t mask = _output_mask & up_pwm_servo_get_rate_group(group);
 
 			if (mask == 0) {
 				continue;
@@ -487,7 +502,7 @@ int PWMOut::task_spawn(int argc, char *argv[])
 	for (unsigned instance = 0; instance < (sizeof(_objects) / sizeof(_objects[0])); instance++) {
 
 		if (instance < PWM_OUT_MAX_INSTANCES) {
-			uint8_t base = instance * 8;  // TODO: configurable
+			uint8_t base = instance * MAX_PER_INSTANCE;  // TODO: configurable
 			PWMOut *dev = new PWMOut(instance, base);
 
 			if (dev) {
@@ -528,7 +543,7 @@ void PWMOut::capture_callback(uint32_t chan_index,
 	fprintf(stdout, "FMU: Capture chan:%d time:%lld state:%d overflow:%d\n", chan_index, edge_time, edge_state, overflow);
 }
 
-void PWMOut::update_pwm_out_state(bool on)
+bool PWMOut::update_pwm_out_state(bool on)
 {
 	if (on && !_pwm_initialized && _pwm_mask != 0) {
 
@@ -537,23 +552,11 @@ void PWMOut::update_pwm_out_state(bool on)
 		// Collect the PWM alt rate channels across all instances
 		uint32_t pwm_alt_rate_channels_new = 0;
 
-		// Collect the minimum default rate
-		unsigned default_rate_min = 0;
-		// Collect the maximum alternative rate (400 Hz or DSHOT outputs)
-		unsigned alt_rate_max = 0;
-
 		for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) {
 			if (_objects[i].load()) {
+
 				pwm_mask_new |= _objects[i].load()->get_pwm_mask();
 				pwm_alt_rate_channels_new |= _objects[i].load()->get_alt_rate_channels();
-
-				if (_objects[i].load()->get_alt_rate() > alt_rate_max) {
-					alt_rate_max = _objects[i].load()->get_alt_rate();
-				}
-
-				if (_objects[i].load()->get_default_rate() < default_rate_min) {
-					default_rate_min = _objects[i].load()->get_default_rate();
-				}
 			}
 		}
 
@@ -563,10 +566,14 @@ void PWMOut::update_pwm_out_state(bool on)
 
 		// Set rate is not affecting non-masked channels, so can be called
 		// individually
+		set_pwm_rate(get_alt_rate_channels(), get_default_rate(), get_alt_rate());
+
 		_pwm_initialized = true;
+		_all_instances_ready.fetch_add(1);
 	}
 
-	up_pwm_servo_arm(on);
+	up_pwm_servo_arm(on, _pwm_mask);
+	return _all_instances_ready.load() == PWM_OUT_MAX_INSTANCES;
 }
 
 bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
@@ -620,8 +627,10 @@ void PWMOut::Run()
 	bool pwm_on = _mixing_output.armed().armed || (_num_disarmed_set > 0) || _mixing_output.armed().in_esc_calibration_mode;
 
 	if (_pwm_on != pwm_on) {
-		_pwm_on = pwm_on;
-		update_pwm_out_state(pwm_on);
+
+		if (update_pwm_out_state(pwm_on)) {
+			_pwm_on = pwm_on;
+		}
 	}
 
 	// check for parameter updates
@@ -1171,7 +1180,7 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	case PWM_SERVO_SET(1):
 	case PWM_SERVO_SET(0):
 		if (arg <= 2100) {
-			up_pwm_servo_set(cmd - PWM_SERVO_SET(0 + _output_base), arg);
+			up_pwm_servo_set(cmd - PWM_SERVO_SET(0) + _output_base, arg);
 
 		} else {
 			ret = -EINVAL;
@@ -1243,7 +1252,7 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	/* FALLTHROUGH */
 	case PWM_SERVO_GET(1):
 	case PWM_SERVO_GET(0):
-		*(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0 + _output_base));
+		*(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0) +  _output_base);
 		break;
 
 	case PWM_SERVO_GET_RATEGROUP(0):
@@ -1268,7 +1277,7 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	case PWM_SERVO_GET_RATEGROUP(12):
 	case PWM_SERVO_GET_RATEGROUP(13):
 #endif
-		*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0 + _output_base));
+		*(uint32_t *)arg = _output_mask & up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
 		break;
 
 	case PWM_SERVO_GET_COUNT:
@@ -1813,7 +1822,7 @@ int fmu_new_i2c_speed(unsigned bus, unsigned clock_hz)
 
 } // namespace
 
-int PWMOut::test()
+int PWMOut::test(const char *dev)
 {
 	int	 fd;
 	unsigned servo_count = 0;
@@ -1828,7 +1837,7 @@ int PWMOut::test()
 		input_capture_config_t  chan;
 	} capture_conf[INPUT_CAPTURE_MAX_CHANNELS];
 
-	fd = ::open(PX4FMU_DEVICE_PATH, O_RDWR);
+	fd = ::open(dev, O_RDWR);
 
 	if (fd < 0) {
 		PX4_ERR("open fail");
@@ -2006,8 +2015,33 @@ err_out_no_test:
 
 int PWMOut::custom_command(int argc, char *argv[])
 {
+
+	int ch = 0;
+	int myoptind = 0;
+	const char *myoptarg = nullptr;
+	const char *dev = PX4FMU_DEVICE_PATH;
+
+	while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'd':
+			if (nullptr == strstr(myoptarg, "/dev/")) {
+				PX4_WARN("device %s not valid", myoptarg);
+				print_usage(nullptr);
+				return 1;
+			}
+
+			dev = myoptarg;
+			break;
+		}
+	}
+
+	if (myoptind >= argc) {
+		print_usage(nullptr);
+		return 1;
+	}
+
 	PortMode new_mode = PORT_MODE_UNSET;
-	const char *verb = argv[0];
+	const char *verb = argv[myoptind];
 
 	/* does not operate on a FMU instance */
 	if (!strcmp(verb, "i2c")) {
@@ -2151,7 +2185,7 @@ int PWMOut::custom_command(int argc, char *argv[])
 	}
 
 	if (!strcmp(verb, "test")) {
-		return test();
+		return test(dev);
 	}
 
 	return print_usage("unknown command");

src/drivers/pwm_out/PWMOut.hpp

@@ -151,7 +151,7 @@ public:
 	/** change the FMU mode of the running module */
 	static int fmu_new_mode(PortMode new_mode);
 
-	static int test();
+	static int test(const char *dev);
 
 	virtual int	ioctl(file *filp, int cmd, unsigned long arg);
 
@@ -182,6 +182,9 @@ private:
 
 	const int _instance;
 	const uint32_t _output_base;
+	uint32_t _output_mask;
+
+	static const int MAX_PER_INSTANCE{8};
 
 	MixingOutput _mixing_output{FMU_MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, true};
 
@@ -218,7 +221,7 @@ private:
 	int			set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate);
 	int			pwm_ioctl(file *filp, int cmd, unsigned long arg);
 
-	void		update_pwm_out_state(bool on);
+	bool		update_pwm_out_state(bool on);
 
 	void		update_params();
 

src/drivers/uavcan_v1/public_regulated_data_types

@@ -1 +1 @@
-Subproject commit 309b251a7e8d713d6bf428e18e28d91d5f07b73e
+Subproject commit 1337b1c86fee5bd3f3c3c0f1027bcf19e5c08aae

src/drivers/uavcannode_gps_demo/public_regulated_data_types

@@ -1 +1 @@
-Subproject commit 309b251a7e8d713d6bf428e18e28d91d5f07b73e
+Subproject commit 1337b1c86fee5bd3f3c3c0f1027bcf19e5c08aae

src/lib/collision_prevention/CollisionPrevention.cpp

@@ -389,7 +389,7 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec
 	// read parameters
 	const float col_prev_d = _param_cp_dist.get();
 	const float col_prev_dly = _param_cp_delay.get();
-	const bool move_no_data = _param_cp_go_nodata.get() > 0;
+	const bool move_no_data = _param_cp_go_nodata.get();
 	const float xy_p = _param_mpc_xy_p.get();
 	const float max_jerk = _param_mpc_jerk_max.get();
 	const float max_accel = _param_mpc_acc_hor.get();

src/lib/collision_prevention/CollisionPrevention.hpp

@@ -145,7 +145,7 @@ private:
 		(ParamFloat<px4::params::CP_DIST>) _param_cp_dist, /**< collision prevention keep minimum distance */
 		(ParamFloat<px4::params::CP_DELAY>) _param_cp_delay, /**< delay of the range measurement data*/
 		(ParamFloat<px4::params::CP_GUIDE_ANG>) _param_cp_guide_ang, /**< collision prevention change setpoint angle */
-		(ParamFloat<px4::params::CP_GO_NO_DATA>) _param_cp_go_nodata, /**< movement allowed where no data*/
+		(ParamBool<px4::params::CP_GO_NO_DATA>) _param_cp_go_nodata, /**< movement allowed where no data*/
 		(ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p, /**< p gain from position controller*/
 		(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max, /**< vehicle maximum jerk*/
 		(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor /**< vehicle maximum horizontal acceleration*/

src/lib/collision_prevention/collisionprevention_params.c

@@ -83,4 +83,4 @@ PARAM_DEFINE_FLOAT(CP_GUIDE_ANG, 30.f);
  * @boolean
  * @group Multicopter Position Control
  */
-PARAM_DEFINE_FLOAT(CP_GO_NO_DATA, 0);
+PARAM_DEFINE_INT32(CP_GO_NO_DATA, 0);

src/lib/ecl

@@ -1 +1 @@
-Subproject commit 5d34d7a24ef72b826c320a3259ee0ec68b1936df
+Subproject commit a7b8afe420f438554ad90bcba0f1f4872325e75b