github: fix workspace path

github: fix env var context
github: fix deprecated set-env
2026-07-31 08:40:35 +08:00 · 2022-02-14 11:05:24 -08:00 · 2022-02-14 09:59:20 -08:00 · 2022-02-12 10:29:00 -08:00 · 2022-02-11 12:50:01 -08:00 · 2022-02-11 12:48:05 -08:00
167 changed files with 3666 additions and 3753 deletions
@@ -73,8 +73,10 @@ 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'
                }
              }
@@ -143,8 +145,10 @@ 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'
                }
              }
@@ -213,8 +217,10 @@ 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'
                }
              }
@@ -282,8 +288,10 @@ 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'
                }
              }
@@ -352,8 +360,10 @@ 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'
                }
              }
@@ -442,8 +452,10 @@ 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'
                }
              }
@@ -524,8 +536,10 @@ 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'
                }
              }
@@ -594,8 +608,10 @@ 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'
                }
              }
@@ -664,8 +680,10 @@ 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'
                }
              }
@@ -775,7 +793,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" || 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 "ver all"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "work_queue status"'
  // stop logger
@@ -791,7 +809,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 6001 8001 10016'
+  sh './Tools/HIL/test_airframes.sh `find /dev/serial -name *usb-*` 1000 1001 2100 3000 4001 4018 6001 8001 10016'

  resetParameters()

@@ -844,7 +862,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" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1 -a"'

  // 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'
@@ -0,0 +1,154 @@
+# 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"
@@ -1,54 +0,0 @@
-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
@@ -1,51 +0,0 @@
-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
@@ -1,120 +0,0 @@
-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,11 +1,17 @@
-name: Deploy metadata for all targets
+name: Build and deploy all targets with metadata

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

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

    - name: parameter & events metadata
