drivers/rc_input: cleanup and simplify data processing per type

- don't store data in class
drivers/rc_input: poll file descriptor to minimize latency
2026-05-26 17:50:04 +08:00 · 2022-02-09 17:11:11 -05:00 · 2022-02-09 16:48:08 -05:00 · 2022-02-09 13:11:52 -05:00 · 2022-02-09 13:11:52 -05:00 · 2022-02-09 11:39:47 -05:00
168 changed files with 3792 additions and 3705 deletions
@@ -73,10 +73,8 @@ pipeline {
                }
              }
              post {
-                failure {
-                  sh 'cat /tmp/pyserial_spy_file.txt'
-                }
                always {
+                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/cubepilot_cubeorange_test/cubepilot_cubeorange_test.elf || true'
                }
              }
@@ -145,10 +143,8 @@ pipeline {
                }
              }
              post {
-                failure {
-                  sh 'cat /tmp/pyserial_spy_file.txt'
-                }
                always {
+                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/cuav_x7pro_test/cuav_x7pro_test.elf || true'
                }
              }
@@ -217,10 +213,8 @@ pipeline {
                }
              }
              post {
-                failure {
-                  sh 'cat /tmp/pyserial_spy_file.txt'
-                }
                always {
+                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v3_test/px4_fmu-v3_test.elf || true'
                }
              }
@@ -288,10 +282,8 @@ pipeline {
                }
              }
              post {
-                failure {
-                  sh 'cat /tmp/pyserial_spy_file.txt'
-                }
                always {
+                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v4_test/px4_fmu-v4_test.elf || true'
                }
              }
@@ -360,10 +352,8 @@ pipeline {
                }
              }
              post {
-                failure {
-                  sh 'cat /tmp/pyserial_spy_file.txt'
-                }
                always {
+                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v4pro_test/px4_fmu-v4pro_test.elf || true'
                }
              }
@@ -452,10 +442,8 @@ pipeline {
                }
              }
              post {
-                failure {
-                  sh 'cat /tmp/pyserial_spy_file.txt'
-                }
                always {
+                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v5_debug/px4_fmu-v5_debug.elf || true'
                }
              }
@@ -536,10 +524,8 @@ pipeline {
                }
              }
              post {
-                failure {
-                  sh 'cat /tmp/pyserial_spy_file.txt'
-                }
                always {
+                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v5_stackcheck/px4_fmu-v5_stackcheck.elf || true'
                }
              }
@@ -608,10 +594,8 @@ pipeline {
                }
              }
              post {
-                failure {
-                  sh 'cat /tmp/pyserial_spy_file.txt'
-                }
                always {
+                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v5_test/px4_fmu-v5_test.elf || true'
                }
              }
@@ -680,10 +664,8 @@ pipeline {
                }
              }
              post {
-                failure {
-                  sh 'cat /tmp/pyserial_spy_file.txt'
-                }
                always {
+                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/nxp_fmuk66-v3_test/nxp_fmuk66-v3_test.elf || true'
                }
              }