-      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 }}/'
-
+      run: make ${{matrix.target}} ver_gen events_json actuators_json
@@ -44,8 +44,6 @@ 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
-	gimbal start
+	vmount start
 fi

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

 . ${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,25 +13,6 @@

 . ${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,32 +31,6 @@ 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,20 +23,6 @@ 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,18 +23,6 @@

 . ${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,16 +21,3 @@
 #

 . ${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
-
@@ -0,0 +1,39 @@
+#!/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 0
+param set-default MNT_MODE_IN 1
 param set-default MNT_MODE_OUT 1
 param set-default MNT_MAN_PITCH 2
 param set-default MNT_RC_IN_MODE 1
@@ -23,23 +23,6 @@

 . ${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
@@ -0,0 +1,52 @@
+#!/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,23 +23,6 @@

 . ${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,28 +25,6 @@

 . ${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,6 +72,7 @@ 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
@@ -99,6 +100,7 @@ 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 gimbal to control mounts such as gimbals, disabled by default.
+	# Start vmount to control mounts such as gimbals, disabled by default.
 	#
 	if param greater -s MNT_MODE_IN -1
 	then
-		gimbal start
+		vmount start
 	fi

 	# Check for flow sensor
@@ -35,6 +35,7 @@ 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,35 +1,5 @@
 #! /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)
@@ -38,30 +8,23 @@ fi
 #Delete all 'untagged/dangling' (<none>) images
 #docker rmi $(docker images -q -f dangling=true)

-echo "PX4_DOCKER_REPO: $PX4_DOCKER_REPO";
+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

 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=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"
+  --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"
@@ -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,6 +23,10 @@ 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
@@ -35,48 +39,66 @@ 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
@@ -85,18 +107,37 @@ 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
-        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)
+
+        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)


 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
-gimbal,CONFIG_MODULES_GIMBAL=y
+vmount,CONFIG_MODULES_VMOUNT=y
 vtol_att_control,CONFIG_MODULES_VTOL_ATT_CONTROL=y
 bl_update,CONFIG_SYSTEMCMDS_BL_UPDATE=y
 dmesg,CONFIG_SYSTEMCMDS_DMESG=y
@@ -0,0 +1,55 @@
+#
+# 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"]
@@ -0,0 +1,28 @@
+#!/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,20 +2,22 @@

 set -e

-## Bash script to setup PX4 development environment on Ubuntu LTS (20.04, 18.04, 16.04).
+## Bash script to setup PX4 development environment on Ubuntu LTS (20.04, 18.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 Gazebo9 simulator (omit with arg: --no-sim-tools)
-##
-## Not Installs:
-## - FastRTPS and FastCDR
+## - jMAVSim and Gazebo simulator (omit with arg: --no-sim-tools)
+## Optional:
+## - FastRTPS and FastCDR (with args: --with-rtps)

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

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

-done
+	if [[ $arg == "--with-rtps" ]]; then
+		INSTALL_RTPS="true"
+	fi

-# 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
+	if [[ $arg == "--with-java" ]]; then
+		INSTALL_JAVA="true"
+	fi
+
+	if [[ $arg == "--from-docker" ]]; then
+		INSIDE_DOCKER="true"
+	fi
+
+done

 # script directory
 DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )
@@ -69,88 +71,119 @@ 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 "Installing PX4 general dependencies"
+echo $VERBOSE_BAR
+echo "🍻 Installing System Dependencies"
+echo $VERBOSE_BAR
+echo

 sudo apt-get update -y --quiet
 sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
-	astyle \
 	build-essential \
-	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 \
-	;
+  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 \
+  ;

-# Python3 dependencies
+# Python 3 dependencies
 echo
-echo "Installing PX4 Python3 dependencies"
+echo $VERBOSE_BAR
+echo "🍻 Installing Python dependencies"
+echo $VERBOSE_BAR
+echo
+
 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
-	python3 -m pip install --user -r ${DIR}/requirements.txt
+  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
 fi

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

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

 	sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
-		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
+    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


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

-	# arm-none-eabi-gcc
-	NUTTX_GCC_VERSION="9-2020-q2-update"
-	NUTTX_GCC_VERSION_SHORT="9-2020q2"
+  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

-	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_FOUND_VER=$(echo $GCC_VER_STR | grep -c "${NUTTX_GCC_VERSION}")
-	fi
+    GCC_VER_STR=$(arm-none-eabi-gcc --version)
+    GCC_VER_FOUND=$(echo $GCC_VER_STR | grep -c "${NUTTX_GCC_VERSION}")
+  fi

-	if [[ "$GCC_FOUND_VER" == "1" ]]; then
-		echo "arm-none-eabi-gcc-${NUTTX_GCC_VERSION} found, skipping installation"
+  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

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

-		# add arm-none-eabi-gcc to user's PATH
-		exportline="export PATH=/opt/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}/bin:\$PATH"
+    # 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

-		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 "Installing PX4 simulation dependencies"
+  echo
+  echo $VERBOSE_BAR
+  echo "🍻 Installing PX4 Simulation Tools"
+  echo

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

-	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 \
-		;
+  echo "  * Gazebo Version $gazebo_version"
+  echo $VERBOSE_BAR

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

-	# 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 \
-		;
+  # 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 \
+    ;

-	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 [[ $INSTALL_NUTTX == "true" ]]; then
-	echo
-	echo "Relogin or reboot computer before attempting to build NuttX targets"
+if [[ $INSIDE_DOCKER == "true" ]]; then
+  # cleanup installation
+  rm -rf /tmp/
 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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_DYN=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
@@ -17,6 +17,7 @@ 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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -21,6 +21,7 @@ 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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -2,7 +2,6 @@ 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
@@ -16,7 +15,6 @@ 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,6 +3,8 @@
 # Matek M9NF4 CAN specific board sensors init
 #------------------------------------------------------------------------------

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

 rm3100 -b 2 -s start
@@ -0,0 +1,23 @@
+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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_DMESG=y
@@ -1,10 +1,10 @@
 {
-    "board_id": 1013,
+    "board_id": 139,
    "magic": "PX4FWv1",
-    "description": "Firmware for the MatekH743 board",
+    "description": "Firmware for the MatekH743-slim board",
    "image": "",
    "build_time": 0,
-    "summary": "MatekH743",
+    "summary": "MatekH743-slim",
    "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=0x1013
-CONFIG_CDCACM_PRODUCTSTR="MatekH743"
+CONFIG_CDCACM_PRODUCTID=0x004b
+CONFIG_CDCACM_PRODUCTSTR="Matek H743-slim"
 CONFIG_CDCACM_RXBUFSIZE=600
 CONFIG_CDCACM_TXBUFSIZE=12000
-CONFIG_CDCACM_VENDORID=0x1209
+CONFIG_CDCACM_VENDORID=0x3162
 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=0x1013
-CONFIG_CDCACM_PRODUCTSTR="MatekH743"
+CONFIG_CDCACM_PRODUCTID=0x0036
+CONFIG_CDCACM_PRODUCTSTR="MatekH743-slim"
 CONFIG_CDCACM_RXBUFSIZE=600
 CONFIG_CDCACM_TXBUFSIZE=12000
-CONFIG_CDCACM_VENDORID=0x1209
-CONFIG_CDCACM_VENDORSTR="Matek"
+CONFIG_CDCACM_VENDORID=0x1B8C
+CONFIG_CDCACM_VENDORSTR="PX4"
 CONFIG_CLOCK_MONOTONIC=y
 CONFIG_DEBUG_FULLOPT=y
 CONFIG_DEBUG_HARDFAULT_ALERT=y
@@ -119,9 +119,9 @@

 /* Spare GPIO */

-#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)
+// #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)


 /* Tone alarm output */
@@ -174,8 +174,6 @@
 		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                     1013
+#define BOARD_TYPE                     139
 #define _FLASH_KBYTES                  (*(uint32_t *)0x1FF1E880)
 #define BOARD_FLASH_SECTORS            (15)
 #define BOARD_FLASH_SIZE               (_FLASH_KBYTES * 1024)
@@ -60,10 +60,6 @@
 #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);
@@ -178,25 +174,6 @@ __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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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 -M start
+	mpu9250 -s -R 8 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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=n
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=n
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=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_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_DMESG=y
@@ -11,7 +11,6 @@ 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}
@@ -48,11 +47,6 @@ 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,5 +16,3 @@ float32 angular_velocity_y
 float32 angular_velocity_z

 uint32 failure_flags
-
-bool received_from_mavlink
@@ -175,5 +175,3 @@ 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,6 +52,3 @@
 #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()
 {
-	free(_data);
+	delete[] _data;

 	const char *devname = get_devname();

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

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

 			unlock();
@@ -50,24 +50,6 @@ __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;
@@ -222,7 +204,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", SERIAL_PASSTHRU_UBLOX_DEV,   nullptr};
+								static const char *passthru_argv[] {"serial_passthru", "start", "-t", "-b", baudstring, "-e", USB_DEVICE_PATH, "-d", CONFIG_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_telemetry_disable         = 32,
+	DShot_cmd_signal_line_telemeetry_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 rover antenna is in
+ * Set this to 0 if the antennas are parallel to the forward-facing direction of the vehicle and the first antenna is in
 * front. The offset angle increases clockwise.
 *
- * 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.
+ * Set this to 90 if the first antenna is placed on the right side and the second on the left side of the vehicle.
 *
 * @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
-	px4_udelay(10);
 	transfer(cmd, cmd, sizeof(cmd));
+	px4_udelay(10);
 	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
-	px4_udelay(10);
 	transfer(cmd, cmd, sizeof(cmd));
+	px4_udelay(10);
 }

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

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

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

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

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

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

 	return retval;
 }

 static constexpr int16_t combine_xy_int13(const uint8_t msb, const uint8_t lsb)
 {
-	// 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);
+	int16_t x = ((msb << 8) | lsb);
+	return x / 8; // arithmetic shift by 3 (13 bit signed integer)
 }

 static constexpr int16_t combine_z_int15(const uint8_t msb, const uint8_t lsb)
 {
-	// 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);
+	int16_t z = ((msb << 8) | lsb);
+	return z / 2; // arithmetic shift by 1 (15 bit signed integer)
 }

 static constexpr uint16_t combine_rhall_uint14(const uint8_t msb, const uint8_t lsb)
 {
-	// 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);
+	uint16_t rhall = ((msb << 8) | lsb);
+	return (rhall >> 2) & 0x3FFF; // 14 bit unsigned integer
 }

 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_SET, OP_MODE_BIT::ODR_20HZ_CLEAR | OP_MODE_BIT::Opmode_Sleep | OP_MODE_BIT::Self_Test },
+		{ Register::OP_MODE,       OP_MODE_BIT::ODR_20Hz, 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,12 +96,11 @@ enum POWER_CONTROL_BIT : uint8_t {
 // OP_MODE
 enum OP_MODE_BIT : uint8_t {
 	// 5:3 Data rate control
-	ODR_20HZ_SET   = Bit5 | Bit3,
-	ODR_20HZ_CLEAR = Bit4,
+	ODR_20Hz     = Bit5 | Bit3, // ODR 20 Hz

 	// 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 += Sp2HeaderSize + payload_len;
+		i += payload_len;

 	}
 }
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2021 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -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,8 +44,21 @@ constexpr char const *RCInput::RC_SCAN_STRING[];

 RCInput::RCInput(const char *device) :
 	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device))
+	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"))
 {
+	// 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';
@@ -64,7 +77,6 @@ RCInput::~RCInput()
 	delete _ghst_telemetry;

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

@@ -76,11 +88,6 @@ 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);
@@ -157,60 +164,89 @@ RCInput::task_spawn(int argc, char *argv[])
 	_object.store(instance);
 	_task_id = task_id_is_work_queue;

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

 	return PX4_OK;
 }

-void RCInput::FillRssi(input_rc_s &input_rc)
+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)
 {
-	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)) {
+	// 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)) {
 			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 = ((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)
+			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);

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

-			input_rc.rssi = math::constrain((int)roundf(rssi_analog), 0, (int)input_rc_s::RSSI_MAX);
-#endif // ADC_RC_RSSI_CHANNEL
+			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;
 		}
-	}
-}

-void RCInput::PublishInputRc(input_rc_s &input_rc)
-{
-	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;
+	} else {
+		_rc_in.rssi = rssi;
 	}

-	PX4_DEBUG("RC scan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[new_state]);
+	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)
+{
+	PX4_DEBUG("RCscan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[newState]);
 	_rc_scan_begin = 0;
-	_rc_scan_state = static_cast<RC_SCAN>(new_state);
+	_rc_scan_state = newState;
 	_rc_scan_locked = false;

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

 	} else {

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

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

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

@@ -329,8 +370,9 @@ 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;
@@ -349,25 +391,18 @@ 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
-		static constexpr hrt_abstime rc_scan_max = 300_ms;
+		constexpr hrt_abstime rc_scan_max = 300_ms;

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

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

 		// read all available data from the serial RC input UART
 		int newBytes = ::read(_rcs_fd, &_rcs_buf[0], RC_MAX_BUFFER_SIZE);
@@ -376,48 +411,7 @@ 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;
@@ -429,40 +423,30 @@ 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) {
-					// 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.
-						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);
+						_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;
 					}
 				}

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

 			break;
@@ -477,38 +461,29 @@ 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)) {
-				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 dsm_11_bit = false;
-					unsigned frame_drops = 0;
-					int8_t dsm_rssi = 0;
+			} else if (_rc_scan_locked
+				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {

-					rc_updated = dsm_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count,
+				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,
 							       &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.
-						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);
+						_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;
 					}
 				}

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

 			break;