@@ -793,7 +775,7 @@ void checkStatus() {
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "top once"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uavcan status" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb status" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1 -a"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1 -a" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ver all"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "work_queue status"'
  // stop logger
@@ -809,7 +791,7 @@ void resetParameters() {
 void runTests() {

  // test loading a range of airframes
-  sh './Tools/HIL/test_airframes.sh `find /dev/serial -name *usb-*` 1000 1001 2100 3000 4001 4018 6001 8001 10016'
+  sh './Tools/HIL/test_airframes.sh `find /dev/serial -name *usb-*` 1000 1001 2100 3000 4001 6001 8001 10016'

  resetParameters()

@@ -862,7 +844,7 @@ void runTests() {
 void printTopics() {
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /obj"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1 -a"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1 -a" || true'

  // these are for casually inspecting the system, output failure doesn't matter
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_armed" || true'
@@ -1,154 +0,0 @@
-# build docker images on:
-# * every pull request
-# * push to master
-# * stable release published
-# * tag name is:
-#   - commit sha if pull request
-#   - 'latest' if push to master
-#   - release name if release
-
-# pushes to registry if:
-# * is not a pull request
-# * is master branch
-# * is the result of release
-
-# builds all nuttx targets and deploys metadata
-
-name: Build docker
-
-on:
-  release:
-    types: [released]
-  push:
-    branches:
-      - 'master'
-  pull_request:
-
-jobs:
-  set_docker_tag:
-    name: Set docker tag
-    runs-on: ubuntu-latest
-    env:
-      DOCKER_TAG: "px4io/px4-dev"
-    outputs:
-      docker_tag: ${{ steps.set-outputs.outputs.docker_tag }}
-
-    steps:
-      - name: Set pull request docker tag
-        if: github.event_name == 'pull_request'
-        run: echo "DOCKER_TAG=px4io/px4-dev:${{ github.sha }}" >> $GITHUB_ENV
-
-      - name: Set push docker tag
-        if: github.event_name == 'push'
-        run: echo "DOCKER_TAG=px4io/px4-dev:latest" >> $GITHUB_ENV
-
-      - name: Set release docker tag
-        if: github.event_name == 'release'
-        run: echo "DOCKER_TAG=px4io/px4-dev:${{ github.event.release.name }}" >> $GITHUB_ENV
-
-      - name: Set docker tag outputs
-        id: set-outputs
-        run: echo "::set-output name=docker_tag::$DOCKER_TAG"
-
-  build_docker:
-    name: Build Docker image
-    runs-on: ubuntu-latest
-    needs: set_docker_tag
-
-    steps:
-      - name: Checkout code
-        uses: actions/checkout@v2
-
-      - name: Login to Github Registry
-        uses: docker/login-action@v1
-        if: github.event_name == 'push'
-        with:
-          registry: ghrc.io
-          username: ${{ github.actor }}
-          password: ${{ secrets.GITHUB_TOKEN }}
-
-      - name: Set up image builder
-        uses: docker/setup-buildx-action@v1
-
-      - name: Build image
-        uses: docker/build-push-action@v2
-        id: docker_build
-        with:
-          file: Tools/setup/Dockerfile
-          push: ${{ github.event_name != 'pull_request' }}
-          tags: ${{ needs.set_docker_tag.outputs.docker_tag }}
-          outputs: type=tar,dest=/tmp/px4_docker_image.tar
-
-      - name: Save container to artifacts
-        uses: actions/upload-artifact@v2
-        with:
-          name: px4_docker_image
-          path: /tmp/px4_docker_image.tar
-
-      - name: Push to Github Registry
-        uses: docker/build-push-action@v2
-        if: github.event_name == 'push'
-
-      - name: Image Digest
-        run: echo ${{ steps.docker_build.outputs.digest }}
-
-  list_targets:
-    name: Generate target list
-    runs-on: ubuntu-latest
-    needs: build_docker
-    outputs:
-      matrix: ${{ steps.set-matrix.outputs.matrix }}
-
-    steps:
-      - name: Checkout Code
-        uses: actions/checkout@v2
-
-      - name: Setup Python
-        uses: actions/setup-python@v2
-        with:
-          python-version: 3.9
-
-      #- name: Install Python packages
-        #run: pip install -r Tools/setup/requirements.txt
-        #run: pip install kconfiglib
-
-      - id: set-matrix
-        name: Get all nuttx targets
-        run: echo "::set-output name=matrix::$(./Tools/generate_board_targets_json.py -a)"
-
-  build_px4:
-    name: Build targets with Docker
-    runs-on: ubuntu-latest
-    needs: [list_targets, set_docker_tag]
-    #strategy:
-      #matrix: ${{ fromJson(needs.enumerate_targets.outputs.matrix) }}
-
-    steps:
-      - name: Get container from artifacts
-        uses: actions/download-artifact@v2
-        with:
-          name: px4_docker_image
-          path: /tmp
-
-      - name: Load Docker image
-        run: |
-          docker import /tmp/px4_docker_image.tar ${{ needs.set_docker_tag.outputs.docker_tag }}
-          docker image ls -a
-
-      - name: Checkout Code
-        uses: actions/checkout@v2
-
-      - name: Build targets
-        #run: |
-          #docker run --rm -w "${GITHUB_ACTION_PATH}" \
-            #--env=LOCAL_USER_ID="$(id -u)" \
-            #--volume=/tmp:/tmp:rw \
-            #--volume=${GITHUB_ACTION_PATH}:${GITHUB_ACTION_PATH}:rw \
-            #px4io/px4-dev:${{ needs.set_docker_tag.outputs.docker_tag }} /bin/bash -c "make ${{ matrix.target }}"
-
-        run: |
-          docker run --rm -w ${{ github.workspace }} \
-            --env=LOCAL_USER_ID="$(id -u)" \
-            --volume=/tmp:/tmp:rw \
-            --volume=${{ github.workspace }}:${{ github.workspace }}:rw \
-            ${{ needs.set_docker_tag.outputs.docker_tag }} /bin/bash -c "make px4_fmu-v6x"
@@ -0,0 +1,54 @@
+name: Linux Targets
+
+on:
+  push:
+    branches:
+    - 'master'
+  pull_request:
+    branches:
+    - '*'
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    container: px4io/px4-dev-armhf:2021-09-08
+    strategy:
+      matrix:
+        config: [
+          beaglebone_blue_default,
+          emlid_navio2_default,
+          px4_raspberrypi_default,
+          scumaker_pilotpi_default,
+          ]
+    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: ${{matrix.config}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}}
+        restore-keys: ${{matrix.config}}-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 = 6" >> ~/.ccache/ccache.conf
+          echo "max_size = 100M" >> ~/.ccache/ccache.conf
+          echo "hash_dir = false" >> ~/.ccache/ccache.conf
+          ccache -s
+          ccache -z
+
+    - name: make ${{matrix.config}}
+      run: make ${{matrix.config}}
+    - name: ccache post-run
+      run: ccache -s
@@ -0,0 +1,51 @@
+name: Linux ARM64 Targets
+
+on:
+  push:
+    branches:
+    - 'master'
+  pull_request:
+    branches:
+    - '*'
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    container: px4io/px4-dev-aarch64:2021-09-08
+    strategy:
+      matrix:
+        config: [
+          scumaker_pilotpi_arm64,
+          ]
+    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: ${{matrix.config}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}}
+        restore-keys: ${{matrix.config}}-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 = 6" >> ~/.ccache/ccache.conf
+          echo "max_size = 100M" >> ~/.ccache/ccache.conf
+          echo "hash_dir = false" >> ~/.ccache/ccache.conf
+          ccache -s
+          ccache -z
+
+    - name: make ${{matrix.config}}
+      run: make ${{matrix.config}}
+    - name: ccache post-run
+      run: ccache -s
@@ -0,0 +1,120 @@
+name: Nuttx Targets
+
+on:
+  push:
+    branches:
+    - 'master'
+  pull_request:
+    branches:
+    - '*'
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    container: px4io/px4-dev-nuttx-focal:2021-09-08
+    strategy:
+      fail-fast: false
+      matrix:
+        config: [
+          airmind_mindpx-v2,
+          ark_can-flow,
+          ark_can-gps,
+          ark_can-rtk-gps,
+          atl_mantis-edu,
+          av_x-v1,
+          bitcraze_crazyflie,
+          bitcraze_crazyflie21,
+          cuav_can-gps-v1,
+          cuav_nora,
+          cuav_x7pro,
+          cubepilot_cubeorange,
+          cubepilot_cubeyellow,
+          freefly_can-rtk-gps,
+          holybro_can-gps-v1,
+          holybro_durandal-v1,
+          holybro_kakutef7,
+          holybro_kakuteh7,
+          holybro_pix32v5,
+          matek_h743-slim,
+          matek_gnss-m9n-f4,
+          modalai_fc-v1,
+          modalai_fc-v2,
+          mro_ctrl-zero-f7,
+          mro_ctrl-zero-f7-oem,
+          mro_ctrl-zero-h7,
+          mro_ctrl-zero-h7-oem,
+          mro_pixracerpro,
+          mro_x21,
+          mro_x21-777,
+          nxp_fmuk66-e,
+          nxp_fmuk66-v3,
+          nxp_fmurt1062-v1,
+          nxp_ucans32k146,
+          omnibus_f4sd,
+          raspberrypi_pico,
+          px4_fmu-v2,
+          px4_fmu-v3,
+          px4_fmu-v4,
+          px4_fmu-v4pro,
+          px4_fmu-v5,
+          px4_fmu-v5x,
+          px4_fmu-v6u,
+          px4_fmu-v6x,
+          spracing_h7extreme,
+          uvify_core
+          ]
+    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: ${{matrix.config}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}}
+        restore-keys: ${{matrix.config}}-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 = 6" >> ~/.ccache/ccache.conf
+          echo "max_size = 120M" >> ~/.ccache/ccache.conf
+          echo "hash_dir = false" >> ~/.ccache/ccache.conf
+          ccache -s
+          ccache -z
+
+    - name: make all_variants_${{matrix.config}}
+      run: make all_variants_${{matrix.config}}
+      timeout-minutes: 45
+    - name: make ${{matrix.config}} bloaty_compileunits
+      run: make ${{matrix.config}} bloaty_compileunits || true
+    - name: make ${{matrix.config}} bloaty_inlines
+      run: make ${{matrix.config}} bloaty_inlines || true
+    - name: make ${{matrix.config}} bloaty_segments
+      run: make ${{matrix.config}} bloaty_segments || true
+    - name: make ${{matrix.config}} bloaty_symbols
+      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
+      run: ccache -s
+
+    - name: Upload px4 package
+      uses: actions/upload-artifact@v2
+      with:
+        name: px4_package_${{matrix.config}}
+        path: |
+          build/**/*.px4
+          build/**/*.bin
@@ -1,17 +1,11 @@
-name: Build and deploy all targets with metadata
+name: Deploy metadata for all targets

 on:
-  release:
-    types: [published]
-  create:
-    tags:
-      - 'v*'
  push:
    branches:
    - 'master'
    - 'release/*'
    - 'pr-metadata-test'
-    - 'build_and_deploy_mix'

 jobs:
  enumerate_targets:
@@ -24,8 +18,8 @@ jobs:
      with:
        token: ${{secrets.ACCESS_TOKEN}}
    - id: set-matrix
-      run: echo "::set-output name=matrix::$(./Tools/generate_board_targets_json.py -a)"
-  build_all:
+      run: echo "::set-output name=matrix::$(./Tools/generate_board_targets_json.py)"
+  build:
    runs-on: ubuntu-latest
    needs: enumerate_targets
    strategy:
@@ -40,4 +34,21 @@ jobs:
      run: make ${{matrix.target}}

    - name: parameter & events metadata
-      run: make ${{matrix.target}} ver_gen events_json actuators_json
+      run: |
+        make ${{matrix.target}} ver_gen events_json actuators_json
+        ./src/lib/version/get_git_tag_or_branch_version.sh build/${{ matrix.target }} >> $GITHUB_ENV
+        cd build/${{ matrix.target }}
+        mkdir _metadata || true
+        cp parameters.* events/*.xz actuators.json* _metadata
+
+    - uses: jakejarvis/s3-sync-action@master
+      with:
+        args: --acl public-read
+      env:
+        AWS_S3_BUCKET: 'px4-travis'
+        AWS_ACCESS_KEY_ID: ${{ secrets.AWS_ACCESS_KEY_ID }}
+        AWS_SECRET_ACCESS_KEY: ${{ secrets.AWS_SECRET_ACCESS_KEY }}
+        AWS_REGION: 'us-west-1'
+        SOURCE_DIR: 'build/${{ matrix.target }}/_metadata/'
+        DEST_DIR: 'Firmware/${{ env.version }}/${{ matrix.target }}/'
+
@@ -44,6 +44,8 @@ param set-default MIS_TAKEOFF_ALT 30
 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2

+param set-default FW_USE_NPFG 1
+
 param set-default RWTO_TKOFF 1

 set MIXER_FILE etc/mixers-sitl/plane_sitl.main.mix
@@ -234,7 +234,7 @@ fi

 if param greater -s MNT_MODE_IN -1
 then
-	vmount start
+	gimbal start
 fi

 if param greater -s TRIG_MODE 0
@@ -24,6 +24,27 @@

 . ${R}etc/init.d/rc.vtol_defaults

+param set-default CA_AIRFRAME 2
+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.15
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.15
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.15
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.15
+param set-default CA_ROTOR3_KM -0.05
+param set-default CA_SV_CS_COUNT 4
+param set-default CA_SV_CS0_TYPE 1
+param set-default CA_SV_CS0_TRQ_R -0.5
+param set-default CA_SV_CS1_TYPE 2
+param set-default CA_SV_CS1_TRQ_R 0.5
+param set-default CA_SV_CS2_TYPE 3
+param set-default CA_SV_CS2_TRQ_P 1.0
+param set-default CA_SV_CS3_TRQ_Y 1.0
+param set-default CA_SV_CS3_TYPE 4

 param set-default PWM_AUX_DIS5 950

@@ -13,6 +13,25 @@

 . ${R}etc/init.d/rc.vtol_defaults

+param set-default CA_AIRFRAME 4
+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.15
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.15
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.15
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.15
+param set-default CA_ROTOR3_KM -0.05
+param set-default CA_SV_CS_COUNT 2
+param set-default CA_SV_CS0_TRQ_P 0.5
+param set-default CA_SV_CS0_TRQ_Y 0.5
+param set-default CA_SV_CS0_TYPE 5
+param set-default CA_SV_CS1_TRQ_P 0.5
+param set-default CA_SV_CS1_TRQ_Y -0.5
+param set-default CA_SV_CS1_TYPE 6

 param set-default PWM_MAIN_MAX 2000

@@ -31,6 +31,32 @@ param set-default VT_IDLE_PWM_MC 1100
 param set-default VT_TYPE 1
 param set-default VT_MOT_ID 1234
 param set-default VT_FW_MOT_OFFID 24
+
+param set-default CA_AIRFRAME 3
+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.15
+param set-default CA_ROTOR0_TILT 1
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.15
+param set-default CA_ROTOR1_TILT 2
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.15
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR2_TILT 3
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.15
+param set-default CA_ROTOR3_KM -0.05
+param set-default CA_ROTOR3_TILT 4
+param set-default CA_SV_CS_COUNT 4
+param set-default CA_SV_CS1_TRQ_R 0.5
+param set-default CA_SV_CS1_TYPE 2
+param set-default CA_SV_CS2_TRQ_P 1.0
+param set-default CA_SV_CS2_TYPE 3
+param set-default CA_SV_CS3_TRQ_Y 1.0
+param set-default CA_SV_CS3_TYPE 4
+param set-default CA_SV_TL_COUNT 4
+
 set MAV_TYPE 21

 set MIXER quad_x
@@ -23,6 +23,20 @@ param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_MOT_COUNT 2
 param set-default VT_TYPE 0

+param set-default CA_AIRFRAME 4
+param set-default CA_ROTOR_COUNT 2
+param set-default CA_ROTOR0_KM -0.05
+param set-default CA_ROTOR0_PY 0.2
+param set-default CA_ROTOR1_KM -0.05
+param set-default CA_ROTOR1_PY -0.2
+param set-default CA_SV_CS_COUNT 2
+param set-default CA_SV_CS0_TRQ_P 0.5
+param set-default CA_SV_CS0_TRQ_Y 0.5
+param set-default CA_SV_CS0_TYPE 5
+param set-default CA_SV_CS1_TRQ_P 0.5
+param set-default CA_SV_CS1_TRQ_Y -0.5
+param set-default CA_SV_CS1_TYPE 6
+
 param set-default MAV_TYPE 19
 set MAV_TYPE 19
 set MIXER vtol_tailsitter_duo
@@ -23,6 +23,18 @@

 . ${R}etc/init.d/rc.fw_defaults

+param set-default CA_AIRFRAME 1
+param set-default CA_ROTOR_COUNT 1
+param set-default CA_ROTOR0_PX 0.3
+param set-default CA_SV_CS_COUNT 4
+param set-default CA_SV_CS0_TRQ_R -0.5
+param set-default CA_SV_CS1_TRQ_R 0.5
+param set-default CA_SV_CS1_TYPE 2
+param set-default CA_SV_CS2_TRQ_P 1.0
+param set-default CA_SV_CS2_TYPE 3
+param set-default CA_SV_CS3_TRQ_Y 1.0
+param set-default CA_SV_CS3_TYPE 4
+
 param set-default PWM_AUX_RATE 50
 param set-default PWM_MAIN_RATE 50

@@ -21,3 +21,16 @@
 #

 . ${R}etc/init.d/rc.mc_defaults
+
+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.15
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.15
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.15
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.15
+param set-default CA_ROTOR3_KM -0.05
+
@@ -1,39 +0,0 @@
-#!/bin/sh
-#
-# @name S500 with control allocation
-#
-# @type Quadrotor x
-# @class Copter
-#
-# @maintainer Silvan Fuhrer
-#
-# @board px4_fmu-v2 exclude
-#
-
-. ${R}etc/init.d/rc.mc_defaults
-
-set MIXER skip
-set MIXER_AUX none
-
-param set-default SYS_CTRL_ALLOC 1
-
-param set-default CA_AIRFRAME 0
-
-param set-default CA_ROTOR_COUNT 4
-param set-default CA_ROTOR0_PX 0.177
-param set-default CA_ROTOR0_PY 0.177
-param set-default CA_ROTOR0_KM 0.05
-param set-default CA_ROTOR1_PX -0.177
-param set-default CA_ROTOR1_PY -0.177
-param set-default CA_ROTOR1_KM 0.05
-param set-default CA_ROTOR2_PX 0.177
-param set-default CA_ROTOR2_PY -0.177
-param set-default CA_ROTOR2_KM -0.05
-param set-default CA_ROTOR3_PX -0.177
-param set-default CA_ROTOR3_PY 0.177
-param set-default CA_ROTOR3_KM -0.05
-
-param set-default PWM_MAIN_FUNC1 101
-param set-default PWM_MAIN_FUNC2 102
-param set-default PWM_MAIN_FUNC3 103
-param set-default PWM_MAIN_FUNC4 104
@@ -147,7 +147,7 @@ param set-default MPC_Z_VEL_P 0.27


 # gimbal configuration
-param set-default MNT_MODE_IN 1
+param set-default MNT_MODE_IN 0
 param set-default MNT_MODE_OUT 1
 param set-default MNT_MAN_PITCH 2
 param set-default MNT_RC_IN_MODE 1
@@ -23,6 +23,23 @@

 . ${R}etc/init.d/rc.mc_defaults

+param set-default CA_ROTOR_COUNT 6
+param set-default CA_ROTOR0_PX 0.0
+param set-default CA_ROTOR0_PY 0.5
+param set-default CA_ROTOR0_KM -0.05
+param set-default CA_ROTOR1_PX 0.0
+param set-default CA_ROTOR1_PY -0.5
+param set-default CA_ROTOR2_PX 0.43
+param set-default CA_ROTOR2_PY -0.25
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.43
+param set-default CA_ROTOR3_PY 0.25
+param set-default CA_ROTOR4_PX 0.43
+param set-default CA_ROTOR4_PY 0.25
+param set-default CA_ROTOR5_PX -0.43
+param set-default CA_ROTOR5_PY -0.25
+param set-default CA_ROTOR5_KM -0.05
+
 set MIXER hexa_x

 # Need to set all 8 channels
@@ -1,52 +0,0 @@
-#!/bin/sh
-#
-# @name Hex X with control allocation
-#
-# @type Hexarotor x
-# @class Copter
-#
-# @maintainer Silvan Fuhrer
-#
-# @board px4_fmu-v2 exclude
-#
-
-. ${R}etc/init.d/rc.mc_defaults
-
-param set-default SYS_CTRL_ALLOC 1
-
-param set-default CA_AIRFRAME 0
-param set-default CA_ROTOR_COUNT 6
-
-param set-default CA_ROTOR0_PX 0.0
-param set-default CA_ROTOR0_PY 0.275
-param set-default CA_ROTOR0_KM -0.05
-
-param set-default CA_ROTOR1_PX 0.0
-param set-default CA_ROTOR1_PY -0.275
-param set-default CA_ROTOR1_KM 0.05
-
-param set-default CA_ROTOR2_PX 0.238
-param set-default CA_ROTOR2_PY -0.1375
-param set-default CA_ROTOR2_KM -0.05
-
-param set-default CA_ROTOR3_PX -0.238
-param set-default CA_ROTOR3_PY 0.1375
-param set-default CA_ROTOR3_KM 0.05
-
-param set-default CA_ROTOR4_PX 0.238
-param set-default CA_ROTOR4_PY 0.1375
-param set-default CA_ROTOR4_KM 0.05
-
-param set-default CA_ROTOR5_PX -0.238
-param set-default CA_ROTOR5_PY -0.1375
-param set-default CA_ROTOR5_KM -0.05
-
-param set-default PWM_MAIN_FUNC1 101
-param set-default PWM_MAIN_FUNC2 102
-param set-default PWM_MAIN_FUNC3 103
-param set-default PWM_MAIN_FUNC4 104
-param set-default PWM_MAIN_FUNC5 105
-param set-default PWM_MAIN_FUNC6 106
-
-set MIXER skip
-set MIXER_AUX none
@@ -23,6 +23,23 @@

 . ${R}etc/init.d/rc.mc_defaults

+param set-default CA_ROTOR_COUNT 6
+param set-default CA_ROTOR0_PX 0.5
+param set-default CA_ROTOR0_PY 0.0
+param set-default CA_ROTOR0_KM -0.05
+param set-default CA_ROTOR1_PX -0.5
+param set-default CA_ROTOR1_PY 0.0
+param set-default CA_ROTOR2_PX -0.25
+param set-default CA_ROTOR2_PY -0.43
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX 0.25
+param set-default CA_ROTOR3_PY 0.43
+param set-default CA_ROTOR4_PX 0.25
+param set-default CA_ROTOR4_PY -0.43
+param set-default CA_ROTOR5_PX -0.25
+param set-default CA_ROTOR5_PY 0.43
+param set-default CA_ROTOR5_KM -0.05
+
 set MIXER hexa_+

 # Need to set all 8 channels
@@ -25,6 +25,28 @@

 . ${R}etc/init.d/rc.mc_defaults

+param set-default CA_ROTOR_COUNT 8
+param set-default CA_ROTOR0_KM -0.05
+param set-default CA_ROTOR0_PX 0.46
+param set-default CA_ROTOR0_PY 0.19
+param set-default CA_ROTOR1_KM -0.05
+param set-default CA_ROTOR1_PX -0.46
+param set-default CA_ROTOR1_PY -0.19
+param set-default CA_ROTOR2_PX 0.19
+param set-default CA_ROTOR2_PY 0.46
+param set-default CA_ROTOR3_PX -0.46
+param set-default CA_ROTOR3_PY 0.19
+param set-default CA_ROTOR4_PX 0.46
+param set-default CA_ROTOR4_PY -0.19
+param set-default CA_ROTOR5_PX -0.19
+param set-default CA_ROTOR5_PY -0.46
+param set-default CA_ROTOR6_KM -0.05
+param set-default CA_ROTOR6_PX 0.19
+param set-default CA_ROTOR6_PY -0.46
+param set-default CA_ROTOR7_KM -0.05
+param set-default CA_ROTOR7_PX -0.19
+param set-default CA_ROTOR7_PY 0.46
+
 set MIXER octo_x

 set PWM_OUT 12345678
@@ -72,7 +72,6 @@ px4_add_romfs_files(
 	4015_holybro_s500
 	4016_holybro_px4vision
 	4017_nxp_hovergames
-	4018_s500_ctrlalloc
 	4019_x500_v2
 	4030_3dr_solo
 	4031_3dr_quad
@@ -100,7 +99,6 @@ px4_add_romfs_files(
 	# [6000, 6999] Hexarotor x"
 	6001_hexa_x
 	6002_draco_r
-	6003_hexa_x_ctrlalloc

 	# [7000, 7999] Hexarotor +"
 	7001_hexa_+
@@ -486,11 +486,11 @@ else
 	. ${R}etc/init.d/rc.thermal_cal

 	#
-	# Start vmount to control mounts such as gimbals, disabled by default.
+	# Start gimbal to control mounts such as gimbals, disabled by default.
 	#
 	if param greater -s MNT_MODE_IN -1
 	then
-		vmount start
+		gimbal start
 	fi

 	# Check for flow sensor
@@ -35,7 +35,6 @@ do
 	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param set MAV_0_CONFIG 101' || true
 	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param set MAV_1_CONFIG 102' || true
 	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param set MAV_2_CONFIG 103' || true
-	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param set MAV_3_CONFIG 104' || true

 	# enable all GPS
 	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param set GPS_1_CONFIG 201' || true
@@ -1,5 +1,35 @@
 #! /bin/bash

+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-09-08"
+	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-08-18"
+	elif [[ $@ =~ .*pilotpi.arm64 ]]; then
+		# scumaker_pilotpi_arm64
+		PX4_DOCKER_REPO="px4io/px4-dev-aarch64:latest"
+	elif [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*bebop.* ]]; then
+		# posix_rpi_cross, posix_bebop_default
+		PX4_DOCKER_REPO="px4io/px4-dev-armhf:2021-08-18"
+	elif [[ $@ =~ .*clang.* ]] || [[ $@ =~ .*scan-build.* ]]; then
+		# clang tools
+		PX4_DOCKER_REPO="px4io/px4-dev-clang:2021-02-04"
+	elif [[ $@ =~ .*tests* ]]; then
+		# run all tests with simulation
+		PX4_DOCKER_REPO="px4io/px4-dev-simulation-bionic:2021-12-11"
+	fi
+else
+	echo "PX4_DOCKER_REPO is set to '$PX4_DOCKER_REPO'";
+fi
+
+# otherwise default to nuttx
+if [ -z ${PX4_DOCKER_REPO+x} ]; then
+	PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2021-09-08"
+fi
+
 # docker hygiene

 #Delete all stopped containers (including data-only containers)
@@ -8,23 +38,30 @@
 #Delete all 'untagged/dangling' (<none>) images
 #docker rmi $(docker images -q -f dangling=true)

-if [[ -z "${DOCKER_TAG}" ]]; then
-  TAG_NAME=""
-else
-  TAG_NAME=":${DOCKER_TAG}"
-fi
-
-PX4_DOCKER_REPO="px4io/px4-dev$TAG_NAME"
-
-echo $PX4_DOCKER_REPO
+echo "PX4_DOCKER_REPO: $PX4_DOCKER_REPO";

 PWD=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )
 SRC_DIR=$PWD/../

+CCACHE_DIR=${HOME}/.ccache
+mkdir -p "${CCACHE_DIR}"
+
 docker run -it --rm -w "${SRC_DIR}" \
-  --env=LOCAL_USER_ID="$(id -u)" \
-  --publish 14556:14556/udp \
-  --volume=/tmp/.X11-unix:/tmp/.X11-unix \
-  --volume=/tmp:/tmp:rw \
-  --volume=${SRC_DIR}:${SRC_DIR}:rw \
-  ${PX4_DOCKER_REPO} /bin/bash -c "$1 $2 $3"
+	--env=AWS_ACCESS_KEY_ID \
+	--env=AWS_SECRET_ACCESS_KEY \
+	--env=BRANCH_NAME \
+	--env=CCACHE_DIR="${CCACHE_DIR}" \
+	--env=CI \
+	--env=CODECOV_TOKEN \
+	--env=COVERALLS_REPO_TOKEN \
+	--env=LOCAL_USER_ID="$(id -u)" \
+	--env=PX4_ASAN \
+	--env=PX4_MSAN \
+	--env=PX4_TSAN \
+	--env=PX4_UBSAN \
+	--env=TRAVIS_BRANCH \
+	--env=TRAVIS_BUILD_ID \
+	--publish 14556:14556/udp \
+	--volume=${CCACHE_DIR}:${CCACHE_DIR}:rw \
+	--volume=${SRC_DIR}:${SRC_DIR}:rw \
+	${PX4_DOCKER_REPO} /bin/bash -c "$1 $2 $3"
@@ -6,14 +6,14 @@ import os
 import sys
 import json
 import re
-# from kconfiglib import Kconfig
+from kconfiglib import Kconfig

-# kconf = Kconfig()
+kconf = Kconfig()

 # Supress warning output
-# kconf.warn_assign_undef = False
-# kconf.warn_assign_override = False
-# kconf.warn_assign_redun = False
+kconf.warn_assign_undef = False
+kconf.warn_assign_override = False
+kconf.warn_assign_redun = False

 source_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)), '..')

@@ -23,10 +23,6 @@ parser.add_argument('-v', '--verbose', dest='verbose', action='store_true',
                    help='Verbose Output')
 parser.add_argument('-p', '--pretty', dest='pretty', action='store_true',
                    help='Pretty output instead of a single line')
-parser.add_argument('-a', '--all_variants', dest='all_variants', action='store_true',
-                    help='Prints targets into a single all_variants_* target')
-parser.add_argument('-b', '--bloaty', dest='bloaty', action='store_true',
-                    help='Includes all bloaty targets')

 args = parser.parse_args()
 verbose = args.verbose
@@ -39,66 +35,48 @@ excluded_labels = [
    'nolockstep', 'replay', 'test',
    'uavcanv1' # TODO: fix and enable
    ]
-bloaty_helpers = [
-    'bloaty_compileunits',
-    'bloaty_inlines',
-    'bloaty_segments',
-    'bloaty_symbols',
-    'bloaty_templates',
-    'bloaty_ram',
-    'bloaty_compare_master',
-    ]

-# def process_target(px4board_file, target_name):
-    # ret = None
-    # platform = None
-    # toolchain = None
+def process_target(px4board_file, target_name):
+    ret = None
+    platform = None
+    toolchain = None

-    # if px4board_file.endswith("default.px4board") or \
-        # px4board_file.endswith("recovery.px4board") or \
-        # px4board_file.endswith("bootloader.px4board"):
-        # kconf.load_config(px4board_file, replace=True)
-    # else: # Merge config with default.px4board
-        # default_kconfig = re.sub(r'[a-zA-Z\d_]+\.px4board', 'default.px4board', px4board_file)
-        # kconf.load_config(default_kconfig, replace=True)
-        # kconf.load_config(px4board_file, replace=False)
+    if px4board_file.endswith("default.px4board") or \
+        px4board_file.endswith("recovery.px4board") or \
+        px4board_file.endswith("bootloader.px4board"):
+        kconf.load_config(px4board_file, replace=True)
+    else: # Merge config with default.px4board
+        default_kconfig = re.sub(r'[a-zA-Z\d_]+\.px4board', 'default.px4board', px4board_file)
+        kconf.load_config(default_kconfig, replace=True)
+        kconf.load_config(px4board_file, replace=False)

-    # if "BOARD_TOOLCHAIN" in kconf.syms:
-        # toolchain = kconf.syms["BOARD_TOOLCHAIN"].str_value
+    if "BOARD_TOOLCHAIN" in kconf.syms:
+        toolchain = kconf.syms["BOARD_TOOLCHAIN"].str_value

-    # if "BOARD_PLATFORM" in kconf.syms:
-        # platform = kconf.syms["BOARD_PLATFORM"].str_value
+    if "BOARD_PLATFORM" in kconf.syms:
+        platform = kconf.syms["BOARD_PLATFORM"].str_value

-    # assert platform, f"PLATFORM not found in {px4board_file}"
+    assert platform, f"PLATFORM not found in {px4board_file}"

-    # if platform not in excluded_platforms:
-        # # get the container based on the platform and toolchain
-        # container = platform
-        # if platform == 'posix':
-            # container = 'base-focal'
-            # if toolchain:
-                # if toolchain.startswith('aarch64'):
-                    # container = 'aarch64'
-                # elif toolchain == 'arm-linux-gnueabihf':
-                    # container = 'armhf'
-                # else:
-                    # if verbose: print(f'possibly unmatched toolchain: {toolchain}')
-        # elif platform == 'nuttx':
-            # container = 'nuttx-focal'
+    if platform not in excluded_platforms:
+        # get the container based on the platform and toolchain
+        container = platform
+        if platform == 'posix':
+            container = 'base-focal'
+            if toolchain:
+                if toolchain.startswith('aarch64'):
+                    container = 'aarch64'
+                elif toolchain == 'arm-linux-gnueabihf':
+                    container = 'armhf'
+                else:
+                    if verbose: print(f'possibly unmatched toolchain: {toolchain}')
+        elif platform == 'nuttx':
+            container = 'nuttx-focal'

-        # ret = {'target': target_name, 'container': container}
+        ret = {'target': target_name, 'container': container}

-    # return ret
+    return ret

-def process_bloaty(target_path, target_name):
-    response = []
-    for bloat in bloaty_helpers:
-        bloaty_name = target_name + ' ' + bloat + ' || true'
-        processed_target = process_target(target_path, bloaty_name)
-        response.append(processed_target)
-    return response
-
-# Look up boards from each manufacturer
 for manufacturer in os.scandir(os.path.join(source_dir, 'boards')):
    if not manufacturer.is_dir():
        continue
@@ -107,37 +85,18 @@ for manufacturer in os.scandir(os.path.join(source_dir, 'boards')):
        continue

    for board in os.scandir(manufacturer.path):
-        # Only boards are directories don't proceed if otherwise
        if not board.is_dir():
            continue
-
-        if args.all_variants:
-            # The all_variants target makes all targets for a board
-            target_name = 'all_variants_' + manufacturer.name + '_' + board.name
-            default_target_path = f'{board.path}/default.px4board'
-            # target = process_target(default_target_path, target_name)
-            target = {'target': target_name}
-            if target is not None:
-                build_configs.append(target)
-            if args.bloaty and target is not None:
-                # bloaty targets
-                bloat_target_name = manufacturer.name + '_' + board.name
-                # bloaty_targets = process_bloaty(default_target_path, bloat_target_name)
-                bloaty_targets = {'target': bloat_target_name}
-                build_configs += bloaty_targets
-        else:
-            # Each board can have multiple variant targets
-            for files in os.scandir(board.path):
-                if files.is_file() and files.name.endswith('.px4board'):
-                    label = files.name[:-9]
-                    target_name = manufacturer.name + '_' + board.name + '_' + label
-                    if label in excluded_labels:
-                        if verbose: print(f'excluding label {label} ({target_name})')
-                        continue
-                    # target = process_target(files.path, target_name)
-                    target = {'target': target_name}
-                    if target is not None:
-                        build_configs.append(target)
+        for files in os.scandir(board.path):
+            if files.is_file() and files.name.endswith('.px4board'):
+                label = files.name[:-9]
+                target_name = manufacturer.name + '_' + board.name + '_' + label
+                if label in excluded_labels:
+                    if verbose: print(f'excluding label {label} ({target_name})')
+                    continue
+                target = process_target(files.path, target_name)
+                if target is not None:
+                    build_configs.append(target)


 github_action_config = { 'include': build_configs }
@@ -150,7 +150,7 @@ simulator,CONFIG_MODULES_SIMULATOR=y
 temperature_compensation,CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 uuv_att_control,CONFIG_MODULES_UUV_ATT_CONTROL=y
 uuv_pos_control,CONFIG_MODULES_UUV_POS_CONTROL=y
-vmount,CONFIG_MODULES_VMOUNT=y
+gimbal,CONFIG_MODULES_GIMBAL=y
 vtol_att_control,CONFIG_MODULES_VTOL_ATT_CONTROL=y
 bl_update,CONFIG_SYSTEMCMDS_BL_UPDATE=y
 dmesg,CONFIG_SYSTEMCMDS_DMESG=y
@@ -1,55 +0,0 @@
-#
-# PX4 base development environment
-#
-
-FROM ubuntu:20.04
-LABEL maintainer="Daniel Agar <daniel@agar.ca>, Ramon Roche <mrpollo@gmail.com>"
-
-ENV DEBIAN_FRONTEND noninteractive
-ENV LANG C.UTF-8
-ENV LC_ALL C.UTF-8
-
-# Installing required utilities
-RUN apt-get update && apt-get -y --quiet --no-install-recommends install \
-  ca-certificates \
-  gnupg \
-  lsb-core \
-  lsb-release \
-  sudo \
-  software-properties-common \
-  wget \
-  gosu \
-  ;
-
-# Install PX4 Requirements
-COPY Tools/setup/requirements.txt /tmp/requirements.txt
-COPY Tools/setup/ubuntu.sh /tmp/ubuntu.sh
-# We support pre-downloading the gcc arm none eabi compiler
-# to speed up build times, if the file is not present when
-# building, the ubuntu.sh script will download it from source
-COPY *gcc-arm-none-eabi-9-2020-q2-update-linux.tar.bz2 /tmp/gcc-arm-none-eabi-9-2020-q2-update-linux.tar.bz2
-# The PATH env is set within ubuntu.sh, but given how we
-# are running the image using `gosu` to avoid read-only problems
-# with the filesystem the env variable does not persist
-ENV PATH="/opt/gcc-arm-none-eabi-9-2020-q2-update/bin:$PATH"
-ENV PATH="/opt/jdk-14.0.2+12/bin:$PATH"
-RUN bash /tmp/ubuntu.sh --from-docker --with-java --with-rtps --no-sim-tools
-
-ENV DISPLAY :99
-
-ENV FASTRTPSGEN_DIR="/usr/local/bin/"
-ENV TERM=xterm
-ENV TZ=UTC
-
-# SITL UDP PORTS
-EXPOSE 14556/udp
-EXPOSE 14557/udp
-
-# create user with id 1001 (jenkins docker workflow default)
-RUN useradd --shell /bin/bash -u 1001 -c "" -m user && usermod -a -G dialout user
-
-# create and start as LOCAL_USER_ID
-COPY Tools/setup/docker-entrypoint.sh /usr/local/bin/entrypoint.sh
-ENTRYPOINT ["/usr/local/bin/entrypoint.sh"]
-
-CMD ["/bin/bash"]
@@ -1,28 +0,0 @@
-#!/bin/bash
-
-# Start virtual X server in the background
-# - DISPLAY default is :99, set in dockerfile
-# - Users can override with `-e DISPLAY=` in `docker run` command to avoid
-#   running Xvfb and attach their screen
-if [[ -x "$(command -v Xvfb)" && "$DISPLAY" == ":99" ]]; then
-	echo "Starting Xvfb"
-	Xvfb :99 -screen 0 1600x1200x24+32 &
-fi
-
-# Check if the ROS_DISTRO is passed and use it
-# to source the ROS environment
-if [ -n "${ROS_DISTRO}" ]; then
-	source "/opt/ros/$ROS_DISTRO/setup.bash"
-fi
-
-# Use the LOCAL_USER_ID if passed in at runtime
-if [ -n "${LOCAL_USER_ID}" ]; then
-	echo "Starting with UID : $LOCAL_USER_ID"
-	# modify existing user's id
-	usermod -u $LOCAL_USER_ID user
-
-	# run as user
-  exec gosu user "$@"
-else
-	exec "$@"
-fi
@@ -2,22 +2,20 @@

 set -e

-## Bash script to setup PX4 development environment on Ubuntu LTS (20.04, 18.04).
+## Bash script to setup PX4 development environment on Ubuntu LTS (20.04, 18.04, 16.04).
 ## Can also be used in docker.
 ##
 ## Installs:
-## - Common dependencies and tools for NuttX, jMAVSim, Gazebo
+## - Common dependencies and tools for nuttx, jMAVSim, Gazebo
 ## - NuttX toolchain (omit with arg: --no-nuttx)
-## - jMAVSim and Gazebo simulator (omit with arg: --no-sim-tools)
-## Optional:
-## - FastRTPS and FastCDR (with args: --with-rtps)
+## - jMAVSim and Gazebo9 simulator (omit with arg: --no-sim-tools)
+##
+## Not Installs:
+## - FastRTPS and FastCDR

 INSTALL_NUTTX="true"
 INSTALL_SIM="true"
 INSTALL_ARCH=`uname -m`
-INSTALL_RTPS="false"
-INSTALL_JAVA="false"
-INSIDE_DOCKER="false"

 # Parse arguments
 for arg in "$@"
@@ -30,20 +28,20 @@ do
 		INSTALL_SIM="false"
 	fi

-	if [[ $arg == "--with-rtps" ]]; then
-		INSTALL_RTPS="true"
-	fi
-
-	if [[ $arg == "--with-java" ]]; then
-		INSTALL_JAVA="true"
-	fi
-
-	if [[ $arg == "--from-docker" ]]; then
-		INSIDE_DOCKER="true"
-	fi
-
 done

+# detect if running in docker
+if [ -f /.dockerenv ]; then
+	echo "Running within docker, installing initial dependencies";
+	apt-get --quiet -y update && DEBIAN_FRONTEND=noninteractive apt-get --quiet -y install \
+		ca-certificates \
+		gnupg \
+		lsb-core \
+		sudo \
+		wget \
+		;
+fi
+
 # script directory
 DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )

@@ -71,119 +69,88 @@ elif [[ "${UBUNTU_RELEASE}" == "20.04" ]]; then
 	echo "Ubuntu 20.04"
 fi

-VERBOSE_BAR="####################"
-echo
-echo $VERBOSE_BAR
-echo "#⚡️ Starting PX4 Dependency Installer for Ubuntu ${UBUNTU_RELEASE} (${INSTALL_ARCH})"
-echo "# Options:
-#
-#  - Install NuttX = ${INSTALL_NUTTX}
-#  - Install Java = ${INSTALL_JAVA}
-#  - Install Simulation = ${INSTALL_SIM}
-#  - Install RTPS = ${INSTALL_RTPS}"
-echo $VERBOSE_BAR
-echo

 echo
-echo $VERBOSE_BAR
-echo "🍻 Installing System Dependencies"
-echo $VERBOSE_BAR
-echo
+echo "Installing PX4 general dependencies"

 sudo apt-get update -y --quiet
 sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
+	astyle \
 	build-essential \
-  g++ \
-  gcc \
-  gdb \
-  astyle \
-  cmake \
-  cppcheck \
-  file \
-  git \
-  lcov \
-  libxml2-dev \
-  libxml2-utils \
-  make \
-  ninja-build \
-  python3 \
-  python3-dev \
-  python3-pip \
-  python3-setuptools \
-  python3-wheel \
-  rsync \
-  shellcheck \
-  unzip \
-  zip \
-  libssl-dev \
-  ;
+	cmake \
+	cppcheck \
+	file \
+	g++ \
+	gcc \
+	gdb \
+	git \
+	lcov \
+	libxml2-dev \
+	libxml2-utils \
+	make \
+	ninja-build \
+	python3 \
+	python3-dev \
+	python3-pip \
+	python3-setuptools \
+	python3-wheel \
+	rsync \
+	shellcheck \
+	unzip \
+	zip \
+	;

-# Python 3 dependencies
+# Python3 dependencies
 echo
-echo $VERBOSE_BAR
-echo "🍻 Installing Python dependencies"
-echo $VERBOSE_BAR
-echo
-
+echo "Installing PX4 Python3 dependencies"
 if [ -n "$VIRTUAL_ENV" ]; then
 	# virtual envrionments don't allow --user option
-  python -m pip install -r ${DIR}/requirements.txt
+	python -m pip install -r ${DIR}/requirements.txt
 else
 	# older versions of Ubuntu require --user option
-  if [[ $INSIDE_DOCKER == "true" ]]; then
-    # when running inside a docker container we don't need to install
-    # under --user since the installer user is root
-    # its best to install packages globaly for any user to find
-    python3 -m pip install -r /tmp/requirements.txt
-  else
-    python3 -m pip install --user -r ${DIR}/requirements.txt
-  fi
+	python3 -m pip install --user -r ${DIR}/requirements.txt
 fi

 # NuttX toolchain (arm-none-eabi-gcc)
 if [[ $INSTALL_NUTTX == "true" ]]; then

 	echo
-  echo $VERBOSE_BAR
-	echo "🍻 Installing NuttX dependencies"
-  echo $VERBOSE_BAR
-	echo
+	echo "Installing NuttX dependencies"

 	sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
-    automake \
-    binutils-dev \
-    bison \
-    flex \
-    genromfs \
-    gettext \
-    gperf \
-    libelf-dev \
-    libexpat-dev \
-    libgmp-dev \
-    libisl-dev \
-    libmpc-dev \
-    libmpfr-dev \
-    libncurses5 \
-    libncurses5-dev \
-    libncursesw5-dev \
-    libtool \
-    pkg-config \
-    screen \
-    texinfo \
-    u-boot-tools \
-    util-linux \
-    vim-common \
-    g++-arm-linux-gnueabihf \
-    gcc-arm-linux-gnueabihf \
-    g++-aarch64-linux-gnu \
-    gcc-aarch64-linux-gnu \
-    ;
-
-  if [[ "${UBUNTU_RELEASE}" == "20.04" ]]; then
-    sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
-    kconfig-frontends \
-    ;
-  fi
+		automake \
+		binutils-dev \
+		bison \
+		build-essential \
+		flex \
+		g++-multilib \
+		gcc-multilib \
+		gdb-multiarch \
+		genromfs \
+		gettext \
+		gperf \
+		libelf-dev \
+		libexpat-dev \
+		libgmp-dev \
+		libisl-dev \
+		libmpc-dev \
+		libmpfr-dev \
+		libncurses5 \
+		libncurses5-dev \
+		libncursesw5-dev \
+		libtool \
+		pkg-config \
+		screen \
+		texinfo \
+		u-boot-tools \
+		util-linux \
+		vim-common \
+		;
+	if [[ "${UBUNTU_RELEASE}" == "20.04" ]]; then
+		sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
+		kconfig-frontends \
+		;
+	fi


 	if [ -n "$USER" ]; then
@@ -191,186 +158,97 @@ if [[ $INSTALL_NUTTX == "true" ]]; then
 		sudo usermod -a -G dialout $USER
 	fi

-  NUTTX_GCC_VERSION="9-2020-q2-update"
-  NUTTX_GCC_VERSION_SHORT="9-2020q2"
-  echo
-  echo $VERBOSE_BAR
-  echo "🍻 Verifying proper gcc version (${NUTTX_GCC_VERSION}), and installing if not found"
-  echo
+	# arm-none-eabi-gcc
+	NUTTX_GCC_VERSION="9-2020-q2-update"
+	NUTTX_GCC_VERSION_SHORT="9-2020q2"

-  source $HOME/.profile # load changed path for the case the script is reran before relogin
+	source $HOME/.profile # load changed path for the case the script is reran before relogin
 	if [ $(which arm-none-eabi-gcc) ]; then
-    GCC_VER_STR=$(arm-none-eabi-gcc --version)
-    GCC_VER_FOUND=$(echo $GCC_VER_STR | grep -c "${NUTTX_GCC_VERSION}")
-  fi
+		GCC_VER_STR=$(arm-none-eabi-gcc --version)
+		GCC_FOUND_VER=$(echo $GCC_VER_STR | grep -c "${NUTTX_GCC_VERSION}")
+	fi

-  if [[ $(echo $GCC_VER_STR | grep -c "${NUTTX_GCC_VERSION}") == "1" ]]; then
-    echo "📌 Skipping installation, the arm cross compiler was found"
-    echo $VERBOSE_BAR
-	  echo
+	if [[ "$GCC_FOUND_VER" == "1" ]]; then
+		echo "arm-none-eabi-gcc-${NUTTX_GCC_VERSION} found, skipping installation"

-  else
-    echo "📌 The arm cross compiler was not found";
-    echo " * Installing arm-none-eabi-gcc-${NUTTX_GCC_VERSION}";
-    # The arm cross compiler hosting provider is known to throttle download speeds
-    # for users who reach a certain limit of downloads in a given time frame
-    # for this reason we allow for using a previously downloaded file
-    # this is specially helpful when debugging this installer script
-    # from within a container COMPILER_PATH="/tmp/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}-linux.tar.bz2"
-    COMPILER_NAME="gcc-arm-none-eabi-${NUTTX_GCC_VERSION}"
-    COMPILER_PATH="/tmp/$COMPILER_NAME-linux.tar.bz2"
-    if [ ! -f "$COMPILER_PATH" ]; then
-      wget -O $COMPILER_PATH https://armkeil.blob.core.windows.net/developer/Files/downloads/gnu-rm/${NUTTX_GCC_VERSION_SHORT}/${COMPILER_NAME}-${INSTALL_ARCH}-linux.tar.bz2
-    fi
-    sudo tar -jxf $COMPILER_PATH -C /opt/;
+	else
+		echo "Installing arm-none-eabi-gcc-${NUTTX_GCC_VERSION}";
+		wget -O /tmp/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}-linux.tar.bz2 https://armkeil.blob.core.windows.net/developer/Files/downloads/gnu-rm/${NUTTX_GCC_VERSION_SHORT}/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}-${INSTALL_ARCH}-linux.tar.bz2 && \
+			sudo tar -jxf /tmp/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}-linux.tar.bz2 -C /opt/;

-    # add arm-none-eabi-gcc to user's PATH
-    exportline="export PATH=\"/opt/${COMPILER_NAME}/bin:\$PATH\""
-    if [[ $INSIDE_DOCKER == "true" ]]; then
-      # when running on a docker container its best to set the environment globally
-      # since we don't know which user is going to be running commands on the container
-      touch /etc/profile.d/px4env.sh
-      echo $exportline >> /etc/profile.d/px4env.sh
-    else
-      if grep -Fxq "$exportline" $HOME/.profile; then
-        echo "${NUTTX_GCC_VERSION} path already set.";
-      else
-        echo $exportline >> $HOME/.profile;
-      fi
-    fi
-    echo " * arm-none-eabi-gcc (${NUTTX_GCC_VERSION}) Installed Succesful to /opt/${COMPILER_NAME}/bin"
-    echo $VERBOSE_BAR
-	  echo
-  fi
-fi
-
-# Install JAVA
-if [[ $INSTALL_JAVA == "true" ]]; then
-  JDK_VERSION="14.0.2_12"
-  echo
-  echo $VERBOSE_BAR
-  echo "🍻 Installing Java JDK
-
-  * Version: $JDK_VERSION
-  * Path: /opt/jdk-14.0.2+12"
-  echo $VERBOSE_BAR
-  echo
-
-  JDK_DOWNLOAD="/tmp/OpenJDK14U-jdk_x64_linux_hotspot_$JDK_VERSION.tar.gz"
-  wget -O $JDK_DOWNLOAD https://github.com/AdoptOpenJDK/openjdk14-binaries/releases/download/jdk-14.0.2%2B12/OpenJDK14U-jdk_x64_linux_hotspot_14.0.2_12.tar.gz
-  sudo tar -xzf $JDK_DOWNLOAD -C /opt/
-  export PATH="/opt/jdk-14.0.2+12/bin:$PATH"
-fi
-
-# Fast-RTPS
-if [[ $INSTALL_RTPS == "true" ]]; then
-  echo
-  echo $VERBOSE_BAR
-  echo "🍻 Installing Fast-RTPS"
-  echo $VERBOSE_BAR
-  echo
-
-  GRADLE_VERSION="6.4.1"
-  wget -O "/tmp/gradle-$GRADLE_VERSION-bin.zip" "https://services.gradle.org/distributions/gradle-$GRADLE_VERSION-bin.zip" \
-    && unzip -d /opt/gradle "/tmp/gradle-$GRADLE_VERSION-bin.zip"
-  export PATH="$PATH:/opt/gradle/gradle-$GRADLE_VERSION/bin"
-
-  # Intall foonathan_memory from source as it is required to Fast-RTPS >= 1.9
-  git clone https://github.com/eProsima/foonathan_memory_vendor.git /tmp/foonathan_memory \
-    && cd /tmp/foonathan_memory \
-    && mkdir build && cd build \
-    && cmake .. \
-    && cmake --build . --target install -- -j $(nproc)
-
-  # Fast-DDS (Fast-RTPS 2.1.1)
-  git clone --recursive https://github.com/eProsima/Fast-DDS.git -b v2.1.1 /tmp/FastRTPS-2.1.1 \
-    && cd /tmp/FastRTPS-2.1.1 \
-    && mkdir build && cd build \
-    && cmake -DTHIRDPARTY=ON -DSECURITY=ON .. \
-    && cmake --build . --target install -- -j $(nproc)
-
-  # Fast-RTPS-Gen 1.0.4
-  git clone --recursive https://github.com/eProsima/Fast-DDS-Gen.git -b v1.0.4 /tmp/Fast-RTPS-Gen-1.0.4 \
-    && cd /tmp/Fast-RTPS-Gen-1.0.4 \
-    && gradle assemble \
-    && gradle install
+		# add arm-none-eabi-gcc to user's PATH
+		exportline="export PATH=/opt/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}/bin:\$PATH"

+		if grep -Fxq "$exportline" $HOME/.profile; then
+			echo "${NUTTX_GCC_VERSION} path already set.";
+		else
+			echo $exportline >> $HOME/.profile;
+		fi
+	fi
 fi

 # Simulation tools
 if [[ $INSTALL_SIM == "true" ]]; then

-  echo
-  echo $VERBOSE_BAR
-  echo "🍻 Installing PX4 Simulation Tools"
-  echo
+	echo
+	echo "Installing PX4 simulation dependencies"

-  if [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then
-    gazebo_version=9
-  elif [[ "${UBUNTU_RELEASE}" == "20.04" ]]; then
-    gazebo_version=11
-  fi
+	# General simulation dependencies
+	sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
+		bc \
+		;

-  echo "  * Gazebo Version $gazebo_version"
-  echo $VERBOSE_BAR
+	if [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then
+		java_version=11
+		gazebo_version=9
+	elif [[ "${UBUNTU_RELEASE}" == "20.04" ]]; then
+		java_version=13
+		gazebo_version=11
+	else
+		java_version=14
+		gazebo_version=11
+	fi
+	# Java (jmavsim or fastrtps)
+	sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
+		ant \
+		openjdk-$java_version-jre \
+		openjdk-$java_version-jdk \
+		libvecmath-java \
+		;

-  # General simulation dependencies
-  sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
-    bc \
-    ant \
-    libvecmath-java \
-    ;
+	# Set Java 11 as default
+	sudo update-alternatives --set java $(update-alternatives --list java | grep "java-$java_version")

-  # Installing Gazebo and dependencies
-  # Setup OSRF Gazebo repository
-  sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
-  wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
-  # Update list, since new gazebo-stable.list has been added
-  sudo apt-get update -y --quiet
-  sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
-    dmidecode \
-    gazebo$gazebo_version \
-    gstreamer1.0-plugins-bad \
-    gstreamer1.0-plugins-base \
-    gstreamer1.0-plugins-good \
-    gstreamer1.0-plugins-ugly \
-    gstreamer1.0-libav \
-    libeigen3-dev \
-    libgazebo$gazebo_version-dev \
-    libgstreamer-plugins-base1.0-dev \
-    libimage-exiftool-perl \
-    libopencv-dev \
-    libxml2-utils \
-    pkg-config \
-    protobuf-compiler \
-    ;
+	# Gazebo
+	sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
+	wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
+	# Update list, since new gazebo-stable.list has been added
+	sudo apt-get update -y --quiet
+	sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
+		dmidecode \
+		gazebo$gazebo_version \
+		gstreamer1.0-plugins-bad \
+		gstreamer1.0-plugins-base \
+		gstreamer1.0-plugins-good \
+		gstreamer1.0-plugins-ugly \
+		gstreamer1.0-libav \
+		libeigen3-dev \
+		libgazebo$gazebo_version-dev \
+		libgstreamer-plugins-base1.0-dev \
+		libimage-exiftool-perl \
+		libopencv-dev \
+		libxml2-utils \
+		pkg-config \
+		protobuf-compiler \
+		;

-  if sudo dmidecode -t system | grep -q "Manufacturer: VMware, Inc." ; then
-    # fix VMWare 3D graphics acceleration for gazebo
-    echo "export SVGA_VGPU10=0" >> ~/.profile
-  fi
+	if sudo dmidecode -t system | grep -q "Manufacturer: VMware, Inc." ; then
+		# fix VMWare 3D graphics acceleration for gazebo
+		echo "export SVGA_VGPU10=0" >> ~/.profile
+	fi
 fi

-if [[ $INSIDE_DOCKER == "true" ]]; then
-  # cleanup installation
-  rm -rf /tmp/
+if [[ $INSTALL_NUTTX == "true" ]]; then
+	echo
+	echo "Relogin or reboot computer before attempting to build NuttX targets"
 fi
-
-if [[ $INSIDE_DOCKER == "false" ]] && [[ $INSTALL_NUTTX == "true" ]]; then
-  echo
-  echo $VERBOSE_BAR
-  echo "💡 We recommend you relogin/reboot before attempting to build NuttX targets"
-  echo $VERBOSE_BAR
-  echo
-fi
-
-echo
-echo
-echo $VERBOSE_BAR
-echo "#⚡️ PX4 Dependency Installer Ended Succesfully
-#
-#  For more information on PX4 Autopilot check out our docs
-#  at docs.px4.io, if you find a bug please file an issue
-#  on GitHub https://github.com/px4/px4-autopilot"
-echo $VERBOSE_BAR
-echo
@@ -74,7 +74,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -31,7 +31,7 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_DMESG=y
@@ -71,7 +71,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -56,7 +56,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_DYN=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
@@ -17,7 +17,6 @@ CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
-CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
@@ -76,7 +76,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -77,7 +77,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -72,7 +72,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -74,7 +74,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -58,7 +58,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_DYN=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
@@ -69,7 +69,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -21,7 +21,6 @@ CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
-CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LOGGER=y
@@ -62,7 +62,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -79,7 +79,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -2,6 +2,7 @@ CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m4"
 CONFIG_BOARD_ROMFSROOT="cannode"
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
+CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2"
 CONFIG_DRIVERS_BAROMETER_DPS310=y
 CONFIG_DRIVERS_BOOTLOADERS=y
 CONFIG_DRIVERS_GPS=y
@@ -15,6 +16,7 @@ CONFIG_SYSTEMCMDS_NSHTERM=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_REBOOT=y
 CONFIG_SYSTEMCMDS_SERIAL_PASSTHRU=y
+CONFIG_SERIAL_PASSTHRU_UBLOX=y
 CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
 CONFIG_SYSTEMCMDS_UORB=y
@@ -3,8 +3,6 @@
 # Matek M9NF4 CAN specific board sensors init
 #------------------------------------------------------------------------------

-gps start -d /dev/ttyS2
-
 icm20602 -s start

 rm3100 -b 2 -s start
@@ -1,23 +0,0 @@
-menu "SERIAL_PASSTHRU Configuration"
-config SERIAL_PASSTHRU_UBLOX
-	bool "Detect and Auto Connect on U-Center messages"
-	default n
-	---help---
-		This option will enable the cdc_acm_check to launch
-		The passthru driver.
-
-config SERIAL_PASSTHRU_UBLOX_DEV
-	string "Device path of the GPS"
-	depends on SERIAL_PASSTHRU_UBLOX
-	default "/dev/ttyS2"
-	---help---
-		This is the path of the device used as the right side
-		of the passthru.
-
-config SERIAL_PASSTHRU_UBLOX_BAUDRATE
-	string "baudrate"
-	depends on SERIAL_PASSTHRU_UBLOX
-	default "115200"
-	---help---
-		This option sets the baudrate for the passthru.
-endmenu
@@ -52,7 +52,7 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_DMESG=y
@@ -1,10 +1,10 @@
 {
-    "board_id": 139,
+    "board_id": 1013,
    "magic": "PX4FWv1",
-    "description": "Firmware for the MatekH743-slim board",
+    "description": "Firmware for the MatekH743 board",
    "image": "",
    "build_time": 0,
-    "summary": "MatekH743-slim",
+    "summary": "MatekH743",
    "version": "0.1",
    "image_size": 0,
    "image_maxsize": 1966080,
@@ -30,11 +30,11 @@ CONFIG_BOARD_RESET_ON_ASSERT=2
 CONFIG_C99_BOOL8=y
 CONFIG_CDCACM=y
 CONFIG_CDCACM_IFLOWCONTROL=y
-CONFIG_CDCACM_PRODUCTID=0x004b
-CONFIG_CDCACM_PRODUCTSTR="Matek H743-slim"
+CONFIG_CDCACM_PRODUCTID=0x1013
+CONFIG_CDCACM_PRODUCTSTR="MatekH743"
 CONFIG_CDCACM_RXBUFSIZE=600
 CONFIG_CDCACM_TXBUFSIZE=12000
-CONFIG_CDCACM_VENDORID=0x3162
+CONFIG_CDCACM_VENDORID=0x1209
 CONFIG_CDCACM_VENDORSTR="Matek"
 CONFIG_CLOCK_MONOTONIC=y
 CONFIG_DEBUG_FULLOPT=y
@@ -46,12 +46,12 @@ CONFIG_BUILTIN=y
 CONFIG_C99_BOOL8=y
 CONFIG_CDCACM=y
 CONFIG_CDCACM_IFLOWCONTROL=y
-CONFIG_CDCACM_PRODUCTID=0x0036
-CONFIG_CDCACM_PRODUCTSTR="MatekH743-slim"
+CONFIG_CDCACM_PRODUCTID=0x1013
+CONFIG_CDCACM_PRODUCTSTR="MatekH743"
 CONFIG_CDCACM_RXBUFSIZE=600
 CONFIG_CDCACM_TXBUFSIZE=12000
-CONFIG_CDCACM_VENDORID=0x1B8C
-CONFIG_CDCACM_VENDORSTR="PX4"
+CONFIG_CDCACM_VENDORID=0x1209
+CONFIG_CDCACM_VENDORSTR="Matek"
 CONFIG_CLOCK_MONOTONIC=y
 CONFIG_DEBUG_FULLOPT=y
 CONFIG_DEBUG_HARDFAULT_ALERT=y
@@ -119,9 +119,9 @@

 /* Spare GPIO */

-// #define GPIO_PG6                        /* PG6  */  (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN6)
-// #define GPIO_PD15                       /* PD15 */  (GPIO_INPUT|GPIO_FLOAT|GPIO_PORTD|GPIO_PIN15)
-// #define GPIO_PG15                       /* PG15 */  (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN15)
+#define GPIO_VIDEO_PWR          /* PD10 */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN10)
+#define GPIO_VIDEO_CAM          /* PD11 */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN11)
+// #define GPIO_PG15               /* PG15 */  (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN15)


 /* Tone alarm output */
@@ -174,6 +174,8 @@
 		GPIO_nLED_BLUE, \
 		GPIO_nLED_GREEN, \
 		GPIO_TONE_ALARM_IDLE, \
+		GPIO_VIDEO_PWR, \
+		GPIO_VIDEO_CAM, \
 	}

 #define BOARD_ENABLE_CONSOLE_BUFFER
@@ -96,7 +96,7 @@
 #define INTERFACE_USART                1
 #define INTERFACE_USART_CONFIG         "/dev/ttyS0,57600"
 #define BOOT_DELAY_ADDRESS             0x000001a0
-#define BOARD_TYPE                     139
+#define BOARD_TYPE                     1013
 #define _FLASH_KBYTES                  (*(uint32_t *)0x1FF1E880)
 #define BOARD_FLASH_SECTORS            (15)
 #define BOARD_FLASH_SIZE               (_FLASH_KBYTES * 1024)
@@ -60,6 +60,10 @@
 #include <px4_platform/gpio.h>
 #include <px4_platform/board_dma_alloc.h>

+// # if defined(FLASH_BASED_PARAMS)
+// #  include <parameters/flashparams/flashfs.h>
+// #endif
+
 __BEGIN_DECLS
 extern void led_init(void);
 extern void led_on(int led);
@@ -174,6 +178,25 @@ __EXPORT int board_app_initialize(uintptr_t arg)

 #endif

+
+// #if defined(FLASH_BASED_PARAMS)
+// 	static sector_descriptor_t params_sector_map[] = {
+// 		{6, 128 * 1024, 0x081C0000},
+// 		{7, 128 * 1024, 0x081E0000},
+// 		{0, 0, 0},
+// 	};
+
+// 	/* Initialize the flashfs layer to use heap allocated memory */
+// 	int result = parameter_flashfs_init(params_sector_map, NULL, 0);
+
+// 	if (result != OK) {
+// 		syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result);
+// 		led_on(LED_BLUE);
+// 		return -ENODEV;
+// 	}
+
+// #endif
+
 	/* Configure the HW based on the manifest */
 	px4_platform_configure();

@@ -74,7 +74,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -70,7 +70,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -72,7 +72,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -72,7 +72,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -71,7 +71,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
@@ -72,7 +72,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
@@ -70,7 +70,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -73,7 +73,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -74,7 +74,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -72,7 +72,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
@@ -76,7 +76,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
@@ -48,7 +48,7 @@ CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
@@ -77,7 +77,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -78,7 +78,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -32,5 +32,5 @@ then
 	mpu6500 -s -R 0 start
 else
 	# mpu9250 internal SPI bus mpu9250
-	mpu9250 -s -R 8 start
+	mpu9250 -s -R 8 -M start
 fi
@@ -75,7 +75,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -32,7 +32,7 @@ CONFIG_MODULES_SIH=n
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=n
 CONFIG_MODULES_UUV_ATT_CONTROL=n
 CONFIG_MODULES_UUV_POS_CONTROL=n
-CONFIG_MODULES_VMOUNT=n
+CONFIG_MODULES_GIMBAL=n
 CONFIG_SYSTEMCMDS_SERIAL_TEST=n
 CONFIG_BOARD_CONSTRAINED_FLASH=y
 CONFIG_BOARD_TESTING=y
@@ -80,7 +80,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
@@ -28,6 +28,6 @@ CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_MODULES_ROVER_POS_CONTROL=n
 CONFIG_MODULES_SIH=n
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=n
-CONFIG_MODULES_VMOUNT=n
+CONFIG_MODULES_GIMBAL=n
 CONFIG_BOARD_CONSTRAINED_FLASH=y
 CONFIG_BOARD_TESTING=y
@@ -80,7 +80,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
@@ -72,7 +72,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -68,7 +68,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -55,7 +55,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_DYN=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
@@ -43,7 +43,7 @@ CONFIG_MODULES_SIMULATOR=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_DYN=y
@@ -53,7 +53,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_DYN=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
@@ -39,7 +39,7 @@ CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
@@ -59,7 +59,7 @@ CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_DMESG=y
@@ -11,6 +11,7 @@ set(MENUCONFIG_PATH ${PYTHON_EXECUTABLE} -m menuconfig CACHE INTERNAL "menuconfi
 set(GUICONFIG_PATH ${PYTHON_EXECUTABLE} -m guiconfig CACHE INTERNAL "guiconfig program" FORCE)
 set(DEFCONFIG_PATH ${PYTHON_EXECUTABLE} -m defconfig CACHE INTERNAL "defconfig program" FORCE)
 set(SAVEDEFCONFIG_PATH ${PYTHON_EXECUTABLE} -m savedefconfig CACHE INTERNAL "savedefconfig program" FORCE)
+set(GENCONFIG_PATH ${PYTHON_EXECUTABLE} -m genconfig CACHE INTERNAL "genconfig program" FORCE)

 set(COMMON_KCONFIG_ENV_SETTINGS
 	PYTHON_EXECUTABLE=${PYTHON_EXECUTABLE}
@@ -47,6 +48,11 @@ if(EXISTS ${BOARD_DEFCONFIG})
                        OUTPUT_VARIABLE DUMMY_RESULTS)
    endif()

+    # Generate header file for C/C++ preprocessor
+    execute_process(COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS}
+                    ${GENCONFIG_PATH} --header-path ${PX4_BINARY_DIR}/px4_boardconfig.h
+                    WORKING_DIRECTORY ${PX4_SOURCE_DIR}
+                    OUTPUT_VARIABLE DUMMY_RESULTS)

    # parse board config options for cmake
    file(STRINGS ${BOARD_CONFIG} ConfigContents)
@@ -16,3 +16,5 @@ float32 angular_velocity_y
 float32 angular_velocity_z

 uint32 failure_flags
+
+bool received_from_mavlink
@@ -175,3 +175,5 @@ uint8 source_system		# System sending the command
 uint8 source_component		# Component sending the command
 uint8 confirmation		# 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)
 bool from_external
+
+# TOPICS vehicle_command gimbal_v1_command
@@ -52,3 +52,6 @@
 #include <board_config.h>

 #endif
+
+/* PX4 board kconfig symbols */
+#include <px4_boardconfig.h>
@@ -84,7 +84,7 @@ uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t inst

 uORB::DeviceNode::~DeviceNode()
 {
-	delete[] _data;
+	free(_data);

 	const char *devname = get_devname();

@@ -186,7 +186,9 @@ uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen)

 			/* re-check size */
 			if (nullptr == _data) {
-				_data = new uint8_t[_meta->o_size * _queue_size];
+				const size_t data_size = _meta->o_size * _queue_size;
+				_data = (uint8_t *) px4_cache_aligned_alloc(data_size);
+				memset(_data, 0, data_size);
 			}

 			unlock();
@@ -50,6 +50,24 @@ __END_DECLS

 #define USB_DEVICE_PATH "/dev/ttyACM0"

+#if defined(CONFIG_SERIAL_PASSTHRU_UBLOX)
+#  undef SERIAL_PASSTHRU_UBLOX_DEV
+#  if defined(CONFIG_SERIAL_PASSTHRU_GPS1) && defined(CONFIG_BOARD_SERIAL_GPS1)
+#    define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS1
+#  elif defined(CONFIG_SERIAL_PASSTHRU_GPS2)&& defined(CONFIG_BOARD_SERIAL_GPS2)
+#    define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS2
+#  elif defined(CONFIG_SERIAL_PASSTHRU_GPS3)&& defined(CONFIG_BOARD_SERIAL_GPS3)
+#    define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS3
+#  elif defined(CONFIG_SERIAL_PASSTHRU_GPS4)&& defined(CONFIG_BOARD_SERIAL_GPS4)
+#    define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS4
+#  elif defined(CONFIG_SERIAL_PASSTHRU_GPS5) && defined(CONFIG_BOARD_SERIAL_GPS5)
+#    define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS5
+#  endif
+#  if !defined(SERIAL_PASSTHRU_UBLOX_DEV)
+#    error "CONFIG_SERIAL_PASSTHRU_GPSn and CONFIG_BOARD_SERIAL_GPSn must be defined"
+#  endif
+#endif
+
 static struct work_s usb_serial_work;
 static bool vbus_present_prev = false;
 static int ttyacm_fd = -1;
@@ -204,7 +222,7 @@ static void mavlink_usb_check(void *arg)
 								snprintf(baudstring, sizeof(baudstring), "%d", baudrate);
 								static const char *gps_argv[] {"gps", "stop", nullptr};

-								static const char *passthru_argv[] {"serial_passthru", "start", "-t", "-b", baudstring, "-e", USB_DEVICE_PATH, "-d", CONFIG_SERIAL_PASSTHRU_UBLOX_DEV,   nullptr};
+								static const char *passthru_argv[] {"serial_passthru", "start", "-t", "-b", baudstring, "-e", USB_DEVICE_PATH, "-d", SERIAL_PASSTHRU_UBLOX_DEV,   nullptr};
 #endif
 								char **exec_argv = nullptr;

@@ -241,7 +241,7 @@ typedef enum {
 	DShot_cmd_led4_off,     // BLHeli32 only
 	DShot_cmd_audio_stream_mode_on_off              = 30, // KISS audio Stream mode on/off
 	DShot_cmd_silent_mode_on_off                    = 31, // KISS silent Mode on/off
-	DShot_cmd_signal_line_telemeetry_disable        = 32,
+	DShot_cmd_signal_line_telemetry_disable         = 32,
 	DShot_cmd_signal_line_continuous_erpm_telemetry = 33,
 	DShot_cmd_MAX          = 47,     // >47 are throttle values
 	DShot_cmd_MIN_throttle = 48,
@@ -101,10 +101,10 @@ PARAM_DEFINE_INT32(GPS_UBX_MODE, 0);
 *
 * Heading offset angle for dual antenna GPS setups that support heading estimation.
 *
- * Set this to 0 if the antennas are parallel to the forward-facing direction of the vehicle and the first antenna is in
+ * Set this to 0 if the antennas are parallel to the forward-facing direction of the vehicle and the rover antenna is in
 * front. The offset angle increases clockwise.
 *
- * Set this to 90 if the first antenna is placed on the right side and the second on the left side of the vehicle.
+ * Set this to 90 if the rover antenna is placed on the right side of the vehicle and the moving base antenna is on the left side.
 *
 * @min 0
 * @max 360
@@ -436,8 +436,8 @@ uint8_t MPU6000::RegisterRead(Register reg)
 	uint8_t cmd[2] {};
 	cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
 	set_frequency(SPI_SPEED); // low speed for regular registers
-	transfer(cmd, cmd, sizeof(cmd));
 	px4_udelay(10);
+	transfer(cmd, cmd, sizeof(cmd));
 	return cmd[1];
 }

@@ -445,8 +445,8 @@ void MPU6000::RegisterWrite(Register reg, uint8_t value)
 {
 	uint8_t cmd[2] { (uint8_t)reg, value };
 	set_frequency(SPI_SPEED); // low speed for regular registers
-	transfer(cmd, cmd, sizeof(cmd));
 	px4_udelay(10);
+	transfer(cmd, cmd, sizeof(cmd));
 }

 void MPU6000::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
@@ -108,15 +108,18 @@ float BMM150::compensate_x(int16_t mag_data_x, uint16_t data_rhall)
 {
 	float retval = 0;

-	// Processing compensation equations
-	//  not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1624-L1633 as of 2020-09-25
-	float process_comp_x0 = (((float)_trim_data.dig_xyz1) * 16384.f / data_rhall);
-	retval = (process_comp_x0 - 16384.f);
-	float process_comp_x1 = ((float)_trim_data.dig_xy2) * (retval * retval / 268435456.f);
-	float process_comp_x2 = process_comp_x1 + retval * ((float)_trim_data.dig_xy1) / 16384.f;
-	float process_comp_x3 = ((float)_trim_data.dig_x2) + 160.f;
-	float process_comp_x4 = mag_data_x * ((process_comp_x2 + 256.f) * process_comp_x3);
-	retval = ((process_comp_x4 / 8192.f) + (((float)_trim_data.dig_x1) * 8.f)) / 16.f;
+	// Overflow condition check
+	if ((mag_data_x != OVERFLOW_XYAXES) && (data_rhall != 0) && (_trim_data.dig_xyz1 != 0)) {
+		// Processing compensation equations
+		//  not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1624-L1633 as of 2020-09-25
+		float process_comp_x0 = (((float)_trim_data.dig_xyz1) * 16384.0f / data_rhall);
+		retval = (process_comp_x0 - 16384.0f);
+		float process_comp_x1 = ((float)_trim_data.dig_xy2) * (retval * retval / 268435456.0f);
+		float process_comp_x2 = process_comp_x1 + retval * ((float)_trim_data.dig_xy1) / 16384.0f;
+		float process_comp_x3 = ((float)_trim_data.dig_x2) + 160.0f;
+		float process_comp_x4 = mag_data_x * ((process_comp_x2 + 256.0f) * process_comp_x3);
+		retval = ((process_comp_x4 / 8192.0f) + (((float)_trim_data.dig_x1) * 8.0f)) / 16.0f;
+	}

 	return retval;
 }
@@ -125,15 +128,17 @@ float BMM150::compensate_y(int16_t mag_data_y, uint16_t data_rhall)
 {
 	float retval = 0;

-	// Processing compensation equations
-	//  not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1660-L1667 as of 2020-09-25
-	float process_comp_y0 = ((float)_trim_data.dig_xyz1) * 16384.f / data_rhall;
-	retval = process_comp_y0 - 16384.f;
-	float process_comp_y1 = ((float)_trim_data.dig_xy2) * (retval * retval / 268435456.f);
-	float process_comp_y2 = process_comp_y1 + retval * ((float)_trim_data.dig_xy1) / 16384.f;
-	float process_comp_y3 = ((float)_trim_data.dig_y2) + 160.0f;
-	float process_comp_y4 = mag_data_y * (((process_comp_y2) + 256.f) * process_comp_y3);
-	retval = ((process_comp_y4 / 8192.f) + (((float)_trim_data.dig_y1) * 8.f)) / 16.f;
+	// Overflow condition check
+	if ((mag_data_y != OVERFLOW_XYAXES) && (data_rhall != 0) && (_trim_data.dig_xyz1 != 0)) {
+		// Processing compensation equations
+		float process_comp_y0 = ((float)_trim_data.dig_xyz1) * 16384.0f / data_rhall;
+		retval = process_comp_y0 - 16384.0f;
+		float process_comp_y1 = ((float)_trim_data.dig_xy2) * (retval * retval / 268435456.0f);
+		float process_comp_y2 = process_comp_y1 + retval * ((float)_trim_data.dig_xy1) / 16384.0f;
+		float process_comp_y3 = ((float)_trim_data.dig_y2) + 160.0f;
+		float process_comp_y4 = mag_data_y * (((process_comp_y2) + 256.0f) * process_comp_y3);
+		retval = ((process_comp_y4 / 8192.0f) + (((float)_trim_data.dig_y1) * 8.0f)) / 16.0f;
+	}

 	return retval;
 }
@@ -142,35 +147,52 @@ float BMM150::compensate_z(int16_t mag_data_z, uint16_t data_rhall)
 {
 	float retval = 0;

-	// Processing compensation equations
-	//  not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1696-L1703 as of 2020-09-25
-	float process_comp_z0 = ((float)mag_data_z) - ((float)_trim_data.dig_z4);
-	float process_comp_z1 = ((float)data_rhall) - ((float)_trim_data.dig_xyz1);
-	float process_comp_z2 = (((float)_trim_data.dig_z3) * process_comp_z1);
-	float process_comp_z3 = ((float)_trim_data.dig_z1) * ((float)data_rhall) / 32768.f;
-	float process_comp_z4 = ((float)_trim_data.dig_z2) + process_comp_z3;
-	float process_comp_z5 = (process_comp_z0 * 131072.f) - process_comp_z2;
-	retval = (process_comp_z5 / ((process_comp_z4) * 4.f)) / 16.f;
+	// Overflow condition check
+	if ((mag_data_z != OVERFLOW_ZAXIS)
+	    && (_trim_data.dig_z2 != 0) && (_trim_data.dig_z1 != 0) && (_trim_data.dig_xyz1 != 0)
+	    && (data_rhall != 0)) {
+		// Processing compensation equations
+		//  not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1696-L1703 as of 2020-09-25
+		float process_comp_z0 = ((float)mag_data_z) - ((float)_trim_data.dig_z4);
+		float process_comp_z1 = ((float)data_rhall) - ((float)_trim_data.dig_xyz1);
+		float process_comp_z2 = (((float)_trim_data.dig_z3) * process_comp_z1);
+		float process_comp_z3 = ((float)_trim_data.dig_z1) * ((float)data_rhall) / 32768.0f;
+		float process_comp_z4 = ((float)_trim_data.dig_z2) + process_comp_z3;
+		float process_comp_z5 = (process_comp_z0 * 131072.0f) - process_comp_z2;
+		retval = (process_comp_z5 / ((process_comp_z4) * 4.0f)) / 16.0f;
+	}

 	return retval;
 }

 static constexpr int16_t combine_xy_int13(const uint8_t msb, const uint8_t lsb)
 {
-	int16_t x = ((msb << 8) | lsb);
-	return x / 8; // arithmetic shift by 3 (13 bit signed integer)
+	// msb: 8-bit MSB part [12:5] of the 13 bit output data
+	// lsb: 5-bit LSB part [4:0] of the 13 bit output data
+	int16_t msb_data = ((int16_t)((int8_t)msb)) << 5;
+	int16_t lsb_data = ((lsb & 0xF8) >> 3);
+
+	return (int16_t)(msb_data | lsb_data);
 }

 static constexpr int16_t combine_z_int15(const uint8_t msb, const uint8_t lsb)
 {
-	int16_t z = ((msb << 8) | lsb);
-	return z / 2; // arithmetic shift by 1 (15 bit signed integer)
+	// msb: 8-bit MSB part [12:5] of the 13 bit output data
+	// lsb: 7-bit LSB part [6:0] of the 15 bit output data
+	int16_t msb_data = ((int16_t)((int8_t)msb)) << 7;
+	int16_t lsb_data = ((lsb & 0xFE) >> 1);
+
+	return (int16_t)(msb_data | lsb_data);
 }

 static constexpr uint16_t combine_rhall_uint14(const uint8_t msb, const uint8_t lsb)
 {
-	uint16_t rhall = ((msb << 8) | lsb);
-	return (rhall >> 2) & 0x3FFF; // 14 bit unsigned integer
+	// msb: 8-bit MSB part [13:6] of the 14 bit output data
+	// lsb: 6-bit LSB part [5:0] of the 14 bit output data
+	uint16_t msb_data = ((uint16_t)((uint16_t)msb)) << 6;
+	uint16_t lsb_data = ((lsb & 0xFC) >> 2);
+
+	return (uint16_t)(msb_data | lsb_data);
 }

 void BMM150::RunImpl()
@@ -128,7 +128,7 @@ private:
 	register_config_t _register_cfg[size_register_cfg] {
 		// Register                | Set bits, Clear bits
 		{ Register::POWER_CONTROL, POWER_CONTROL_BIT::PowerControl, POWER_CONTROL_BIT::SoftReset },
-		{ Register::OP_MODE,       OP_MODE_BIT::ODR_20Hz, OP_MODE_BIT::Opmode_Sleep | OP_MODE_BIT::Self_Test },
+		{ Register::OP_MODE,       OP_MODE_BIT::ODR_20HZ_SET, OP_MODE_BIT::ODR_20HZ_CLEAR | OP_MODE_BIT::Opmode_Sleep | OP_MODE_BIT::Self_Test },
 		{ Register::REPXY,         REPXY_BIT::XY_HA_SET, REPXY_BIT::XY_HA_CLEAR },
 		{ Register::REPZ,          REPZ_BIT::Z_HA_SET, REPZ_BIT::Z_HA_CLEAR },
 	};
@@ -96,11 +96,12 @@ enum POWER_CONTROL_BIT : uint8_t {
 // OP_MODE
 enum OP_MODE_BIT : uint8_t {
 	// 5:3 Data rate control
-	ODR_20Hz     = Bit5 | Bit3, // ODR 20 Hz
+	ODR_20HZ_SET   = Bit5 | Bit3,
+	ODR_20HZ_CLEAR = Bit4,

 	// 2:1 Operation mode control
-	Opmode_Sleep = Bit2 | Bit1, // Sleep mode
-	Self_Test    = Bit0,
+	Opmode_Sleep   = Bit2 | Bit1, // Sleep mode
+	Self_Test      = Bit0,
 };

 // STATUS
@@ -548,7 +548,7 @@ void DevCommon::scan_for_packets()

 		perf_set_count(header_bytes_received_count, _read_buffer->header_bytes_received);

-		i += payload_len;
+		i += Sp2HeaderSize + payload_len;

 	}
 }
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2012-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2022 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
@@ -33,9 +33,9 @@

 #include "RCInput.hpp"