@@ -523,43 +498,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 = 0;
-					uint8_t lost_count = 0;
-					uint16_t raw_rc_count = 0;
-					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
+					uint8_t st24_rssi, lost_count;
+
+					rc_updated = 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 */
 						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) {
-						// 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;
+						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;

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

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

 			break;
@@ -574,47 +549,74 @@ 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 = 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;
+					uint8_t sumd_rssi, rx_count;
+					bool sumd_failsafe;
+
+					rc_updated = 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.
-						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);
+						_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;
 					}
 				}

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

 			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;
@@ -625,44 +627,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)) {
-				if (newBytes > 0) {
-					// parse new data
-					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
-					uint16_t raw_rc_count = 0;
+			} else if (_rc_scan_locked
+				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {

-					rc_updated = crsf_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count,
+				// parse new data
+				if (newBytes > 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.
-						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;
+						_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);

-						for (int i = 0; i < input_rc.channel_count; i++) {
-							input_rc.values[i] = raw_rc_values[i];
+						// 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);
 						}

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

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

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

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

 			break;
@@ -677,55 +675,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.
-						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;
+						_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);

-						for (int i = 0; i < input_rc.channel_count; i++) {
-							input_rc.values[i] = raw_rc_values[i];
+						// 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);
 						}

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

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

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

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

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

-		if (!rc_updated && !_armed && (hrt_elapsed_time(&_last_publish_time) > 1_s)) {
+		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)) {
 			_rc_scan_locked = false;
 		}

@@ -733,16 +731,6 @@ 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);
 	}
 }