-#include "crsf_telemetry.h"
 #include <uORB/topics/vehicle_command_ack.h>

+#include <poll.h>
 #include <termios.h>

 using namespace time_literals;
@@ -44,21 +44,8 @@ constexpr char const *RCInput::RC_SCAN_STRING[];

 RCInput::RCInput(const char *device) :
 	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device)),
-	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")),
-	_publish_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval"))
+	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device))
 {
-	// rc input, published to ORB
-	_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
-
-	// initialize it as RC lost
-	_rc_in.rc_lost = true;
-
-	// initialize raw_rc values and count
-	for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
-		_raw_rc_values[i] = UINT16_MAX;
-	}
-
 	if (device) {
 		strncpy(_device, device, sizeof(_device) - 1);
 		_device[sizeof(_device) - 1] = '\0';
@@ -77,6 +64,7 @@ RCInput::~RCInput()
 	delete _ghst_telemetry;

 	perf_free(_cycle_perf);
+	perf_free(_cycle_interval_perf);
 	perf_free(_publish_interval_perf);
 }

@@ -88,6 +76,11 @@ RCInput::init()
 	RF_RADIO_POWER_CONTROL(true);
 #endif // RF_RADIO_POWER_CONTROL

+
+#if defined(RC_SERIAL_PORT)
+	_rc_serial_port_output = (strcmp(_device, RC_SERIAL_PORT) != 0);
+#endif // RC_SERIAL_PORT
+
 	// dsm_init sets some file static variables and returns a file descriptor
 	// it also powers on the radio if needed
 	_rcs_fd = dsm_init(_device);
@@ -164,89 +157,60 @@ RCInput::task_spawn(int argc, char *argv[])
 	_object.store(instance);
 	_task_id = task_id_is_work_queue;

-	instance->ScheduleOnInterval(_current_update_interval);
+	instance->ScheduleOnInterval(_backup_update_interval);

 	return PX4_OK;
 }

-void
-RCInput::fill_rc_in(uint16_t raw_rc_count_local,
-		    uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
-		    hrt_abstime now, bool frame_drop, bool failsafe,
-		    unsigned frame_drops, int rssi = -1)
+void RCInput::FillRssi(input_rc_s &input_rc)
 {
-	// fill rc_in struct for publishing
-	_rc_in.channel_count = raw_rc_count_local;
-
-	if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
-		_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
-	}
-
-	unsigned valid_chans = 0;
-
-	for (unsigned i = 0; i < _rc_in.channel_count; i++) {
-		_rc_in.values[i] = raw_rc_values_local[i];
-
-		if (raw_rc_values_local[i] != UINT16_MAX) {
-			valid_chans++;
-		}
-
-		// once filled, reset values back to default
-		_raw_rc_values[i] = UINT16_MAX;
-	}
-
-	_rc_in.timestamp = now;
-	_rc_in.timestamp_last_signal = _rc_in.timestamp;
-	_rc_in.rc_ppm_frame_length = 0;
-
-	/* fake rssi if no value was provided */
-	if (rssi == -1) {
-		if ((_param_rc_rssi_pwm_chan.get() > 0) && (_param_rc_rssi_pwm_chan.get() < _rc_in.channel_count)) {
+	if (input_rc.rssi < 0 || input_rc.rssi > input_rc_s::RSSI_MAX) {
+		if ((_param_rc_rssi_pwm_chan.get() > 0) && (_param_rc_rssi_pwm_chan.get() < input_rc.channel_count)) {
 			const int32_t rssi_pwm_chan = _param_rc_rssi_pwm_chan.get();
 			const int32_t rssi_pwm_min = _param_rc_rssi_pwm_min.get();
 			const int32_t rssi_pwm_max = _param_rc_rssi_pwm_max.get();

 			// get RSSI from input channel
-			int rc_rssi = ((_rc_in.values[rssi_pwm_chan - 1] - rssi_pwm_min) * 100) / (rssi_pwm_max - rssi_pwm_min);
-			_rc_in.rssi = math::constrain(rc_rssi, 0, 100);
+			int rc_rssi = ((input_rc.values[rssi_pwm_chan - 1] - rssi_pwm_min) * 100) / (rssi_pwm_max - rssi_pwm_min);
+
+			input_rc.rssi = math::constrain(rc_rssi, 0, (int)input_rc_s::RSSI_MAX);
+
+#if defined(ADC_RC_RSSI_CHANNEL)

 		} else if (_analog_rc_rssi_stable) {
 			// set RSSI if analog RSSI input is present
 			float rssi_analog = ((_analog_rc_rssi_volt - 0.2f) / 3.0f) * 100.0f;

-			if (rssi_analog > 100.0f) {
-				rssi_analog = 100.0f;
-			}
-
-			if (rssi_analog < 0.0f) {
-				rssi_analog = 0.0f;
-			}
-
-			_rc_in.rssi = rssi_analog;
-
-		} else {
-			_rc_in.rssi = 255;
+			input_rc.rssi = math::constrain((int)roundf(rssi_analog), 0, (int)input_rc_s::RSSI_MAX);
+#endif // ADC_RC_RSSI_CHANNEL
 		}
-
-	} else {
-		_rc_in.rssi = rssi;
 	}
-
-	if (valid_chans == 0) {
-		_rc_in.rssi = 0;
-	}
-
-	_rc_in.rc_failsafe = failsafe;
-	_rc_in.rc_lost = (valid_chans == 0);
-	_rc_in.rc_lost_frame_count = frame_drops;
-	_rc_in.rc_total_frame_count = 0;
 }

-void RCInput::set_rc_scan_state(RC_SCAN newState)
+void RCInput::PublishInputRc(input_rc_s &input_rc)
 {
-	PX4_DEBUG("RCscan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[newState]);
+	FillRssi(input_rc); // requires input_rc.values[]
+
+	input_rc.timestamp = hrt_absolute_time();
+	_input_rc_pub.publish(input_rc);
+
+	perf_count(_publish_interval_perf);
+
+	_last_publish_time = input_rc.timestamp;
+	_rc_scan_locked = true;
+}
+
+void RCInput::set_next_rc_scan_state()
+{
+	int new_state = _rc_scan_state + 1;
+
+	if (new_state >= RC_SCAN::RC_SCAN_MAX) {
+		new_state = 0;
+	}
+
+	PX4_DEBUG("RC scan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[new_state]);
 	_rc_scan_begin = 0;
-	_rc_scan_state = newState;
+	_rc_scan_state = static_cast<RC_SCAN>(new_state);
 	_rc_scan_locked = false;

 	_report_lock = true;
@@ -288,8 +252,6 @@ void RCInput::Run()

 	} else {

-		perf_begin(_cycle_perf);
-
 		// Check if parameters have changed
 		if (_parameter_update_sub.updated()) {
 			// clear update
@@ -307,13 +269,10 @@ void RCInput::Run()
 			}
 		}

-		const hrt_abstime cycle_timestamp = hrt_absolute_time();
-
-
 		/* vehicle command */
 		vehicle_command_s vcmd;

-		if (_vehicle_cmd_sub.update(&vcmd)) {
+		while (_vehicle_cmd_sub.update(&vcmd)) {
 			// Check for a pairing command
 			if (vcmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) {

@@ -370,9 +329,8 @@ void RCInput::Run()
 			if (_adc_report_sub.copy(&adc)) {
 				for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) {
 					if (adc.channel_id[i] == ADC_RC_RSSI_CHANNEL) {
-						float adc_volt = adc.raw_data[i] *
-								 adc.v_ref /
-								 adc.resolution;
+
+						float adc_volt = adc.raw_data[i] * adc.v_ref / adc.resolution;

 						if (_analog_rc_rssi_volt < 0.0f) {
 							_analog_rc_rssi_volt = adc_volt;
@@ -391,18 +349,25 @@ void RCInput::Run()

 #endif // ADC_RC_RSSI_CHANNEL

-		bool rc_updated = false;

 		// This block scans for a supported serial RC input and locks onto the first one found
 		// Scan for 300 msec, then switch protocol
-		constexpr hrt_abstime rc_scan_max = 300_ms;
+		static constexpr hrt_abstime rc_scan_max = 300_ms;

-		unsigned frame_drops = 0;
+		// poll with 1 second timeout
+		pollfd fds[1];
+		fds[0].fd = _rcs_fd;
+		fds[0].events = POLLIN;
+		int ret = poll(fds, 1, 1000);

-		// TODO: needs work (poll _rcs_fd)
-		// int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 100);
-		// then update priority to SCHED_PRIORITY_FAST_DRIVER
-		// read all available data from the serial RC input UART
+		perf_begin(_cycle_perf);
+		perf_count(_cycle_interval_perf);
+
+		if (ret < 0) {
+			PX4_DEBUG("poll error %d", ret);
+		}
+
+		const hrt_abstime cycle_timestamp = hrt_absolute_time();

 		// read all available data from the serial RC input UART
 		int newBytes = ::read(_rcs_fd, &_rcs_buf[0], RC_MAX_BUFFER_SIZE);
@@ -411,7 +376,48 @@ void RCInput::Run()
 			_bytes_rx += newBytes;
 		}

+		bool rc_updated = false;
+
 		switch (_rc_scan_state) {
+#if defined(HRT_PPM_CHANNEL)
+
+		case RC_SCAN_PPM:
+			if (_rc_scan_begin == 0) {
+				_rc_scan_begin = cycle_timestamp;
+				// Configure timer input pin for CPPM
+				px4_arch_configgpio(GPIO_PPM_IN);
+
+			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
+				// see if we have new PPM input data
+				if ((ppm_last_valid_decode != _last_publish_time) && (ppm_decoded_channels >= 4)) {
+					// we have a new PPM frame. Publish it.
+					rc_updated = true;
+
+					input_rc_s input_rc{};
+					input_rc.timestamp_last_signal = ppm_last_valid_decode;
+					input_rc.channel_count = math::max(ppm_decoded_channels, (unsigned)input_rc_s::RC_INPUT_MAX_CHANNELS);
+					input_rc.rc_lost = (ppm_decoded_channels == 0);
+					input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
+					input_rc.rc_ppm_frame_length = ppm_frame_length;
+
+					for (int i = 0; i < input_rc.channel_count; i++) {
+						input_rc.values[i] = ppm_buffer[i];
+					}
+
+					PublishInputRc(input_rc);
+				}
+
+			} else {
+				// disable CPPM input by mapping it away from the timer capture input
+				px4_arch_unconfiggpio(GPIO_PPM_IN);
+
+				// Scan the next protocol
+				set_next_rc_scan_state();
+			}
+
+			break;
+#endif  // HRT_PPM_CHANNEL
+
 		case RC_SCAN_SBUS:
 			if (_rc_scan_begin == 0) {
 				_rc_scan_begin = cycle_timestamp;
@@ -423,30 +429,40 @@ void RCInput::Run()
 				tcflush(_rcs_fd, TCIOFLUSH);
 				memset(_rcs_buf, 0, sizeof(_rcs_buf));

-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
-				// parse new data
+			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
 				if (newBytes > 0) {
+					// parse new data
+					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
+					uint16_t raw_rc_count = 0;
 					bool sbus_failsafe = false;
 					bool sbus_frame_drop = false;
+					unsigned frame_drops = 0;

-					rc_updated = sbus_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count, &sbus_failsafe,
+					rc_updated = sbus_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count, &sbus_failsafe,
 								&sbus_frame_drop, &frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS);

 					if (rc_updated) {
 						// we have a new SBUS frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
-							   sbus_frame_drop, sbus_failsafe, frame_drops);
-						_rc_scan_locked = true;
+						input_rc_s input_rc{};
+						input_rc.timestamp_last_signal = cycle_timestamp;
+						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
+						input_rc.rc_failsafe = sbus_failsafe;
+						input_rc.rc_lost = (raw_rc_count == 0);
+						input_rc.rc_lost_frame_count = frame_drops;
+						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS;
+
+						for (int i = 0; i < input_rc.channel_count; i++) {
+							input_rc.values[i] = raw_rc_values[i];
+						}
+
+						PublishInputRc(input_rc);
 					}
 				}

 			} else {
 				// Scan the next protocol
 				rc_io_invert(false);
-				set_rc_scan_state(RC_SCAN_DSM);
+				set_next_rc_scan_state();
 			}

 			break;
@@ -461,29 +477,38 @@ void RCInput::Run()
 				tcflush(_rcs_fd, TCIOFLUSH);
 				memset(_rcs_buf, 0, sizeof(_rcs_buf));

-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
+			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
 				if (newBytes > 0) {
-					int8_t dsm_rssi = 0;
-					bool dsm_11_bit = false;
-
 					// parse new data
-					rc_updated = dsm_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count,
+					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
+					uint16_t raw_rc_count = 0;
+					bool dsm_11_bit = false;
+					unsigned frame_drops = 0;
+					int8_t dsm_rssi = 0;
+
+					rc_updated = dsm_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count,
 							       &dsm_11_bit, &frame_drops, &dsm_rssi, input_rc_s::RC_INPUT_MAX_CHANNELS);

 					if (rc_updated) {
 						// we have a new DSM frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
-							   false, false, frame_drops, dsm_rssi);
-						_rc_scan_locked = true;
+						input_rc_s input_rc{};
+						input_rc.timestamp_last_signal = cycle_timestamp;
+						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
+						input_rc.rc_lost = (raw_rc_count == 0);
+						input_rc.rc_lost_frame_count = frame_drops;
+						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM;
+
+						for (int i = 0; i < input_rc.channel_count; i++) {
+							input_rc.values[i] = raw_rc_values[i];
+						}
+
+						PublishInputRc(input_rc);
 					}
 				}

 			} else {
 				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_ST24);
+				set_next_rc_scan_state();
 			}

 			break;
@@ -498,43 +523,43 @@ void RCInput::Run()
 				tcflush(_rcs_fd, TCIOFLUSH);
 				memset(_rcs_buf, 0, sizeof(_rcs_buf));

-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
+			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
 				if (newBytes > 0) {
 					// parse new data
-					uint8_t st24_rssi, lost_count;
-
-					rc_updated = false;
+					uint8_t st24_rssi = 0;
+					uint8_t lost_count = 0;
+					uint16_t raw_rc_count = 0;
+					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];

 					for (unsigned i = 0; i < (unsigned)newBytes; i++) {
-						/* set updated flag if one complete packet was parsed */
+						// set updated flag if one complete packet was parsed
 						st24_rssi = input_rc_s::RSSI_MAX;
-						rc_updated = (OK == st24_decode(_rcs_buf[i], &st24_rssi, &lost_count,
-										&_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS));
+						rc_updated = (OK == st24_decode(_rcs_buf[i], &st24_rssi, &lost_count, &raw_rc_count, raw_rc_values,
+										input_rc_s::RC_INPUT_MAX_CHANNELS));
 					}

 					// The st24 will keep outputting RC channels and RSSI even if RC has been lost.
 					// The only way to detect RC loss is therefore to look at the lost_count.
-
 					if (rc_updated) {
-						if (lost_count == 0) {
-							// we have a new ST24 frame. Publish it.
-							_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24;
-							fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
-								   false, false, frame_drops, st24_rssi);
-							_rc_scan_locked = true;
+						// we have a new ST24 frame. Publish it.
+						input_rc_s input_rc{};
+						input_rc.timestamp_last_signal = cycle_timestamp;
+						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
+						input_rc.rssi = st24_rssi;
+						input_rc.rc_lost = (raw_rc_count == 0) || (lost_count > 0);
+						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24;

-						} else {
-							// if the lost count > 0 means that there is an RC loss
-							_rc_in.rc_lost = true;
+						for (int i = 0; i < input_rc.channel_count; i++) {
+							input_rc.values[i] = raw_rc_values[i];
 						}
+
+						PublishInputRc(input_rc);
 					}
 				}

 			} else {
 				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_SUMD);
+				set_next_rc_scan_state();
 			}

 			break;