@@ -817,6 +805,8 @@ 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);
@@ -838,23 +828,42 @@ int RCInput::print_status()
 			PX4_INFO("SBUS frame drops: %u", sbus_dropped_frames());
 			break;

-		default:
+
+		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
 			break;
 		}
 	}

-#if defined(ADC_RC_RSSI_CHANNEL)
+#if ADC_RC_RSSI_CHANNEL

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

-#endif // ADC_RC_RSSI_CHANNEL
+#endif

 	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,31 +91,23 @@ public:
 private:

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

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

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

-	void FillRssi(input_rc_s &input_rc);
-	void PublishInputRc(input_rc_s &input_rc);
+	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 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	_backup_update_interval{100000}; // 10 Hz (backup schedule)
+	static constexpr unsigned	_current_update_interval{4000}; // 250 Hz

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

-#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	_adc_report_sub{ORB_ID(adc_report)};
 	uORB::Subscription	_vehicle_cmd_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription	_vehicle_status_sub{ORB_ID(vehicle_status)};

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

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

 	int		_rcs_fd{-1};
 	char		_device[20] {};					///< device / serial port path
@@ -161,13 +155,14 @@ 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_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")};
-
+	perf_counter_t	_cycle_perf;
+	perf_counter_t	_publish_interval_perf;
 	uint32_t	_bytes_rx{0};

 	DEFINE_PARAMETERS(
@@ -64,12 +64,6 @@ 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;
@@ -82,14 +76,6 @@ 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)
 {
@@ -149,6 +135,7 @@ void crypto_init()
 {
 	keystore_init();
 	clear_key_cache();
+	libtomcrypt_init();
 }

 crypto_session_handle_t crypto_open(px4_crypto_algorithm_t algorithm)
@@ -282,8 +269,6 @@ 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) &&
@@ -428,8 +413,6 @@ 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,25 +617,6 @@ 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,21 +248,6 @@ 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
Ramon Roche	2057dd3381	github: fix workspace path	2022-02-14 11:05:24 -08:00
Ramon Roche	771049799f	github: fix env var context	2022-02-14 09:59:20 -08:00
Ramon Roche	a3d5466ac3	github: fix deprecated set-env	2022-02-12 10:29:00 -08:00
Ramon Roche	4243be3a4b	tools: leave out kconf platform targets	2022-02-11 12:50:01 -08:00
Ramon Roche	af8c6d3cef	tools: generate single target platform	2022-02-11 12:48:05 -08:00
Ramon Roche	cf9584c36a	github: replace env with outputs	2022-02-11 12:47:06 -08:00
Ramon Roche	fb9de0c32a	github: debug variables take three	2022-02-09 10:34:56 -08:00
Ramon Roche	e82c68da8b	github: debug variables in group	2022-02-09 10:32:38 -08:00
Ramon Roche	3de4e46cdc	github: debug variables in run scope	2022-02-09 10:24:34 -08:00
Ramon Roche	85ab62dd67	github: try to fix env variables	2022-02-09 09:23:50 -08:00
Ramon Roche	b26ba88d40	github: try removing newline again	2022-02-08 21:56:21 -08:00
Ramon Roche	5132ffc0e0	github: remove whitespace from env variable	2022-02-08 21:49:49 -08:00
Ramon Roche	195082ccd1	github: docker run fix environemnt variables	2022-02-08 20:03:33 -08:00
Ramon Roche	f0f82b6994	github: import tagged image	2022-02-08 15:45:59 -08:00
Ramon Roche	e0dc8166dd	github: import with tag name	2022-02-08 11:36:34 -08:00
Ramon Roche	c181aefff7	github: debug docker build	2022-02-03 11:06:39 -08:00
Ramon Roche	963e5f5df9	github: install only required pip dep	2022-02-03 10:14:11 -08:00
Ramon Roche	991b288afb	github: fix docker builder run path	2022-02-03 09:16:43 -08:00
Ramon Roche	f3bfad4e8a	github: bump runner to python 3.9	2022-02-03 08:59:12 -08:00
Ramon Roche	0d319f7a2f	github: run non interactive	2022-02-03 08:17:37 -08:00
Ramon Roche	c09f7af718	github: fix container name	2022-02-02 22:24:51 -08:00
Ramon Roche	a2fc0f7924	container: docker import file path	2022-02-02 21:49:40 -08:00
Ramon Roche	ae0ba7cd98	container: build with no sim tools we are removing the gazebo sim tools from this image for now as we are trying to slim down the image	2022-02-02 10:38:45 -08:00
Ramon Roche	083a043776	github: docker import from previous job	2022-02-02 08:20:13 -08:00
Ramon Roche	854d529c81	github: generate target list	2022-02-01 21:49:52 -08:00
Ramon Roche	419c849a8e	github: remove replaced actions workflows were replaced by a single nuttx build	2022-02-01 20:56:04 -08:00
Ramon Roche	c19ed79237	container: add gnu c compiler for the arm64 arch	2022-02-01 20:40:49 -08:00
Ramon Roche	104ea0c53a	github: build nuttx with docker	2022-02-01 20:30:58 -08:00
Ramon Roche	fc8cdb0b6f	tools: docker runner accepts tags	2022-02-01 20:30:58 -08:00
Ramon Roche	64276b0c8d	github: docker build and push to registry build docker images on: * every pull request * push to master * stable release published pushes to registry if: * is not a pull request * is master branch * tag as 'latest' * is the result of release * tag with release name	2022-02-01 20:30:57 -08:00
Ramon Roche	939186fa7d	github: build docker on pr workflow	2022-02-01 20:30:57 -08:00
Ramon Roche	fc7e422821	container: general purpose multiarch docker * new docker container using ubuntu focal (20.04) * adds nuttx building for any supported target * adds initial support for gazebo simulation via X11 * updates docker_run.sh script to initialize container * NEEDS: updated ubuntu.sh script How to run: ``` ./Tools/docker_run.sh make all_variants_px4_fmu-v5x ```	2022-02-01 20:30:57 -08:00
Ramon Roche	92deb37f64	setup: robustify install script and update deps * increased support for running within a container * optionally install OpenJDK 14 * optionally install RTPS * adds better output messages	2022-02-01 20:30:57 -08:00
Ramon Roche	c6a019a9d3	ci: build every target on a single step Switching to use an all_variants approach to reduce the number of jobs we are creating per workflow since are starting to get throttled by GitHub, and we have limited resources. TODO: Build artifact uploadeds to S3, and GH Releases	2022-02-01 20:30:57 -08:00