@@ -549,74 +574,47 @@ void RCInput::Run()
 				tcflush(_rcs_fd, TCIOFLUSH);
 				memset(_rcs_buf, 0, sizeof(_rcs_buf));

-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
+			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
 				if (newBytes > 0) {
 					// parse new data
-					uint8_t sumd_rssi, rx_count;
-					bool sumd_failsafe;
-
-					rc_updated = false;
+					uint8_t sumd_rssi = 0;
+					uint8_t rx_count = 0;
+					uint16_t raw_rc_count = 0;
+					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
+					bool sumd_failsafe = false;

 					for (unsigned i = 0; i < (unsigned)newBytes; i++) {
-						/* set updated flag if one complete packet was parsed */
+						// set updated flag if one complete packet was parsed
 						sumd_rssi = input_rc_s::RSSI_MAX;
-						rc_updated = (OK == sumd_decode(_rcs_buf[i], &sumd_rssi, &rx_count,
-										&_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS, &sumd_failsafe));
+						rc_updated = (OK == sumd_decode(_rcs_buf[i], &sumd_rssi, &rx_count, &raw_rc_count, raw_rc_values,
+										input_rc_s::RC_INPUT_MAX_CHANNELS, &sumd_failsafe));
 					}

 					if (rc_updated) {
 						// we have a new SUMD frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
-							   false, sumd_failsafe, frame_drops, sumd_rssi);
-						_rc_scan_locked = true;
+						input_rc_s input_rc{};
+						input_rc.timestamp_last_signal = cycle_timestamp;
+						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
+						input_rc.rssi = sumd_rssi;
+						input_rc.rc_failsafe = sumd_failsafe;
+						input_rc.rc_lost = (raw_rc_count == 0);
+						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD;
+
+						for (int i = 0; i < input_rc.channel_count; i++) {
+							input_rc.values[i] = raw_rc_values[i];
+						}
+
+						PublishInputRc(input_rc);
 					}
 				}

 			} else {
 				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_PPM);
+				set_next_rc_scan_state();
 			}

 			break;

-		case RC_SCAN_PPM:
-			// skip PPM if it's not supported
-#ifdef HRT_PPM_CHANNEL
-			if (_rc_scan_begin == 0) {
-				_rc_scan_begin = cycle_timestamp;
-				// Configure timer input pin for CPPM
-				px4_arch_configgpio(GPIO_PPM_IN);
-
-			} else if (_rc_scan_locked || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
-				// see if we have new PPM input data
-				if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) && ppm_decoded_channels > 3) {
-					// we have a new PPM frame. Publish it.
-					rc_updated = true;
-					_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
-					fill_rc_in(ppm_decoded_channels, ppm_buffer, cycle_timestamp, false, false, 0);
-					_rc_scan_locked = true;
-					_rc_in.rc_ppm_frame_length = ppm_frame_length;
-					_rc_in.timestamp_last_signal = ppm_last_valid_decode;
-				}
-
-			} else {
-				// disable CPPM input by mapping it away from the timer capture input
-				px4_arch_unconfiggpio(GPIO_PPM_IN);
-				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_CRSF);
-			}
-
-#else   // skip PPM if it's not supported
-			set_rc_scan_state(RC_SCAN_CRSF);
-
-#endif  // HRT_PPM_CHANNEL
-
-			break;
-
 		case RC_SCAN_CRSF:
 			if (_rc_scan_begin == 0) {
 				_rc_scan_begin = cycle_timestamp;
@@ -627,40 +625,44 @@ void RCInput::Run()
 				tcflush(_rcs_fd, TCIOFLUSH);
 				memset(_rcs_buf, 0, sizeof(_rcs_buf));

-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
-				// parse new data
+			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
 				if (newBytes > 0) {
-					rc_updated = crsf_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count,
+					// parse new data
+					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
+					uint16_t raw_rc_count = 0;
+
+					rc_updated = crsf_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count,
 								input_rc_s::RC_INPUT_MAX_CHANNELS);

 					if (rc_updated) {
 						// we have a new CRSF frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0);
+						input_rc_s input_rc{};
+						input_rc.timestamp_last_signal = cycle_timestamp;
+						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
+						input_rc.rc_lost = (raw_rc_count == 0);
+						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF;

-						// on Pixhawk (-related) boards we cannot write to the RC UART
-						// another option is to use a different UART port
-#ifdef BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT
-
-						if (!_rc_scan_locked && !_crsf_telemetry) {
-							_crsf_telemetry = new CRSFTelemetry(_rcs_fd);
+						for (int i = 0; i < input_rc.channel_count; i++) {
+							input_rc.values[i] = raw_rc_values[i];
 						}

-#endif /* BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT */
+						PublishInputRc(input_rc);

-						_rc_scan_locked = true;
+						if (_rc_serial_port_output) {
+							if (!_rc_scan_locked && !_crsf_telemetry) {
+								_crsf_telemetry = new CRSFTelemetry(_rcs_fd);
+							}

-						if (_crsf_telemetry) {
-							_crsf_telemetry->update(cycle_timestamp);
+							if (_crsf_telemetry) {
+								_crsf_telemetry->update(cycle_timestamp);
+							}
 						}
 					}
 				}

 			} else {
 				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_GHST);
+				set_next_rc_scan_state();
 			}

 			break;
@@ -675,55 +677,55 @@ void RCInput::Run()
 				tcflush(_rcs_fd, TCIOFLUSH);
 				memset(_rcs_buf, 0, sizeof(_rcs_buf));

-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
+			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
 				// parse new data
 				if (newBytes > 0) {
+					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
+					uint16_t raw_rc_count = 0;
 					int8_t ghst_rssi = -1;
-					rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &ghst_rssi,
-								&_raw_rc_count, input_rc_s::RC_INPUT_MAX_CHANNELS);
+					rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &raw_rc_values[0], &ghst_rssi, &raw_rc_count,
+								input_rc_s::RC_INPUT_MAX_CHANNELS);

 					if (rc_updated) {
 						// we have a new GHST frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, ghst_rssi);
+						input_rc_s input_rc{};
+						input_rc.timestamp_last_signal = cycle_timestamp;
+						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
+						input_rc.rssi = ghst_rssi;
+						input_rc.rc_lost = (raw_rc_count == 0);
+						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST;

-						// ghst telemetry works on fmu-v5
-						// on other Pixhawk (-related) boards we cannot write to the RC UART
-						// another option is to use a different UART port
-#ifdef BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT
-
-						if (!_rc_scan_locked && !_ghst_telemetry) {
-							_ghst_telemetry = new GHSTTelemetry(_rcs_fd);
+						for (int i = 0; i < input_rc.channel_count; i++) {
+							input_rc.values[i] = raw_rc_values[i];
 						}

-#endif /* BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT */
+						PublishInputRc(input_rc);

-						_rc_scan_locked = true;
+						if (_rc_serial_port_output) {
+							if (!_rc_scan_locked && !_ghst_telemetry) {
+								_ghst_telemetry = new GHSTTelemetry(_rcs_fd);
+							}

-						if (_ghst_telemetry) {
-							_ghst_telemetry->update(cycle_timestamp);
+							if (_ghst_telemetry) {
+								_ghst_telemetry->update(cycle_timestamp);
+							}
 						}
 					}
 				}

 			} else {
 				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_SBUS);
+				set_next_rc_scan_state();
 			}

 			break;
+
+		default:
+			// Scan the next protocol
+			set_next_rc_scan_state();
 		}

-		perf_end(_cycle_perf);
-
-		if (rc_updated) {
-			perf_count(_publish_interval_perf);
-
-			_to_input_rc.publish(_rc_in);
-
-		} else if (!rc_updated && !_armed && (hrt_elapsed_time(&_rc_in.timestamp_last_signal) > 1_s)) {
+		if (!rc_updated && !_armed && (hrt_elapsed_time(&_last_publish_time) > 1_s)) {
 			_rc_scan_locked = false;
 		}

@@ -731,6 +733,16 @@ void RCInput::Run()
 			_report_lock = false;
 			PX4_INFO("RC scan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]);
 		}
+
+		// reschedule immediately if RC is locked
+		if (_rc_scan_locked) {
+			ScheduleNow();
+
+		} else {
+			ScheduleDelayed(_backup_update_interval);
+		}
+
+		perf_end(_cycle_perf);
 	}
 }

@@ -805,8 +817,6 @@ int RCInput::custom_command(int argc, char *argv[])

 int RCInput::print_status()
 {
-	PX4_INFO("Max update rate: %u Hz", 1000000 / _current_update_interval);
-
 	if (_device[0] != '\0') {
 		PX4_INFO("UART device: %s", _device);
 		PX4_INFO("UART RX bytes: %"  PRIu32, _bytes_rx);
@@ -828,42 +838,23 @@ int RCInput::print_status()
 			PX4_INFO("SBUS frame drops: %u", sbus_dropped_frames());
 			break;

-
-		case RC_SCAN_DSM:
-			// DSM status output
-#if defined(SPEKTRUM_POWER)
-#endif
-			break;
-
-		case RC_SCAN_PPM:
-			// PPM status output
-			break;
-
-		case RC_SCAN_SUMD:
-			// SUMD status output
-			break;
-
-		case RC_SCAN_ST24:
-			// SUMD status output
+		default:
 			break;
 		}
 	}

-#if ADC_RC_RSSI_CHANNEL
+#if defined(ADC_RC_RSSI_CHANNEL)

 	if (_analog_rc_rssi_stable) {
 		PX4_INFO("vrssi: %dmV", (int)(_analog_rc_rssi_volt * 1000.0f));
 	}

-#endif
+#endif // ADC_RC_RSSI_CHANNEL

 	perf_print_counter(_cycle_perf);
+	perf_print_counter(_cycle_interval_perf);
 	perf_print_counter(_publish_interval_perf);

-	if (hrt_elapsed_time(&_rc_in.timestamp) < 1_s) {
-		print_message(ORB_ID(input_rc), _rc_in);
-	}
-
 	return 0;
 }

@@ -91,23 +91,31 @@ public:
 private:

 	enum RC_SCAN {
-		RC_SCAN_PPM = 0,
+#if defined(HRT_PPM_CHANNEL)
+		RC_SCAN_PPM,
+#endif // HRT_PPM_CHANNEL
 		RC_SCAN_SBUS,
 		RC_SCAN_DSM,
 		RC_SCAN_SUMD,
 		RC_SCAN_ST24,
 		RC_SCAN_CRSF,
-		RC_SCAN_GHST
+		RC_SCAN_GHST,
+
+		RC_SCAN_MAX
 	} _rc_scan_state{RC_SCAN_SBUS};

-	static constexpr char const *RC_SCAN_STRING[7] {
+	static constexpr char const *RC_SCAN_STRING[] {
+#if defined(HRT_PPM_CHANNEL)
 		"PPM",
+#endif // HRT_PPM_CHANNEL
 		"SBUS",
 		"DSM",
 		"SUMD",
 		"ST24",
 		"CRSF",
-		"GHST"
+		"GHST",
+
+		"NONE"
 	};

 	void Run() override;
@@ -116,38 +124,36 @@ private:
 	bool bind_spektrum(int arg = DSMX8_BIND_PULSES) const;
 #endif // SPEKTRUM_POWER

-	void fill_rc_in(uint16_t raw_rc_count_local,
-			uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
-			hrt_abstime now, bool frame_drop, bool failsafe,
-			unsigned frame_drops, int rssi);
-
-	void set_rc_scan_state(RC_SCAN _rc_scan_state);
+	void FillRssi(input_rc_s &input_rc);
+	void PublishInputRc(input_rc_s &input_rc);

 	void rc_io_invert(bool invert);
+	void set_next_rc_scan_state();

 	hrt_abstime _rc_scan_begin{0};
+	hrt_abstime _last_publish_time{0};

 	bool _initialized{false};
 	bool _rc_scan_locked{false};
 	bool _report_lock{true};

-	static constexpr unsigned	_current_update_interval{4000}; // 250 Hz
+	static constexpr unsigned	_backup_update_interval{100000}; // 10 Hz (backup schedule)

 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};

-	uORB::Subscription	_adc_report_sub{ORB_ID(adc_report)};
+#if defined(ADC_RC_RSSI_CHANNEL)
+	uORB::Subscription	_adc_report_sub {ORB_ID(adc_report)};
+	float _analog_rc_rssi_volt{-1.0f};
+	bool _analog_rc_rssi_stable{false};
+#endif // ADC_RC_RSSI_CHANNEL
+
 	uORB::Subscription	_vehicle_cmd_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription	_vehicle_status_sub{ORB_ID(vehicle_status)};

-	input_rc_s	_rc_in{};
-
-	float		_analog_rc_rssi_volt{-1.0f};
-	bool		_analog_rc_rssi_stable{false};
-
+	bool _rc_serial_port_output{true};
 	bool _armed{false};

-
-	uORB::PublicationMulti<input_rc_s>	_to_input_rc{ORB_ID(input_rc)};
+	uORB::PublicationMulti<input_rc_s> _input_rc_pub{ORB_ID(input_rc)};

 	int		_rcs_fd{-1};
 	char		_device[20] {};					///< device / serial port path
@@ -155,14 +161,13 @@ private:
 	static constexpr size_t RC_MAX_BUFFER_SIZE{SBUS_BUFFER_SIZE};
 	uint8_t _rcs_buf[RC_MAX_BUFFER_SIZE] {};

-	uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {};
-	uint16_t _raw_rc_count{};
-
 	CRSFTelemetry *_crsf_telemetry{nullptr};
 	GHSTTelemetry *_ghst_telemetry{nullptr};

-	perf_counter_t	_cycle_perf;
-	perf_counter_t	_publish_interval_perf;
+	perf_counter_t	_cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
+	perf_counter_t	_cycle_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};
+	perf_counter_t	_publish_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")};
+
 	uint32_t	_bytes_rx{0};

 	DEFINE_PARAMETERS(
@@ -64,6 +64,12 @@ extern void libtomcrypt_init(void);
 */
 static int crypto_open_count = 0;

+/*
+ * Status of libtomcrypt initialization. This is a large library, which
+ * is initialized & pulled in by linker only when it is actually used
+ */
+static bool tomcrypt_initialized = false;
+
 typedef struct {
 	size_t key_size;
 	uint8_t *key;
@@ -76,6 +82,14 @@ typedef struct {
 	uint64_t ctr;
 } chacha20_context_t;

+static inline void initialize_tomcrypt(void)
+{
+	if (!tomcrypt_initialized) {
+		libtomcrypt_init();
+		tomcrypt_initialized = true;
+	}
+}
+
 /* Clear key cache */
 static void clear_key_cache(void)
 {
@@ -135,7 +149,6 @@ void crypto_init()
 {
 	keystore_init();
 	clear_key_cache();
-	libtomcrypt_init();
 }

 crypto_session_handle_t crypto_open(px4_crypto_algorithm_t algorithm)
@@ -269,6 +282,8 @@ bool crypto_encrypt_data(crypto_session_handle_t handle,
 			uint8_t *public_key = (uint8_t *)crypto_get_key_ptr(handle.keystore_handle, key_idx, &key_sz);
 			*cipher_size = 0;

+			initialize_tomcrypt();
+
 			if (public_key &&
 			    rsa_import(public_key, key_sz, &key) == CRYPT_OK) {
 				if (outlen >= ltc_mp.unsigned_size(key.N) &&
@@ -413,6 +428,8 @@ size_t crypto_get_min_blocksize(crypto_session_handle_t handle, uint8_t key_idx)
 			size_t pub_key_sz;
 			uint8_t *pub_key = (uint8_t *)crypto_get_key_ptr(handle.keystore_handle, key_idx, &pub_key_sz);

+			initialize_tomcrypt();
+
 			if (pub_key &&
 			    rsa_import(pub_key, pub_key_sz, &enc_key) == CRYPT_OK) {
 				ret = ltc_mp.unsigned_size(enc_key.N);
@@ -617,6 +617,25 @@ void NPFG::navigateLoiter(const Vector2d &loiter_center, const Vector2d &vehicle
 	updateRollSetpoint();
 } // navigateLoiter

+
+void NPFG::navigatePathTangent(const matrix::Vector2d &vehicle_pos, const matrix::Vector2d &position_setpoint,
+			       const matrix::Vector2f &tangent_setpoint,
+			       const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature)
+{
+	path_type_loiter_ = false;
+
+	// set unit tangent directly
+	unit_path_tangent_ = tangent_setpoint.normalized();
+
+	// closest point to vehicle
+	matrix::Vector2f error_vector = getLocalPlanarVector(position_setpoint, vehicle_pos);
+	signed_track_error_ = cross2D(unit_path_tangent_, error_vector);
+
+	guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, curvature);
+
+	updateRollSetpoint();
+} // navigatePathTangent
+
 void NPFG::navigateHeading(float heading_ref, const Vector2f &ground_vel, const Vector2f &wind_vel)
 {
 	path_type_loiter_ = false;
@@ -248,6 +248,21 @@ public:
 			    float radius, int8_t loiter_direction, const matrix::Vector2f &ground_vel,
 			    const matrix::Vector2f &wind_vel);

+	/*
+	 * Path following logic. Takes poisiton, path tangent, curvature and
+	 * then updates control setpoints to follow a path setpoint.
+	 *
+	 * @param[in] vehicle_pos vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg]
+	 * @param[in] position_setpoint closest point on a path in WGS84 coordinates (lat,lon) [deg]
+	 * @param[in] tangent_setpoint unit tangent vector of the path [m]
+	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
+	 * @param[in] wind_vel Wind velocity vector [m/s]
+	 * @param[in] curvature of the path setpoint [1/m]
+	 */
+	void navigatePathTangent(const matrix::Vector2d &vehicle_pos, const matrix::Vector2d &position_setpoint,
+				 const matrix::Vector2f &tangent_setpoint,
+				 const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature);
+
 	/*
 	 * Navigate on a fixed heading.
 	 *
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Daniel Agar	edb9fce9b3	drivers/rc_input: cleanup and simplify data processing per type - don't store data in class	2022-02-09 17:11:11 -05:00
Daniel Agar	c4f9f2980d	drivers/rc_input: poll file descriptor to minimize latency	2022-02-09 16:48:08 -05:00
David Sidrane	fd1aa3cfb9	matek_gnss-m9n-f4:Use CONFIG_BOARD_SERIAL_GPSn for serial_passthru	2022-02-09 13:11:52 -05:00
David Sidrane	0c936e4fd2	serial_passthru:Move CONFIG_xxx to serial_passthru	2022-02-09 13:11:52 -05:00
Silvan Fuhrer	81b08a0168	FW Pos C: set position_sp type to position during VTOL backtransition Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-02-09 11:39:47 -05:00
Silvan Fuhrer	1d6396b418	Navigator: VTOL: track virtual WP 1m ahead of vehicle during backtransition in Land mode Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-02-09 11:39:47 -05:00
Jukka Laitinen	dcde0d0559	src/drivers/sw_crypto: Late initialize tomcypt This saves a lot of flash space, in case functions from libtomcrypt are not used (currently only RSA related). When RSA is not used, the linker can now drop all libtomcrypt related things. This is especially relevant for bootloaders using the SW crypto. Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2022-02-09 10:28:06 -05:00
Daniel Agar	a95da715d5	Jenkins: HIL remove airframe 4018 test	2022-02-09 10:08:59 +01:00
Beat Küng	86860808e3	ROMFS: set CA_* geometry params for some of the generic airframes Not enabled, makes it easier to switch.	2022-02-09 10:08:59 +01:00
Beat Küng	a2ba613254	ROMFS: remove 4018 + 6003 ctrlalloc airframes	2022-02-09 10:08:59 +01:00
PX4 BuildBot	33ce1b9b64	Update submodule mavlink to latest Wed Feb 9 00:39:13 UTC 2022 - mavlink in PX4/Firmware (ed8c1dca2e792e95d9ab1ba72a22bc9658f10b79): https://github.com/mavlink/mavlink/commit/51abf3c82b3d7137406459dc9b337e57637711ae - mavlink current upstream: https://github.com/mavlink/mavlink/commit/311eee010bb82f5fb2e4e0f64f7961a83212b003 - Changes: https://github.com/mavlink/mavlink/compare/51abf3c82b3d7137406459dc9b337e57637711ae...311eee010bb82f5fb2e4e0f64f7961a83212b003 311eee01 2022-02-04 Peter Barker - common.xml: deprecate SET_HOME_POSITION message (#1791) 63d62522 2022-02-02 Julian Oes - minimal: add MAV_TYPE_WINCH (#1789)	2022-02-08 21:50:55 -05:00
PX4 BuildBot	421ca2fc48	Update submodule sitl_gazebo to latest Wed Feb 9 00:39:03 UTC 2022 - sitl_gazebo in PX4/Firmware (a088277fe866860272f1ef21773f5be1b6832b11): https://github.com/PX4/PX4-SITL_gazebo/commit/2750fe233c6a38dbe60e545ac0ec878d6ac20b1e - sitl_gazebo current upstream: https://github.com/PX4/PX4-SITL_gazebo/commit/25138e803ee8525ee5fe4e6d511506e88e3f819c - Changes: https://github.com/PX4/PX4-SITL_gazebo/compare/2750fe233c6a38dbe60e545ac0ec878d6ac20b1e...25138e803ee8525ee5fe4e6d511506e88e3f819c 25138e8 2021-07-16 Julian Oes - gimbal_controller: fix attitude status	2022-02-08 20:52:12 -05:00
Daniel Agar	b24aa071b6	Jenkins: hardware always dump pyserial debug (silently)	2022-02-08 17:22:57 -05:00
Daniel Agar	6686736cff	drv_pwm_output.h fix dshot cmd typo	2022-02-08 17:11:49 -05:00
Daniel Agar	86f81680fb	sensors: check uORB::SubscriptionData validity before use	2022-02-08 13:19:01 -05:00
Oleg Kalachev	21b78f9d05	Enable mpu9250’s magnetometer on fmu-v4	2022-02-08 13:18:39 -05:00
Steve Nogar	dce067df83	add configurable rtps rate parameter	2022-02-08 19:03:20 +01:00
Daniel Agar	133fe0cfb1	local_position_estimator: move to INS0 work queue (for significantly more stack)	2022-02-08 11:16:49 -05:00
Daniel Agar	be3da5089c	uORB: uORBDeviceNode use px4_cache_aligned_alloc	2022-02-08 10:20:50 -05:00
Viktor Vladic	f4d02a2937	MatekH743 board_id, usb vid/pid changes, and MPU6000 delay before transfer - not after (#18733 ) * Make board_id compatible with ardupilot * Initialize outputs for CAM1/CAM2 and Vsw pad * Correct board type 1013 in bootloader to match AP * Change usb vendor string to "Matek" * Change cdcacm pid to 1013 * Comment out FLASH_BASED_PARAMS because of #15331	2022-02-07 19:55:49 -05:00
Julian Oes	490a0c473b	Rename vmount to gimbal	2022-02-07 19:21:15 -05:00
Julian Oes	853047c643	Mantis: use gimbal auto input This allows RC and mavlink gimbal v2 input.	2022-02-07 19:21:15 -05:00
Julian Oes	39f0e97245	vmount: refactor for v2 auto input, test command This is a bigger refactor of vmount to clean it up and plug some holes in functionality. The changes include: - Fixing/simplifying the test command. - Changing the dependencies of the input and output classes to just the parameter list. - Adding a separate topic to publish gimbal v1 commands to avoid polluting the vehicle_command topic. - Removing outdated comments and author lists. - Extracting the gimbal v2 "in control" notion outside into control_data rather than only the v2 input.	2022-02-07 19:21:15 -05:00
Julian Oes	f2216dff55	mavlink: don't send gimbal_device_attitude_status If we receive gimbal_device_attitude_status by mavlink we should not re-send it as we are already supposed to be forwarding mavlink traffic from the gimbal to the ground station.	2022-02-07 19:21:15 -05:00
Julian Oes	7e7a99b542	mavlink: handle for GIMBAL_DEVICE_ATTITUDE_STATUS That way we can log it later.	2022-02-07 19:21:15 -05:00
Daniel Agar	5b07398b3e	Tools/HIL/test_airframes.sh: only attempt setting first 3 MAV_x_CONFIG - MAV_3_CONFIG doesn't exist	2022-02-07 16:31:27 -05:00
Daniel Agar	f9d87fd97a	sensors/vehicle_angular_velocity: improve error handling (especially during initial selection)	2022-02-07 12:47:02 -05:00
Daniel Agar	3a37fd92e6	Jenkins: HIL temporarily tolerate ERRORs during 'uorb top' (long running command) - on older boards if the sensor auto cal completes while this is running it causes a brief sensor timeout	2022-02-07 10:26:48 -05:00
Daniel Agar	052adfbfd9	borads: holybro_kakutef7 disable modules/events to save flash	2022-02-06 16:53:13 -05:00
Daniel Agar	3a741cb9c9	boards: bitcrazy_crazyflie disable gyro_fft to save flash	2022-02-06 16:53:13 -05:00
Thomas Debrunner	b4087ebd2b	mc_pos_control: increase the default crawl speed for three-stage-landing approach for a more robust landing detection	2022-02-04 21:56:20 -05:00
Matthias Grob	4ef8cead3d	mc_pos_control_params: correct crawl speed reference	2022-02-04 21:56:20 -05:00
Matthias Grob	1df9d6fca6	MulticopterLandDetector: fix crawl speed parameter fetching	2022-02-04 21:56:20 -05:00
Thomas Debrunner	fb8b9b647a	land-detector: switch to crawl speed for intent detection	2022-02-04 21:56:20 -05:00
Thomas Debrunner	2a6d9bc1dd	fligh-mode-manager: First implementation of a three-stage-landing for multirotos, in case LIDAR is available	2022-02-04 21:56:20 -05:00
Daniel Agar	893cdf8f38	ekf2: test external vision adjustments after lowering fake position fusion variance	2022-02-04 21:48:32 -05:00
Daniel Agar	6b1750d8be	ekf2: lower fake position observation variance when at rest	2022-02-04 21:48:32 -05:00
Daniel Agar	c028b964e2	bmm150: minor changes to match reference driver	2022-02-04 19:57:24 -05:00
Beat Küng	8b2016b4ed	fix protocol_splitter: increment i properly in scan_for_packets This also guarantees that i is increased in every loop iteration. Before it was possible to enter a busy loop.	2022-02-04 09:42:52 -05:00
Daniel Agar	cf3db0d313	mavlink: don't send_mission_current if mission invalid	2022-02-04 09:40:25 -05:00
Silvan Fuhrer	26dba1407b	Commander: VTOL quadchuate also triggers a RTL in Loiter and VTOL_Takeoff modes, not just in Mission (#19123 ) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-02-03 14:18:28 +01:00
JaeyoungLim	f82c722653	Publish correct orbit status with goto (#19102 ) For fixedwings, the orbit status was publishing zero radius when a goto waypoint was being passed. This commit corrects this by passing the default loiter radius as the guidance logic was using	2022-02-03 10:16:39 +01:00
bresch	917910f3e2	ekf: pass a float by copy instead of a constant ref	2022-02-02 17:43:25 -05:00
bresch	3077f27821	ekf: use float instead of Vector2f for innov gate in pos/vel fusion We always use the same innovation gate for X and Y anyway	2022-02-02 17:43:25 -05:00
Daniel Agar	5fb0084524	ekf2: remove unused shared fields for last velocity observation and variance	2022-02-02 17:43:25 -05:00
Daniel Agar	8a0581516c	ekf2: don't store _auxvel_sample_delayed	2022-02-02 17:43:25 -05:00
Daniel Agar	452b5e94b4	ekf2: don't store _drag_sample_delayed	2022-02-02 17:43:25 -05:00
Daniel Agar	138ff7a316	sensors: ensure that best accel/gyro is in bounds	2022-02-02 17:14:46 -05:00
Daniel Agar	07c273fc31	sensors/vehicle_angular_velocity: print full sensor_selection if device id not found	2022-02-02 17:14:46 -05:00
Daniel Agar	94604ff21a	Jenkins: hardware pyserial cat shouldn't fail the build	2022-02-02 14:17:02 -05:00
Peter van der Perk	9f97793491	Generate C/C++ header to expose px4board kconfig symbols to the preprocessor	2022-02-02 13:23:21 -05:00
Jaeyoung Lim	2fc95bb369	Rebase fix Use getnavspeed_2d for groundspeed	2022-02-02 17:16:19 +01:00
Jaeyoung Lim	3dd5c1fbaf	Enable NPFG by default on the believer	2022-02-02 17:16:19 +01:00
Jaeyoung Lim	3a9c5c3178	Use acceleration to pass path curvature	2022-02-02 17:16:19 +01:00
Jaeyoung Lim	e2741f988a	Add interface for passing path tangent and closest point directly This commit adds an interface to pass the path tangent and closest point directly to NPFG using the offboard interface	2022-02-02 17:16:19 +01:00
David Sidrane	f460611098	Fix serial_test stack warning	2022-02-02 11:12:53 -05:00
Peter van der Perk	efd3bc1794	Serial_test move g_cl and g_mod from heap to stack to save memory by not preallocating them	2022-02-02 07:21:43 -08:00
Daniel Agar	88ffc177f7	ekf2: utils add getEulerYaw() that uses the best rotation sequence	2022-02-02 08:54:03 -05:00
Daniel Agar	c3e0b93fc8	ekf2: remove unnecessary inlines	2022-02-02 08:50:49 -05:00
Hamish Willee	26d5ac4f58	GPS_YAW_OFFSET param docs: use rover and moving base terminology	2022-02-02 07:39:01 +01:00