Merge tag '8.0.0' into handle_guidedmode

.github/PULL_REQUEST_TEMPLATE/new_target_pull_request.md

@@ -0,0 +1,38 @@
+This pull requests adds support for ``$TARGET_NAME``
+
+ * [ ] Target complies with ``docs/policies/NEW_HARDWARE_POLICY.md``
+
+The following items have been checked and confirmed working by an INAV developer on actual hardware: (feel free to remove non-relevant items)
+
+ * [ ] Samples received
+ * [ ] Flash firmware
+ * [ ] Calibrate
+ * [ ] Orientation matches
+ * [ ] Gyro working
+ * [ ] Accel working
+ * [ ] Voltage correct
+ * [ ] Current correct
+ * [ ] Baro working
+ * [ ] Mag I2C Bus
+ * [ ] Additional I2C2 Buses (Airspeed/other accessories)
+ * [ ] UART1
+ * [ ] UART2
+ * [ ] UART3
+ * [ ] UART4
+ * [ ] UART5
+ * [ ] UART6
+ * [ ] UART7
+ * [ ] UART8
+ * [ ] Analog Camera working
+ * [ ] Video Out working
+ * [ ] OSD working
+ * [ ] LEDs working
+ * [ ] Buzzer working
+ * [ ] Motor outputs
+ * [ ] DShot support on m1-4
+ * [ ] Servo outputs
+ * [ ] Blackbox
+ * [ ] PINIO1
+ * [ ] PINIO2
+ * [ ] PINIO3
+ * [ ] PINIO4

.github/workflows/ci.yml

@@ -13,12 +13,21 @@ on:
       - 'CMakeLists.txt'
       - '*.sh'
 
+  workflow_call:
+    #inputs:
+    #  release_build:
+    #   description: 'Specifies if it is a build that should include commit hash in hex file names or not'
+    #   default: false
+    #   required: false
+    #   type: boolean
+
+
 jobs:
   build:
     runs-on: ubuntu-latest
     strategy:
         matrix:
-          id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15]
+          id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14]
 
     steps:
       - uses: actions/checkout@v4
@@ -37,7 +46,7 @@ jobs:
           # why we try github.event.pull_request.head.sha first
           COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
           BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
-          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
+          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/[ \t)]/, "", $2); print $2 }')
           echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
           echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
           echo "NUM_CORES=$(grep processor /proc/cpuinfo  | wc -l)" >> $GITHUB_ENV
@@ -72,7 +81,7 @@ jobs:
           # why we try github.event.pull_request.head.sha first
           COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
           BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
-          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
+          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/[ \t)]/, "", $2); print $2 }')
           echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
           echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
           echo "NUM_CORES=$(grep processor /proc/cpuinfo  | wc -l)" >> $GITHUB_ENV
@@ -96,6 +105,42 @@ jobs:
           name: targets
           path: targets.txt
 
+  build-SITL-Linux-arm64:
+    runs-on: ubuntu-22.04-arm
+    steps:
+      - uses: actions/checkout@v4
+      - name: Install dependencies
+        run: sudo apt-get update && sudo apt-get -y install ninja-build
+      - name: Setup environment
+        env:
+          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
+        run: |
+          # This is the hash of the commit for the PR
+          # when the action is triggered by PR, empty otherwise
+          COMMIT_ID=${{ github.event.pull_request.head.sha }}
+          # This is the hash of the commit when triggered by push
+          # but the hash of refs/pull/<n>/merge, which is different
+          # from the hash of the latest commit in the PR, that's
+          # why we try github.event.pull_request.head.sha first
+          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
+          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
+          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/[ \t)]/, "", $2); print $2 }')
+          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
+          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
+          echo "NUM_CORES=$(grep processor /proc/cpuinfo  | wc -l)" >> $GITHUB_ENV
+      - name: Build SITL
+        run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja -j${{ env.NUM_CORES }}
+      - name: Strip version number
+        run: |
+          for f in build_SITL/*_SITL; do
+            mv $f $(echo $f | sed -e 's/_[0-9]\+\.[0-9]\+\.[0-9]\+//')
+          done
+      - name: Upload artifacts
+        uses: actions/upload-artifact@v4
+        with:
+          name: ${{ env.BUILD_NAME }}_SITL-Linux-aarch64
+          path: ./build_SITL/*_SITL
+
   build-SITL-Linux:
     runs-on: ubuntu-latest
     steps:
@@ -115,12 +160,17 @@ jobs:
           # why we try github.event.pull_request.head.sha first
           COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
           BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
-          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
+          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/[ \t)]/, "", $2); print $2 }')
           echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
           echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
           echo "NUM_CORES=$(grep processor /proc/cpuinfo  | wc -l)" >> $GITHUB_ENV
       - name: Build SITL
         run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja -j${{ env.NUM_CORES }}
+      - name: Strip version number
+        run: |
+          for f in build_SITL/*_SITL; do
+            mv $f $(echo $f | sed -e 's/_[0-9]\+\.[0-9]\+\.[0-9]\+//')
+          done
       - name: Upload artifacts
         uses: actions/upload-artifact@v4
         with:
@@ -148,7 +198,7 @@ jobs:
           # why we try github.event.pull_request.head.sha first
           COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
           BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
-          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
+          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/[ \t)]/, "", $2); print $2 }')
           echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
           echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
           echo "NUM_CORES=$(grep processor /proc/cpuinfo  | wc -l)" >> $GITHUB_ENV
@@ -157,7 +207,11 @@ jobs:
           mkdir -p build_SITL && cd build_SITL
           cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -DCMAKE_OSX_ARCHITECTURES="arm64;x86_64" -G Ninja ..
           ninja -j4
-
+      - name: Strip version number
+        run: |
+          for f in build_SITL/*_SITL; do
+            mv -v $f $(echo $f | sed -Ee 's/_[0-9]+\.[0-9]+\.[0-9]+//')
+          done
       - name: Upload artifacts
         uses: actions/upload-artifact@v4
         with:
@@ -188,19 +242,23 @@ jobs:
           # why we try github.event.pull_request.head.sha first
           COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
           BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
-          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
+          VERSION=$( grep project CMakeLists.txt|awk -F VERSION '{ gsub(/[ \t)]/, "", $2); print $2 }' )
           echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
           echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
           
       - name: Build SITL
         run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja -j4
+      - name: Strip version number
+        run: |
+          for f in ./build_SITL/*_SITL.exe; do
+            mv $f $(echo $f | sed -e 's/_[0-9]\+\.[0-9]\+\.[0-9]\+//')
+          done
       - name: Upload artifacts
         uses: actions/upload-artifact@v4
         with:
           name: ${{ env.BUILD_NAME }}_SITL-WIN
           path: ./build_SITL/*.exe
 
-
   test:
     #needs: [build]
     runs-on: ubuntu-latest

.github/workflows/dev-builds.yml

@@ -1,245 +0,0 @@
-name: Build pre-release
-# Don't enable CI on push, just on PR. If you
-# are working on the main repo and want to trigger
-# a CI build submit a draft PR.
-on: 
-  push:
-    branches:
-    - master
-    paths:
-      - 'src/**'
-      - '.github/**'
-      - 'cmake/**'
-      - 'lib/**'
-      - 'docs/Settings.md'
-      - 'CMakeLists.txt'
-      - '*.sh'
-
-jobs:
-  build:
-    runs-on: ubuntu-latest
-    strategy:
-        matrix:
-          id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15]
-
-    steps:
-      - uses: actions/checkout@v4
-      - name: Install dependencies
-        run: sudo apt-get update && sudo apt-get -y install ninja-build
-      - name: Setup environment
-        env:
-          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
-        run: |
-          # This is the hash of the commit for the PR
-          # when the action is triggered by PR, empty otherwise
-          COMMIT_ID=${{ github.event.pull_request.head.sha }}
-          # This is the hash of the commit when triggered by push
-          # but the hash of refs/pull/<n>/merge, which is different
-          # from the hash of the latest commit in the PR, that's
-          # why we try github.event.pull_request.head.sha first
-          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
-          BUILD_SUFFIX=dev-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
-          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
-          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
-          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
-          echo "NUM_CORES=$(grep processor /proc/cpuinfo | wc -l)" >> $GITHUB_ENV
-      - uses: actions/cache@v4
-        with:
-          path: downloads
-          key: ${{ runner.os }}-downloads-${{ hashFiles('CMakeLists.txt') }}-${{ hashFiles('**/cmake/*')}}
-      - name: Build targets (${{ matrix.id }})
-        run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -DVERSION_TYPE=dev -G Ninja .. && ninja -j${{ env.NUM_CORES }} ci
-      - name: Upload artifacts
-        uses: actions/upload-artifact@v4
-        with:
-          name: ${{ env.BUILD_NAME }}.${{ matrix.id }}
-          path: ./build/*.hex
-
-  build-SITL-Linux:
-    runs-on: ubuntu-latest
-    steps:
-      - uses: actions/checkout@v4
-      - name: Install dependencies
-        run: sudo apt-get update && sudo apt-get -y install ninja-build
-      - name: Setup environment
-        env:
-          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
-        run: |
-          # This is the hash of the commit for the PR
-          # when the action is triggered by PR, empty otherwise
-          COMMIT_ID=${{ github.event.pull_request.head.sha }}
-          # This is the hash of the commit when triggered by push
-          # but the hash of refs/pull/<n>/merge, which is different
-          # from the hash of the latest commit in the PR, that's
-          # why we try github.event.pull_request.head.sha first
-          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
-          BUILD_SUFFIX=dev-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
-          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/[ \t\)]+/, "", $2); print $2 }')
-          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
-          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
-          echo "NUM_CORES=$(grep processor /proc/cpuinfo | wc -l)" >> $GITHUB_ENV
-      - name: Build SITL
-        run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja -DVERSION_TYPE=dev .. && ninja -j${{ env.NUM_CORES }}
-      - name: Upload artifacts
-        uses: actions/upload-artifact@v4
-        with:
-          name: sitl-${{ env.BUILD_NAME }}-Linux
-          path: ./build_SITL/*_SITL
-
-  build-SITL-Mac:
-    runs-on: macos-latest
-    steps:
-      - uses: actions/checkout@v4
-      - name: Install dependencies
-        run: |
-          brew install cmake ninja ruby
-
-      - name: Setup environment
-        env:
-          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
-        run: |
-          # This is the hash of the commit for the PR
-          # when the action is triggered by PR, empty otherwise
-          COMMIT_ID=${{ github.event.pull_request.head.sha }}
-          # This is the hash of the commit when triggered by push
-          # but the hash of refs/pull/<n>/merge, which is different
-          # from the hash of the latest commit in the PR, that's
-          # why we try github.event.pull_request.head.sha first
-          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
-          BUILD_SUFFIX=dev-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
-          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/[ \t\)]+/, "", $2); print $2 }')
-          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
-          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
-      - name: Build SITL
-        run: |
-          mkdir -p build_SITL && cd build_SITL
-          cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -DCMAKE_OSX_ARCHITECTURES="arm64;x86_64" -DVERSION_TYPE=dev -G Ninja ..
-          ninja -j4
-
-      - name: Upload artifacts
-        uses: actions/upload-artifact@v4
-        with:
-          name: sitl-${{ env.BUILD_NAME }}-MacOS
-          path: ./build_SITL/*_SITL
-
-  build-SITL-Windows:
-    runs-on: windows-latest
-    defaults:
-      run:
-        shell: C:\tools\cygwin\bin\bash.exe -o igncr '{0}'
-    steps:
-      - uses: actions/checkout@v4
-      - name: Setup Cygwin
-        uses: egor-tensin/setup-cygwin@v4
-        with:
-          packages: cmake ruby ninja gcc-g++
-      - name: Setup environment
-        env:
-          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
-        run: |
-          # This is the hash of the commit for the PR
-          # when the action is triggered by PR, empty otherwise
-          COMMIT_ID=${{ github.event.pull_request.head.sha }}
-          # This is the hash of the commit when triggered by push
-          # but the hash of refs/pull/<n>/merge, which is different
-          # from the hash of the latest commit in the PR, that's
-          # why we try github.event.pull_request.head.sha first
-          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
-          BUILD_SUFFIX=dev-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
-          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/[ \t\)]+/, "", $2); print $2 }')
-          #echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
-          #echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
-          #echo "VERSION_TAG=-$(date '+%Y%m%d')" >> $GITHUB_ENV
-          echo "version=${VERSION}" >> $GITHUB_OUTPUT
-      - name: Build SITL
-        run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -DVERSION_TYPE=dev -G Ninja .. && ninja -j4
-      - name: Upload artifacts
-        uses: actions/upload-artifact@v4
-        with:
-          name: sitl-${{ env.BUILD_NAME }}-WIN
-          path: ./build_SITL/*.exe
-
-
-  test:
-    runs-on: ubuntu-latest
-    steps:
-      - uses: actions/checkout@v4
-      - name: Install dependencies
-        run: sudo apt-get update && sudo apt-get -y install ninja-build
-      - name: Run Tests
-        run: mkdir -p build && cd build && cmake -DTOOLCHAIN=none -G Ninja .. && ninja check
-
-  release:
-    needs: [build, build-SITL-Linux, build-SITL-Mac, build-SITL-Windows, test]
-    runs-on: ubuntu-latest
-    steps:
-      - uses: actions/checkout@v4
-      - name: Get version
-        id: version
-        run: |
-          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/[ \t\)]+/, "", $2); print $2 }')
-          echo "version=${VERSION}" >> $GITHUB_OUTPUT
-      - name: Get current date
-        id: date
-        run: echo "today=$(date '+%Y%m%d')" >> $GITHUB_OUTPUT
-      - name: download artifacts
-        uses: actions/download-artifact@v4
-        with:
-          path: hexes
-          pattern: inav-*
-          merge-multiple: true
-      - name: download sitl linux
-        uses: actions/download-artifact@v4
-        with:
-          path: resources/sitl/linux
-          pattern: sitl-*-Linux
-          merge-multiple: true
-      - name: download sitl windows
-        uses: actions/download-artifact@v4
-        with:
-          path: resources/sitl/windows
-          pattern: sitl-*-WIN
-          merge-multiple: true
-      - name: download sitl mac
-        uses: actions/download-artifact@v4
-        with:
-          path: resources/sitl/macos
-          pattern: sitl-*-MacOS
-          merge-multiple: true
-      - name: Consolidate sitl files
-        run: |
-          zip -r -9 sitl-resources.zip resources/
-      - name: Upload release artifacts
-        uses: softprops/action-gh-release@v2
-        with:
-          name: inav-${{ steps.version.outputs.version }}-dev-${{ steps.date.outputs.today }}-${{ github.run_number }}-${{ github.sha }}
-          tag_name: v${{ steps.version.outputs.version }}-${{ steps.date.outputs.today }}.${{ github.run_number }}
-          # To create release on a different repo, we need a token setup
-          token: ${{ secrets.NIGHTLY_TOKEN }}
-          repository: iNavFlight/inav-nightly
-          prerelease: true
-          draft: false
-          #generate_release_notes: true
-          make_latest: false
-          files: |
-            hexes/*.hex
-            sitl-resources.zip
-          body: |
-            ${{ steps.notes.outputs.notes }}
-
-            ### Flashing
-            These are nightly builds and configuration settings can be added and removed often. Flashing with Full chip erase is strongly recommended to avoid issues.
-            Firmware related issues should be opened in the iNavflight/inav repository, not in inav-nightly.
-
-            ### Repository:
-            ${{ github.repository }} ([link](${{ github.event.repository.html_url }}))
-
-            ### Branch:
-            ${{ github.ref_name }} ([link](${{ github.event.repository.html_url }}/tree/${{ github.ref_name }}))
-
-            ### Latest changeset:
-            ${{ github.event.head_commit.id }} ([link](${{ github.event.head_commit.url }}))
-
-            ### Changes:
-            ${{ github.event.head_commit.message }}
-

.github/workflows/nightly-build.yml

@@ -0,0 +1,103 @@
+name: Build pre-release
+# Don't enable CI on push, just on PR. If you
+# are working on the main repo and want to trigger
+# a CI build submit a draft PR.
+on: 
+  push:
+    branches:
+    - master
+    paths:
+      - 'src/**'
+      - '.github/**'
+      - 'cmake/**'
+      - 'lib/**'
+      - 'docs/Settings.md'
+      - 'CMakeLists.txt'
+      - '*.sh'
+
+jobs:
+  build:
+    name: build
+    uses: ./.github/workflows/ci.yml
+
+  release:
+    name: Release
+    needs: [build]
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v4
+      - name: Get version
+        id: version
+        run: |
+          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/[ \t\)]/, "", $2); print $2 }')
+          echo "version=${VERSION}" >> $GITHUB_OUTPUT
+      - name: Get current date
+        id: date
+        run: echo "today=$(date '+%Y%m%d')" >> $GITHUB_OUTPUT
+      - name: download artifacts
+        uses: actions/download-artifact@v4
+        with:
+          path: hexes
+          pattern: matrix-inav-*
+          merge-multiple: true
+      - name: download sitl linux
+        uses: actions/download-artifact@v4
+        with:
+          path: resources/sitl/linux
+          pattern: inav-*SITL-Linux
+          merge-multiple: true
+      - name: download sitl linux aarch64
+        uses: actions/download-artifact@v4
+        with:
+          path: resources/sitl/linux/arm64
+          pattern: inav-*SITL-Linux-aarch64
+          merge-multiple: true
+      - name: download sitl windows
+        uses: actions/download-artifact@v4
+        with:
+          path: resources/sitl/windows
+          pattern: inav-*SITL-WIN
+          merge-multiple: true
+      - name: download sitl mac
+        uses: actions/download-artifact@v4
+        with:
+          path: resources/sitl/macos
+          pattern: inav-*SITL-MacOS
+          merge-multiple: true
+      - name: Consolidate sitl files
+        run: |
+          zip -r -9 sitl-resources.zip resources/
+      - name: Upload release artifacts
+        uses: softprops/action-gh-release@v2
+        with:
+          name: inav-${{ steps.version.outputs.version }}-dev-${{ steps.date.outputs.today }}-${{ github.run_number }}-${{ github.sha }}
+          tag_name: v${{ steps.version.outputs.version }}-${{ steps.date.outputs.today }}.${{ github.run_number }}
+          # To create release on a different repo, we need a token setup
+          token: ${{ secrets.NIGHTLY_TOKEN }}
+          repository: iNavFlight/inav-nightly
+          prerelease: true
+          draft: false
+          #generate_release_notes: true
+          make_latest: false
+          files: |
+            hexes/*.hex
+            sitl-resources.zip
+          body: |
+            ${{ steps.notes.outputs.notes }}
+
+            ### Flashing
+            These are nightly builds and configuration settings can be added and removed often. Flashing with Full chip erase is strongly recommended to avoid issues.
+            Firmware related issues should be opened in the iNavflight/inav repository, not in inav-nightly.
+
+            ### Repository:
+            ${{ github.repository }} ([link](${{ github.event.repository.html_url }}))
+
+            ### Branch:
+            ${{ github.ref_name }} ([link](${{ github.event.repository.html_url }}/tree/${{ github.ref_name }}))
+
+            ### Latest changeset:
+            ${{ github.event.head_commit.id }} ([link](${{ github.event.head_commit.url }}))
+
+            ### Changes:
+            ${{ github.event.head_commit.message }}
+

.vscode/c_cpp_properties.json

@@ -34,8 +34,26 @@
                 "USE_I2C_IO_EXPANDER",
                 "USE_PCF8574",
                 "USE_ESC_SENSOR",
+                "USE_PROGRAMMING_FRAMEWORK",
+                "USE_SERIALRX_GHST",
+                "USE_TELEMETRY_GHST",
+                "USE_CMS",
+                "USE_DJI_HD_OSD",
+                "USE_GYRO_KALMAN",
+                "USE_RANGEFINDER",
+                "USE_RATE_DYNAMICS",
+                "USE_SMITH_PREDICTOR",
+                "USE_ALPHA_BETA_GAMMA_FILTER",
+                "USE_MAG_VCM5883",
+                "USE_TELEMETRY_JETIEXBUS",
+                "USE_NAV",
+                "USE_SDCARD_SDIO",
+                "USE_SDCARD",
+                "USE_Q_TUNE",
+                "USE_GYRO_FFT_FILTER",
+                "USE_BARO_DPS310",
                 "USE_ADAPTIVE_FILTER",
-                "MCU_FLASH_SIZE 1024",
+                "MCU_FLASH_SIZE 1024"
             ],
             "configurationProvider": "ms-vscode.cmake-tools"
         }

.vscode/settings.json

@@ -4,7 +4,7 @@
         "cmath": "c",
         "ranges": "c",
         "navigation.h": "c",
-        "rth_trackback.h": "c"
+        "rth_trackback.h": "c",
         "platform.h": "c",
         "timer.h": "c",
         "bus.h": "c"

cmake/ci.cmake

@@ -3,13 +3,20 @@ if(DEFINED CI_JOB_INDEX AND DEFINED CI_JOB_COUNT)
     message("-- configuring CI job ${job_name}/${CI_JOB_COUNT}")
     get_property(targets GLOBAL PROPERTY RELEASE_TARGETS)
     list(LENGTH targets count)
+    message("-- ${count} total targets")
     math(EXPR per_job "(${count}+${CI_JOB_COUNT}-1)/${CI_JOB_COUNT}")
-    message("-- ${per_job} targets per job")
     math(EXPR start "${CI_JOB_INDEX}*${per_job}")
-    list(SUBLIST targets ${start} ${per_job} ci_targets)
-    message("-- will build targets: ${ci_targets}")
-    add_custom_target(ci
-        ${CMAKE_COMMAND} -E true
-        DEPENDS ${ci_targets}
-)
+    message("-- ${per_job} targets per job, starting at ${start}")
+    if(${start} LESS ${count})
+        list(SUBLIST targets ${start} ${per_job} ci_targets)
+        message("-- will build targets: ${ci_targets}")
+        add_custom_target(ci
+            ${CMAKE_COMMAND} -E true
+            DEPENDS ${ci_targets}
+        )
+    else()
+        add_custom_target(ci
+            ${CMAKE_COMMAND} -E true
+        )
+    endif()
 endif()

cmake/stm32f7.cmake

@@ -38,6 +38,8 @@ set(STM32F7_HAL_SRC
     stm32f7xx_ll_tim.c
     stm32f7xx_ll_usb.c
     stm32f7xx_ll_utils.c
+    stm32f7xx_hal_sd.c
+    stm32f7xx_ll_sdmmc.c
 )
 list(TRANSFORM STM32F7_HAL_SRC PREPEND "${STM32F7_HAL_DIR}/Src/")
 
@@ -74,7 +76,7 @@ main_sources(STM32F7_SRC
     drivers/system_stm32f7xx.c
     drivers/serial_uart_stm32f7xx.c
     drivers/serial_uart_hal.c
-    drivers/sdcard/sdmmc_sdio_f7xx.c
+    drivers/sdcard/sdmmc_sdio_hal.c
 )
 
 main_sources(STM32F7_MSC_SRC

cmake/stm32h7.cmake

@@ -161,7 +161,7 @@ main_sources(STM32H7_SRC
     drivers/serial_uart_stm32h7xx.c
     drivers/serial_uart_hal.c
     drivers/sdio.h
-    drivers/sdcard/sdmmc_sdio_h7xx.c
+    drivers/sdcard/sdmmc_sdio_hal.c
 )
 
 main_sources(STM32H7_MSC_SRC

docs/Geozones.md

@@ -0,0 +1,135 @@
+# Geozones
+
+## Introduction
+The Geozone feature allows pilots to define one or multiple areas on the map in Mission Control, to prevent accidental flying outside of allowed zones (Flight-Zones, FZ) or to avoid certain areas they are not allowed or not willing to fly in (No-Flight-Zone, NFZ). 
+This type of feature might be known to many pilots as a "Geofence" and despite providing the same core functionality, INAV Geozones are significantly more versatile and provide more safety and features. 
+
+Geozones can not only inform the Pilot on the OSD, if the aircraft is approaching a geozone border, it also gives the distance and direction to the closest border and the remaining flight distance to the breach point. Additionally, it provides autonomous avoidance features, if the Aircraft is in any kind of self-leveling flight mode. 
+The most important feature for safety is the automatic path planning for RTH (Start Return To Home), that automatically avoids NFZ areas if possible. 
+
+![image](https://github.com/user-attachments/assets/48e3e5cc-8517-4096-8497-399cf00ee541)
+
+
+## Compatibility
+- [INAV Version: 8.0 or later](https://github.com/iNavFlight/inav/releases)
+- [INAV Configurator: 8.0 or Later](https://github.com/iNavFlight/inav-configurator/releases)
+- [MWPTools: Snapshot 2024-11-15 or later](https://github.com/stronnag/mwptools) 
+- Only flight controller with more than 512k of Flash (STM32F405, STM32F765, STM32H743, etc.)
+- Plane, Multirotor (Rover and Boat are untested at time of writing)
+
+## Setup Procedure
+- In the INAV Configurator, switch to the Configuration Panel and enable "Geozone" in the features. 
+- Switch to the Mission Control Panel and you will see a new Icon to call up the Geozone Editor. If Zones are already set up, they will be loaded automatically, as soon as you enter Mission Control.
+  ![image](https://github.com/user-attachments/assets/23cd2149-b6a2-4945-b349-ee99863e74f0)
+- Click on the "+" Symbol to add a new zone and define its parameters to your desire.
+- The following Options are available:
+  - Shape: Configures a Zone as a Circle or a Polygon
+  - Type: Inclusive (FZ, green) or Exclusive (NFZ, red)
+  - Min. Alt (cm): lower ceiling of the Zone (0 represents the ground relative from the launch location or AMSL. No action will be taken at a minimum altitude of 0, so the aircraft can "dive out" of an Inclusive FZ on a hill. To have a Minimum Altitude action, set a negative altitude of -1 or lower)
+  - Max. Alt (cm): upper ceiling of the Zone (A value if 0 means no upper altitude limit)
+  - Action: Action to execute if an aircraft approaches the border of that Zone
+  - Radius: Circular Zone only, Radius of the Circle
+- Move the Zone-Markers to the desired locations, to create a bordered area with the shape and size needed (Or change the radius in case of a Circular Zone)
+- To add additional vertices, click on the borderline of the zone you are editing. This will add a new vertex to that line to move around.
+  ![image](https://github.com/user-attachments/assets/eacb6d3c-62d3-4bab-8874-1543c0a6b06d)
+- Add additional Zones as you like, Zones can be separated but also overlapping (See [Limitations]( ) for details)
+- After finishing the zone setup, click the "Store in EEPROM" Button to save the zones on the Flight Controller. It is important that the FC reboots after storing, as the Zones can only be used after a fresh boot process.
+  ![image](https://github.com/user-attachments/assets/4b278dd0-aa65-45f6-b914-22bdd753feaf)
+
+## Global Settings
+- In the Advanced Tuning Panel, you will find additional global settings for Geozones
+  ![image](https://github.com/user-attachments/assets/db567521-e256-4fb6-8ca6-6e6b8b57d7a9)
+  - Detection Distance `geozone_detection_distance`: Defines at what distance a Geozone will be shown as a System Message if a breach is imminent.
+  - Avoid Altitude Range `geozone_avoid_altitude_range`: When the Aircraft approaches an NFZ that has a upper limit (can be overflown at higher altitude), INAV will climb above the Zone automatically if the altitude difference between Zone ceiling and current Aircraft altitude is less, than this value. For fixed wing, you need to consider how steep the possible climb angle is.
+  - Safe Altitude Distance `geozone_safe_altitude_distance`: Vertical safety margin to avoid a ceiling or floor altitude breach at high vertical speed. If your FZ Ceiling is at 100m and this value set to 10m, the aircraft will not allow you to fly above 90m and descents if the Aircraft overshoots.
+  - Safehome as Inclusive `geozone_safehome_as_inclusive`: Defines every Safehome location as a circular Inclusive zone with the radius of `safehome_max_distance` to allow a FZ at ground altitude (For Landings) if the general FZ around it might have a minimum altitude.
+  - Safehome Zone Action `geozone_safehome_zone_action`: Defines the action on zone breach if Safehome is enabled as inclusive. This is helpful for flying fields with distance or altitude restrictions for LOS Pilots.
+  - Multirotor Stop Distance `geozone_mr_stop_distance:`: Distance from the Border a Multirotor will stop, if the Fence Action is Avoid or Loiter (For fixed wings, this will be calculated from the Loiter-Radius of the Plane).
+  - No Way Home Action `geozone_no_way_home_action`: If RTH cannot find a possible route in FS or RTH modes, the Aircraft will either emergency land or fly straight back home and ignores NFZ. 
+
+## Functions and Behaviors
+- Zone Type: Inclusive
+  - If craft is armed inside the Inclusive FZ, everything outside that zone is considered an NFZ.
+  - Inclusive FZ can be combined if they overlap and will be handled as one zone.
+  - Overlapping multiple FZ allows different upper and lower altitude limits for each zone, as long as they still overlap in 3D Space (Both overlapping zones have to have a overlapping altitude range as well).
+  - Arming the aircraft outside of an Inclusive Zone is prohibited within a 2km distance to the next vertex (Distance to a border between two vertex is not checked). Arming override can be used. Arming at a distance bigger than 2km is possible. 
+  - Arming a craft outside of an Inclusive FZ will disable all Inclusive zones. 
+- Zone Type: Exclusive
+  - Arming inside an Exclusive NFZ is prohibited. Arming override can be used but not recommended. If armed inside an NFZ the Pilot keeps control until the NFZ is left, then any configured Action will be executed on re-enter.
+  - Exclusive Zones can be combined and overlapped as needed.
+  - Exclusive NFZ with an upper or lower limit other than 0 can be overflown and underflown. The Automatic avoidance will only fly above NFZ if possible and never below.
+- Actions:
+  - Avoid: Also called “Bounce” (only airplanes): The aircraft flies away from the boundary at the same angle it approached it, like a pool ball bouncing off the table border. Multirotor will switch into "Position Hold".
+  - Hold: Position in front of the boundary is held. Airplances will adjust their loiter center according to the loider radius, to stay away from the border while circling.
+  - RTH: Triggers Return To Home. The Failsafe RTH Procedure is executed, so RTH Trackback is also used if enabled for Failsafe situations.
+  - None: No action (only info in OSD).
+- RTH:
+  - If RTH is enabled by Failsafe, User Command or Zone Action, INAV will calculate a path to the Home Location that automatically avoids NFZ and tries to stay inside the current FZ.
+  - If no Path can be calculated (Not able to climb over a blocking NFZ, No Intersection between FZ, too tight gaps between blocking NFZ) a configurable alternative action will be executed.
+    - Direct RTH: Ignores Flight zones and comes back in a direct path.
+    - Emergency Land: Executes a GPS enabled Emergency Landing (Circle down with horizontal position hold active on Planes).
+  - When direct "Line of sight" with the Home location is reached (No zones blocking path), regular RTH settings are executed.
+  - To abort the Smart-RTH feature and come back on a direct way, disable and Re-Enable RTH within 1 Second. This temporarily ignores all FZ and NFZ borders.
+- Return to Zone:
+  - If the Aircraft breaches into an NFZ or out of a FZ (by avoiding tight angled Zones or flown in Acro mode and then the mode switches to any Self-Level mode), RTZ is initiated and the aircraft flies back into the last permitted zone on the shortest possible course.
+
+## OSD Elements
+- Three dedicated OSD Elements have been added:
+  - Fence-Distance Horizontal shows the distance to the nearest Fence Border and the heading to that border. (ID 145)
+  - Fence-Distance Vertical shows the distance to the nearest ceiling or floor of a zone. (ID 146)
+  - Fence-Direction Vertical is an optional element to show if the nearest vertical border is above or below the aircraft. (ID 144)
+  ![image](https://github.com/user-attachments/assets/87dd3c5a-1046-4bd4-93af-5f8c9078b868)
+- The Flight-Mode will show AUTO if the Aircraft executes any kind of Fence-Action.
+- The System-Message shows the distance to a potential fence breach point, based on the current aircraft Attitude and Heading.
+- Additionally, the System Message shows the current Fence Action that is Executed.
+
+  
+## Limitations
+- The maximum number of dedicated zones of any type is 63.
+- The maximum number of vertices of all zones combined is 127. Every circular zone uses 2 vertices while every polygon has to consist of at least 3 vertices.
+- INAV can only execute one border-breach action at a time. This is especially important to consider for Airplanes that cannot hover. 
+  - Complicated Zone setups with overlaps and tight areas can cause a loiter or "bounce" into another NFZ that was not considered before.
+  - This can lead to a "Return to FZ" action that tries to find the shortest path into an allowed area.
+- All Geozone Actions are disabled when in Waypoint Mode. The Pilot is responsible for planning his mission accordingly, to not create a path that crosses NFZ areas. If a mission leads to such an area and the pilot disables WP mode, a "Return to FZ" action will be executed.
+- All Geozone Actions are disabled in ACRO and MANUAL Mode. INAV will not take over control in these modes and only OSD Warnings are shown.
+- Planning the Geozone as a polygon, needs to have the vertex numbering to be counter clockwise in ascending order. One vertex must not be dragged over another border to create crossing borders within one zone. INAV Configurator and MWP will check for that before uploading.
+  - Examples of Zones that are not allowed:
+    ![image](https://github.com/user-attachments/assets/50f1a441-39da-4f1c-9128-7375bc593fa5)
+- To properly combine multiple Inclusion FZ into one area, the Zones need to overlap at 2 borders and the points where the borders touch, must be at least 2.5x Loiter-Radius apart from Airplanes at least 2.5x Multirotor Stop Distance apart for Multirotor.
+  - Example:
+    ![image](https://github.com/user-attachments/assets/cc50e24b-dc83-4408-bcba-90d6da33eb63)
+- If multiple zones with different minimum and maximum altitudes are combined, they need to vertically overlap at least 50m.
+- There is a chance that Smart RTH cannot find a path around NFZ areas, if there are multiple very big zones blocking the path. Due to hardware limitations, the amount of waypoints that Smart RTH can create are limited. Many Zones with very long border lines (>500m) cause additional waypoints.
+- It is not recommended to edit geozones in CLI by hand as this bypasses a lot of sanity checks. Potential errors in zones will disable them or can lead to unexpected behaviors. Transferring Geozones with a DIFF between aircraft is fine.
+
+## CLI
+The Geozone Information are stored in two separate data arrays. The first array holds the main Geozone Information and settings. The second array holds the Geozone vertices. 
+The following commands are available for users: 
+
+- `geozone` without argument lists the current settings
+- `geozone reset <id>` resets a specific geozone and all related vertices. If no ID proveded, all geozones and vertices will be deleted.
+- `geozone vertex` - lists all vertices.
+- `geozone vertex reset` - deletes all vertices.
+- `geozone vertex reset <zone id>` - Deletes all vertices of the zone.
+- `geozone vertex reset <zone id> <vertex id>` - Deletes the vertex with the corresponding id from a zone.
+
+The following information are for app-developers. _DO NOT EDIT GEOZONES MANUALLY CLI_!
+
+`geozone <id> <shape> <type> <minimum altitude> <maximum altitude> <is_amsl> <fence action> <vertices count>`
+
+- id: 0 - 63
+- shape: 0 = Circular, 1 = Polygonal
+- type: 0 = Exclusive, 1 = Inclusive
+- minimum altitude: In centimetres, 0 = ground
+- maximum altitude: In centimetres, 0 = infinity
+- is_amsl: 0 = relative, 1 = AMSL
+- fence action: 0 = None, 1 = Avoid, 2 = Position hold, 3 = Return To Home
+- vertices count: 0-126 - Sanity check if number of vertices matches with configured zones
+
+`geozone vertex <zone id> <vertex idx> <latitude> <logitude>`
+
+- zone id: (0-63) The zone id to which this vertex belongs
+- vertex idx: Index of the vertex (0-126)
+- latitude/ logitude: Longitude and latitude of the vertex. Values in decimal degrees * 1e7. Example:the value 47.562004o becomes 475620040
+
+

docs/Navigation.md

@@ -5,13 +5,13 @@ The navigation system in INAV is responsible for assisting the pilot allowing al
 ## NAV ALTHOLD mode - altitude hold
 
 Altitude hold requires a valid source of altitude - barometer, GPS or rangefinder. The best source is chosen automatically. 
-In this mode THROTTLE stick controls climb rate (vertical velocity). When pilot moves stick up - quad goes up, pilot moves stick down - quad descends, you keep stick at neutral position - quad hovers.
+In this mode THROTTLE stick controls climb rate (vertical velocity). When pilot moves stick up - aircraft goes up, pilot moves stick down - 
+aircraft descends, you keep stick at neutral position - aircraft maintains current altitude.
 
-By default, GPS is available as an altitude source for airplanes only. Multirotor requires barometer, unless *inav_use_gps_no_baro* is enabled.
 
 ### CLI parameters affecting ALTHOLD mode:
 * *nav_use_midthr_for_althold* - when set to "0", firmware will remember where your throttle stick was when ALTHOLD was activated - this will be considered neutral position. When set to "1" - 50% throttle will be considered neutral position.
-* *inav_use_gps_no_baro* - Multirotor only: enable althold based on GPS only, without baromemer installed. Default is OFF. 
+
 
 ### Related PIDs
 PIDs affecting altitude hold: ALT & VEL
@@ -25,12 +25,12 @@ Throttle tilt compensation attempts to maintain constant vertical thrust when co
 
 ## NAV POSHOLD mode - position hold
 
-Position hold requires GPS, accelerometer and compass sensors. Multirotor requires barometer, unless *inav_use_gps_no_baro* is enabled. Flight modes that require a compass (POSHOLD, RTH) are locked until compass is properly calibrated.
+Position hold requires GPS, accelerometer and compass sensors. Multirotor requires barometer, unless is enabled. Flight modes that require a compass (POSHOLD, RTH) are locked until compass is properly calibrated.
 When activated, this mode will attempt to keep copter where it is (based on GPS coordinates). From INAV 2.0, POSHOLD is a full 3D position hold. Heading hold in this mode is assumed and activated automatically.
 
 ### CLI parameters affecting POSHOLD mode:
 * *nav_user_control_mode* - can be set to "0" (GPS_ATTI) or "1" (GPS_CRUISE), controls how firmware will respond to roll/pitch stick movement. When in GPS_ATTI mode, right stick controls attitude, when it is released, new position is recorded and held. When in GPS_CRUISE mode right stick controls velocity and firmware calculates required attitude on its own.
-* *inav_use_gps_no_baro* - Multirotor only: enable althold based on GPS only, without baromemer installed. Default is OFF. 
+
  
 ### Related PIDs
 PIDs affecting position hold: POS, POSR
@@ -42,7 +42,7 @@ PID meaning:
 
 Home for RTH is the position where vehicle was first armed. This position may be offset by the CLI settings `nav_rth_home_offset_distance` and `nav_rth_home_offset_direction`. This position may also be overridden with Safehomes. RTH requires accelerometer, compass and GPS sensors.
 
-RTH requires barometer for multirotor, unless unless *inav_use_gps_no_baro* is enabled.
+RTH requires barometer for multirotor.
 
 RTH will maintain altitude during the return. When home is reached, a copter will attempt automated landing. An airplane will either loiter around the home position, or attempt an automated landing, depending on your settings.
 When deciding what altitude to maintain, RTH has 6 different modes of operation (controlled by *nav_rth_alt_mode* and *nav_rth_altitude* cli variables):

docs/OSD.md

@@ -4,7 +4,7 @@ The On Screen Display, or OSD, is a feature that overlays flight data over the v
 
 
 General OSD information is in this document. Other documents cover specific OSD-related topics:
-* [Custom OSD Messages](https://github.com/iNavFlight/inav/wiki/OSD-custom-messages)
+* [Custom OSD Elements](https://github.com/iNavFlight/inav/wiki/Custom-OSD-Elements)
 * [OSD Hud and ESP32 radars](https://github.com/iNavFlight/inav/wiki/OSD-Hud-and-ESP32-radars)
 * [OSD Joystick](https://github.com/iNavFlight/inav/blob/master/docs/OSD%20Joystick.md)
 * [DJI compatible OSD.md](https://github.com/iNavFlight/inav/blob/master/docs/DJI%20compatible%20OSD.md)
@@ -189,6 +189,13 @@ Here are the OSD Elements provided by INAV.
 | 156 | OSD_CUSTOM_ELEMENT_6                             | 8.0.0  |       |
 | 157 | OSD_CUSTOM_ELEMENT_7                             | 8.0.0  |       |
 | 158 | OSD_CUSTOM_ELEMENT_8                             | 8.0.0  |       |
+| 159 | OSD_LQ_DOWNLINK                                  | 8.0.0  |       |
+| 160 | OSD_RX_POWER_DOWNLINK                            | 8.0.0  |       |
+| 161 | OSD_RX_BAND                                      | 8.0.0  |       |
+| 162 | OSD_RX_MODE                                      | 8.0.0  |       |
+| 163 | OSD_COURSE_TO_FENCE                              | 8.0.0  |       |
+| 164 | OSD_H_DIST_TO_FENCE                              | 8.0.0  |       |
+| 165 | OSD_V_DIST_TO_FENCE                              | 8.0.0  |       |
 
 # Pilot Logos
 

docs/Programming Framework.md

@@ -96,7 +96,7 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn
 | 47            | Edge                          | Momentarily true when triggered by `Operand A`. `Operand A` is the activation operator [`boolean`], `Operand B` _(Optional)_ is the time for the edge to stay active [ms]. After activation, operator will return `true` until the time in Operand B is reached. If a pure momentary edge is wanted. Just leave `Operand B` as the default `Value: 0` setting. |
 | 48            | Delay                         | Delays activation after being triggered. This will return `true` when `Operand A` _is_ true, and the delay time in `Operand B` [ms] has been exceeded. |
 | 49            | Timer                         | A simple on - off timer. `true` for the duration of `Operand A` [ms]. Then `false` for the duration of `Operand B` [ms]. |
-| 50            | Delta (|A| >= B)              | This returns `true` when the value of `Operand A` has changed by the value of `Operand B` or greater within 100ms. |
+| 50            | Delta             | This returns `true` when the value of `Operand A` has changed by the value of `Operand B` or greater within 100ms. ( \|ΔA\| >= B )  |
 | 51            | Approx Equals (A ~ B)         | `true` if `Operand B` is within 1% of `Operand A`. |
 | 52            | LED Pin PWM                   | Value `Operand A` from [`0` : `100`] starts PWM generation on LED Pin. See [LED pin PWM](LED%20pin%20PWM.md). Any other value stops PWM generation (stop to allow ws2812 LEDs updates in shared modes). |
 | 53            | Disable GPS Sensor Fix        | Disables the GNSS sensor fix. For testing GNSS failure. |

docs/SITL/SITL.md

@@ -62,7 +62,7 @@ All other interfaces (I2C, SPI, etc.) are not emulated.
 ## Remote control
 Multiple methods for connecting RC Controllers are available:
 - MSP_RX (TCP/IP)
-- joystick (via simulator)
+- joystick / radio attached via USB (via simulator)
 - serial receiver via USB to serial converter
 - any receiver with proxy flight controller
 
@@ -75,6 +75,7 @@ MSP_RX is the default, 18 channels are supported over TCP/IP connection.
 Only 8 channels are supported.
 
 Select "SIM (SITL)" as the receiver and set up a joystick in the simulator.
+Many RC transmittters (radios) can function as a joystick by plugging them in to the computer via USB, making this the simplest option in many cases.
 
 *Not available with INAV-X-Plane-HITL plugin.*
 

docs/Settings.md

@@ -674,7 +674,7 @@ Defines debug values exposed in debug variables (developer / debugging setting)
 
 ### disarm_always
 
-Disarms the motors independently of throttle value. Setting to OFF reverts to the old behaviour of disarming only when the throttle is low.
+When you switch to Disarm, do so regardless of throttle position. If this Setting is `OFF`. It will only disarm only when the throttle is low. This is similar to the previous `disarm_kill_switch` option. Default setting is the same as the old default behaviour.
 
 | Default | Min | Max |
 | --- | --- | --- |
@@ -842,6 +842,16 @@ Q factor for dynamic notches
 
 ---
 
+### enable_broken_o4_workaround
+
+DJI O4 release firmware has a broken MSP DisplayPort implementation. This enables a workaround to restore ARM detection.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
 ### esc_sensor_listen_only
 
 Enable when BLHeli32 Auto Telemetry function is used. Disable in every other case
@@ -908,7 +918,7 @@ EzTune filter cutoff frequency
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 110 | 10 | 300 |
+| 110 | 20 | 300 |
 
 ---
 
@@ -1178,7 +1188,7 @@ The target percentage of maximum mixer output used for determining the rates in
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 80 | 50 | 100 |
+| 90 | 50 | 100 |
 
 ---
 
@@ -1472,6 +1482,76 @@ Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solv
 
 ---
 
+### geozone_avoid_altitude_range
+
+Altitude range in which an attempt is made to avoid a geozone upwards
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 5000 | 0 | 1000000 |
+
+---
+
+### geozone_detection_distance
+
+Distance from which a geozone is detected
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 50000 | 0 | 1000000 |
+
+---
+
+### geozone_mr_stop_distance
+
+Distance in which multirotors stops before the border
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 15000 | 0 | 100000 |
+
+---
+
+### geozone_no_way_home_action
+
+Action if RTH with active geozones is unable to calculate a course to home
+
+| Default | Min | Max |
+| --- | --- | --- |
+| RTH |  |  |
+
+---
+
+### geozone_safe_altitude_distance
+
+Vertical distance that must be maintained to the upper and lower limits of the zone.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 1000 | 0 | 10000 |
+
+---
+
+### geozone_safehome_as_inclusive
+
+Treat nearest safehome as inclusive geozone
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
+### geozone_safehome_zone_action
+
+Fence action for safehome zone
+
+| Default | Min | Max |
+| --- | --- | --- |
+| NONE |  |  |
+
+---
+
 ### gimbal_pan_channel
 
 Gimbal pan rc channel index. 0 is no channel.
@@ -2002,6 +2082,16 @@ Uncertainty value for barometric sensor [cm]
 
 ---
 
+### inav_default_alt_sensor
+
+Sets the default altitude sensor to use. Settings GPS and BARO always use both sensors unless there is an altitude error between the sensors that exceeds a set limit. In this case only the selected sensor will be used while the altitude error limit is exceeded. GPS error limit = 2 * inav_max_eph_epv. BARO error limit = 4 * inav_baro_epv. Settings GPS_ONLY and BARO_ONLY will use only the selected sensor even if the other sensor is working. The other sensor will only be used as a backup if the selected sensor is no longer available to use.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| GPS |  |  |
+
+---
+
 ### inav_gravity_cal_tolerance
 
 Unarmed gravity calibration tolerance level. Won't finish the calibration until estimated gravity error falls below this value.
@@ -2122,6 +2212,16 @@ Weight of barometer measurements in estimated altitude and climb rate. Setting i
 
 ---
 
+### inav_w_z_baro_v
+
+Weight of barometer climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0.1 | 0 | 10 |
+
+---
+
 ### inav_w_z_gps_p
 
 Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors.
@@ -2552,6 +2652,16 @@ Rate of the extra3 message for MAVLink telemetry
 
 ---
 
+### mavlink_min_txbuffer
+
+Minimum percent of TX buffer space free, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 33 | 0 | 100 |
+
+---
+
 ### mavlink_pos_rate
 
 Rate of the position message for MAVLink telemetry
@@ -2562,6 +2672,16 @@ Rate of the position message for MAVLink telemetry
 
 ---
 
+### mavlink_radio_type
+
+Mavlink radio type. Affects how RSSI and LQ are reported on OSD.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| GENERIC |  |  |
+
+---
+
 ### mavlink_rc_chan_rate
 
 Rate of the RC channels message for MAVLink telemetry
@@ -2908,7 +3028,7 @@ If enabled, motor will stop when throttle is low on this mixer_profile
 
 | Default | Min | Max |
 | --- | --- | --- |
-| OFF | OFF | ON |
+| ON | OFF | ON |
 
 ---
 
@@ -3008,7 +3128,7 @@ Adjusts the deceleration response of fixed wing altitude control as the target a
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 20 | 5 | 100 |
+| 40 | 5 | 100 |
 
 ---
 
@@ -3492,6 +3612,16 @@ D gain of altitude PID controller (Fixedwing)
 
 ---
 
+### nav_fw_pos_z_ff
+
+FF gain of altitude PID controller (Fixedwing)
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 10 | 0 | 255 |
+
+---
+
 ### nav_fw_pos_z_i
 
 I gain of altitude PID controller (Fixedwing)
@@ -3508,7 +3638,7 @@ P gain of altitude PID controller (Fixedwing)
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 40 | 0 | 255 |
+| 30 | 0 | 255 |
 
 ---
 
@@ -4542,6 +4672,26 @@ Value above which the OSD current consumption element will start blinking. Measu
 
 ---
 
+### osd_decimals_altitude
+
+Number of decimals for the altitude displayed in the OSD [3-5].
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 3 | 3 | 5 |
+
+---
+
+### osd_decimals_distance
+
+Number of decimals for distance displayed in the OSD [3-5]. This includes distance from home, total distance, and distance remaining.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 3 | 3 | 5 |
+
+---
+
 ### osd_dist_alarm
 
 Value above which to make the OSD distance from home indicator blink (meters)
@@ -4908,7 +5058,7 @@ Number of digits used for mAh precision. Currently used by mAh Used and Battery
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 4 | 4 | 6 |
+| 4 | 3 | 6 |
 
 ---
 
@@ -5118,7 +5268,7 @@ Value below which Crossfire SNR Alarm pops-up. (dB)
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 4 | -20 | 10 |
+| 4 | -20 | 99 |
 
 ---
 

docs/VTOL.md

@@ -113,7 +113,7 @@ set mc_iterm_relax = RPY
 save
 ```
 
-# STEP1&2: Configuring as a Normal fixed-wing/Multi-Copter in two profiles separately
+# STEP1: Configuring as a normal fixed-wing in Profile 1
 
 1. **Select the fisrt Mixer Profile and PID Profile:**
    - In the CLI, switch to the mixer_profile and pid_profile you wish to set first. You can also switch mixer_profile/pid_profile through gui if with aforementioned presets loaded.
@@ -132,7 +132,10 @@ save
 
 ![Alt text](Screenshots/mixerprofile_fw_mixer.png)
 
-3. **Switch to Another Mixer Profile with PID Profile:**
+
+# STEP2: Configuring as a Multi-Copter in Profile 2
+
+1. **Switch to Another Mixer Profile with PID Profile:**
    - In the CLI, switch to another mixer_profile along with the appropriate pid_profile. You can also switch mixer_profile/pid_profile through gui if with aforementioned presets loaded.
      ```
      mixer_profile 2
@@ -141,7 +144,7 @@ save
      save
      ```
 
-4. **Configure the Multi-Copter/fixed-wing:**
+2. **Configure the Multicopter/tricopter:**
    - Set up your multi-copter/fixed-wing as usual, this time for mixer_profile 2 and pid_profile 2.
    - Utilize the 'MAX' input in the servo mixer to tilt the motors without altering the servo midpoint.
    - At this stage, focus on configuring profile-specific settings. You can streamline this process by copying and pasting the default PID settings.
@@ -174,7 +177,59 @@ save
 
 Conduct a bench test on the model (without props attached). The model can now switch between fixed-wing and multi-copter modes while armed. Furthermore, it is capable of mid-air switching, resulting in an immediate stall upon entering fixed-wing profile
 
-# STEP4: Transition Mixing (Multi-Rotor Profile)(Recommended)
+# STEP4: Tilting Servo Setup (Recommended)
+### Setting up the tilting servos to operate correclty is crucial for correct yaw control of the craft. Using the default setup works, but will most likely result in your craft crawling forward with evey yaw input.
+The steps below describe how you can fine-tune the tilting servos such as to obtian the desired result.
+
+1. **Set the tilt servos at 45 degrees:**
+   - Connect and power the tilting servos with your flight controller.
+   - Enter transition mode (your switch should be in the mid-position).
+   - Check from the Outputs tab output that the tilt servo channels are exactly at 1500μs.
+   - In this mode, your tilt servos should be at the 45-degree position and you can now mount the motor and prop to your tilt servo such that the angle of the motor mounting plate is at 45 degrees upwards.
+   - NOTE1: If you have dedicated tilt servos, you may have engraved indeces on the servos and tilting motor sassembly to help you with this step. If the servos don't end up exactly at 45 degrees due to the teeth on the servo and the control arm/plate, don't worry, this will be automatically adjusted after completing the other steps below.
+   - NOTE2: If you are using control rods to adjust the tilt of the servos, adjust the lenth of your control rod and the position of the control arm position the control arm as close as possible to the mid position. It will depend on the oriatation of the servo, but generally speaking, the control arm of the servo should be pointed perpendicular to the fuselage when the motor mounts are at the 45 degree setting.
+
+2. **Switch to Multicopter/Tricopter:**
+   - Assuming that you have set up your mixer similar to STEP1 and STEP2, you can now switch to the tricopter/multicopter mode and your servos should be tilting the motors upwards. If this is not the case, reverse the servo(s) in the Outputs tab such that the servo(s) is/are pointed upwards.
+   - It is OK for the servos not to point exactly 90 degrees upwards, but they should be as close as possible to that position.
+   - Also, ensure that your MAX values in the Mixer tab are at 100 and -100, so that your servo will move to the maximum position, as shown in the screenshots in STEP1 and STEP2.
+
+3. **Adjust the maximum throws for the Multicopter/Tricopter mode:**
+   - While in tricopter mode, go to the Outputs tab and adjust the endpoints MIN and MAX values such that when your motors are pointed slightly backwards.
+   - Rotate the prop such that it is pointed backwards towards the wing/motor mount and ensure that the gap is the same on both sides by adjusting the MIN and MAX values for the tilt servo channels.
+   - NOTE: You can check the distance with calipers or gauge blocks. Alternatively, you can adjust the MIN and MAX for your tilting servos such that the props are just touching the top of the wing or motor mount, and then you can increase/degrease the MIN and MAX values for each channel by the same ammount for both servos. This should ensure that you have the same gap between the tip of the prop and the wing or motor mount for both sides.
+
+4. **Adjust the minimum position for the Fixed-wing mode:**
+   - Repeat the same step as point 3 with the model in fixed-wing mode, where the servos are tilted forwards.
+   - For this step, you just have to make sure that the motors are pointed exactly forwards.
+   - You can do this by adjusting the respective MIN and MAX values in the Output tab for the tilt servo channels while in fixed-wing mode.
+   - NOTE: Ensuring that your servos are tilted exactly forward is a crucial step as it can cause the plane to roll slightly if that it is not the case. However, ensuring the exact aligment will depend on your specific setup. If you are using dedicated tilting motor servos rather than standard servos with control arms and pushrods, you can make sure that you are exact by measuring the distance between the front edge of the tilting servo and the of the motor mounting plate. If the disances are the uniform across each mount and same on both motors, your servos are pointed forwads correctly.
+
+5. **Adjsut the vertival position of the tilt servos:**
+   - Switch back to multicopter/tricopter mode and open the Mixer tab.
+   - Start adjusting the `MAX` mixer lines from STEP2 such that the servos are pointed exactly upwards. In other words, start reducing the values of 100 and -100 to something like 80 and -80 until the motors are are pointed exaxctly upwards.
+   - You will have to `Save & reboot` for adjustement for the changes to take effect, so be patient, take your time and don't forget to `Save & reboot`.
+   - Move the YAW stick to either extreme position and ensure that the servos are tilting the motors both forwards and backwards.
+   - NOTE: When yawing fully left, the left motor should tilt backwards and the right motor should tilt forwards. 
+  
+6. **Adjsut the throws of the tilt servos:**
+   - The final step is to adjust the throws of the servos such that they are the same in both directions.
+   - To do this, move back to the Mixer tab while in multicopter/tricopter mode and start adjusting the previously set up 50 and -50 values from the Stabilised Yaw lines.
+   - You can try with lower values of about 30 and -30 and then increase the values until you reach the maximum travel point.
+   - NOTE: The maximum is reached when both servos are moving the same ammount in oposite directions and one servo does not continue to move after the other has stopped. 
+
+7. **Check correct operation and direction:**
+   - Cycle back and forth between the plane, transition and tricopter modes to make sure that your servos are maintaining the same setting.
+   - For the fixed wing setting, your tilt servos should point the motors exactly forwards.
+   - For the multicoper/tricopter mode, the tilt servos should point the motors exactly upwards and when moving the yaw stick, both servos should tilt the motors the same ammount in opposite directions.
+
+Optional Setup Step for Tilt Servos:
+
+8. **Reversing tilt servos and mixer signs:**
+If you have set up the mixer as suggested in STEP1 and STEP2, you may have to deal with negative values for the mixer. You may wish to reverese a servo so that you don't have to deal with the negative signs. In that case, you may have to adjust the MIN and MAX values from point 4 again, so that your tilt servos are operating correctly. Check the operation of the servos once again for the YAW control in multicopter/tricipter mode as well as the horixontal position of the tilt servos in fixed-wing mode.
+
+
+# STEP5: Transition Mixing (Multi-Rotor Profile)(Recommended)
 ### Transition Mixing is typically useful in multi-copter profile to gain airspeed in prior to entering the fixed-wing profile. When the `MIXER TRANSITION` mode is activated, the associated motor or servo will move according to your configured Transition Mixing. 
 
 Please note that transition input is disabled when a navigation mode is activated. The use of Transition Mixing is necessary to enable additional features such as VTOL RTH with out stalling.

docs/boards/Omnibus F4.md

@@ -59,6 +59,12 @@ More target options:
 * Note that in multirotor configuration, servos are not enabled on S5 and S6
 * Uses target **OMNIBUSF4V6**
 
+For the Omnibus V6 and the Fireworks V2 boards, run the following command in the CLI tab to select the correct accelerometer / gyro:
+
+set gyro_to_use = 1
+set acc_hardware = auto
+save
+
 
 ### [Omnibus F4 Pro](https://inavflight.com/shop/p/OMNIBUSF4PRO)
 

docs/boards/RADIOLINKF722.md

@@ -0,0 +1,3 @@
+# RADIOLINKF722
+
+> Notice: DSHOT does not work on M6 output. If using M6, please use a different motor protocol. 

docs/development/Converting Betaflight Targets.md

@@ -0,0 +1,74 @@
+If your flight controller does not have an official INAV target, it is possible to use src/utils/bf2inav.py script to generate a good starting point for an unofficial INAV target.
+
+This script can read the config.h from [Betaflight's target configuration repository](https://github.com/betaflight/config) and it currently supports STM32F405, STM32F722, STM32F745, STM32H743 and AT32F435 targets.
+Locate your Flight Controller target config.h in BetaFlight's repo, eg.: [config.h](https://github.com/betaflight/config/blob/master/configs/BETAFPVF405/config.h), for BETAFPVF435 target.
+
+It is also advisable to record the output of the timer command from BetaFlight, as it will provide useful information on `timer` usage that can be used to adjust the generated target later.
+
+```
+# timer
+timer B08 AF3
+# pin B08: TIM10 CH1 (AF3)
+timer C08 AF3
+# pin C08: TIM8 CH3 (AF3)
+timer B00 AF2
+# pin B00: TIM3 CH3 (AF2)
+timer B01 AF2
+# pin B01: TIM3 CH4 (AF2)
+timer A03 AF1
+# pin A03: TIM2 CH4 (AF1)
+timer A02 AF1
+# pin A02: TIM2 CH3 (AF1)
+timer B06 AF2
+# pin B06: TIM4 CH1 (AF2)
+timer A08 AF1
+# pin A08: TIM1 CH1 (AF1)
+timer A09 AF1
+# pin A09: TIM1 CH2 (AF1)
+timer A10 AF1
+# pin A10: TIM1 CH3 (AF1)
+```
+In the above example, `pin B08: TIM10 CH1 (AF3)` means that pind to CH1. This information can be used to fix the generated timer assigned to match BetaFlight's allocation by editing the `target.c` file generated by the `bf2inav.py` script.
+
+
+Using the BETAFPVF405 target mentioned above, to create the target now we need to:
+
+1. Download INAV source code and be able to build
+2. Download the config.h from BetaFlight repository
+3. Create a target folder that will be used as the output folder for the `bf2inav.py` script, eg: `inav/src/main/targets/BETAFPVF405`
+4. Navigate to the script folder in `inav/src/utils/`
+5. `python3 ./bf2inav.py -i config.h -o ../main/target/BETAFPVF405/`
+6. Edit generated `target.c` and  chose the correct timer definitions to match Betaflight's timer definitions.
+```
+timerHardware_t timerHardware[] = {
+    DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0),
+    //DEF_TIM(TIM8, CH2N, PB0, TIM_USE_OUTPUT_AUTO, 0, 0),
+    //DEF_TIM(TIM1, CH2N, PB0, TIM_USE_OUTPUT_AUTO, 0, 0),
+
+    DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0),
+    //DEF_TIM(TIM8, CH3N, PB1, TIM_USE_OUTPUT_AUTO, 0, 0),
+    //DEF_TIM(TIM1, CH3N, PB1, TIM_USE_OUTPUT_AUTO, 0, 0),
+
+    //DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0),
+    //DEF_TIM(TIM9, CH2, PA3, TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0),
+
+    //DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0),
+    //DEF_TIM(TIM9, CH1, PA2, TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0),
+
+    DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0),
+    //DEF_TIM(TIM3, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0),
+
+    DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0),
+
+    //DEF_TIM(TIM3, CH1, PB4, TIM_USE_BEEPER, 0, 0),
+
+    DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0),
+
+};
+```
+In this particular example, PA3, PA2 were changed to match Betaflight's mapping, and the timer PB4 was disabled, due to a timer conflict. Normal channels are prefered over N channels (CH1, over CH1N) or C channels in AT32 architectures.
+7. Now update yout build scripts by running `cmake` and build the target you just created. The target name can be checked in the generated `CMakeLists.txt`, but should match the Betaflight target name.
+
+For information on how to build INAV, check the documents in the [docs/development](https://github.com/iNavFlight/inav/tree/master/docs/development) folder.

readme.md

@@ -1,3 +1,11 @@
+# INAV 8.0 feature freeze
+
+It is that time of the year again, and the time for a new INAV release is near!
+
+The current plan is to have a feature freeze on **15th of November 2024**.
+
+For a preview of what is comming, have a look at ![milestone 8.0](https://github.com/iNavFlight/inav/milestone/43).
+
 # INAV - navigation capable flight controller
 
 # F411 PSA

src/main/CMakeLists.txt

@@ -576,6 +576,7 @@ main_sources(COMMON_SRC
     navigation/navigation_pos_estimator_flow.c
     navigation/navigation_private.h
     navigation/navigation_rover_boat.c
+    navigation/navigation_geozone.c
     navigation/sqrt_controller.c
     navigation/sqrt_controller.h
     navigation/rth_trackback.c

src/main/cms/cms_menu_imu.c

@@ -227,6 +227,7 @@ static const OSD_Entry cmsx_menuPidAltMagEntries[] =
     OTHER_PIDFF_ENTRY("ALT P", &cmsx_pidPosZ.P),
     OTHER_PIDFF_ENTRY("ALT I", &cmsx_pidPosZ.I),
     OTHER_PIDFF_ENTRY("ALT D", &cmsx_pidPosZ.D),
+    OTHER_PIDFF_ENTRY("ALT FF", &cmsx_pidPosZ.FF),
 
     OTHER_PIDFF_ENTRY("VEL P", &cmsx_pidVelZ.P),
     OTHER_PIDFF_ENTRY("VEL I", &cmsx_pidVelZ.I),

src/main/cms/cms_menu_osd.c

@@ -147,10 +147,10 @@ static const OSD_Entry menuCrsfRxEntries[]=
     OSD_SETTING_ENTRY("LQ ALARM LEVEL", SETTING_OSD_LINK_QUALITY_ALARM),
     OSD_SETTING_ENTRY("SNR ALARM LEVEL", SETTING_OSD_SNR_ALARM),
     OSD_SETTING_ENTRY("RX SENSITIVITY", SETTING_OSD_RSSI_DBM_MIN),
-    OSD_ELEMENT_ENTRY("RX RSSI DBM", OSD_CRSF_RSSI_DBM),
-    OSD_ELEMENT_ENTRY("RX LQ", OSD_CRSF_LQ),
-    OSD_ELEMENT_ENTRY("RX SNR ALARM", OSD_CRSF_SNR_DB),
-    OSD_ELEMENT_ENTRY("TX POWER", OSD_CRSF_TX_POWER),
+    OSD_ELEMENT_ENTRY("RX RSSI DBM", OSD_RSSI_DBM),
+    OSD_ELEMENT_ENTRY("RX LQ", OSD_LQ_UPLINK),
+    OSD_ELEMENT_ENTRY("RX SNR ALARM", OSD_SNR_DB),
+    OSD_ELEMENT_ENTRY("TX POWER", OSD_TX_POWER_UPLINK),
 
     OSD_BACK_AND_END_ENTRY,
 };

src/main/common/streambuf.c

@@ -98,6 +98,11 @@ uint8_t sbufReadU8(sbuf_t *src)
     return *src->ptr++;
 }
 
+int8_t sbufReadI8(sbuf_t *src)
+{
+    return *src->ptr++;
+}
+
 uint16_t sbufReadU16(sbuf_t *src)
 {
     uint16_t ret;

src/main/common/streambuf.h

@@ -42,6 +42,7 @@ void sbufWriteU16BigEndian(sbuf_t *dst, uint16_t val);
 void sbufWriteU32BigEndian(sbuf_t *dst, uint32_t val);
 
 uint8_t sbufReadU8(sbuf_t *src);
+int8_t sbufReadI8(sbuf_t *src);
 uint16_t sbufReadU16(sbuf_t *src);
 uint32_t sbufReadU32(sbuf_t *src);
 void sbufReadData(const sbuf_t *dst, void *data, int len);

src/main/common/vector.h

@@ -29,6 +29,13 @@ typedef union {
     };
 } fpVector3_t;
 
+typedef union {
+    float v[2];
+    struct {
+       float x,y;
+    };
+} fpVector2_t;
+
 typedef struct {
     float m[3][3];
 } fpMat3_t;
@@ -116,3 +123,79 @@ static inline fpVector3_t * vectorScale(fpVector3_t * result, const fpVector3_t
     *result = ab;
     return result;
 }
+
+static inline fpVector3_t* vectorSub(fpVector3_t* result, const fpVector3_t* a, const fpVector3_t* b)
+{
+    fpVector3_t ab;
+
+    ab.x = a->x - b->x;
+    ab.y = a->y - b->y;
+    ab.z = a->z - b->z;
+
+    *result = ab;
+    return result;
+}
+
+static inline float vectorDotProduct(const fpVector3_t* a, const fpVector3_t* b)
+{
+    return  a->x * b->x + a->y * b->y + a->z * b->z;
+}
+
+static inline float vector2NormSquared(const fpVector2_t * v)
+{
+    return sq(v->x) + sq(v->y);
+}
+
+static inline fpVector2_t* vector2Normalize(fpVector2_t* result, const fpVector2_t* v)
+{
+    float sqa = vector2NormSquared(v);
+    float length = sqrtf(sqa);
+    if (length != 0) {
+        result->x = v->x / length;
+        result->y = v->y / length;
+    } else {
+        result->x = 0;
+        result->y = 0;
+    }
+    return result;
+}
+
+
+static inline fpVector2_t* vector2Sub(fpVector2_t* result, const fpVector2_t* a, const fpVector2_t* b)
+{
+    fpVector2_t ab;
+
+    ab.x = a->x - b->x;
+    ab.y = a->y - b->y;
+
+    *result = ab;
+    return result;
+}
+
+static inline fpVector2_t * vector2Add(fpVector2_t * result, const fpVector2_t * a, const fpVector2_t * b)
+{
+    fpVector2_t ab;
+
+    ab.x = a->x + b->x;
+    ab.y = a->y + b->y;
+
+    *result = ab;
+    return result;
+}
+
+
+static inline float vector2DotProduct(const fpVector2_t* a, const fpVector2_t* b)
+{
+    return  a->x * b->x + a->y * b->y;
+}
+
+static inline fpVector2_t * vector2Scale(fpVector2_t * result, const fpVector2_t * a, const float b)
+{
+    fpVector2_t ab;
+
+    ab.x = a->x * b;
+    ab.y = a->y * b;
+
+    *result = ab;
+    return result;
+}

src/main/config/parameter_group_ids.h

@@ -129,7 +129,10 @@
 #define PG_GIMBAL_CONFIG 1039
 #define PG_GIMBAL_SERIAL_CONFIG 1040
 #define PG_HEADTRACKER_CONFIG 1041
-#define PG_INAV_END PG_HEADTRACKER_CONFIG
+#define PG_GEOZONE_CONFIG 1042
+#define PG_GEOZONES 1043
+#define PG_GEOZONE_VERTICES 1044
+#define PG_INAV_END PG_GEOZONE_VERTICES
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047

src/main/drivers/accgyro/accgyro_icm42605.c

@@ -62,6 +62,7 @@
 
 #define ICM42605_RA_GYRO_DATA_X1                    0x25
 #define ICM42605_RA_ACCEL_DATA_X1                   0x1F
+#define ICM42605_RA_TEMP_DATA1                      0x1D
 
 #define ICM42605_RA_INT_CONFIG                      0x14
 #define ICM42605_INT1_MODE_PULSED                   (0 << 2)
@@ -321,6 +322,20 @@ static bool icm42605GyroRead(gyroDev_t *gyro)
     return true;
 }
 
+static bool icm42605ReadTemperature(gyroDev_t *gyro, int16_t * temp)
+{
+    uint8_t data[2];
+
+    const bool ack = busReadBuf(gyro->busDev, ICM42605_RA_TEMP_DATA1, data, 2);
+    if (!ack) {
+        return false;
+    }
+    // From datasheet: Temperature in Degrees Centigrade = (TEMP_DATA / 132.48) + 25 
+    *temp = ( int16_val_big_endian(data, 0) / 13.248 ) + 250; // Temperature stored as degC*10
+
+    return true;
+}
+
 bool icm42605GyroDetect(gyroDev_t *gyro)
 {
     gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_ICM42605, gyro->imuSensorToUse, OWNER_MPU);
@@ -340,7 +355,7 @@ bool icm42605GyroDetect(gyroDev_t *gyro)
     gyro->initFn = icm42605AccAndGyroInit;
     gyro->readFn = icm42605GyroRead;
     gyro->intStatusFn = gyroCheckDataReady;
-    gyro->temperatureFn = NULL;
+    gyro->temperatureFn = icm42605ReadTemperature;
     gyro->scale = 1.0f / 16.4f;     // 16.4 dps/lsb scalefactor
     gyro->gyroAlign = gyro->busDev->param;
 

src/main/drivers/barometer/barometer_bmp085.c

@@ -34,7 +34,9 @@
 
 #if defined(USE_BARO_BMP085)
 
+#if !defined(BMP085_I2C_ADDR)
 #define BMP085_I2C_ADDR         0x77
+#endif
 #define BMP085_CHIP_ID          0x55
 
 typedef struct {

src/main/drivers/barometer/barometer_bmp388.c

@@ -37,10 +37,13 @@
 #include "drivers/barometer/barometer.h"
 #include "drivers/barometer/barometer_bmp388.h"
 
-#if defined(USE_BARO) && (defined(USE_BARO_BMP388) || defined(USE_BARO_SPI_BMP388))
+#if defined(USE_BARO) && (defined(USE_BARO_BMP388) || defined(USE_BARO_SPI_BMP388) || defined(USE_BARO_BMP390) || defined(USE_BARO_SPI_BMP390))
 
+#if !defined(BMP388_I2C_ADDR)
 #define BMP388_I2C_ADDR                                 (0x76) // same as BMP280/BMP180
+#endif
 #define BMP388_DEFAULT_CHIP_ID                          (0x50) // from https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/bmp3_defs.h#L130
+#define BMP390_DEFAULT_CHIP_ID                          (0x60) // from https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/bmp3_defs.h#L133
 
 #define BMP388_CMD_REG                                  (0x7E)
 #define BMP388_RESERVED_UPPER_REG                       (0x7D)
@@ -314,7 +317,7 @@ static bool deviceDetect(busDevice_t * busDev)
 
         bool ack = busReadBuf(busDev, BMP388_CHIP_ID_REG, chipId, nRead);
 
-        if (ack && *pId == BMP388_DEFAULT_CHIP_ID) {
+        if (ack && (*pId == BMP388_DEFAULT_CHIP_ID || *pId == BMP390_DEFAULT_CHIP_ID)) {
             return true;
         }
     };

src/main/drivers/barometer/barometer_dps310.c

@@ -61,6 +61,7 @@
 
 
 #define DPS310_ID_REV_AND_PROD_ID       (0x10)
+#define SPL07_003_CHIP_ID               (0x11)
 
 #define DPS310_RESET_BIT_SOFT_RST       (0x09)    // 0b1001
 
@@ -96,6 +97,8 @@ typedef struct {
     int16_t c20;    // 16bit
     int16_t c21;    // 16bit
     int16_t c30;    // 16bit
+    int16_t c31;    // 12bit
+    int16_t c40;    // 12bit
 } calibrationCoefficients_t;
 
 typedef struct {
@@ -105,6 +108,7 @@ typedef struct {
 } baroState_t;
 
 static baroState_t  baroState;
+static uint8_t chipId[1];
 
 
 // Helper functions
@@ -167,7 +171,10 @@ static bool deviceConfigure(busDevice_t * busDev)
 
     // 1. Read the pressure calibration coefficients (c00, c10, c20, c30, c01, c11, and c21) from the Calibration Coefficient register.
     //   Note: The coefficients read from the coefficient register are 2's complement numbers.
-    uint8_t coef[18];
+
+    unsigned coefficientLength = chipId[0] == SPL07_003_CHIP_ID ? 21 : 18;
+    uint8_t coef[coefficientLength];
+
     if (!busReadBuf(busDev, DPS310_REG_COEF, coef, sizeof(coef))) {
         return false;
     }
@@ -199,6 +206,17 @@ static bool deviceConfigure(busDevice_t * busDev)
     // 0x20 c30 [15:8] + 0x21 c30 [7:0]
     baroState.calib.c30 = getTwosComplement(((uint32_t)coef[16] << 8) | (uint32_t)coef[17], 16);
 
+    if (chipId[0] == SPL07_003_CHIP_ID) {
+        // 0x23 c31 [3:0] + 0x22 c31 [11:4]
+        baroState.calib.c31 = getTwosComplement(((uint32_t)coef[18] << 4) | (((uint32_t)coef[19] >> 4) & 0x0F), 12);
+
+        // 0x23 c40 [11:8] + 0x24 c40 [7:0]
+        baroState.calib.c40 = getTwosComplement((((uint32_t)coef[19] & 0x0F) << 8) | (uint32_t)coef[20], 12);
+    } else {
+        baroState.calib.c31 = 0;
+        baroState.calib.c40 = 0; 
+    }
+
     // MEAS_CFG: Make sure the device is in IDLE mode
     registerWriteBits(busDev, DPS310_REG_MEAS_CFG, DPS310_MEAS_CFG_MEAS_CTRL_MASK, DPS310_MEAS_CFG_MEAS_IDLE);
 
@@ -218,8 +236,12 @@ static bool deviceConfigure(busDevice_t * busDev)
     registerSetBits(busDev, DPS310_REG_PRS_CFG, DPS310_PRS_CFG_BIT_PM_RATE_32HZ | DPS310_PRS_CFG_BIT_PM_PRC_16);
 
     // TMP_CFG: temperature measurement rate (32 Hz) and oversampling (16 times)
-    const uint8_t TMP_COEF_SRCE = registerRead(busDev, DPS310_REG_COEF_SRCE) & DPS310_COEF_SRCE_BIT_TMP_COEF_SRCE;
-    registerSetBits(busDev, DPS310_REG_TMP_CFG, DPS310_TMP_CFG_BIT_TMP_RATE_32HZ | DPS310_TMP_CFG_BIT_TMP_PRC_16 | TMP_COEF_SRCE);
+    if (chipId[0] == SPL07_003_CHIP_ID) {
+        registerSetBits(busDev, DPS310_REG_TMP_CFG, DPS310_TMP_CFG_BIT_TMP_RATE_32HZ | DPS310_TMP_CFG_BIT_TMP_PRC_16);
+    } else {
+        const uint8_t TMP_COEF_SRCE = registerRead(busDev, DPS310_REG_COEF_SRCE) & DPS310_COEF_SRCE_BIT_TMP_COEF_SRCE;
+        registerSetBits(busDev, DPS310_REG_TMP_CFG, DPS310_TMP_CFG_BIT_TMP_RATE_32HZ | DPS310_TMP_CFG_BIT_TMP_PRC_16 | TMP_COEF_SRCE);
+    }
 
     // CFG_REG: set pressure and temperature result bit-shift (required when the oversampling rate is >8 times)
     registerSetBits(busDev, DPS310_REG_CFG_REG, DPS310_CFG_REG_BIT_T_SHIFT | DPS310_CFG_REG_BIT_P_SHIFT);
@@ -265,9 +287,17 @@ static bool deviceReadMeasurement(baroDev_t *baro)
     const float c20 = baroState.calib.c20;
     const float c21 = baroState.calib.c21;
     const float c30 = baroState.calib.c30;
+    const float c31 = baroState.calib.c31;
+    const float c40 = baroState.calib.c40;
 
     // See section 4.9.1, How to Calculate Compensated Pressure Values, of datasheet
-    baroState.pressure = c00 + Praw_sc * (c10 + Praw_sc * (c20 + Praw_sc * c30)) + Traw_sc * c01 + Traw_sc * Praw_sc * (c11 + Praw_sc * c21);
+    // baroState.pressure = c00 + Praw_sc * (c10 + Praw_sc * (c20 + Praw_sc * c30)) + Traw_sc * c01 + Traw_sc * Praw_sc * (c11 + Praw_sc * c21);
+    if (chipId[0] == SPL07_003_CHIP_ID) {
+        baroState.pressure = c00 + Praw_sc * (c10 + Praw_sc * (c20 + Praw_sc * (c30 + Praw_sc * c40))) + Traw_sc * c01 + Traw_sc * Praw_sc * (c11 + Praw_sc * (c21 + Praw_sc * c31));
+    } else {
+        baroState.pressure = c00 + Praw_sc * (c10 + Praw_sc * (c20 + Praw_sc * c30)) + Traw_sc * c01 + Traw_sc * Praw_sc * (c11 + Praw_sc * c21);
+    }
+
 
     const float c0 = baroState.calib.c0;
     const float c1 = baroState.calib.c1;
@@ -299,13 +329,11 @@ static bool deviceCalculate(baroDev_t *baro, int32_t *pressure, int32_t *tempera
 static bool deviceDetect(busDevice_t * busDev)
 {
     for (int retry = 0; retry < DETECTION_MAX_RETRY_COUNT; retry++) {
-        uint8_t chipId[1];
-
         delay(100);
 
         bool ack = busReadBuf(busDev, DPS310_REG_ID, chipId, 1);
 
-        if (ack && chipId[0] == DPS310_ID_REV_AND_PROD_ID) {
+        if (ack && (chipId[0] == DPS310_ID_REV_AND_PROD_ID || chipId[0] == SPL07_003_CHIP_ID)) {
             return true;
         }
     };

src/main/drivers/barometer/barometer_spl06.h

@@ -17,7 +17,9 @@
 
 #pragma once
 
+#if !defined(SPL06_I2C_ADDR)
 #define SPL06_I2C_ADDR                         0x76
+#endif
 #define SPL06_DEFAULT_CHIP_ID                  0x10
 
 #define SPL06_PRESSURE_START_REG               0x00

src/main/drivers/gimbal_common.h

@@ -61,10 +61,10 @@ typedef struct gimbalConfig_s {
     uint8_t panChannel;
     uint8_t tiltChannel;
     uint8_t rollChannel;
-    uint8_t sensitivity;
-    uint16_t panTrim;
-    uint16_t tiltTrim;
-    uint16_t rollTrim;
+    int8_t sensitivity;
+    int16_t panTrim;
+    int16_t tiltTrim;
+    int16_t rollTrim;
 } gimbalConfig_t;
 
 PG_DECLARE(gimbalConfig_t, gimbalConfig);

src/main/drivers/osd_symbols.h

@@ -162,9 +162,9 @@
 
 #define SYM_MAX                     0xCE  // 206 MAX symbol
 #define SYM_PROFILE                 0xCF  // 207 Profile symbol
-#define SYM_SWITCH_INDICATOR_LOW    0xD0  // 208 Switch High
-#define SYM_SWITCH_INDICATOR_MID    0xD1  // 209 Switch Mid
-#define SYM_SWITCH_INDICATOR_HIGH   0xD2  // 210 Switch Low
+#define SYM_SWITCH_INDICATOR_LOW    0xD0  // 208 Switch Indicator Down/Low - Note: Some systems have HIGH us values for a switch in the down position
+#define SYM_SWITCH_INDICATOR_MID    0xD1  // 209 Switch Indicator Mid
+#define SYM_SWITCH_INDICATOR_HIGH   0xD2  // 210 Switch Indicator Up/High - Note: Some systems have LOW us values for a switch in the up position
 #define SYM_AH                      0xD3  // 211 Amphours symbol
 #define SYM_GLIDE_DIST              0xD4  // 212 Glide Distance
 #define SYM_GLIDE_MINS              0xD5  // 213 Glide Minutes
@@ -234,6 +234,8 @@
 #define SYM_AH_CH_CENTER            0x166 // 358 Crossair center
 #define SYM_FLIGHT_DIST_REMAINING   0x167 // 359 Flight distance reminaing
 #define SYM_ODOMETER                0x168 // 360 Odometer
+#define SYM_RX_BAND                 0x169 // 361 RX Band
+#define SYM_RX_MODE                 0x16A // 362 RX Mode
 
 #define SYM_AH_CH_TYPE3             0x190 // 400 to 402, crosshair 3
 #define SYM_AH_CH_TYPE4             0x193 // 403 to 405, crosshair 4

src/main/drivers/sdcard/sdcard_sdio.c

@@ -95,14 +95,21 @@ static bool sdcardSdio_isFunctional(void)
  */
 static void sdcardSdio_reset(void)
 {
-    if (SD_Init() != 0) {
-        sdcard.failureCount++;
-        if (sdcard.failureCount >= SDCARD_MAX_CONSECUTIVE_FAILURES || !sdcard_isInserted()) {
-            sdcard.state = SDCARD_STATE_NOT_PRESENT;
-        } else {
-            sdcard.operationStartTime = millis();
-            sdcard.state = SDCARD_STATE_RESET;
-        }
+    if (!sdcard_isInserted()) {
+        sdcard.state = SDCARD_STATE_NOT_PRESENT;
+        return;
+    }
+    if (SD_Init() != SD_OK) {
+        sdcard.state = SDCARD_STATE_NOT_PRESENT;
+        return;
+    }
+
+    sdcard.failureCount++;
+    if (sdcard.failureCount >= SDCARD_MAX_CONSECUTIVE_FAILURES) {
+        sdcard.state = SDCARD_STATE_NOT_PRESENT;
+    } else {
+        sdcard.operationStartTime = millis();
+        sdcard.state = SDCARD_STATE_RESET;
     }
 }
 
@@ -347,8 +354,20 @@ static bool sdcardSdio_poll(void)
                         break; // Timeout not reached yet so keep waiting
                     }
                     // Timeout has expired, so fall through to convert to a fatal error
+                    FALLTHROUGH;
 
                 case SDCARD_RECEIVE_ERROR:
+                    sdcardSdio_reset();
+
+                    if (sdcard.pendingOperation.callback) {
+                        sdcard.pendingOperation.callback(
+                            SDCARD_BLOCK_OPERATION_READ,
+                            sdcard.pendingOperation.blockIndex,
+                            NULL,
+                            sdcard.pendingOperation.callbackData
+                        );
+                    }
+
                     goto doMore;
                 break;
             }
@@ -561,17 +580,13 @@ void sdcardSdio_init(void)
         return;
     }
 
-    if (!SD_Initialize_LL(sdcard.dma->ref)) {
+    if (!SD_Initialize_LL(sdcard.dma)) {
         sdcard.dma = NULL;
         sdcard.state = SDCARD_STATE_NOT_PRESENT;
         return;
     }
-#else
-    if (!SD_Initialize_LL(0)) {
-        sdcard.state = SDCARD_STATE_NOT_PRESENT;
-        return;
-    }
 #endif
+
     // We don't support hot insertion
     if (!sdcard_isInserted()) {
         sdcard.state = SDCARD_STATE_NOT_PRESENT;

src/main/drivers/sdcard/sdmmc_sdio.h

@@ -31,22 +31,7 @@
 #if defined(USE_SDCARD_SDIO)
 
 #include "platform.h"
-
-#ifdef STM32F4
-#include "stm32f4xx.h"
-#endif
-
-#ifdef STM32F7
-#include "stm32f7xx.h"
-#endif
-
-#ifdef STM32H7
-#include "stm32h7xx.h"
-#endif
-
-#ifdef AT32F43x
-#include "at32f435_437.h"
-#endif
+#include "drivers/dma.h"
 
  /* SDCARD pinouts
  *
@@ -221,9 +206,9 @@ extern SD_CardType_t SD_CardType;
 
 #ifdef AT32F43x
 // TODO:AT32 TARGES NOT USE SD CARD ANT TF CARD FOR NOW
-void             SD_Initialize_LL            (dma_channel_type *dma);
+void             SD_Initialize_LL            (DMA_t dma);
 #else
-bool             SD_Initialize_LL            (DMA_Stream_TypeDef *dma);
+bool             SD_Initialize_LL            (DMA_t dma);
 #endif
 bool SD_Init(void);
 bool SD_IsDetected(void);

src/main/drivers/sdcard/sdmmc_sdio_f4xx.c

@@ -1356,8 +1356,10 @@ static SD_Error_t SD_FindSCR(uint32_t *pSCR)
 /**
   * @brief  Initialize the SDIO module, DMA, and IO
   */
-bool SD_Initialize_LL(DMA_Stream_TypeDef *dmaRef)
+bool SD_Initialize_LL(DMA_t dma)
 {
+    DMA_Stream_TypeDef *dmaRef = dma->ref;
+
     // Sanity check DMA stread - we only support two possible
     if (((uint32_t)dmaRef != (uint32_t)DMA2_Stream3) && ((uint32_t)dmaRef != (uint32_t)DMA2_Stream6)) {
         return false;

src/main/drivers/sdcard/sdmmc_sdio_hal.c

@@ -33,7 +33,6 @@
 #ifdef USE_SDCARD_SDIO
 
 #include "sdmmc_sdio.h"
-#include "stm32h7xx.h"
 
 #include "drivers/sdio.h"
 #include "drivers/io.h"
@@ -53,12 +52,12 @@ typedef struct SD_Handle_s
     volatile uint32_t TXCplt;          // SD TX Complete is equal 0 when no transfer
 } SD_Handle_t;
 
-SD_HandleTypeDef hsd1;
-
 SD_CardInfo_t                      SD_CardInfo;
 SD_CardType_t                      SD_CardType;
 
-static SD_Handle_t                 SD_Handle;
+static SD_Handle_t SD_Handle;
+static DMA_HandleTypeDef sd_dma;
+static SD_HandleTypeDef hsd;
 
 typedef struct sdioPin_s {
     ioTag_t pin;
@@ -75,6 +74,18 @@ typedef struct sdioPin_s {
 
 #define SDIO_MAX_PINDEFS 2
 
+#define IOCFG_SDMMC       IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL)
+
+#ifdef STM32F7
+#define SDMMC_CLK_DIV SDMMC_TRANSFER_CLK_DIV
+#else
+#if defined(SDCARD_SDIO_NORMAL_SPEED)
+#define SDMMC_CLK_DIV SDMMC_NSpeed_CLK_DIV
+#else
+#define SDMMC_CLK_DIV SDMMC_HSpeed_CLK_DIV
+#endif
+#endif
+
 typedef struct sdioHardware_s {
     SDMMC_TypeDef *instance;
     IRQn_Type irqn;
@@ -90,6 +101,7 @@ typedef struct sdioHardware_s {
 
 #define PINDEF(device, pin, afnum) { DEFIO_TAG_E(pin), GPIO_AF ## afnum ## _SDMMC ## device }
 
+#ifdef STM32H7
 static const sdioHardware_t sdioPinHardware[SDIODEV_COUNT] = {
     {
         .instance = SDMMC1,
@@ -112,6 +124,36 @@ static const sdioHardware_t sdioPinHardware[SDIODEV_COUNT] = {
         .sdioPinD3  = { PINDEF(2, PB4,   9) },
     }
 };
+#endif
+
+#ifdef STM32F7
+static const sdioHardware_t sdioPinHardware[SDIODEV_COUNT] = {
+    {
+        .instance = SDMMC1,
+        .irqn = SDMMC1_IRQn,
+        .sdioPinCK  = { PINDEF(1, PC12, 12) },
+        .sdioPinCMD = { PINDEF(1, PD2,  12) },
+        .sdioPinD0  = { PINDEF(1, PC8,  12) },
+        .sdioPinD1  = { PINDEF(1, PC9,  12) },
+        .sdioPinD2  = { PINDEF(1, PC10, 12) },
+        .sdioPinD3  = { PINDEF(1, PC11, 12) },
+        //.sdioPinD4  = { PINDEF(1, PB8, 12) },
+        //.sdioPinD5  = { PINDEF(1, PB9, 12) },
+        //.sdioPinD6  = { PINDEF(1, PC7, 12) },
+        //.sdioPinD7  = { PINDEF(1, PC11, 12) },
+    },
+    {
+        .instance = SDMMC2,
+        .irqn = SDMMC2_IRQn,
+        .sdioPinCK  = { PINDEF(2, PD6,  11) },
+        .sdioPinCMD = { PINDEF(2, PD7,  11) },
+        .sdioPinD0  = { PINDEF(2, PB14, 10), PINDEF(2, PG0,  11) },
+        .sdioPinD1  = { PINDEF(2, PB15, 10), PINDEF(2, PG10, 11) },
+        .sdioPinD2  = { PINDEF(2, PB3, 10), PINDEF(2, PG11, 10) },
+        .sdioPinD3  = { PINDEF(2, PB4, 10), PINDEF(2, PG12, 11) },
+    }
+};
+#endif
 
 #undef PINDEF
 
@@ -139,81 +181,31 @@ void sdioPinConfigure(void)
 #endif
     sdioPin[SDIO_PIN_D0] = sdioHardware->sdioPinD0[0];
 
-#ifdef SDCARD_SDIO_4BIT
-    sdioPin[SDIO_PIN_D1] = sdioHardware->sdioPinD1[0];
-    sdioPin[SDIO_PIN_D2] = sdioHardware->sdioPinD2[0];
-    sdioPin[SDIO_PIN_D3] = sdioHardware->sdioPinD3[0];
-#endif
-}
-
-#define IOCFG_SDMMC       IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL)
-
-void HAL_SD_MspInit(SD_HandleTypeDef* hsd)
-{
-    UNUSED(hsd);
-
-    if (!sdioHardware) {
-        return;
-    }
-
-    if (sdioHardware->instance == SDMMC1) {
-        __HAL_RCC_SDMMC1_CLK_DISABLE();
-        __HAL_RCC_SDMMC1_FORCE_RESET();
-        __HAL_RCC_SDMMC1_RELEASE_RESET();
-        __HAL_RCC_SDMMC1_CLK_ENABLE();
-    } else if (sdioHardware->instance == SDMMC2) {
-        __HAL_RCC_SDMMC2_CLK_DISABLE();
-        __HAL_RCC_SDMMC2_FORCE_RESET();
-        __HAL_RCC_SDMMC2_RELEASE_RESET();
-        __HAL_RCC_SDMMC2_CLK_ENABLE();
-    }
-
     const IO_t clk = IOGetByTag(sdioPin[SDIO_PIN_CK].pin);
     const IO_t cmd = IOGetByTag(sdioPin[SDIO_PIN_CMD].pin);
     const IO_t d0 = IOGetByTag(sdioPin[SDIO_PIN_D0].pin);
-    const IO_t d1 = IOGetByTag(sdioPin[SDIO_PIN_D1].pin);
-    const IO_t d2 = IOGetByTag(sdioPin[SDIO_PIN_D2].pin);
-    const IO_t d3 = IOGetByTag(sdioPin[SDIO_PIN_D3].pin);
-
-    IOConfigGPIOAF(clk, IOCFG_SDMMC, sdioPin[SDIO_PIN_CK].af);
-    IOConfigGPIOAF(cmd, IOCFG_SDMMC, sdioPin[SDIO_PIN_CMD].af);
-    IOConfigGPIOAF(d0, IOCFG_SDMMC, sdioPin[SDIO_PIN_D0].af);
-
-#ifdef SDCARD_SDIO_4BIT
-    IOConfigGPIOAF(d1, IOCFG_SDMMC, sdioPin[SDIO_PIN_D1].af);
-    IOConfigGPIOAF(d2, IOCFG_SDMMC, sdioPin[SDIO_PIN_D2].af);
-    IOConfigGPIOAF(d3, IOCFG_SDMMC, sdioPin[SDIO_PIN_D3].af);
-#endif
-
-    HAL_NVIC_SetPriority(sdioHardware->irqn, 0, 0);
-    HAL_NVIC_EnableIRQ(sdioHardware->irqn);
-}
-
-void SDIO_GPIO_Init(void)
-{
-    if (!sdioHardware) {
-        return;
-    }
-
-    const IO_t clk = IOGetByTag(sdioPin[SDIO_PIN_CK].pin);
-    const IO_t cmd = IOGetByTag(sdioPin[SDIO_PIN_CMD].pin);
-    const IO_t d0 = IOGetByTag(sdioPin[SDIO_PIN_D0].pin);
-    const IO_t d1 = IOGetByTag(sdioPin[SDIO_PIN_D1].pin);
-    const IO_t d2 = IOGetByTag(sdioPin[SDIO_PIN_D2].pin);
-    const IO_t d3 = IOGetByTag(sdioPin[SDIO_PIN_D3].pin);
 
     IOInit(clk, OWNER_SDCARD, RESOURCE_NONE, 0);
     IOInit(cmd, OWNER_SDCARD, RESOURCE_NONE, 0);
     IOInit(d0, OWNER_SDCARD, RESOURCE_NONE, 0);
 
 #ifdef SDCARD_SDIO_4BIT
+    sdioPin[SDIO_PIN_D1] = sdioHardware->sdioPinD1[0];
+    sdioPin[SDIO_PIN_D2] = sdioHardware->sdioPinD2[0];
+    sdioPin[SDIO_PIN_D3] = sdioHardware->sdioPinD3[0];
+
+    const IO_t d1 = IOGetByTag(sdioPin[SDIO_PIN_D1].pin);
+    const IO_t d2 = IOGetByTag(sdioPin[SDIO_PIN_D2].pin);
+    const IO_t d3 = IOGetByTag(sdioPin[SDIO_PIN_D3].pin);
+
     IOInit(d1, OWNER_SDCARD, RESOURCE_NONE, 0);
     IOInit(d2, OWNER_SDCARD, RESOURCE_NONE, 0);
     IOInit(d3, OWNER_SDCARD, RESOURCE_NONE, 0);
 #endif
 
     //
-    // Setting all the SDIO pins to high for a short time results in more robust initialisation.
+    // Setting all the SDIO pins to high for a short time results in more robust
+    // initialisation.
     //
     IOHi(d0);
     IOConfigGPIO(d0, IOCFG_OUT_PP);
@@ -233,50 +225,133 @@ void SDIO_GPIO_Init(void)
     IOConfigGPIO(cmd, IOCFG_OUT_PP);
 }
 
-bool SD_Initialize_LL(DMA_Stream_TypeDef *dma)
+void SDMMC_DMA_IRQHandler(DMA_t channel) 
 {
+    UNUSED(channel);
+
+    HAL_DMA_IRQHandler(&sd_dma);
+}
+
+void HAL_SD_MspInit(SD_HandleTypeDef* hsd)
+{
+    if (!sdioHardware) {
+        return;
+    }
+
+    if (sdioHardware->instance == SDMMC1) {
+        __HAL_RCC_SDMMC1_CLK_DISABLE();
+        __HAL_RCC_SDMMC1_CLK_ENABLE();
+
+        __HAL_RCC_SDMMC1_FORCE_RESET();
+        __HAL_RCC_SDMMC1_RELEASE_RESET();
+    } else if (sdioHardware->instance == SDMMC2) {
+        __HAL_RCC_SDMMC2_CLK_DISABLE();
+        __HAL_RCC_SDMMC2_CLK_ENABLE();
+
+        __HAL_RCC_SDMMC2_FORCE_RESET();
+        __HAL_RCC_SDMMC2_RELEASE_RESET();
+    }
+
+    const IO_t clk = IOGetByTag(sdioPin[SDIO_PIN_CK].pin);
+    const IO_t cmd = IOGetByTag(sdioPin[SDIO_PIN_CMD].pin);
+    const IO_t d0 = IOGetByTag(sdioPin[SDIO_PIN_D0].pin);
+    const IO_t d1 = IOGetByTag(sdioPin[SDIO_PIN_D1].pin);
+    const IO_t d2 = IOGetByTag(sdioPin[SDIO_PIN_D2].pin);
+    const IO_t d3 = IOGetByTag(sdioPin[SDIO_PIN_D3].pin);
+
+    IOConfigGPIOAF(clk, IOCFG_SDMMC, sdioPin[SDIO_PIN_CK].af);
+    IOConfigGPIOAF(cmd, IOCFG_SDMMC, sdioPin[SDIO_PIN_CMD].af);
+    IOConfigGPIOAF(d0, IOCFG_SDMMC, sdioPin[SDIO_PIN_D0].af);
+
+#ifdef SDCARD_SDIO_4BIT
+    IOConfigGPIOAF(d1, IOCFG_SDMMC, sdioPin[SDIO_PIN_D1].af);
+    IOConfigGPIOAF(d2, IOCFG_SDMMC, sdioPin[SDIO_PIN_D2].af);
+    IOConfigGPIOAF(d3, IOCFG_SDMMC, sdioPin[SDIO_PIN_D3].af);
+#endif
+
+#ifdef STM32F7
+    sd_dma.Init.Direction = DMA_PERIPH_TO_MEMORY;
+    sd_dma.Init.PeriphInc = DMA_PINC_DISABLE;
+    sd_dma.Init.MemInc = DMA_MINC_ENABLE;
+    sd_dma.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD;
+    sd_dma.Init.MemDataAlignment = DMA_MDATAALIGN_WORD;
+    sd_dma.Init.Mode = DMA_PFCTRL;
+    sd_dma.Init.Priority = DMA_PRIORITY_LOW;
+    sd_dma.Init.FIFOMode = DMA_FIFOMODE_ENABLE;
+    sd_dma.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL;
+    sd_dma.Init.MemBurst = DMA_MBURST_INC4;
+    sd_dma.Init.PeriphBurst = DMA_PBURST_INC4;
+
+    dmaInit(dmaGetByRef(sd_dma.Instance), OWNER_SDCARD, 0);
+    if (HAL_DMA_Init(&sd_dma) != HAL_OK) {
+        return;
+    }
+    dmaSetHandler(dmaGetByRef(sd_dma.Instance), SDMMC_DMA_IRQHandler, 1, 0);
+
+    __HAL_LINKDMA(hsd, hdmarx, sd_dma);
+    __HAL_LINKDMA(hsd, hdmatx, sd_dma);
+#else
+    UNUSED(hsd);
+#endif
+
+    HAL_NVIC_SetPriority(sdioHardware->irqn, 2, 0);
+    HAL_NVIC_EnableIRQ(sdioHardware->irqn);
+}
+
+bool SD_Initialize_LL(DMA_t dma)
+{
+#ifdef STM32F7
+    sd_dma.Instance = dma->ref;
+    sd_dma.Init.Channel = dmaGetChannelByTag(SDCARD_SDIO_DMA);
+#else
     UNUSED(dma);
+#endif
     return true;
 }
 
 bool SD_GetState(void)
 {
-    HAL_SD_CardStateTypedef cardState = HAL_SD_GetCardState(&hsd1);
+    HAL_SD_CardStateTypedef cardState = HAL_SD_GetCardState(&hsd);
 
     return (cardState == HAL_SD_CARD_TRANSFER);
 }
 
 bool SD_Init(void)
 {
-    HAL_StatusTypeDef status;
+    memset(&hsd, 0, sizeof(hsd));
 
-    memset(&hsd1, 0, sizeof(hsd1));
+    hsd.Instance = sdioHardware->instance;
 
-    hsd1.Instance = sdioHardware->instance;
+    // falling seems to work better ?? no idea whats "right" here
+    hsd.Init.ClockEdge = SDMMC_CLOCK_EDGE_FALLING;
 
-    hsd1.Init.ClockEdge = SDMMC_CLOCK_EDGE_RISING;
-    hsd1.Init.ClockPowerSave = SDMMC_CLOCK_POWER_SAVE_ENABLE;
-#ifdef SDCARD_SDIO_4BIT
-    hsd1.Init.BusWide = SDMMC_BUS_WIDE_4B;
-#else
-    hsd1.Init.BusWide = SDMMC_BUS_WIDE_1B; // FIXME untested
-#endif
-    hsd1.Init.HardwareFlowControl = SDMMC_HARDWARE_FLOW_CONTROL_ENABLE;
-#ifdef SDCARD_SDIO_NORMAL_SPEED
-        hsd1.Init.ClockDiv = SDMMC_NSpeed_CLK_DIV;
-#else
-        hsd1.Init.ClockDiv = 1; // 200Mhz / (2 * 1 ) = 100Mhz, used for "UltraHigh speed SD card" only, see   HAL_SD_ConfigWideBusOperation, SDMMC_HSpeed_CLK_DIV, SDMMC_NSpeed_CLK_DIV
+    // drastically increases the time to respond from the sdcard
+    // lets leave it off
+    hsd.Init.ClockPowerSave = SDMMC_CLOCK_POWER_SAVE_DISABLE;
+#ifdef STM32F7
+    hsd.Init.ClockBypass = SDMMC_CLOCK_BYPASS_DISABLE;
 #endif
+    hsd.Init.BusWide = SDMMC_BUS_WIDE_1B;
+    hsd.Init.HardwareFlowControl = SDMMC_HARDWARE_FLOW_CONTROL_ENABLE;
+    hsd.Init.ClockDiv = SDMMC_CLK_DIV;
 
-    status = HAL_SD_Init(&hsd1); // Will call HAL_SD_MspInit
-
-    if (status != HAL_OK) {
+    // Will call HAL_SD_MspInit
+    if (HAL_SD_Init(&hsd) != HAL_OK) {
+        return SD_ERROR;
+    }
+    if (hsd.SdCard.BlockNbr == 0) {
         return SD_ERROR;
     }
 
-    switch(hsd1.SdCard.CardType) {
+#ifdef SDCARD_SDIO_4BIT
+    if (HAL_SD_ConfigWideBusOperation(&hsd, SDMMC_BUS_WIDE_4B) != HAL_OK) {
+        return SD_ERROR;
+    }
+#endif
+
+    switch(hsd.SdCard.CardType) {
     case CARD_SDSC:
-        switch (hsd1.SdCard.CardVersion) {
+        switch (hsd.SdCard.CardVersion) {
         case CARD_V1_X:
             SD_CardType = SD_STD_CAPACITY_V1_1;
             break;
@@ -296,11 +371,11 @@ bool SD_Init(void)
         return SD_ERROR;
     }
 
-    // STATIC_ASSERT(sizeof(SD_Handle.CSD) == sizeof(hsd1.CSD), hal-csd-size-error);
-    memcpy(&SD_Handle.CSD, &hsd1.CSD, sizeof(SD_Handle.CSD));
+    // STATIC_ASSERT(sizeof(SD_Handle.CSD) == sizeof(hsd.CSD), hal-csd-size-error);
+    memcpy(&SD_Handle.CSD, &hsd.CSD, sizeof(SD_Handle.CSD));
 
-    // STATIC_ASSERT(sizeof(SD_Handle.CID) == sizeof(hsd1.CID), hal-cid-size-error);
-    memcpy(&SD_Handle.CID, &hsd1.CID, sizeof(SD_Handle.CID));
+    // STATIC_ASSERT(sizeof(SD_Handle.CID) == sizeof(hsd.CID), hal-cid-size-error);
+    memcpy(&SD_Handle.CID, &hsd.CID, sizeof(SD_Handle.CID));
 
     return SD_OK;
 }
@@ -525,15 +600,24 @@ SD_Error_t SD_WriteBlocks_DMA(uint64_t WriteAddress, uint32_t *buffer, uint32_t
         return SD_ERROR; // unsupported.
     }
 
+#ifndef STM32F7
     if ((uint32_t)buffer & 0x1f) {
         return SD_ADDR_MISALIGNED;
     }
+#endif
 
     // Ensure the data is flushed to main memory
     SCB_CleanDCache_by_Addr(buffer, NumberOfBlocks * BlockSize);
 
-    HAL_StatusTypeDef status;
-    if ((status = HAL_SD_WriteBlocks_DMA(&hsd1, (uint8_t *)buffer, WriteAddress, NumberOfBlocks)) != HAL_OK) {
+#ifdef STM32F7
+    if (sd_dma.Init.Direction != DMA_MEMORY_TO_PERIPH) {
+        sd_dma.Init.Direction = DMA_MEMORY_TO_PERIPH;
+        if (HAL_DMA_Init(&sd_dma) != HAL_OK) {
+            return SD_ERROR;
+        }
+    }
+#endif
+    if (HAL_SD_WriteBlocks_DMA(&hsd, (uint8_t *)buffer, WriteAddress, NumberOfBlocks) != HAL_OK) {
         return SD_ERROR;
     }
 
@@ -556,9 +640,11 @@ SD_Error_t SD_ReadBlocks_DMA(uint64_t ReadAddress, uint32_t *buffer, uint32_t Bl
         return SD_ERROR; // unsupported.
     }
 
+#ifndef STM32F7
     if ((uint32_t)buffer & 0x1f) {
         return SD_ADDR_MISALIGNED;
     }
+#endif
 
     SD_Handle.RXCplt = 1;
 
@@ -566,8 +652,15 @@ SD_Error_t SD_ReadBlocks_DMA(uint64_t ReadAddress, uint32_t *buffer, uint32_t Bl
     sdReadParameters.BlockSize = BlockSize;
     sdReadParameters.NumberOfBlocks = NumberOfBlocks;
 
-    HAL_StatusTypeDef status;
-    if ((status = HAL_SD_ReadBlocks_DMA(&hsd1, (uint8_t *)buffer, ReadAddress, NumberOfBlocks)) != HAL_OK) {
+#ifdef STM32F7
+    if (sd_dma.Init.Direction != DMA_PERIPH_TO_MEMORY) {
+        sd_dma.Init.Direction = DMA_PERIPH_TO_MEMORY;
+        if (HAL_DMA_Init(&sd_dma) != HAL_OK) {
+            return SD_ERROR;
+        }
+    }
+#endif
+    if (HAL_SD_ReadBlocks_DMA(&hsd, (uint8_t *)buffer, ReadAddress, NumberOfBlocks) != HAL_OK) {
         return SD_ERROR;
     }
 
@@ -615,12 +708,12 @@ void HAL_SD_AbortCallback(SD_HandleTypeDef *hsd)
 
 void SDMMC1_IRQHandler(void)
 {
-    HAL_SD_IRQHandler(&hsd1);
+    HAL_SD_IRQHandler(&hsd);
 }
 
 void SDMMC2_IRQHandler(void)
 {
-    HAL_SD_IRQHandler(&hsd1);
+    HAL_SD_IRQHandler(&hsd);
 }
 
 #endif

src/main/drivers/sdio.h

@@ -29,7 +29,6 @@ typedef enum {
 
 #define SDIODEV_COUNT 2
 
-#if defined(STM32H7)
+#if defined(STM32H7) || defined(STM32F7)
 void sdioPinConfigure(void);
-void SDIO_GPIO_Init(void);
 #endif

src/main/drivers/sdmmc_sdio.h

@@ -1,241 +0,0 @@
-/*
- * This file is part of Cleanflight and Betaflight.
- *
- * Cleanflight and Betaflight are free software. You can redistribute
- * this software and/or modify this software under the terms of the
- * GNU General Public License as published by the Free Software
- * Foundation, either version 3 of the License, or (at your option)
- * any later version.
- *
- * Cleanflight and Betaflight are distributed in the hope that they
- * will be useful, but WITHOUT ANY WARRANTY; without even the implied
- * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- * See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this software.
- *
- * If not, see <http://www.gnu.org/licenses/>.
- */
-
-/*
- * Original author: Alain (https://github.com/aroyer-qc)
- * Modified for F4 and BF source: Chris Hockuba (https://github.com/conkerkh)
- *
- */
-
-#ifndef __fatfs_sd_sdio_H__
-#define __fatfs_sd_sdio_H__
-
-#include <stdint.h>
-
-#include "platform.h"
-#ifdef STM32F4
-#include "stm32f4xx.h"
-#endif
-
-#ifdef STM32F7
-#include "stm32f7xx.h"
-#endif
-
- /* SDCARD pinouts
- *
- * SD CARD PINS
-   _________________
-  / 1 2 3 4 5 6 7 8 |  NR   |SDIO INTERFACE
- /                  |       |NAME     STM32F746     DESCRIPTION
-/ 9                 |       |         4-BIT  1-BIT
-|                   |       |
-|                   |   1   |CD/DAT3  PC11   -      Connector data line 3
-|                   |   2   |CMD      PD2    PD2    Command/Response line
-|                   |   3   |VSS1     GND    GND    GND
-|   SD CARD Pinout  |   4   |VDD      3.3V   3.3V   3.3V Power supply
-|                   |   5   |CLK      PC12   PC12   Clock
-|                   |   6   |VSS2     GND    GND    GND
-|                   |   7   |DAT0     PC8    PC8    Connector data line 0
-|                   |   8   |DAT1     PC9    -      Connector data line 1
-|___________________|   9   |DAT2     PC10   -      Connector data line 2
-
- */
-
-/* Define(s) --------------------------------------------------------------------------------------------------------*/
-
-//#define MSD_OK                        		    ((uint8_t)0x00)
-#define MSD_ERROR                     		    ((uint8_t)0x01)
-#define MSD_ERROR_SD_NOT_PRESENT      		    ((uint8_t)0x02)
-
-#define SD_PRESENT               				((uint8_t)0x01)
-#define SD_NOT_PRESENT           				((uint8_t)0x00)
-
-#define SD_DATATIMEOUT           				((uint32_t)100000000)
-
-#define SD_DETECT_GPIO_PORT                   	GPIOC
-#define SD_DETECT_PIN             				GPIO_PIN_13
-
-/* Structure(s) -----------------------------------------------------------------------------------------------------*/
-
-typedef enum
-{
-  // SD specific error defines
-    SD_CMD_CRC_FAIL                    = (1),   // Command response received (but CRC check failed)
-    SD_DATA_CRC_FAIL                   = (2),   // Data block sent/received (CRC check failed)
-    SD_CMD_RSP_TIMEOUT                 = (3),   // Command response TimeOut
-    SD_DATA_TIMEOUT                    = (4),   // Data TimeOut
-    SD_TX_UNDERRUN                     = (5),   // Transmit FIFO underrun
-    SD_RX_OVERRUN                      = (6),   // Receive FIFO overrun
-    SD_START_BIT_ERR                   = (7),   // Start bit not detected on all data signals in wide bus mode
-    SD_CMD_OUT_OF_RANGE                = (8),   // Command's argument was out of range.
-    SD_ADDR_MISALIGNED                 = (9),   // Misaligned address
-    SD_BLOCK_LEN_ERR                   = (10),  // Transferred block length is not allowed for the card or the number of transferred bytes does not match the block length
-    SD_ERASE_SEQ_ERR                   = (11),  // An error in the sequence of erase command occurs.
-    SD_BAD_ERASE_PARAM                 = (12),  // An invalid selection for erase groups
-    SD_WRITE_PROT_VIOLATION            = (13),  // Attempt to program a write protect block
-    SD_LOCK_UNLOCK_FAILED              = (14),  // Sequence or password error has been detected in unlock command or if there was an attempt to access a locked card
-    SD_COM_CRC_FAILED                  = (15),  // CRC check of the previous command failed
-    SD_ILLEGAL_CMD                     = (16),  // Command is not legal for the card state
-    SD_CARD_ECC_FAILED                 = (17),  // Card internal ECC was applied but failed to correct the data
-    SD_CC_ERROR                        = (18),  // Internal card controller error
-    SD_GENERAL_UNKNOWN_ERROR           = (19),  // General or unknown error
-    SD_STREAM_READ_UNDERRUN            = (20),  // The card could not sustain data transfer in stream read operation.
-    SD_STREAM_WRITE_OVERRUN            = (21),  // The card could not sustain data programming in stream mode
-    SD_CID_CSD_OVERWRITE               = (22),  // CID/CSD overwrite error
-    SD_WP_ERASE_SKIP                   = (23),  // Only partial address space was erased
-    SD_CARD_ECC_DISABLED               = (24),  // Command has been executed without using internal ECC
-    SD_ERASE_RESET                     = (25),  // Erase sequence was cleared before executing because an out of erase sequence command was received
-    SD_AKE_SEQ_ERROR                   = (26),  // Error in sequence of authentication.
-    SD_INVALID_VOLTRANGE               = (27),
-    SD_ADDR_OUT_OF_RANGE               = (28),
-    SD_SWITCH_ERROR                    = (29),
-    SD_SDMMC_DISABLED                  = (30),
-    SD_SDMMC_FUNCTION_BUSY             = (31),
-    SD_SDMMC_FUNCTION_FAILED           = (32),
-    SD_SDMMC_UNKNOWN_FUNCTION          = (33),
-    SD_OUT_OF_BOUND                    = (34),
-
-
-    // Standard error defines
-    SD_INTERNAL_ERROR                  = (35),
-    SD_NOT_CONFIGURED                  = (36),
-    SD_REQUEST_PENDING                 = (37),
-    SD_REQUEST_NOT_APPLICABLE          = (38),
-    SD_INVALID_PARAMETER               = (39),
-    SD_UNSUPPORTED_FEATURE             = (40),
-    SD_UNSUPPORTED_HW                  = (41),
-    SD_ERROR                           = (42),
-    SD_BUSY                            = (43),
-    SD_OK                              = (0)
-} SD_Error_t;
-
-
-typedef struct
-{
-    uint8_t  DAT_BUS_WIDTH;           // Shows the currently defined data bus width
-    uint8_t  SECURED_MODE;            // Card is in secured mode of operation
-    uint16_t SD_CARD_TYPE;            // Carries information about card type
-    uint32_t SIZE_OF_PROTECTED_AREA;  // Carries information about the capacity of protected area
-    uint8_t  SPEED_CLASS;             // Carries information about the speed class of the card
-    uint8_t  PERFORMANCE_MOVE;        // Carries information about the card's performance move
-    uint8_t  AU_SIZE;                 // Carries information about the card's allocation unit size
-    uint16_t ERASE_SIZE;              // Determines the number of AUs to be erased in one operation
-    uint8_t  ERASE_TIMEOUT;           // Determines the TimeOut for any number of AU erase
-    uint8_t  ERASE_OFFSET;            // Carries information about the erase offset
-} SD_CardStatus_t;
-
-typedef struct
-{
-    uint8_t  CSDStruct;                 // CSD structure
-    uint8_t  SysSpecVersion;            // System specification version
-    uint8_t  Reserved1;                 // Reserved
-    uint8_t  TAAC;                      // Data read access time 1
-    uint8_t  NSAC;                      // Data read access time 2 in CLK cycles
-    uint8_t  MaxBusClkFrec;             // Max. bus clock frequency
-    uint16_t CardComdClasses;           // Card command classes
-    uint8_t  RdBlockLen;                // Max. read data block length
-    uint8_t  PartBlockRead;             // Partial blocks for read allowed
-    uint8_t  WrBlockMisalign;           // Write block misalignment
-    uint8_t  RdBlockMisalign;           // Read block misalignment
-    uint8_t  DSRImpl;                   // DSR implemented
-    uint8_t  Reserved2;                 // Reserved
-    uint32_t DeviceSize;                // Device Size
-    uint8_t  MaxRdCurrentVDDMin;        // Max. read current @ VDD min
-    uint8_t  MaxRdCurrentVDDMax;        // Max. read current @ VDD max
-    uint8_t  MaxWrCurrentVDDMin;        // Max. write current @ VDD min
-    uint8_t  MaxWrCurrentVDDMax;        // Max. write current @ VDD max
-    uint8_t  DeviceSizeMul;             // Device size multiplier
-    uint8_t  EraseGrSize;               // Erase group size
-    uint8_t  EraseGrMul;                // Erase group size multiplier
-    uint8_t  WrProtectGrSize;           // Write protect group size
-    uint8_t  WrProtectGrEnable;         // Write protect group enable
-    uint8_t  ManDeflECC;                // Manufacturer default ECC
-    uint8_t  WrSpeedFact;               // Write speed factor
-    uint8_t  MaxWrBlockLen;             // Max. write data block length
-    uint8_t  WriteBlockPaPartial;       // Partial blocks for write allowed
-    uint8_t  Reserved3;                 // Reserved
-    uint8_t  ContentProtectAppli;       // Content protection application
-    uint8_t  FileFormatGrouop;          // File format group
-    uint8_t  CopyFlag;                  // Copy flag (OTP)
-    uint8_t  PermWrProtect;             // Permanent write protection
-    uint8_t  TempWrProtect;             // Temporary write protection
-    uint8_t  FileFormat;                // File format
-    uint8_t  ECC;                       // ECC code
-    uint8_t  CSD_CRC;                   // CSD CRC
-    uint8_t  Reserved4;                 // Always 1
-} SD_CSD_t;
-
-typedef struct
-{
-    uint8_t  ManufacturerID;            // Manufacturer ID
-    uint16_t OEM_AppliID;               // OEM/Application ID
-    uint32_t ProdName1;                 // Product Name part1
-    uint8_t  ProdName2;                 // Product Name part2
-    uint8_t  ProdRev;                   // Product Revision
-    uint32_t ProdSN;                    // Product Serial Number
-    uint8_t  Reserved1;                 // Reserved1
-    uint16_t ManufactDate;              // Manufacturing Date
-    uint8_t  CID_CRC;                   // CID CRC
-    uint8_t  Reserved2;                 // Always 1
-
-} SD_CID_t;
-
-typedef enum
-{
-    SD_STD_CAPACITY_V1_1       = 0,
-    SD_STD_CAPACITY_V2_0       = 1,
-    SD_HIGH_CAPACITY           = 2,
-    SD_MULTIMEDIA              = 3,
-    SD_SECURE_DIGITAL_IO       = 4,
-    SD_HIGH_SPEED_MULTIMEDIA   = 5,
-    SD_SECURE_DIGITAL_IO_COMBO = 6,
-    SD_HIGH_CAPACITY_MMC       = 7,
-} SD_CardType_t;
-
-typedef struct
-{
-  volatile SD_CSD_t    SD_csd;          // SD card specific data register
-  volatile SD_CID_t    SD_cid;          // SD card identification number register
-  uint64_t             CardCapacity;    // Card capacity
-  uint32_t             CardBlockSize;   // Card block size
-} SD_CardInfo_t;
-
-/* Prototype(s) -----------------------------------------------------------------------------------------------------*/
-
-extern           SD_CardInfo_t               SD_CardInfo;
-extern           SD_CardType_t               SD_CardType;
-
-void             SD_Initialize_LL            (DMA_Stream_TypeDef *dma);
-bool             SD_Init                     (void);
-bool             SD_IsDetected				(void);
-bool             SD_GetState                 (void);
-SD_Error_t       SD_GetCardInfo              (void);
-
-SD_Error_t       SD_ReadBlocks_DMA           (uint64_t ReadAddress, uint32_t *buffer, uint32_t BlockSize, uint32_t NumberOfBlocks);
-SD_Error_t       SD_CheckRead                (void);
-SD_Error_t       SD_WriteBlocks_DMA          (uint64_t WriteAddress, uint32_t *buffer, uint32_t BlockSize, uint32_t NumberOfBlocks);
-SD_Error_t       SD_CheckWrite               (void);
-
-SD_Error_t       SD_Erase                    (uint64_t StartAddress, uint64_t EndAddress);
-SD_Error_t       SD_GetCardStatus            (SD_CardStatus_t* pCardStatus);
-
-/* ------------------------------------------------------------------------------------------------------------------*/
-
-#endif // __fatfs_sd_sdio_H__

src/main/drivers/serial_uart_at32f43x.c

@@ -36,25 +36,38 @@ typedef struct uartDevice_s {
     usart_type* dev;
     uartPort_t port;
     ioTag_t rx;
+    uint8_t rx_af;
     ioTag_t tx;
+    uint8_t tx_af;
     volatile uint8_t rxBuffer[UART_RX_BUFFER_SIZE];
     volatile uint8_t txBuffer[UART_TX_BUFFER_SIZE];
     uint32_t rcc_ahb1;
     rccPeriphTag_t rcc_apb2;
     rccPeriphTag_t rcc_apb1;
-    uint8_t af;
     uint8_t irq;
     uint32_t irqPriority;
     bool pinSwap;
 } uartDevice_t;
 
 #ifdef USE_UART1
-static uartDevice_t uart1 =
-{
+static uartDevice_t uart1 = {
     .dev = USART1,
     .rx = IO_TAG(UART1_RX_PIN),
     .tx = IO_TAG(UART1_TX_PIN),
-    .af = GPIO_MUX_7,
+#if defined(UART1_RX_AF)
+    .rx_af = CONCAT(GPIO_MUX_, UART1_RX_AF),
+#elif defined(UART1_AF)
+    .rx_af = CONCAT(GPIO_MUX_, UART1_AF),
+#else
+    .rx_af = GPIO_MUX_7,
+#endif
+#if defined(UART1_TX_AF)
+    .tx_af = CONCAT(GPIO_MUX_, UART1_TX_AF),
+#elif defined(UART1_AF)
+    .tx_af = CONCAT(GPIO_MUX_, UART1_AF),
+#else
+    .tx_af = GPIO_MUX_7,
+#endif
 #ifdef UART1_AHB1_PERIPHERALS
     .rcc_ahb1 = UART1_AHB1_PERIPHERALS,
 #endif
@@ -75,7 +88,20 @@ static uartDevice_t uart2 =
     .dev = USART2,
     .rx = IO_TAG(UART2_RX_PIN),
     .tx = IO_TAG(UART2_TX_PIN),
-    .af = GPIO_MUX_7,
+#if defined(UART2_RX_AF)
+    .rx_af = CONCAT(GPIO_MUX_, UART2_RX_AF),
+#elif defined(UART2_AF)
+    .rx_af = CONCAT(GPIO_MUX_, UART2_AF),
+#else
+    .rx_af = GPIO_MUX_7,
+#endif
+#if defined(UART2_TX_AF)
+    .tx_af = CONCAT(GPIO_MUX_, UART2_TX_AF),
+#elif defined(UART2_AF)
+    .tx_af = CONCAT(GPIO_MUX_, UART2_AF),
+#else
+    .tx_af = GPIO_MUX_7,
+#endif
 #ifdef UART2_AHB1_PERIPHERALS
     .rcc_ahb1 = UART2_AHB1_PERIPHERALS,
 #endif
@@ -96,7 +122,20 @@ static uartDevice_t uart3 =
     .dev = USART3,
     .rx = IO_TAG(UART3_RX_PIN),
     .tx = IO_TAG(UART3_TX_PIN),
-    .af = GPIO_MUX_7,
+#if defined(UART3_RX_AF)
+    .rx_af = CONCAT(GPIO_MUX_, UART3_RX_AF),
+#elif defined(UART3_AF)
+    .rx_af = CONCAT(GPIO_MUX_, UART3_AF),
+#else
+    .rx_af = GPIO_MUX_7,
+#endif
+#if defined(UART3_TX_AF)
+    .tx_af = CONCAT(GPIO_MUX_, UART3_TX_AF),
+#elif defined(UART3_AF)
+    .tx_af = CONCAT(GPIO_MUX_, UART3_AF),
+#else
+    .tx_af = GPIO_MUX_7,
+#endif
 #ifdef UART3_AHB1_PERIPHERALS
     .rcc_ahb1 = UART3_AHB1_PERIPHERALS,
 #endif
@@ -117,7 +156,20 @@ static uartDevice_t uart4 =
     .dev = UART4,
     .rx = IO_TAG(UART4_RX_PIN),
     .tx = IO_TAG(UART4_TX_PIN),
-    .af = GPIO_MUX_8,
+#if defined(UART4_RX_AF)
+    .rx_af = CONCAT(GPIO_MUX_, UART4_RX_AF),
+#elif defined(UART4_AF)
+    .rx_af = CONCAT(GPIO_MUX_, UART4_AF),
+#else
+    .rx_af = GPIO_MUX_8,
+#endif
+#if defined(UART4_TX_AF)
+    .tx_af = CONCAT(GPIO_MUX_, UART4_TX_AF),
+#elif defined(UART4_AF)
+    .tx_af = CONCAT(GPIO_MUX_, UART4_AF),
+#else
+    .tx_af = GPIO_MUX_8,
+#endif
 #ifdef UART4_AHB1_PERIPHERALS
     .rcc_ahb1 = UART4_AHB1_PERIPHERALS,
 #endif
@@ -138,7 +190,20 @@ static uartDevice_t uart5 =
     .dev = UART5,
     .rx = IO_TAG(UART5_RX_PIN),
     .tx = IO_TAG(UART5_TX_PIN),
-    .af = GPIO_MUX_8,
+#if defined(UART5_RX_AF)
+    .rx_af = CONCAT(GPIO_MUX_, UART5_RX_AF),
+#elif defined(UART5_AF)
+    .rx_af = CONCAT(GPIO_MUX_, UART5_AF),
+#else
+    .rx_af = GPIO_MUX_8,
+#endif
+#if defined(UART5_TX_AF)
+    .tx_af = CONCAT(GPIO_MUX_, UART5_TX_AF),
+#elif defined(UART5_AF)
+    .tx_af = CONCAT(GPIO_MUX_, UART5_AF),
+#else
+    .tx_af = GPIO_MUX_8,
+#endif
 #ifdef UART5_AHB1_PERIPHERALS
     .rcc_ahb1 = UART5_AHB1_PERIPHERALS,
 #endif
@@ -159,7 +224,20 @@ static uartDevice_t uart6 =
     .dev = USART6,
     .rx = IO_TAG(UART6_RX_PIN),
     .tx = IO_TAG(UART6_TX_PIN),
-    .af = GPIO_MUX_8,
+#if defined(UART6_RX_AF)
+    .rx_af = CONCAT(GPIO_MUX_, UART6_RX_AF),
+#elif defined(UART6_AF)
+    .rx_af = CONCAT(GPIO_MUX_, UART6_AF),
+#else
+    .rx_af = GPIO_MUX_8,
+#endif
+#if defined(UART6_TX_AF)
+    .tx_af = CONCAT(GPIO_MUX_, UART6_TX_AF),
+#elif defined(UART6_AF)
+    .tx_af = CONCAT(GPIO_MUX_, UART6_AF),
+#else
+    .tx_af = GPIO_MUX_8,
+#endif
 #ifdef UART6_AHB1_PERIPHERALS
     .rcc_ahb1 = UART6_AHB1_PERIPHERALS,
 #endif
@@ -180,7 +258,20 @@ static uartDevice_t uart7 =
     .dev = UART7,
     .rx = IO_TAG(UART7_RX_PIN),
     .tx = IO_TAG(UART7_TX_PIN),
-    .af = GPIO_MUX_8,
+#if defined(UART7_RX_AF)
+    .rx_af = CONCAT(GPIO_MUX_, UART7_RX_AF),
+#elif defined(UART7_AF)
+    .rx_af = CONCAT(GPIO_MUX_, UART7_AF),
+#else
+    .rx_af = GPIO_MUX_8,
+#endif
+#if defined(UART7_TX_AF)
+    .tx_af = CONCAT(GPIO_MUX_, UART7_TX_AF),
+#elif defined(UART7_AF)
+    .tx_af = CONCAT(GPIO_MUX_, UART7_AF),
+#else
+    .tx_af = GPIO_MUX_8,
+#endif
     .rcc_apb1 = RCC_APB1(UART7),
     .irq = UART7_IRQn,
     .irqPriority = NVIC_PRIO_SERIALUART,
@@ -198,7 +289,20 @@ static uartDevice_t uart8 =
     .dev = UART8,
     .rx = IO_TAG(UART8_RX_PIN),
     .tx = IO_TAG(UART8_TX_PIN),
-    .af = GPIO_MUX_8,
+#if defined(UART8_RX_AF)
+    .rx_af = CONCAT(GPIO_MUX_, UART8_RX_AF),
+#elif defined(UART8_AF)
+    .rx_af = CONCAT(GPIO_MUX_, UART8_AF),
+#else
+    .rx_af = GPIO_MUX_8,
+#endif
+#if defined(UART8_TX_AF)
+    .tx_af = CONCAT(GPIO_MUX_, UART8_TX_AF),
+#elif defined(UART8_AF)
+    .tx_af = CONCAT(GPIO_MUX_, UART8_AF),
+#else
+    .tx_af = GPIO_MUX_8,
+#endif
     .rcc_apb1 = RCC_APB1(UART8),
     .irq = UART8_IRQn,
     .irqPriority = NVIC_PRIO_SERIALUART,
@@ -357,22 +461,22 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_t mode,
     if (options & SERIAL_BIDIR) {
         IOInit(tx, OWNER_SERIAL, RESOURCE_UART_TXRX, RESOURCE_INDEX(device));
         if (options & SERIAL_BIDIR_PP) {
-            IOConfigGPIOAF(tx, IOCFG_AF_PP, uart->af);
+            IOConfigGPIOAF(tx, IOCFG_AF_PP, uart->tx_af);
         } else {
             IOConfigGPIOAF(tx,
                     (options & SERIAL_BIDIR_NOPULL) ? IOCFG_AF_OD : IOCFG_AF_OD_UP,
-                    uart->af);
+                    uart->tx_af);
         }
     }
     else {
         if (mode & MODE_TX) {
             IOInit(tx, OWNER_SERIAL, RESOURCE_UART_TX, RESOURCE_INDEX(device));
-            IOConfigGPIOAF(tx, IOCFG_AF_PP, uart->af);
+            IOConfigGPIOAF(tx, IOCFG_AF_PP, uart->rx_af);
         }
 
         if (mode & MODE_RX) {
             IOInit(rx, OWNER_SERIAL, RESOURCE_UART_RX, RESOURCE_INDEX(device));
-            IOConfigGPIOAF(rx, IOCFG_AF_PP, uart->af);
+            IOConfigGPIOAF(rx, IOCFG_AF_PP, uart->rx_af);
         }
     } 
 

src/main/drivers/timer_def_stm32h7xx.h

@@ -87,8 +87,10 @@
 #define DEF_TIM_DMA__BTCH_TIM15_CH2   NONE
 
 #define DEF_TIM_DMA__BTCH_TIM16_CH1   DEF_TIM_DMA_FULL(DMA_REQUEST_TIM16_CH1)
+#define DEF_TIM_DMA__BTCH_TIM16_CH1N  NONE
 
 #define DEF_TIM_DMA__BTCH_TIM17_CH1   DEF_TIM_DMA_FULL(DMA_REQUEST_TIM17_CH1)
+#define DEF_TIM_DMA__BTCH_TIM17_CH1N  NONE
 
 // TIM_UP table
 #define DEF_TIM_DMA__BTCH_TIM1_UP     DEF_TIM_DMA_FULL(DMA_REQUEST_TIM1_UP)

src/main/drivers/uart_inverter.c

@@ -145,7 +145,12 @@ void uartInverterSet(USART_TypeDef *USARTx, uartInverterLine_e line, bool enable
 
 // UART4
 #if defined(INVERTER_PIN_UART4_RX) || defined(INVERTER_PIN_UART4_TX)
-    if (USARTx == USART4) {
+#if defined(STM32F4)
+    if (USARTx == UART4)
+#else
+    if (USARTx == USART4)
+#endif
+    {
 #if defined(INVERTER_PIN_UART4_RX)
         rx_pin = IOGetByTag(IO_TAG(INVERTER_PIN_UART4_RX));
 #endif

src/main/drivers/usb_msc_f4xx.c

@@ -42,7 +42,7 @@
 #include "drivers/light_led.h"
 #include "drivers/nvic.h"
 #include "drivers/persistent.h"
-#include "drivers/sdmmc_sdio.h"
+#include "drivers/sdcard/sdmmc_sdio.h"
 #include "drivers/time.h"
 #include "drivers/usb_msc.h"
 

src/main/drivers/vtx_common.h

@@ -36,11 +36,11 @@
 
 #define VTX_SETTINGS_POWER_COUNT        8
 #define VTX_SETTINGS_DEFAULT_POWER      1
-#define VTX_SETTINGS_MIN_POWER          1
+#define VTX_SETTINGS_MIN_POWER          0
 #define VTX_SETTINGS_MIN_USER_FREQ      5000
 #define VTX_SETTINGS_MAX_USER_FREQ      5999
 #define VTX_SETTINGS_FREQCMD
-#define VTX_SETTINGS_MAX_POWER          (VTX_SETTINGS_POWER_COUNT - VTX_SETTINGS_MIN_POWER + 1)
+#define VTX_SETTINGS_MAX_POWER          (VTX_SETTINGS_POWER_COUNT - VTX_SETTINGS_MIN_POWER)
 
 #else
 

src/main/fc/cli.c

@@ -151,7 +151,7 @@ static uint8_t  commandBatchErrorCount = 0;
 
 // sync this with features_e
 static const char * const featureNames[] = {
-    "THR_VBAT_COMP", "VBAT", "TX_PROF_SEL", "BAT_PROF_AUTOSWITCH", "MOTOR_STOP",
+    "THR_VBAT_COMP", "VBAT", "TX_PROF_SEL", "BAT_PROF_AUTOSWITCH", "GEOZONE",
     "", "SOFTSERIAL", "GPS", "RPM_FILTERS",
     "", "TELEMETRY", "CURRENT_METER", "REVERSIBLE_MOTORS", "",
     "", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "",
@@ -1561,6 +1561,285 @@ static void cliSafeHomes(char *cmdline)
 }
 
 #endif
+
+#if defined(USE_GEOZONE)
+static void printGeozones(uint8_t dumpMask, const geoZoneConfig_t *geoZone, const geoZoneConfig_t *defaultGeoZone)
+{
+    const char *format = "geozone %u %u %u %d %d %u %u %u";
+    for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) {
+        bool equalsDefault = false;
+        if (defaultGeoZone) {
+            equalsDefault = geoZone[i].fenceAction == defaultGeoZone->fenceAction
+            && geoZone[i].shape == defaultGeoZone->shape
+            && geoZone[i].type == defaultGeoZone->type
+            && geoZone[i].maxAltitude == defaultGeoZone->maxAltitude
+            && geoZone[i].minAltitude == defaultGeoZone->minAltitude
+            && geoZone[i].isSealevelRef == defaultGeoZone->isSealevelRef
+            && geoZone[i].fenceAction == defaultGeoZone->fenceAction
+            && geoZone[i].vertexCount == defaultGeoZone->vertexCount;
+
+            cliDefaultPrintLinef(dumpMask, equalsDefault, format, defaultGeoZone[i].shape, defaultGeoZone[i].type, defaultGeoZone[i].minAltitude, defaultGeoZone[i].maxAltitude, defaultGeoZone[i].isSealevelRef, defaultGeoZone[i].fenceAction, defaultGeoZone[i].vertexCount);  
+        }
+        cliDumpPrintLinef(dumpMask, equalsDefault, format, i, geoZone[i].shape, geoZone[i].type, geoZone[i].minAltitude, geoZone[i].maxAltitude, geoZone[i].isSealevelRef, geoZone[i].fenceAction, geoZone[i].vertexCount);    
+    }
+} 
+
+static void printGeozoneVertices(uint8_t dumpMask, const vertexConfig_t *vertices, const vertexConfig_t *defaultVertices)
+{
+    const char *format = "geozone vertex %d %u %d %d";
+    for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) {
+        bool equalsDefault = false;
+        if (defaultVertices) {
+            equalsDefault = vertices[i].idx == defaultVertices->idx
+            && vertices[i].lat == defaultVertices->lat
+            && vertices[i].lon == defaultVertices->lon
+            && vertices[i].zoneId == defaultVertices->zoneId;
+            
+            cliDefaultPrintLinef(dumpMask, equalsDefault, format, defaultVertices[i].zoneId, defaultVertices[i].idx, defaultVertices[i].lat, defaultVertices[i].lon);
+        }
+        
+        cliDumpPrintLinef(dumpMask, equalsDefault, format, vertices[i].zoneId, vertices[i].idx, vertices[i].lat, vertices[i].lon);    
+    }
+
+    if (!defaultVertices) {
+        uint8_t totalVertices = geozoneGetUsedVerticesCount();
+        cliPrintLinef("# %u vertices free (Used %u of %u)", MAX_VERTICES_IN_CONFIG - totalVertices, totalVertices, MAX_VERTICES_IN_CONFIG);
+    }
+}
+
+static void cliGeozone(char* cmdLine)
+{  
+    if (isEmpty(cmdLine)) {
+        printGeozones(DUMP_MASTER, geoZonesConfig(0), NULL);
+    } else if (sl_strcasecmp(cmdLine, "vertex") == 0) {    
+        printGeozoneVertices(DUMP_MASTER, geoZoneVertices(0), NULL);
+    } else if (sl_strncasecmp(cmdLine, "vertex reset", 12) == 0) {
+         const char* ptr = &cmdLine[12];
+         uint8_t zoneId = 0, idx = 0;
+         uint8_t argumentCount = 1;
+
+         if ((ptr = nextArg(ptr))) {
+            zoneId = fastA2I(ptr);
+         } else {
+           geozoneResetVertices(-1, -1);
+           return;
+        }
+
+         if ((ptr = nextArg(ptr))) {
+            argumentCount++;
+            idx = fastA2I(ptr);
+        } else {
+            geozoneResetVertices(zoneId, -1);
+            return;
+        }
+
+         if (argumentCount != 2) {
+            cliShowParseError();
+            return;
+        }
+
+        geozoneResetVertices(zoneId, idx);
+
+    } else if (sl_strncasecmp(cmdLine, "vertex", 6) == 0) {
+        int32_t lat = 0, lon = 0;
+        int8_t zoneId = 0;
+        int16_t vertexIdx = -1;
+        uint8_t vertexZoneIdx = 0;
+        const char* ptr = cmdLine;
+        uint8_t argumentCount = 1;
+
+        if ((ptr = nextArg(ptr))) {         
+            zoneId = fastA2I(ptr);
+            if (zoneId < 0) {
+                return;
+            }
+
+            if (zoneId >= MAX_GEOZONES_IN_CONFIG) {
+                cliShowArgumentRangeError("geozone index", 0, MAX_GEOZONES_IN_CONFIG - 1);
+                return;
+            }
+        } else {
+            cliShowParseError();
+            return;
+        }
+
+         if ((ptr = nextArg(ptr))) {
+            argumentCount++;
+            vertexZoneIdx = fastA2I(ptr);
+        } else {
+            cliShowParseError();
+            return;
+        }
+
+        if ((ptr = nextArg(ptr))) {
+            argumentCount++;
+            lat = fastA2I(ptr);
+        } else {
+            cliShowParseError();
+            return;
+        }
+        
+        if ((ptr = nextArg(ptr))) {
+            argumentCount++;
+            lon = fastA2I(ptr);
+        } else {
+            cliShowParseError();
+            return;
+        }
+
+        if ((ptr = nextArg(ptr))) {
+            argumentCount++;
+        }
+
+        if (argumentCount != 4) {
+            cliShowParseError();
+            return;
+        }
+        
+        for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) {
+            if (geoZoneVertices(i)->zoneId == zoneId && geoZoneVertices(i)->idx == vertexZoneIdx)  {
+                geoZoneVerticesMutable(i)->lat = lat;
+                geoZoneVerticesMutable(i)->lon = lon;
+                return;
+            }
+        }
+
+        for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) {
+            if (geoZoneVertices(i)->zoneId == -1) {
+                vertexIdx = i;
+                break;
+            }
+        }
+
+        if (vertexIdx < 0 || vertexIdx >= MAX_VERTICES_IN_CONFIG || vertexZoneIdx > MAX_VERTICES_IN_CONFIG) {
+            cliPrintError("Maximum number of vertices reached.");
+            return;
+        }
+
+        geoZoneVerticesMutable(vertexIdx)->lat = lat;
+        geoZoneVerticesMutable(vertexIdx)->lon = lon;
+        geoZoneVerticesMutable(vertexIdx)->zoneId = zoneId;
+        geoZoneVerticesMutable(vertexIdx)->idx = vertexZoneIdx;  
+        
+        uint8_t totalVertices = geozoneGetUsedVerticesCount();
+        cliPrintLinef("# %u vertices free (Used %u of %u)", MAX_VERTICES_IN_CONFIG - totalVertices, totalVertices, MAX_VERTICES_IN_CONFIG);
+
+    } else if (sl_strncasecmp(cmdLine, "reset", 5) == 0) {
+        const char* ptr = &cmdLine[5];
+        if ((ptr = nextArg(ptr))) {
+            int idx = fastA2I(ptr);
+            geozoneReset(idx);
+            geozoneResetVertices(idx, -1);
+        } else {
+            geozoneReset(-1);
+            geozoneResetVertices(-1, -1);
+        } 
+    } else {
+        int8_t idx = 0, isPolygon = 0, isInclusive = 0, fenceAction = 0, seaLevelRef = 0, vertexCount = 0;
+        int32_t minAltitude = 0, maxAltitude = 0;
+        const char* ptr = cmdLine;
+        uint8_t argumentCount = 1;
+
+        idx = fastA2I(ptr);
+        if (idx < 0 || idx > MAX_GEOZONES_IN_CONFIG) {
+            cliShowArgumentRangeError("geozone index", 0, MAX_GEOZONES_IN_CONFIG - 1);
+            return;
+        }
+        
+        if ((ptr = nextArg(ptr))) {
+            argumentCount++;
+            isPolygon = fastA2I(ptr);
+        } else {
+            cliShowParseError();
+            return;
+        }
+
+        if ((ptr = nextArg(ptr))){
+            argumentCount++;
+            isInclusive = fastA2I(ptr);
+        } else {
+            cliShowParseError();
+            return;
+        }
+
+        if ((ptr = nextArg(ptr))){
+            argumentCount++;
+            minAltitude = fastA2I(ptr);
+        } else {
+            cliShowParseError();
+            return;
+        }
+
+        if ((ptr = nextArg(ptr))){
+            argumentCount++;
+            maxAltitude = fastA2I(ptr);
+        } else {
+            cliShowParseError();
+            return;
+        }
+
+        if ((ptr = nextArg(ptr))){
+            argumentCount++;
+            seaLevelRef = fastA2I(ptr);
+        } else {
+            cliShowParseError();
+            return;
+        }
+
+        if ((ptr = nextArg(ptr))){
+            argumentCount++;        
+            fenceAction = fastA2I(ptr);
+            if (fenceAction < 0 || fenceAction > GEOFENCE_ACTION_RTH) {
+                cliShowArgumentRangeError("fence action", 0, GEOFENCE_ACTION_RTH);
+                return;
+            }
+        } else {
+            cliShowParseError();
+            return;
+        }
+
+        if ((ptr = nextArg(ptr))){
+            argumentCount++;
+            vertexCount = fastA2I(ptr);
+            if (vertexCount < 1 || vertexCount > MAX_VERTICES_IN_CONFIG) {
+                cliShowArgumentRangeError("vertex count", 1, MAX_VERTICES_IN_CONFIG);
+                return;
+            }
+        } else {
+            cliShowParseError();
+            return;
+        }
+
+        if ((ptr = nextArg(ptr))){
+            argumentCount++;
+        } 
+
+        if (argumentCount != 8) {
+            cliShowParseError();
+            return;
+        }
+
+        if (isPolygon) {
+            geoZonesConfigMutable(idx)->shape = GEOZONE_SHAPE_POLYGON;
+        } else {
+            geoZonesConfigMutable(idx)->shape = GEOZONE_SHAPE_CIRCULAR;
+        }
+
+        if (isInclusive) {
+            geoZonesConfigMutable(idx)->type = GEOZONE_TYPE_INCLUSIVE;
+        } else {
+            geoZonesConfigMutable(idx)->type = GEOZONE_TYPE_EXCLUSIVE;
+        }
+
+        geoZonesConfigMutable(idx)->maxAltitude = maxAltitude;
+        geoZonesConfigMutable(idx)->minAltitude = minAltitude;
+        geoZonesConfigMutable(idx)->isSealevelRef = (bool)seaLevelRef;
+        geoZonesConfigMutable(idx)->fenceAction = fenceAction;
+        geoZonesConfigMutable(idx)->vertexCount = vertexCount;
+    }
+}
+#endif
+
 #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) && defined(NAV_NON_VOLATILE_WAYPOINT_CLI)
 static void printWaypoints(uint8_t dumpMask, const navWaypoint_t *navWaypoint, const navWaypoint_t *defaultNavWaypoint)
 {
@@ -2524,11 +2803,11 @@ static void osdCustom(char *cmdline){
         int32_t i = args[INDEX];
         if (
                 i >= 0 && i < MAX_CUSTOM_ELEMENTS &&
-                args[PART0_TYPE] >= 0 && args[PART0_TYPE] <= 7 &&
+                args[PART0_TYPE] >= 0 && args[PART0_TYPE] < CUSTOM_ELEMENT_TYPE_END &&
                 args[PART0_VALUE] >= 0 && args[PART0_VALUE] <= UINT8_MAX &&
-                args[PART1_TYPE] >= 0 && args[PART1_TYPE] <= 7 &&
+                args[PART1_TYPE] >= 0 && args[PART1_TYPE] < CUSTOM_ELEMENT_TYPE_END &&
                 args[PART1_VALUE] >= 0 && args[PART1_VALUE] <= UINT8_MAX &&
-                args[PART2_TYPE] >= 0 && args[PART2_TYPE] <= 7 &&
+                args[PART2_TYPE] >= 0 && args[PART2_TYPE] < CUSTOM_ELEMENT_TYPE_END &&
                 args[PART2_VALUE] >= 0 && args[PART2_VALUE] <= UINT8_MAX &&
                 args[VISIBILITY_TYPE] >= 0 && args[VISIBILITY_TYPE] <= 2 &&
                 args[VISIBILITY_VALUE] >= 0 && args[VISIBILITY_VALUE] <= UINT8_MAX
@@ -3714,6 +3993,17 @@ static void cliSet(char *cmdline)
                 }
 
                 if (changeValue) {
+                    // If changing the battery capacity unit, update the osd stats energy unit to match
+                    if (strcmp(name, "battery_capacity_unit") == 0) {
+                        if (batteryMetersConfig()->capacity_unit != (uint8_t)tmp.int_value) {
+                            if (tmp.int_value == BAT_CAPACITY_UNIT_MAH) {
+                                osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_MAH;
+                            } else {
+                                osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_WH;
+                            }
+                        }
+                    }
+
                     cliSetIntFloatVar(val, tmp);
 
                     cliPrintf("%s set to ", name);
@@ -4167,6 +4457,14 @@ static void printConfig(const char *cmdline, bool doDiff)
         printFwAutolandApproach(dumpMask, fwAutolandApproachConfig_CopyArray, fwAutolandApproachConfig(0));
 #endif
 
+#if defined(USE_GEOZONE)
+        cliPrintHashLine("geozone");
+        printGeozones(dumpMask, geoZonesConfig_CopyArray, geoZonesConfig(0));
+
+        cliPrintHashLine("geozone vertices");
+        printGeozoneVertices(dumpMask, geoZoneVertices_CopyArray, geoZoneVertices(0));
+#endif
+
         cliPrintHashLine("features");
         printFeature(dumpMask, &featureConfig_Copy, featureConfig());
 
@@ -4550,6 +4848,9 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("fwapproach", "Fixed Wing Approach Settings", NULL, cliFwAutolandApproach),
 #endif
     CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet),
+#ifdef USE_GEOZONE
+    CLI_COMMAND_DEF("geozone", "get or set geo zones", NULL, cliGeozone),
+#endif
 #ifdef USE_GPS
     CLI_COMMAND_DEF("gpspassthrough", "passthrough gps to serial", NULL, cliGpsPassthrough),
     CLI_COMMAND_DEF("gpssats", "show GPS satellites", NULL, cliUbloxPrintSatelites),

src/main/fc/config.h

@@ -36,7 +36,7 @@ typedef enum {
     FEATURE_VBAT = 1 << 1,
     FEATURE_TX_PROF_SEL = 1 << 2,       // Profile selection by TX stick command
     FEATURE_BAT_PROFILE_AUTOSWITCH = 1 << 3,
-    FEATURE_UNUSED_12 = 1 << 4,  //was FEATURE_MOTOR_STOP
+    FEATURE_GEOZONE = 1 << 4,  //was FEATURE_MOTOR_STOP
     FEATURE_UNUSED_1 = 1 << 5,   // was FEATURE_SERVO_TILT was FEATURE_DYNAMIC_FILTERS
     FEATURE_SOFTSERIAL = 1 << 6,
     FEATURE_GPS = 1 << 7,

src/main/fc/fc_core.c

@@ -278,6 +278,14 @@ static void updateArmingStatus(void)
         }
 #endif
 
+#ifdef USE_GEOZONE
+    if (feature(FEATURE_GEOZONE) && geozoneIsBlockingArming()) {
+        ENABLE_ARMING_FLAG(ARMING_DISABLED_GEOZONE);
+    } else {
+        DISABLE_ARMING_FLAG(ARMING_DISABLED_GEOZONE);
+    }
+#endif
+
         /* CHECK: */
         if (
             sensors(SENSOR_ACC) &&

src/main/fc/fc_init.c

@@ -372,9 +372,8 @@ void init(void)
     updateHardwareRevision();
 #endif
 
-#if defined(USE_SDCARD_SDIO) && defined(STM32H7)
+#if defined(USE_SDCARD_SDIO) && (defined(STM32H7) || defined(STM32F7))
     sdioPinConfigure();
-    SDIO_GPIO_Init();
 #endif
 
 #ifdef USE_USB_MSC
@@ -524,6 +523,10 @@ void init(void)
     ezTuneUpdate();
 #endif
 
+#ifndef USE_GEOZONE
+    featureClear(FEATURE_GEOZONE);
+#endif
+
     if (!sensorsAutodetect()) {
         // if gyro was not detected due to whatever reason, we give up now.
         failureMode(FAILURE_MISSING_ACC);

src/main/fc/fc_msp.c

@@ -34,6 +34,7 @@
 #include "common/color.h"
 #include "common/maths.h"
 #include "common/streambuf.h"
+#include "common/string_light.h"
 #include "common/bitarray.h"
 #include "common/time.h"
 #include "common/utils.h"
@@ -215,7 +216,7 @@ static void mspSerialPassthroughFn(serialPort_t *serialPort)
 
 static void mspFcSetPassthroughCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
 {
-    const unsigned int dataSize = sbufBytesRemaining(src);
+    const unsigned int dataSize = sbufBytesRemaining(src);  /* Payload size in Bytes */
 
     if (dataSize == 0) {
         // Legacy format
@@ -1487,11 +1488,18 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
                 sbufWriteU8(dst, vtxSettingsConfig()->channel);
                 sbufWriteU8(dst, vtxSettingsConfig()->power);
                 sbufWriteU8(dst, pitmode);
+                // technically there is bug here, we are missing the 16bit
+                // freqency bf is transmitting
+                // sbufWriteU16(dst, vtxSettingsConfig()->freq);
 
                 // Betaflight < 4 doesn't send these fields
                 sbufWriteU8(dst, vtxCommonDeviceIsReady(vtxDevice) ? 1 : 0);
                 sbufWriteU8(dst, vtxSettingsConfig()->lowPowerDisarm);
-                // future extensions here...
+
+                sbufWriteU8(dst, 1);  // vtxtable is available
+                sbufWriteU8(dst, vtxDevice->capability.bandCount);
+                sbufWriteU8(dst, vtxDevice->capability.channelCount);
+                sbufWriteU8(dst, vtxDevice->capability.powerCount);
             }
             else {
                 sbufWriteU8(dst, VTXDEV_UNKNOWN); // no VTX configured
@@ -1674,6 +1682,18 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
             }
         }
         break;
+
+    case MSP2_INAV_ESC_TELEM:
+        {
+            uint8_t motorCount = getMotorCount();
+            sbufWriteU8(dst, motorCount);
+
+            for (uint8_t i = 0; i < motorCount; i++){
+                const escSensorData_t *escState = getEscTelemetry(i); //Get ESC telemetry
+                sbufWriteDataSafe(dst, escState, sizeof(escSensorData_t));
+            }
+        }
+        break;
 #endif
 
 #ifdef USE_EZ_TUNE
@@ -1759,6 +1779,54 @@ static mspResult_e mspFwApproachOutCommand(sbuf_t *dst, sbuf_t *src)
 }
 #endif
 
+#ifdef USE_GEOZONE
+static mspResult_e mspFcGeozoneOutCommand(sbuf_t *dst, sbuf_t *src)
+{
+    const uint8_t idx = sbufReadU8(src);
+    if (idx < MAX_GEOZONES_IN_CONFIG) {
+        sbufWriteU8(dst, idx);
+        sbufWriteU8(dst, geoZonesConfig(idx)->type);
+        sbufWriteU8(dst, geoZonesConfig(idx)->shape);
+        sbufWriteU32(dst, geoZonesConfig(idx)->minAltitude);
+        sbufWriteU32(dst, geoZonesConfig(idx)->maxAltitude);
+        sbufWriteU8(dst, geoZonesConfig(idx)->isSealevelRef);
+        sbufWriteU8(dst, geoZonesConfig(idx)->fenceAction);
+        sbufWriteU8(dst, geoZonesConfig(idx)->vertexCount);
+        return MSP_RESULT_ACK;
+    } else {
+        return MSP_RESULT_ERROR;
+    }
+}
+
+static mspResult_e mspFcGeozoneVerteciesOutCommand(sbuf_t *dst, sbuf_t *src)
+{
+    const uint8_t zoneId = sbufReadU8(src);
+    const uint8_t vertexId = sbufReadU8(src);
+    if (zoneId < MAX_GEOZONES_IN_CONFIG) {
+        int8_t  vertexIdx = geozoneGetVertexIdx(zoneId, vertexId);
+        if (vertexIdx >= 0) {
+            sbufWriteU8(dst, geoZoneVertices(vertexIdx)->zoneId);
+            sbufWriteU8(dst, geoZoneVertices(vertexIdx)->idx);
+            sbufWriteU32(dst, geoZoneVertices(vertexIdx)->lat);
+            sbufWriteU32(dst, geoZoneVertices(vertexIdx)->lon);
+            if (geoZonesConfig(zoneId)->shape == GEOZONE_SHAPE_CIRCULAR) {
+                int8_t vertexRadiusIdx = geozoneGetVertexIdx(zoneId, vertexId + 1);
+                if (vertexRadiusIdx >= 0) {
+                    sbufWriteU32(dst, geoZoneVertices(vertexRadiusIdx)->lat);
+                } else {
+                    return MSP_RESULT_ERROR;
+                }
+            }
+            return MSP_RESULT_ACK;
+        } else {
+            return MSP_RESULT_ERROR;
+        } 
+    } else {
+        return MSP_RESULT_ERROR;
+    }
+}
+#endif
+
 static mspResult_e mspFcLogicConditionCommand(sbuf_t *dst, sbuf_t *src) {
     const uint8_t idx = sbufReadU8(src);
     if (idx < MAX_LOGIC_CONDITIONS) {
@@ -1795,7 +1863,7 @@ static void mspFcWaypointOutCommand(sbuf_t *dst, sbuf_t *src)
 #ifdef USE_FLASHFS
 static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src)
 {
-    const unsigned int dataSize = sbufBytesRemaining(src);
+    const unsigned int dataSize = sbufBytesRemaining(src); /* Payload size in Bytes */
     uint16_t readLength;
 
     const uint32_t readAddress = sbufReadU32(src);
@@ -1819,7 +1887,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
     uint8_t tmp_u8;
     uint16_t tmp_u16;
 
-    const unsigned int dataSize = sbufBytesRemaining(src);
+    const unsigned int dataSize = sbufBytesRemaining(src);  /* Payload size in Bytes */
 
     switch (cmdMSP) {
     case MSP_SELECT_SETTING:
@@ -2082,6 +2150,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             currentBatteryProfileMutable->capacity.value = sbufReadU32(src);
             currentBatteryProfileMutable->capacity.warning = sbufReadU32(src);
             currentBatteryProfileMutable->capacity.critical = sbufReadU32(src);
+            uint8_t currentCapacityUnit = batteryMetersConfigMutable()->capacity_unit;
             batteryMetersConfigMutable()->capacity_unit = sbufReadU8(src);
             if ((batteryMetersConfig()->voltageSource != BAT_VOLTAGE_RAW) && (batteryMetersConfig()->voltageSource != BAT_VOLTAGE_SAG_COMP)) {
                 batteryMetersConfigMutable()->voltageSource = BAT_VOLTAGE_RAW;
@@ -2090,6 +2159,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             if ((batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MAH) && (batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MWH)) {
                 batteryMetersConfigMutable()->capacity_unit = BAT_CAPACITY_UNIT_MAH;
                 return MSP_RESULT_ERROR;
+            } else if (currentCapacityUnit != batteryMetersConfig()->capacity_unit) {
+                if (batteryMetersConfig()->capacity_unit == BAT_CAPACITY_UNIT_MAH) {
+                    osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_MAH;
+                } else {
+                    osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_WH;
+                }
             }
         } else
             return MSP_RESULT_ERROR;
@@ -2121,6 +2196,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             currentBatteryProfileMutable->capacity.value = sbufReadU32(src);
             currentBatteryProfileMutable->capacity.warning = sbufReadU32(src);
             currentBatteryProfileMutable->capacity.critical = sbufReadU32(src);
+            uint8_t currentCapacityUnit = batteryMetersConfigMutable()->capacity_unit;
             batteryMetersConfigMutable()->capacity_unit = sbufReadU8(src);
             if ((batteryMetersConfig()->voltageSource != BAT_VOLTAGE_RAW) && (batteryMetersConfig()->voltageSource != BAT_VOLTAGE_SAG_COMP)) {
                 batteryMetersConfigMutable()->voltageSource = BAT_VOLTAGE_RAW;
@@ -2129,6 +2205,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             if ((batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MAH) && (batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MWH)) {
                 batteryMetersConfigMutable()->capacity_unit = BAT_CAPACITY_UNIT_MAH;
                 return MSP_RESULT_ERROR;
+            } else if (currentCapacityUnit != batteryMetersConfig()->capacity_unit) {
+                if (batteryMetersConfig()->capacity_unit == BAT_CAPACITY_UNIT_MAH) {
+                    osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_MAH;
+                } else {
+                    osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_WH;
+                }
             }
         } else
             return MSP_RESULT_ERROR;
@@ -2666,21 +2748,15 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
                     if (newFrequency <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) {  //value is band and channel
                         const uint8_t newBand = (newFrequency / 8) + 1;
                         const uint8_t newChannel = (newFrequency % 8) + 1;
-
-                        if(vtxSettingsConfig()->band != newBand || vtxSettingsConfig()->channel != newChannel) {
-                            vtxCommonSetBandAndChannel(vtxDevice, newBand, newChannel);
+                        if (vtxSettingsConfig()->band != newBand || vtxSettingsConfig()->channel != newChannel) {
+                            vtxSettingsConfigMutable()->band = newBand;
+                            vtxSettingsConfigMutable()->channel = newChannel;
                         }
-
-                        vtxSettingsConfigMutable()->band = newBand;
-                        vtxSettingsConfigMutable()->channel = newChannel;
                     }
 
                     if (sbufBytesRemaining(src) > 1) {
                         uint8_t newPower = sbufReadU8(src);
-                        uint8_t currentPower = 0;
-                        vtxCommonGetPowerIndex(vtxDevice, &currentPower);
-                        if (newPower != currentPower) {
-                            vtxCommonSetPowerByIndex(vtxDevice, newPower);
+                        if (vtxSettingsConfig()->power != newPower) {
                             vtxSettingsConfigMutable()->power = newPower;
                         }
 
@@ -2696,9 +2772,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
                             vtxSettingsConfigMutable()->lowPowerDisarm = sbufReadU8(src);
                         }
 
-                        // //////////////////////////////////////////////////////////
-                        // this code is taken from BF, it's hack for HDZERO VTX MSP frame
-                        // API version 1.42 - this parameter kept separate since clients may already be supplying
+                        // API version 1.42 - extension for pitmode frequency
                         if (sbufBytesRemaining(src) >= 2) {
                             sbufReadU16(src); //skip pitModeFreq
                         }
@@ -2706,18 +2780,29 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
                         // API version 1.42 - extensions for non-encoded versions of the band, channel or frequency
                         if (sbufBytesRemaining(src) >= 4) {
                             uint8_t newBand = sbufReadU8(src);
+                            if (vtxSettingsConfig()->band != newBand) {
+                                vtxSettingsConfigMutable()->band = newBand;
+                            }
+
                             const uint8_t newChannel = sbufReadU8(src);
-                            vtxSettingsConfigMutable()->band = newBand;
-                            vtxSettingsConfigMutable()->channel = newChannel;
+                            if (vtxSettingsConfig()->channel != newChannel) {
+                                vtxSettingsConfigMutable()->channel = newChannel;
+                            }
                         }
 
-                       /* if (sbufBytesRemaining(src) >= 4) {
-                            sbufRead8(src); // freq_l
-                            sbufRead8(src); // freq_h
-                            sbufRead8(src); // band count
-                            sbufRead8(src); // channel count
-                        }*/
-                        // //////////////////////////////////////////////////////////
+                        if (sbufBytesRemaining(src) >= 2) {
+                            sbufReadU16(src); // freq
+                        }
+
+                        if (sbufBytesRemaining(src) >= 3) {
+                            sbufReadU8(src); // band count
+                            sbufReadU8(src); // channel count
+
+                            uint8_t newPowerCount = sbufReadU8(src);
+                            if (newPowerCount > 0 && newPowerCount < (vtxDevice->capability.powerCount)) {
+                                vtxDevice->capability.powerCount = newPowerCount;
+                            }
+                        }
                     }
                 }
             }
@@ -2888,6 +2973,53 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             return MSP_RESULT_ERROR;
         break;
 
+#ifdef USE_RX_MSP
+    case MSP2_COMMON_SET_MSP_RC_LINK_STATS: {
+            if (dataSize >= 7) {
+                uint8_t sublinkID = sbufReadU8(src); // Sublink ID
+                sbufReadU8(src); // Valid link (Failsafe backup)
+                if (sublinkID == 0) {
+                    setRSSIFromMSP_RC(sbufReadU8(src)); // RSSI %
+                    rxLinkStatistics.uplinkRSSI = -sbufReadU8(src);
+                    rxLinkStatistics.downlinkLQ = sbufReadU8(src);
+                    rxLinkStatistics.uplinkLQ = sbufReadU8(src);
+                    rxLinkStatistics.uplinkSNR = sbufReadI8(src);
+                }
+
+                return MSP_RESULT_NO_REPLY;
+            } else
+                return MSP_RESULT_ERROR;
+        }
+        break;
+
+    case MSP2_COMMON_SET_MSP_RC_INFO: {
+            if (dataSize >= 15) {
+                uint8_t sublinkID = sbufReadU8(src);
+
+                if (sublinkID == 0) {
+                    rxLinkStatistics.uplinkTXPower = sbufReadU16(src);
+                    rxLinkStatistics.downlinkTXPower = sbufReadU16(src);
+
+                    for (int i = 0; i < 4; i++) {
+                        rxLinkStatistics.band[i] = sbufReadU8(src);
+                    }
+
+                    sl_toupperptr(rxLinkStatistics.band);
+
+                    for (int i = 0; i < 6; i++) {
+                        rxLinkStatistics.mode[i] = sbufReadU8(src);
+                    }
+
+                    sl_toupperptr(rxLinkStatistics.mode);
+                }
+
+                return MSP_RESULT_NO_REPLY;
+            } else
+                return MSP_RESULT_ERROR;
+        }
+        break;
+#endif
+
     case MSP_SET_FAILSAFE_CONFIG:
         if (dataSize == 20) {
             failsafeConfigMutable()->failsafe_delay = sbufReadU8(src);
@@ -3292,6 +3424,50 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         gpsUbloxSendCommand(src->ptr, dataSize, 0);
         break;
 
+#ifdef USE_GEOZONE
+    case MSP2_INAV_SET_GEOZONE:
+        if (dataSize == 14) {
+            uint8_t geozoneId;
+            if (!sbufReadU8Safe(&geozoneId, src) || geozoneId >= MAX_GEOZONES_IN_CONFIG) {
+                return MSP_RESULT_ERROR;
+            }
+            
+            geozoneResetVertices(geozoneId, -1);
+            geoZonesConfigMutable(geozoneId)->type = sbufReadU8(src); 
+            geoZonesConfigMutable(geozoneId)->shape = sbufReadU8(src);
+            geoZonesConfigMutable(geozoneId)->minAltitude = sbufReadU32(src);
+            geoZonesConfigMutable(geozoneId)->maxAltitude = sbufReadU32(src);  
+            geoZonesConfigMutable(geozoneId)->isSealevelRef = sbufReadU8(src);   
+            geoZonesConfigMutable(geozoneId)->fenceAction = sbufReadU8(src);
+            geoZonesConfigMutable(geozoneId)->vertexCount = sbufReadU8(src);
+        } else {
+            return MSP_RESULT_ERROR;
+        }
+        break;
+    case MSP2_INAV_SET_GEOZONE_VERTEX:
+        if (dataSize == 10 || dataSize == 14) {
+            uint8_t geozoneId = 0;
+            if (!sbufReadU8Safe(&geozoneId, src) || geozoneId >= MAX_GEOZONES_IN_CONFIG) {
+                return MSP_RESULT_ERROR;
+            }
+            uint8_t vertexId = sbufReadU8(src);
+            int32_t lat = sbufReadU32(src);
+            int32_t lon = sbufReadU32(src);
+            if (!geozoneSetVertex(geozoneId, vertexId, lat, lon)) {
+                return MSP_RESULT_ERROR;
+            }
+
+            if (geoZonesConfig(geozoneId)->shape == GEOZONE_SHAPE_CIRCULAR) {
+                if (!geozoneSetVertex(geozoneId, vertexId + 1, sbufReadU32(src), 0)) {
+                    return MSP_RESULT_ERROR;
+                }
+            }
+        } else {
+            return MSP_RESULT_ERROR;
+        }
+        break;
+#endif
+
 #ifdef USE_EZ_TUNE
 
     case MSP2_INAV_EZ_TUNE_SET:
@@ -3344,7 +3520,11 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         sbufReadU8Safe(&tmp_u8, src);
         if ((dataSize == (OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1) + (CUSTOM_ELEMENTS_PARTS * 3) + 4) && (tmp_u8 < MAX_CUSTOM_ELEMENTS)) {
             for (int i = 0; i < CUSTOM_ELEMENTS_PARTS; i++) {
-                osdCustomElementsMutable(tmp_u8)->part[i].type = sbufReadU8(src);
+                uint8_t type = sbufReadU8(src);
+                if (type >= CUSTOM_ELEMENT_TYPE_END)
+                    return MSP_RESULT_ERROR;
+
+                osdCustomElementsMutable(tmp_u8)->part[i].type = type;
                 osdCustomElementsMutable(tmp_u8)->part[i].value = sbufReadU16(src);
             }
             osdCustomElementsMutable(tmp_u8)->visibility.type = sbufReadU8(src);
@@ -3674,7 +3854,7 @@ void mspWriteSimulatorOSD(sbuf_t *dst)
 		while (bytesCount < 80) //whole response should be less 155 bytes at worst.
 		{
 			bool blink1;
-			uint16_t lastChar;
+			uint16_t lastChar = 0;
 
 			count = 0;
 			while ( true )
@@ -3870,6 +4050,14 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
         *ret = mspFwApproachOutCommand(dst, src);
         break;
 #endif
+#ifdef USE_GEOZONE
+    case MSP2_INAV_GEOZONE:
+        *ret = mspFcGeozoneOutCommand(dst, src);
+        break;
+    case MSP2_INAV_GEOZONE_VERTEX:
+        *ret = mspFcGeozoneVerteciesOutCommand(dst, src);
+        break;
+#endif
 #ifdef USE_SIMULATOR
     case MSP_SIMULATOR:
         tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version
@@ -4084,6 +4272,28 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
         break;
 #endif
 
+    case MSP_VTXTABLE_POWERLEVEL: {
+        vtxDevice_t *vtxDevice = vtxCommonDevice();
+        if (!vtxDevice) {
+            return MSP_RESULT_ERROR;
+        }
+
+        const uint8_t powerLevel = sbufBytesRemaining(src) ? sbufReadU8(src) : 0;
+        if (powerLevel == 0 || powerLevel > vtxDevice->capability.powerCount) {
+            return MSP_RESULT_ERROR;
+        }
+
+        sbufWriteU8(dst, powerLevel);
+        sbufWriteU16(dst, 0);
+
+        const char *str = vtxDevice->capability.powerNames[powerLevel - 1];
+        const uint32_t str_len = strnlen(str, 5);  // these _should_ all be null-terminated
+        sbufWriteU8(dst, str_len);
+        for (uint32_t i = 0; i < str_len; i++)
+            sbufWriteU8(dst, str[i]);
+
+    } break;
+
     default:
         // Not handled
         return false;
@@ -4172,7 +4382,7 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
     if (cmd->flags & MSP_FLAG_DONT_REPLY) {
         ret = MSP_RESULT_NO_REPLY;
     }
-
+    reply->flags = cmd->flags;
     reply->result = ret;
     return ret;
 }

src/main/fc/fc_tasks.c

@@ -120,9 +120,6 @@ void taskHandleSerial(timeUs_t currentTimeUs)
 #ifdef USE_MSP_OSD
 	// Capture MSP Displayport messages to determine if VTX is connected
     mspOsdSerialProcess(mspFcProcessCommand);
-#ifdef USE_VTX_MSP
-    mspVtxSerialProcess(mspFcProcessCommand);
-#endif
 #endif
 
 }
@@ -339,6 +336,15 @@ void taskUpdateAux(timeUs_t currentTimeUs)
 #endif
 }
 
+#ifdef USE_GEOZONE
+void geozoneUpdateTask(timeUs_t currentTimeUs)
+{
+    if (feature(FEATURE_GEOZONE)) {
+        geozoneUpdate(currentTimeUs);
+    }
+}
+#endif
+
 void fcTasksInit(void)
 {
     schedulerInit();
@@ -409,7 +415,7 @@ void fcTasksInit(void)
     setTaskEnabled(TASK_OPFLOW, sensors(SENSOR_OPFLOW));
 #endif
 #ifdef USE_VTX_CONTROL
-#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)
+#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP)
     setTaskEnabled(TASK_VTXCTRL, true);
 #endif
 #endif
@@ -453,6 +459,11 @@ void fcTasksInit(void)
 #if defined(SITL_BUILD)
     serialProxyStart();
 #endif
+
+#ifdef USE_GEOZONE
+    setTaskEnabled(TASK_GEOZONE, feature(FEATURE_GEOZONE));
+#endif
+
 }
 
 cfTask_t cfTasks[TASK_COUNT] = {
@@ -740,4 +751,13 @@ cfTask_t cfTasks[TASK_COUNT] = {
     },
 #endif
 
+#ifdef USE_GEOZONE
+    [TASK_GEOZONE] = {
+        .taskName = "GEOZONE",
+        .taskFunc = geozoneUpdateTask,
+        .desiredPeriod = TASK_PERIOD_HZ(5),
+        .staticPriority = TASK_PRIORITY_MEDIUM,
+    },
+#endif
+
 };

src/main/fc/rc_adjustments.c

@@ -302,6 +302,10 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
         .adjustmentFunction = ADJUSTMENT_NAV_WP_MULTI_MISSION_INDEX,
         .mode = ADJUSTMENT_MODE_STEP,
         .data = { .stepConfig = { .step = 1 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_NAV_FW_ALT_CONTROL_RESPONSE,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
     }
 };
 
@@ -545,6 +549,9 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
             applyAdjustmentPID(ADJUSTMENT_POS_Z_D, &pidBankMutable()->pid[PID_POS_Z].D, delta);
             navigationUsePIDs();
             break;
+        case ADJUSTMENT_NAV_FW_ALT_CONTROL_RESPONSE:
+            applyAdjustmentU8(ADJUSTMENT_NAV_FW_ALT_CONTROL_RESPONSE, &pidProfileMutable()->fwAltControlResponseFactor, delta, SETTING_NAV_FW_ALT_CONTROL_RESPONSE_MIN, SETTING_NAV_FW_ALT_CONTROL_RESPONSE_MAX);
+            break;
         case ADJUSTMENT_HEADING_P:
             applyAdjustmentPID(ADJUSTMENT_HEADING_P, &pidBankMutable()->pid[PID_HEADING].P, delta);
             // TODO: navigationUsePIDs()?
@@ -576,7 +583,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
         case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
             applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, &navConfigMutable()->fw.minThrottleDownPitchAngle, delta, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MIN, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MAX);
             break;
-#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)
+#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP)
         case ADJUSTMENT_VTX_POWER_LEVEL:
             {
                 vtxDeviceCapability_t vtxDeviceCapability;

src/main/fc/rc_adjustments.h

@@ -85,6 +85,7 @@ typedef enum {
     ADJUSTMENT_FW_TPA_TIME_CONSTANT             = 57,
     ADJUSTMENT_FW_LEVEL_TRIM                    = 58,
     ADJUSTMENT_NAV_WP_MULTI_MISSION_INDEX       = 59,
+    ADJUSTMENT_NAV_FW_ALT_CONTROL_RESPONSE      = 60,
     ADJUSTMENT_FUNCTION_COUNT // must be last
 } adjustmentFunction_e;
 

src/main/fc/runtime_config.c

@@ -34,7 +34,7 @@ static EXTENDED_FASTRAM uint32_t enabledSensors = 0;
 #if !defined(CLI_MINIMAL_VERBOSITY)
 const char *armingDisableFlagNames[]= {
     "FS", "ANGLE", "CAL", "OVRLD", "NAV", "COMPASS",
-    "ACC", "ARMSW", "HWFAIL", "BOXFS", "RX",
+    "ACC", "ARMSW", "HWFAIL", "BOXFS", "PLACEHOLDER", "RX",
     "THR", "CLI", "CMS", "OSD", "ROLL/PITCH", "AUTOTRIM", "OOM",
     "SETTINGFAIL", "PWMOUT", "NOPREARM", "DSHOTBEEPER", "LANDED"
 };

src/main/fc/runtime_config.h

@@ -23,7 +23,7 @@ typedef enum {
     WAS_EVER_ARMED                                  = (1 << 3),
     SIMULATOR_MODE_HITL                             = (1 << 4),
     SIMULATOR_MODE_SITL                             = (1 << 5),
-
+    ARMING_DISABLED_GEOZONE                         = (1 << 6),
     ARMING_DISABLED_FAILSAFE_SYSTEM                 = (1 << 7),
     ARMING_DISABLED_NOT_LEVEL                       = (1 << 8),
     ARMING_DISABLED_SENSORS_CALIBRATING             = (1 << 9),
@@ -49,8 +49,8 @@ typedef enum {
     ARMING_DISABLED_DSHOT_BEEPER                    = (1 << 29),
     ARMING_DISABLED_LANDING_DETECTED                = (1 << 30),
 
-    ARMING_DISABLED_ALL_FLAGS                       = (ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | ARMING_DISABLED_SENSORS_CALIBRATING |
-                                                       ARMING_DISABLED_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE |
+    ARMING_DISABLED_ALL_FLAGS                       = (ARMING_DISABLED_GEOZONE | ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | 
+                                                       ARMING_DISABLED_SENSORS_CALIBRATING | ARMING_DISABLED_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE |
                                                        ARMING_DISABLED_COMPASS_NOT_CALIBRATED | ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED |
                                                        ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_HARDWARE_FAILURE | ARMING_DISABLED_BOXFAILSAFE |
                                                        ARMING_DISABLED_RC_LINK | ARMING_DISABLED_THROTTLE | ARMING_DISABLED_CLI |
@@ -65,7 +65,8 @@ typedef enum {
 // situations where we might just need the motors to spin so the
 // aircraft can move (even unpredictably) and get unstuck (e.g.
 // crashed into a high tree).
-#define ARMING_DISABLED_EMERGENCY_OVERRIDE  (ARMING_DISABLED_NOT_LEVEL \
+#define ARMING_DISABLED_EMERGENCY_OVERRIDE  (ARMING_DISABLED_GEOZONE \
+                                            | ARMING_DISABLED_NOT_LEVEL \
                                             | ARMING_DISABLED_NAVIGATION_UNSAFE \
                                             | ARMING_DISABLED_COMPASS_NOT_CALIBRATED \
                                             | ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED \
@@ -106,6 +107,7 @@ typedef enum {
     SOARING_MODE          = (1 << 16),
     ANGLEHOLD_MODE        = (1 << 17),
     NAV_FW_AUTOLAND       = (1 << 18),
+    NAV_SEND_TO           = (1 << 19),
 } flightModeFlags_e;
 
 extern uint32_t flightModeFlags;

src/main/fc/settings.yaml

@@ -198,6 +198,17 @@ tables:
   - name: headtracker_dev_type
     values: ["NONE", "SERIAL", "MSP"]
     enum: headTrackerDevType_e
+  - name: mavlink_radio_type
+    values: ["GENERIC", "ELRS", "SIK"]
+    enum: mavlinkRadio_e
+  - name: default_altitude_source
+    values: ["GPS", "BARO", "GPS_ONLY", "BARO_ONLY"]
+    enum: navDefaultAltitudeSensor_e
+  - name: fence_action
+    values: ["NONE", "AVOID", "POS_HOLD", "RTH"]
+    enum: fenceAction_e
+  - name: geozone_rth_no_way_home
+    values: [RTH, EMRG_LAND]
 
 constants:
   RPYL_PID_MIN: 0
@@ -1220,7 +1231,7 @@ groups:
         max: INT16_MAX
       - name: motorstop_on_low
         description: "If enabled, motor will stop when throttle is low on this mixer_profile"
-        default_value: OFF
+        default_value: ON
         field: mixer_config.motorstopOnLow
         type: bool
       - name: mixer_pid_profile_linking
@@ -1536,7 +1547,7 @@ groups:
         default_value: OFF
         type: bool
       - name: disarm_always
-        description: "Disarms the motors independently of throttle value. Setting to OFF reverts to the old behaviour of disarming only when the throttle is low."
+        description: "When you switch to Disarm, do so regardless of throttle position. If this Setting is `OFF`. It will only disarm only when the throttle is low. This is similar to the previous `disarm_kill_switch` option. Default setting is the same as the old default behaviour."
         default_value: ON
         type: bool
       - name: switch_disarm_delay
@@ -1578,7 +1589,7 @@ groups:
         description: "EzTune filter cutoff frequency"
         default_value: 110
         field: filterHz
-        min: 10
+        min: 20
         max: 300
       - name: ez_axis_ratio
         description: "EzTune axis ratio"
@@ -2120,7 +2131,7 @@ groups:
         max: 100
       - name: nav_fw_pos_z_p
         description: "P gain of altitude PID controller (Fixedwing)"
-        default_value: 40
+        default_value: 30
         field: bank_fw.pid[PID_POS_Z].P
         min: 0
         max: 255
@@ -2136,9 +2147,15 @@ groups:
         field: bank_fw.pid[PID_POS_Z].D
         min: 0
         max: 255
+      - name: nav_fw_pos_z_ff
+        description: "FF gain of altitude PID controller (Fixedwing)"
+        default_value: 10
+        field: bank_fw.pid[PID_POS_Z].FF
+        min: 0
+        max: 255
       - name: nav_fw_alt_control_response
         description: "Adjusts the deceleration response of fixed wing altitude control as the target altitude is approached. Decrease value to help avoid overshooting the target altitude."
-        default_value: 20
+        default_value: 40
         field: fwAltControlResponseFactor
         min: 5
         max: 100
@@ -2331,7 +2348,7 @@ groups:
         type: uint8_t
       - name: fw_autotune_max_rate_deflection
         description: "The target percentage of maximum mixer output used for determining the rates in `AUTO` and `LIMIT`."
-        default_value: 80
+        default_value: 90
         field: fw_max_rate_deflection
         min: 50
         max: 100
@@ -2407,6 +2424,12 @@ groups:
         min: 0
         max: 10
         default_value: 0.35
+      - name: inav_w_z_baro_v
+        description: "Weight of barometer climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors."
+        field: w_z_baro_v
+        min: 0
+        max: 10
+        default_value: 0.1
       - name: inav_w_z_gps_p
         description: "Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors."
         field: w_z_gps_p
@@ -2461,6 +2484,11 @@ groups:
         field: baro_epv
         min: 0
         max: 9999
+      - name: inav_default_alt_sensor
+        description: "Sets the default altitude sensor to use. Settings GPS and BARO always use both sensors unless there is an altitude error between the sensors that exceeds a set limit. In this case only the selected sensor will be used while the altitude error limit is exceeded. GPS error limit = 2 * inav_max_eph_epv. BARO error limit = 4 * inav_baro_epv. Settings GPS_ONLY and BARO_ONLY will use only the selected sensor even if the other sensor is working. The other sensor will only be used as a backup if the selected sensor is no longer available to use."
+        default_value: "GPS"
+        field: default_alt_sensor
+        table: default_altitude_source
 
   - name: PG_NAV_CONFIG
     type: navConfig_t
@@ -3195,6 +3223,18 @@ groups:
         min: 1
         max: 2
         default_value: 2
+      - name: mavlink_min_txbuffer
+        field: mavlink.min_txbuff
+        description: "Minimum percent of TX buffer space free, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits."
+        default_value: 33
+        min: 0
+        max: 100
+      - name: mavlink_radio_type
+        description: "Mavlink radio type. Affects how RSSI and LQ are reported on OSD."
+        field: mavlink.radio_type
+        table: mavlink_radio_type
+        default_value: "GENERIC"
+        type: uint8_t
 
   - name: PG_OSD_CONFIG
     type: osdConfig_t
@@ -3342,35 +3382,35 @@ groups:
         min: -550
         max: 1250
       - name: osd_snr_alarm
-        condition: USE_SERIALRX_CRSF
+        condition: USE_SERIALRX_CRSF || USE_RX_MSP
         description: "Value below which Crossfire SNR Alarm pops-up. (dB)"
         default_value: 4
         field: snr_alarm
         min: -20
-        max: 10
+        max: 99
       - name: osd_link_quality_alarm
-        condition: USE_SERIALRX_CRSF
+        condition: USE_SERIALRX_CRSF || USE_RX_MSP
         description: "LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50%"
         default_value: 70
         field: link_quality_alarm
         min: 0
         max: 100
       - name: osd_rssi_dbm_alarm
-        condition: USE_SERIALRX_CRSF
+        condition: USE_SERIALRX_CRSF || USE_RX_MSP
         description: "RSSI dBm indicator blinks below this value [dBm]. 0 disables this alarm"
         default_value: 0
         field: rssi_dbm_alarm
         min: -130
         max: 0
       - name: osd_rssi_dbm_max
-        condition: USE_SERIALRX_CRSF
+        condition: USE_SERIALRX_CRSF || USE_RX_MSP
         description: "RSSI dBm upper end of curve. Perfect rssi (max) = 100%"
         default_value: -30
         field: rssi_dbm_max
         min: -50
         max: 0
       - name: osd_rssi_dbm_min
-        condition: USE_SERIALRX_CRSF
+        condition: USE_SERIALRX_CRSF || USE_RX_MSP
         description: "RSSI dBm lower end of curve or RX sensitivity level. Worst rssi (min) = 0%"
         default_value: -120
         field: rssi_dbm_min
@@ -3537,6 +3577,20 @@ groups:
         field: main_voltage_decimals
         min: 1
         max: 2
+      - name: osd_decimals_altitude
+        description: "Number of decimals for the altitude displayed in the OSD [3-5]."
+        default_value: 3
+        field: decimals_altitude
+        type: uint8_t
+        min: 3
+        max: 5
+      - name: osd_decimals_distance
+        description: "Number of decimals for distance displayed in the OSD [3-5]. This includes distance from home, total distance, and distance remaining."
+        default_value: 3
+        field: decimals_distance
+        type: uint8_t
+        min: 3
+        max: 5
       - name: osd_coordinate_digits
         description: "Number of digits for the coordinates displayed in the OSD [8-11]."
         field: coordinate_digits
@@ -3690,7 +3744,8 @@ groups:
       - name: osd_mah_precision
         description: Number of digits used for mAh precision. Currently used by mAh Used and Battery Remaining Capacity
         field: mAh_precision
-        min: 4
+        default: 4
+        min: 3
         max: 6
         default_value: 4
       - name: osd_use_pilot_logo
@@ -3721,6 +3776,12 @@ groups:
         field: highlight_djis_missing_characters
         default_value: ON
         type: bool
+      - name: enable_broken_o4_workaround
+        field: enable_broken_o4_workaround
+        description: DJI O4 release firmware has a broken MSP DisplayPort implementation. This enables a workaround to restore ARM detection.
+        condition: USE_DJI_HD_OSD
+        default_value: OFF
+        type: bool
       - name: osd_switch_indicator_zero_name
         description: "Character to use for OSD switch incicator 0."
         field: osd_switch_indicator0_name
@@ -4318,3 +4379,49 @@ groups:
       field: roll_ratio
       min: 0
       max: 5
+  - name: PG_GEOZONE_CONFIG
+    type: geozone_config_t
+    headers: ["navigation/navigation.h"]
+    condition: USE_GEOZONE && USE_GPS
+    members:
+      - name: geozone_detection_distance
+        description: "Distance from which a geozone is detected"
+        default_value: 50000
+        field: fenceDetectionDistance
+        min: 0
+        max: 1000000
+      - name: geozone_avoid_altitude_range
+        description: "Altitude range in which an attempt is made to avoid a geozone upwards"
+        default_value: 5000
+        field: avoidAltitudeRange
+        min: 0
+        max: 1000000
+      - name: geozone_safe_altitude_distance
+        description: "Vertical distance that must be maintained to the upper and lower limits of the zone."
+        default_value: 1000
+        field: safeAltitudeDistance
+        min: 0
+        max: 10000
+      - name: geozone_safehome_as_inclusive
+        description: "Treat nearest safehome as inclusive geozone"
+        type: bool
+        field: nearestSafeHomeAsInclusivZone
+        default_value: OFF
+      - name: geozone_safehome_zone_action
+        description:  "Fence action for safehome zone"
+        default_value: "NONE"
+        field: safeHomeFenceAction
+        table: fence_action
+        type: uint8_t
+      - name: geozone_mr_stop_distance
+        description: "Distance in which multirotors stops before the border"
+        default_value: 15000
+        min: 0
+        max: 100000
+        field: copterFenceStopDistance
+      - name: geozone_no_way_home_action
+        description: "Action if RTH with active geozones is unable to calculate a course to home"
+        default_value: RTH
+        field: noWayHomeAction
+        table: geozone_rth_no_way_home
+        type: uint8_t

src/main/flight/failsafe.c

@@ -535,6 +535,7 @@ void failsafeUpdateState(void)
                             abortForcedRTH();
                             failsafeSetActiveProcedure(FAILSAFE_PROCEDURE_AUTO_LANDING);
                             failsafeActivate(FAILSAFE_LANDING);
+                            activateForcedEmergLanding();
                             reprocessState = true;
                             break;
                     }

src/main/flight/pid.c

@@ -179,7 +179,7 @@ static EXTENDED_FASTRAM bool angleHoldIsLevel = false;
 static EXTENDED_FASTRAM float fixedWingLevelTrim;
 static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController;
 
-PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 9);
+PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 10);
 
 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .bank_mc = {
@@ -242,8 +242,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
                 [PID_POS_Z] = {
                     .P = SETTING_NAV_FW_POS_Z_P_DEFAULT,      // FW_POS_Z_P * 100
                     .I = SETTING_NAV_FW_POS_Z_I_DEFAULT,      // FW_POS_Z_I * 100
-                    .D = SETTING_NAV_FW_POS_Z_D_DEFAULT,      // FW_POS_Z_D * 100
-                    .FF = 0,
+                    .D = SETTING_NAV_FW_POS_Z_D_DEFAULT,      // FW_POS_Z_D * 200
+                    .FF = SETTING_NAV_FW_POS_Z_FF_DEFAULT,    // FW_POS_Z_FF * 100
                 },
                 [PID_POS_XY] = {
                     .P = SETTING_NAV_FW_POS_XY_P_DEFAULT,     // FW_POS_XY_P * 100

src/main/io/displayport_msp_dji_compat.c

@@ -24,6 +24,13 @@
 #include "io/displayport_msp_dji_compat.h"
 #include "io/dji_osd_symbols.h"
 #include "drivers/osd_symbols.h"
+#include <string.h>
+
+//                       0123456789
+static char *dji_logo = " DJI,     "
+                        " PLEASE   "
+                        " FIX YOUR "
+                        " OSD FONT ";
 
 uint8_t getDJICharacter(uint8_t ch, uint8_t page)
 {
@@ -37,12 +44,20 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page)
         return DJI_SYM_AH_DECORATION;
     }
 
+    if (ech >= SYM_LOGO_START && ech <= 297) {
+        return dji_logo[(ech - SYM_LOGO_START) % (strlen(dji_logo) + 1)];
+    }
+
+    if (ech >= SYM_PILOT_LOGO_LRG_START && ech <= 511) {
+        return dji_logo[(ech - SYM_LOGO_START) % (strlen(dji_logo) + 1)];
+    }
+
     switch (ech) {
         case SYM_RSSI:
             return DJI_SYM_RSSI;
 
         case SYM_LQ:
-            return 'Q';
+            return DJI_SYM_LINK_QUALITY;
 
         case SYM_LAT:
             return DJI_SYM_LAT;
@@ -127,13 +142,13 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page)
 
         case SYM_MAH:
             return DJI_SYM_MAH;
-
-        case SYM_AH_KM:
-            return 'K';
-
-        case SYM_AH_MI:
-            return 'M';
 /*
+        case SYM_AH_KM: // AH / KM
+            return '?';
+
+        case SYM_AH_MI: // AH / MI
+            return '?';
+
         case SYM_VTX_POWER:
             return DJI_SYM_VTX_POWER;
 
@@ -210,8 +225,9 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page)
             return DJI_SYM_M;
 
         case SYM_TOTAL:
-            return DJI_SYM_FLY_H;
-/*
+            return DJI_SYM_TOTAL_DISTANCE;
+
+    /*
 
         case SYM_ALT_KM:
             return DJI_SYM_ALT_KM;
@@ -234,20 +250,22 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page)
         case SYM_DIST_NM:
             return DJI_SYM_DIST_NM;
 */
+
         case SYM_M:
             return DJI_SYM_M;
 
         case SYM_KM:
-            return 'K';
+            return DJI_SYM_KM;
 
         case SYM_MI:
-            return 'M';
+            return DJI_SYM_MILES;
+
 /*
         case SYM_NM:
             return DJI_SYM_NM;
-*/
         case SYM_WIND_HORIZONTAL:
             return 'W';     // W for wind
+ */
 
 /*
         case SYM_WIND_VERTICAL:
@@ -255,7 +273,8 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page)
 
         case SYM_3D_KT:
             return DJI_SYM_3D_KT;
-*/
+ */
+/*
         case SYM_AIR:
             return 'A';     // A for airspeed
 
@@ -264,7 +283,7 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page)
 
         case SYM_3D_MPH:
             return DJI_SYM_MPH;
-
+ */
         case SYM_RPM:
             return DJI_SYM_RPM;
 
@@ -273,7 +292,7 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page)
 /*
         case SYM_100FTM:
             return DJI_SYM_100FTM;
-*/
+ */
         case SYM_MS:
             return DJI_SYM_MPS;
 
@@ -291,7 +310,7 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page)
 
         case SYM_MAH_MI_1:
             return DJI_SYM_MAH_MI_1;
-*/
+ */
         case SYM_THR:
             return DJI_SYM_THR;
 
@@ -304,11 +323,13 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page)
         case SYM_BLANK:
             return DJI_SYM_BLANK;
 
+/*
         case SYM_ON_H:
             return DJI_SYM_ON_H;
 
         case SYM_FLY_H:
             return DJI_SYM_FLY_H;
+ */
 
         case SYM_ON_M:
             return DJI_SYM_ON_M;
@@ -330,7 +351,7 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page)
 
         case SYM_ZERO_HALF_LEADING_DOT:
             return DJI_SYM_ZERO_HALF_LEADING_DOT;
-*/
+ */
 
         case SYM_AUTO_THR0:
             return 'A';
@@ -365,7 +386,7 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page)
 
         case SYM_GFORCE_Z:
             return DJI_SYM_GFORCE_Z;
-*/
+ */
         case SYM_BARO_TEMP:
             return DJI_SYM_TEMPERATURE;
 
@@ -386,7 +407,7 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page)
 
         case TEMP_SENSOR_SYM_COUNT:
             return DJI_TEMP_SENSOR_SYM_COUNT;
-*/
+ */
         case SYM_HEADING_N:
             return DJI_SYM_HEADING_N;
 
@@ -410,7 +431,7 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page)
 /*
         case SYM_PROFILE:
             return DJI_SYM_PROFILE;
-*/
+ */
         case SYM_SWITCH_INDICATOR_LOW:
             return DJI_SYM_STICK_OVERLAY_SPRITE_LOW;
 
@@ -452,16 +473,8 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page)
 
         case SYM_CROSS_TRACK_ERROR:
             return DJI_SYM_CROSS_TRACK_ERROR;
+ */
 
-        case SYM_LOGO_START:
-            return DJI_SYM_LOGO_START;
-
-        case SYM_LOGO_WIDTH:
-            return DJI_SYM_LOGO_WIDTH;
-
-        case SYM_LOGO_HEIGHT:
-            return DJI_SYM_LOGO_HEIGHT;
-*/
         case SYM_AH_LEFT:
             return DJI_SYM_AH_LEFT;
 
@@ -470,16 +483,17 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page)
 /*
         case SYM_AH_DECORATION_COUNT:
             return DJI_SYM_AH_DECORATION_COUNT;
-*/
+ */
+
         case SYM_AH_CH_LEFT:
         case SYM_AH_CH_AIRCRAFT1:
-            return DJI_SYM_CROSSHAIR_LEFT;
+            return DJI_SYM_AH_CENTER_LINE;
         case SYM_AH_CH_CENTER:
         case SYM_AH_CH_AIRCRAFT2:
-            return DJI_SYM_CROSSHAIR_CENTRE;
+            return DJI_SYM_AH_CENTER;
         case SYM_AH_CH_RIGHT:
         case SYM_AH_CH_AIRCRAFT3:
-            return DJI_SYM_CROSSHAIR_RIGHT;
+            return DJI_SYM_AH_CENTER_LINE_RIGHT;
         
         case SYM_AH_CH_AIRCRAFT0:
         case SYM_AH_CH_AIRCRAFT4:
@@ -488,21 +502,21 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page)
         case SYM_AH_CH_TYPE3:
             return DJI_SYM_NONE;
         case (SYM_AH_CH_TYPE3+1):
-            return DJI_SYM_SMALL_CROSSHAIR;
+            return DJI_SYM_AH_CENTER;
         case (SYM_AH_CH_TYPE3+2):
             return DJI_SYM_NONE;
         
         case SYM_AH_CH_TYPE4:
             return DJI_SYM_HYPHEN;
         case (SYM_AH_CH_TYPE4+1):
-            return DJI_SYM_SMALL_CROSSHAIR;
+            return DJI_SYM_AH_CENTER;
         case (SYM_AH_CH_TYPE4+2):
             return DJI_SYM_HYPHEN;
         
         case SYM_AH_CH_TYPE5:
             return DJI_SYM_STICK_OVERLAY_HORIZONTAL;
         case (SYM_AH_CH_TYPE5+1):
-            return DJI_SYM_SMALL_CROSSHAIR;
+            return DJI_SYM_AH_CENTER;
         case (SYM_AH_CH_TYPE5+2):
             return DJI_SYM_STICK_OVERLAY_HORIZONTAL;
         
@@ -516,14 +530,14 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page)
         case SYM_AH_CH_TYPE7:
             return DJI_SYM_ARROW_SMALL_LEFT;
         case (SYM_AH_CH_TYPE7+1):
-            return DJI_SYM_SMALL_CROSSHAIR;
+            return DJI_SYM_AH_CENTER;
         case (SYM_AH_CH_TYPE7+2):
             return DJI_SYM_ARROW_SMALL_RIGHT;
         
         case SYM_AH_CH_TYPE8:
             return DJI_SYM_AH_LEFT;
         case (SYM_AH_CH_TYPE8+1):
-            return DJI_SYM_SMALL_CROSSHAIR;
+            return DJI_SYM_AH_CENTER;
         case (SYM_AH_CH_TYPE8+2):
             return DJI_SYM_AH_RIGHT;
 
@@ -683,7 +697,7 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page)
 
         case SYM_FLIGHT_DIST_REMAINING:
             return DJI_SYM_FLIGHT_DIST_REMAINING;
-*/
+ */
         case SYM_HUD_ARROWS_L1:
             return DJI_SYM_ARROW_SMALL_LEFT;
 

src/main/io/displayport_msp_osd.c

@@ -53,6 +53,12 @@
 #include "displayport_msp_osd.h"
 #include "displayport_msp_dji_compat.h"
 
+#include "osd_dji_hd.h"
+#include "fc/fc_msp_box.h"
+#include "scheduler/scheduler.h"
+#include "fc/config.h"
+#include "common/maths.h"
+
 #define FONT_VERSION 3
 
 typedef enum {          // defines are from hdzero code
@@ -531,11 +537,72 @@ static mspResult_e processMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPo
     return mspProcessCommand(cmd, reply, mspPostProcessFn);
 }
 
+#if defined(USE_OSD) && defined(USE_DJI_HD_OSD)
+extern timeDelta_t cycleTime;
+static mspResult_e fixDjiBrokenO4ProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) {
+    UNUSED(mspPostProcessFn);
+
+    sbuf_t *dst = &reply->buf;
+
+    // If users is using a buggy O4 air unit, re-use the OLD DJI FPV system workaround for status messages
+    if (osdConfig()->enable_broken_o4_workaround && ((cmd->cmd == DJI_MSP_STATUS) || (cmd->cmd == DJI_MSP_STATUS_EX))) {
+        // Start initializing the reply message
+        reply->cmd = cmd->cmd;
+        reply->result = MSP_RESULT_ACK;
+
+        // DJI OSD relies on a statically defined bit order and doesn't use
+        // MSP_BOXIDS to get actual BOX order. We need a special
+        // packBoxModeFlags()
+        // This is a regression from O3
+        boxBitmask_t flightModeBitmask;
+        djiPackBoxModeBitmask(&flightModeBitmask);
+
+        sbufWriteU16(dst, (uint16_t)cycleTime);
+        sbufWriteU16(dst, 0);
+        sbufWriteU16(dst, packSensorStatus());
+        sbufWriteData(dst, &flightModeBitmask,
+                      4);  // unconditional part of flags, first 32 bits
+        sbufWriteU8(dst, getConfigProfile());
+
+        sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));
+        if (cmd->cmd == MSP_STATUS_EX) {
+            sbufWriteU8(dst, 3);  // PID_PROFILE_COUNT
+            sbufWriteU8(dst, 1);  // getCurrentControlRateProfileIndex()
+        } else {
+            sbufWriteU16(dst, cycleTime);  // gyro cycle time
+        }
+
+        // Cap BoxModeFlags to 32 bits
+        // write flightModeFlags header. Lowest 4 bits contain number of bytes
+        // that follow
+        sbufWriteU8(dst, 0);
+        // sbufWriteData(dst, ((uint8_t*)&flightModeBitmask) + 4, byteCount);
+
+        // Write arming disable flags
+        sbufWriteU8(dst, DJI_ARMING_DISABLE_FLAGS_COUNT);
+        sbufWriteU32(dst, djiPackArmingDisabledFlags());
+
+        // Extra flags
+        sbufWriteU8(dst, 0);
+        // Process DONT_REPLY flag
+        if (cmd->flags & MSP_FLAG_DONT_REPLY) {
+            reply->result = MSP_RESULT_NO_REPLY;
+        }
+
+        return reply->result;
+    }
+
+    return processMspCommand(cmd, reply, mspPostProcessFn);
+}
+#else
+#define fixDjiBrokenO4ProcessMspCommand processMspCommand 
+#endif
+
 void mspOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn)
 {
     if (mspPort.port) {
         mspProcessCommand = mspProcessCommandFn;
-        mspSerialProcessOnePort(&mspPort, MSP_SKIP_NON_MSP_DATA, processMspCommand);
+        mspSerialProcessOnePort(&mspPort, MSP_SKIP_NON_MSP_DATA, fixDjiBrokenO4ProcessMspCommand);
     }
 }
 

src/main/io/dji_osd_symbols.h

@@ -38,21 +38,25 @@
 #define DJI_SYM_LAT                     0x89
 #define DJI_SYM_LON                     0x98
 #define DJI_SYM_ALTITUDE                0x7F
+#define DJI_SYM_TOTAL_DISTANCE          0x71
 #define DJI_SYM_OVER_HOME               0x05
 
 // RSSI
 #define DJI_SYM_RSSI                    0x01
+#define DJI_SYM_LINK_QUALITY            0x7B
 
 // Throttle Position (%)
 #define DJI_SYM_THR                     0x04
 
 // Unit Icons (Metric)
 #define DJI_SYM_M                       0x0C
+#define DJI_SYM_KM                      0x7D
 #define DJI_SYM_C                       0x0E
 
 // Unit Icons (Imperial)
-#define DJI_SYM_F                       0x0D
 #define DJI_SYM_FT                      0x0F
+#define DJI_SYM_MILES                   0x7E
+#define DJI_SYM_F                       0x0D
 
 // Heading Graphics
 #define DJI_SYM_HEADING_N               0x18
@@ -63,13 +67,12 @@
 #define DJI_SYM_HEADING_LINE            0x1D
 
 // AH Center screen Graphics
-#define DJI_SYM_CROSSHAIR_LEFT          0x72
-#define DJI_SYM_CROSSHAIR_CENTRE        0x73
-#define DJI_SYM_CROSSHAIR_RIGHT         0x74
+#define DJI_SYM_AH_CENTER_LINE          0x72
+#define DJI_SYM_AH_CENTER               0x73
+#define DJI_SYM_AH_CENTER_LINE_RIGHT    0x74
 #define DJI_SYM_AH_RIGHT                0x02
 #define DJI_SYM_AH_LEFT                 0x03
 #define DJI_SYM_AH_DECORATION           0x13
-#define DJI_SYM_SMALL_CROSSHAIR         0x7E
 
 // Satellite Graphics
 #define DJI_SYM_SAT_L                   0x1E
@@ -136,17 +139,19 @@
 #define DJI_SYM_WATT                    0x57  // 0x57 is 'W'
 
 // Time
-#define DJI_SYM_ON_H                    0x70
-#define DJI_SYM_FLY_H                   0x71
 #define DJI_SYM_ON_M                    0x9B
 #define DJI_SYM_FLY_M                   0x9C
 
 // Speed
+#define DJI_SYM_SPEED                   0x70
 #define DJI_SYM_KPH                     0x9E
 #define DJI_SYM_MPH                     0x9D
 #define DJI_SYM_MPS                     0x9F
 #define DJI_SYM_FTPS                    0x99
 
+// Menu cursor
+#define DJI_SYM_CURSOR                  DJI_SYM_AH_LEFT
+
 // Stick overlays
 #define DJI_SYM_STICK_OVERLAY_SPRITE_HIGH 0x08
 #define DJI_SYM_STICK_OVERLAY_SPRITE_MID  0x09

src/main/io/gps.c

@@ -392,11 +392,13 @@ void gpsProcessNewSolutionData(bool timeout)
     // Update time
     gpsUpdateTime();
 
-    // Update timeout
-    gpsSetProtocolTimeout(gpsState.baseTimeoutMs);
+    if (!timeout) {
+        // Update timeout
+        gpsSetProtocolTimeout(gpsState.baseTimeoutMs);
 
-    // Update statistics
-    gpsStats.lastMessageDt = gpsState.lastMessageMs - gpsState.lastLastMessageMs;
+        // Update statistics
+        gpsStats.lastMessageDt = gpsState.lastMessageMs - gpsState.lastLastMessageMs;
+    }
     gpsSol.flags.hasNewData = true;
 
     // Toggle heartbeat

src/main/io/osd.c

@@ -181,7 +181,7 @@ typedef struct statistic_s {
     uint16_t min_voltage; // /100
     int16_t max_current;
     int32_t max_power;
-    int16_t min_rssi;
+    uint8_t min_rssi;
     int16_t min_lq; // for CRSF
     int16_t min_rssi_dbm; // for CRSF
     int32_t max_altitude;
@@ -224,8 +224,8 @@ static bool osdDisplayHasCanvas;
 
 #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees)
 
-PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 12);
-PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 2);
+PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 15);
+PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 3);
 
 void osdStartedSaveProcess(void) {
     savingSettings = true;
@@ -259,10 +259,11 @@ bool osdIsNotMetric(void) {
  * prefixed by a a symbol to indicate the unit used.
  * @param dist Distance in centimeters
  */
-static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals)
+static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals, uint8_t digits)
 {
-    uint8_t digits = 3U;    // Total number of digits (including decimal point)
-    uint8_t sym_index = 3U; // Position (index) at buffer of units symbol
+    if (digits == 0)    // Total number of digits (including decimal point)
+        digits = 3U;
+    uint8_t sym_index = digits; // Position (index) at buffer of units symbol
     uint8_t symbol_m = SYM_DIST_M;
     uint8_t symbol_km = SYM_DIST_KM;
     uint8_t symbol_ft = SYM_DIST_FT;
@@ -485,13 +486,13 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid)
 */
 void osdFormatAltitudeSymbol(char *buff, int32_t alt)
 {
-    uint8_t totalDigits = 4U;
-    uint8_t digits = 4U;
-    uint8_t symbolIndex = 4U;
+    uint8_t digits = osdConfig()->decimals_altitude + 1;
+    uint8_t totalDigits = digits;
+    uint8_t symbolIndex = digits;
     uint8_t symbolKFt = SYM_ALT_KFT;
 
     if (alt >= 0) {
-        digits = 3U;
+        digits--;
         buff[0] = ' ';
     }
 
@@ -616,11 +617,11 @@ char *osdFormatTrimWhiteSpace(char *buff)
 
 /**
  * Converts RSSI into a % value used by the OSD.
+ * Range is [0, 100]
  */
-static uint16_t osdConvertRSSI(void)
+static uint8_t osdConvertRSSI(void)
 {
-    // change range to [0, 99]
-    return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99);
+    return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 100);
 }
 
 static uint16_t osdGetCrsfLQ(void)
@@ -806,7 +807,7 @@ static const char * osdArmingDisabledReasonMessage(void)
                 case NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE:
                     return OSD_MESSAGE_STR(OSD_MSG_DISABLE_NAV_FIRST);
                 case NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR:
-                    osdFormatDistanceSymbol(buf, distanceToFirstWP(), 0);
+                    osdFormatDistanceSymbol(buf, distanceToFirstWP(), 0, 3);
                     tfp_sprintf(messageBuf, "FIRST WP TOO FAR (%s)", buf);
                     return message = messageBuf;
                 case NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR:
@@ -866,6 +867,14 @@ static const char * osdArmingDisabledReasonMessage(void)
             return OSD_MESSAGE_STR(OSD_MSG_NO_PREARM);
         case ARMING_DISABLED_DSHOT_BEEPER:
             return OSD_MESSAGE_STR(OSD_MSG_DSHOT_BEEPER);
+
+        case ARMING_DISABLED_GEOZONE:
+#ifdef USE_GEOZONE
+            return OSD_MESSAGE_STR(OSD_MSG_NFZ);
+#else
+            FALLTHROUGH;
+#endif
+
             // Cases without message
         case ARMING_DISABLED_LANDING_DETECTED:
             FALLTHROUGH;
@@ -1644,6 +1653,20 @@ void osdDisplaySwitchIndicator(const char *swName, int rcValue, char *buff) {
     buff[ptr] = '\0';
 }
 
+static bool osdElementEnabled(uint8_t elementID, bool onlyCurrentLayout) {
+    bool elementEnabled = false;
+
+    if (onlyCurrentLayout) {
+        elementEnabled = OSD_VISIBLE(osdLayoutsConfig()->item_pos[currentLayout][elementID]);
+    } else {
+        for (uint8_t layout = 0; layout < 4 && !elementEnabled; layout++) {
+            elementEnabled = OSD_VISIBLE(osdLayoutsConfig()->item_pos[layout][elementID]);
+        }
+    }
+
+    return elementEnabled;
+}
+
 static bool osdDrawSingleElement(uint8_t item)
 {
     uint16_t pos = osdLayoutsConfig()->item_pos[currentLayout][item];
@@ -1698,9 +1721,13 @@ static bool osdDrawSingleElement(uint8_t item)
     }
     case OSD_RSSI_VALUE:
         {
-            uint16_t osdRssi = osdConvertRSSI();
+            uint8_t osdRssi = osdConvertRSSI();
             buff[0] = SYM_RSSI;
-            tfp_sprintf(buff + 1, "%2d", osdRssi);
+            if (osdRssi < 100)
+                tfp_sprintf(buff + 1, "%2d", osdRssi);
+            else
+                tfp_sprintf(buff + 1, "%c ", SYM_MAX);
+
             if (osdRssi < osdConfig()->rssi_alarm) {
                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
             }
@@ -1753,7 +1780,7 @@ static bool osdDrawSingleElement(uint8_t item)
         } else
 #endif
         {
-            if (osdFormatCentiNumber(buff, getMAhDrawn() * 100, 1000, 0, (mah_digits - 2), mah_digits, false)) {
+            if (osdFormatCentiNumber(buff, getMAhDrawn() * 100, 1000, 0, 2, mah_digits, false)) {
                 // Shown in Ah
                 buff[mah_digits] = SYM_AH;
             } else {
@@ -1795,7 +1822,7 @@ static bool osdDrawSingleElement(uint8_t item)
             } else
 #endif
             {
-                if (osdFormatCentiNumber(buff, getBatteryRemainingCapacity() * 100, 1000, 0, (mah_digits - 2), mah_digits, false)) {
+                if (osdFormatCentiNumber(buff, getBatteryRemainingCapacity() * 100, 1000, 0, 2, mah_digits, false)) {
                     // Shown in Ah
                     buff[mah_digits] = SYM_AH;
                 } else {
@@ -1952,7 +1979,7 @@ static bool osdDrawSingleElement(uint8_t item)
         {
             buff[0] = SYM_HOME;
             uint32_t distance_to_home_cm = GPS_distanceToHome * 100;
-            osdFormatDistanceSymbol(&buff[1], distance_to_home_cm, 0);
+            osdFormatDistanceSymbol(&buff[1], distance_to_home_cm, 0, osdConfig()->decimals_distance);
 
             uint16_t dist_alarm = osdConfig()->dist_alarm;
             if (dist_alarm > 0 && GPS_distanceToHome > dist_alarm) {
@@ -1963,7 +1990,7 @@ static bool osdDrawSingleElement(uint8_t item)
 
     case OSD_TRIP_DIST:
         buff[0] = SYM_TOTAL;
-        osdFormatDistanceSymbol(buff + 1, getTotalTravelDistance(), 0);
+        osdFormatDistanceSymbol(buff + 1, getTotalTravelDistance(), 0, osdConfig()->decimals_distance);
         break;
 
     case OSD_BLACKBOX:
@@ -2082,7 +2109,7 @@ static bool osdDrawSingleElement(uint8_t item)
         {
             if (isWaypointNavTrackingActive()) {
                 buff[0] = SYM_CROSS_TRACK_ERROR;
-                osdFormatDistanceSymbol(buff + 1, navigationGetCrossTrackError(), 0);
+                osdFormatDistanceSymbol(buff + 1, navigationGetCrossTrackError(), 0, 3);
             } else {
                 displayWrite(osdDisplayPort, elemPosX, elemPosY, "     ");
                 return true;
@@ -2241,7 +2268,7 @@ static bool osdDrawSingleElement(uint8_t item)
                 buff[1] = '-';
                 buff[2] = '-';
             } else {
-                osdFormatDistanceSymbol(buff, range, 1);
+                osdFormatDistanceSymbol(buff, range, 1, 3);
             }
         }
         break;
@@ -2323,31 +2350,33 @@ static bool osdDrawSingleElement(uint8_t item)
         displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_FLIGHT_DIST_REMAINING);
 
         if ((!ARMING_FLAG(ARMED)) || (distanceMeters == -1)) {
-            buff[3] = SYM_BLANK;
-            buff[4] = '\0';
+            buff[osdConfig()->decimals_distance] = SYM_BLANK;
+            buff[osdConfig()->decimals_distance + 1] = '\0';
             strcpy(buff, "---");
         } else if (distanceMeters == -2) {
             // Wind is too strong to come back with cruise throttle
-            buff[0] = buff[1] = buff[2] = SYM_WIND_HORIZONTAL;
+            for (uint8_t i = 0; i < osdConfig()->decimals_distance; i++) {
+                buff[i] = SYM_WIND_HORIZONTAL;
+            }
             switch ((osd_unit_e)osdConfig()->units){
                 case OSD_UNIT_UK:
                     FALLTHROUGH;
                 case OSD_UNIT_IMPERIAL:
-                    buff[3] = SYM_DIST_MI;
+                    buff[osdConfig()->decimals_distance] = SYM_DIST_MI;
                     break;
                 case OSD_UNIT_METRIC_MPH:
                     FALLTHROUGH;
                 case OSD_UNIT_METRIC:
-                    buff[3] = SYM_DIST_KM;
+                    buff[osdConfig()->decimals_distance] = SYM_DIST_KM;
                     break;
                 case OSD_UNIT_GA:
-                    buff[3] = SYM_DIST_NM;
+                    buff[osdConfig()->decimals_distance] = SYM_DIST_NM;
                     break;
             }
-            buff[4] = '\0';
+            buff[osdConfig()->decimals_distance+1] = '\0';
             TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
         } else {
-            osdFormatDistanceSymbol(buff, distanceMeters * 100, 0);
+            osdFormatDistanceSymbol(buff, distanceMeters * 100, 0, osdConfig()->decimals_distance);
             if (distanceMeters == 0)
                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
         }
@@ -2361,6 +2390,11 @@ static bool osdDrawSingleElement(uint8_t item)
             if (FLIGHT_MODE(NAV_FW_AUTOLAND))
                 p = "LAND";
             else
+#endif
+#ifdef USE_GEOZONE
+            if (FLIGHT_MODE(NAV_SEND_TO))
+                p = "AUTO";
+            else
 #endif
             if (FLIGHT_MODE(FAILSAFE_MODE))
                 p = "!FS!";
@@ -2447,15 +2481,15 @@ static bool osdDrawSingleElement(uint8_t item)
             return true;
         }
 
-#if defined(USE_SERIALRX_CRSF)
-    case OSD_CRSF_RSSI_DBM:
+#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP)
+    case OSD_RSSI_DBM:
         {
             int16_t rssi = rxLinkStatistics.uplinkRSSI;
             buff[0] = (rxLinkStatistics.activeAntenna == 0) ? SYM_RSSI : SYM_2RSS; // Separate symbols for each antenna
             if (rssi <= -100) {
                 tfp_sprintf(buff + 1, "%4d%c", rssi, SYM_DBM);
             } else {
-                tfp_sprintf(buff + 1, "%3d%c%c", rssi, SYM_DBM, ' ');
+                tfp_sprintf(buff + 1, " %3d%c", rssi, SYM_DBM);
             }
             if (!failsafeIsReceivingRxData()){
                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
@@ -2464,19 +2498,15 @@ static bool osdDrawSingleElement(uint8_t item)
             }
             break;
         }
-    case OSD_CRSF_LQ:
+    case OSD_LQ_UPLINK:
         {
             buff[0] = SYM_LQ;
-            int16_t statsLQ = rxLinkStatistics.uplinkLQ;
-            int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300);
-            switch (osdConfig()->crsf_lq_format) {
-                case OSD_CRSF_LQ_TYPE1:
-                    if (!failsafeIsReceivingRxData()) {
-                        tfp_sprintf(buff+1, "%3d", 0);
-                    } else {
-                        tfp_sprintf(buff+1, "%3d", rxLinkStatistics.uplinkLQ);
-                    }
-                    break;
+            uint8_t lqFormat = osdConfig()->crsf_lq_format;
+
+            if (rxConfig()->receiverType == RX_TYPE_MSP)
+                lqFormat = OSD_CRSF_LQ_TYPE1;
+
+            switch (lqFormat) {
                 case OSD_CRSF_LQ_TYPE2:
                     if (!failsafeIsReceivingRxData()) {
                         tfp_sprintf(buff+1, "%s:%3d", " ", 0);
@@ -2488,9 +2518,18 @@ static bool osdDrawSingleElement(uint8_t item)
                     if (!failsafeIsReceivingRxData()) {
                         tfp_sprintf(buff+1, "%3d", 0);
                     } else {
+                        int16_t scaledLQ = scaleRange(constrain(rxLinkStatistics.uplinkLQ, 0, 100), 0, 100, 170, 300);
                         tfp_sprintf(buff+1, "%3d", rxLinkStatistics.rfMode >= 2 ? scaledLQ : rxLinkStatistics.uplinkLQ);
                     }
                     break;
+                case OSD_CRSF_LQ_TYPE1:
+                default:
+                    if (!failsafeIsReceivingRxData()) {
+                        tfp_sprintf(buff+1, "%3d", 0);
+                    } else {
+                        tfp_sprintf(buff+1, "%3d", rxLinkStatistics.uplinkLQ);
+                    }
+                    break;
             }
             if (!failsafeIsReceivingRxData()) {
                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
@@ -2500,7 +2539,24 @@ static bool osdDrawSingleElement(uint8_t item)
             break;
         }
 
-    case OSD_CRSF_SNR_DB:
+    case OSD_LQ_DOWNLINK:
+        {
+            buff[0] = SYM_LQ;
+            if (!failsafeIsReceivingRxData()) {
+                tfp_sprintf(buff+1, "%3d%c", 0, SYM_AH_DECORATION_DOWN);
+            } else {
+                tfp_sprintf(buff+1, "%3d%c", rxLinkStatistics.downlinkLQ, SYM_AH_DECORATION_DOWN);
+            }
+
+            if (!failsafeIsReceivingRxData()) {
+                TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+            } else if (rxLinkStatistics.downlinkLQ < osdConfig()->link_quality_alarm) {
+                TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+            }
+            break;
+        }
+
+    case OSD_SNR_DB:
         {
             static pt1Filter_t snrFilterState;
             static timeMs_t snrUpdated = 0;
@@ -2519,23 +2575,49 @@ static bool osdDrawSingleElement(uint8_t item)
                 }
             } else if (snrFiltered <= osdConfig()->snr_alarm) {
                 buff[0] = SYM_SNR;
-                if (snrFiltered <= -10) {
+                if (snrFiltered <= -10 || snrFiltered >= 10) {
                     tfp_sprintf(buff + 1, "%3d%c", snrFiltered, SYM_DB);
                 } else {
-                    tfp_sprintf(buff + 1, "%2d%c%c", snrFiltered, SYM_DB, ' ');
+                    tfp_sprintf(buff + 1, " %2d%c", snrFiltered, SYM_DB);
                 }
             }
             break;
         }
 
-    case OSD_CRSF_TX_POWER:
+    case OSD_TX_POWER_UPLINK:
         {
             if (!failsafeIsReceivingRxData())
-                tfp_sprintf(buff, "%s%c", "    ", SYM_BLANK);
+                tfp_sprintf(buff, "%s%c", "    ", SYM_MW);
             else
                 tfp_sprintf(buff, "%4d%c", rxLinkStatistics.uplinkTXPower, SYM_MW);
             break;
         }
+
+    case OSD_RX_POWER_DOWNLINK:
+        {
+            if (!failsafeIsReceivingRxData())
+                tfp_sprintf(buff, "%s%c%c", "    ", SYM_MW, SYM_AH_DECORATION_DOWN);
+            else
+                tfp_sprintf(buff, "%4d%c%c", rxLinkStatistics.downlinkTXPower, SYM_MW, SYM_AH_DECORATION_DOWN);
+            break;
+        }
+    case OSD_RX_BAND:
+        displayWriteChar(osdDisplayPort, elemPosX++, elemPosY, SYM_RX_BAND);
+        strcat(buff, rxLinkStatistics.band);
+        if (strlen(rxLinkStatistics.band) < 4)
+            for (uint8_t i = strlen(rxLinkStatistics.band); i < 4; i++)
+                buff[i] = ' ';
+        buff[4] = '\0';
+        break;
+
+    case OSD_RX_MODE:
+        displayWriteChar(osdDisplayPort, elemPosX++, elemPosY, SYM_RX_MODE);
+        strcat(buff, rxLinkStatistics.mode);
+        if (strlen(rxLinkStatistics.mode) < 6)
+            for (uint8_t i = strlen(rxLinkStatistics.mode); i < 6; i++)
+                buff[i] = ' ';
+        buff[6] = '\0';
+        break;
 #endif
 
     case OSD_FORMATION_FLIGHT:
@@ -2889,7 +2971,7 @@ static bool osdDrawSingleElement(uint8_t item)
             buff[0] = SYM_GLIDE_DIST;
             if (glideSeconds > 0) {
                 uint32_t glideRangeCM = glideSeconds * gpsSol.groundSpeed;
-                osdFormatDistanceSymbol(buff + 1, glideRangeCM, 0);
+                osdFormatDistanceSymbol(buff + 1, glideRangeCM, 0, 3);
             } else {
                 tfp_sprintf(buff + 1, "%s%c", "---", SYM_BLANK);
                 buff[5] = '\0';
@@ -3002,6 +3084,10 @@ static bool osdDrawSingleElement(uint8_t item)
         osdDisplayNavPIDValues(elemPosX, elemPosY, "VZ", PID_VEL_Z, ADJUSTMENT_VEL_Z_P, ADJUSTMENT_VEL_Z_I, ADJUSTMENT_VEL_Z_D);
         return true;
 
+    case OSD_NAV_FW_ALT_CONTROL_RESPONSE:
+        osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "ACR", 0, pidProfile()->fwAltControlResponseFactor, 3, 0, ADJUSTMENT_NAV_FW_ALT_CONTROL_RESPONSE);
+        return true;
+
     case OSD_HEADING_P:
         osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "HP", 0, pidBank()->pid[PID_HEADING].P, 3, 0, ADJUSTMENT_HEADING_P);
         return true;
@@ -3821,6 +3907,52 @@ static bool osdDrawSingleElement(uint8_t item)
             clearMultiFunction = true;
             break;
         }
+#if defined(USE_GEOZONE)
+        case OSD_COURSE_TO_FENCE:
+        {
+            if (navigationPositionEstimateIsHealthy() && isGeozoneActive()) {
+                int16_t panHomeDirOffset = 0;
+                if (!(osdConfig()->pan_servo_pwm2centideg == 0)){
+                    panHomeDirOffset = osdGetPanServoOffset();
+                }
+                int16_t flightDirection = STATE(AIRPLANE) ? CENTIDEGREES_TO_DEGREES(posControl.actualState.cog) : DECIDEGREES_TO_DEGREES(osdGetHeading());
+                int direction = CENTIDEGREES_TO_DEGREES(geozone.directionToNearestZone) - flightDirection + panHomeDirOffset;
+                osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), direction);
+            } else {
+                if (isGeozoneActive()) {
+                    TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+                }
+                displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, '-', elemAttr);
+            }
+        break;
+        }
+
+        case OSD_H_DIST_TO_FENCE:
+        {
+            if (navigationPositionEstimateIsHealthy() && isGeozoneActive()) {
+                char buff2[12];
+                osdFormatDistanceSymbol(buff2, geozone.distanceHorToNearestZone, 0, 3);
+                tfp_sprintf(buff, "FD %s", buff2 );
+            } else {
+                strcpy(buff, "FD ---");
+            }
+        }
+        break;
+
+        case OSD_V_DIST_TO_FENCE:
+        {
+            if (navigationPositionEstimateIsHealthy() && isGeozoneActive()) {
+                char buff2[12];
+                osdFormatAltitudeSymbol(buff2, abs(geozone.distanceVertToNearestZone));
+                tfp_sprintf(buff, "FD%s", buff2);
+                displayWriteCharWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, geozone.distanceVertToNearestZone < 0 ? SYM_DECORATION + 4 : SYM_DECORATION, elemAttr);
+            } else {
+                strcpy(buff, "FD ---");
+            }
+
+            break;
+        }
+#endif
 
     default:
         return false;
@@ -3976,7 +4108,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
     .adsb_distance_alert = SETTING_OSD_ADSB_DISTANCE_ALERT_DEFAULT,
     .adsb_ignore_plane_above_me_limit = SETTING_OSD_ADSB_IGNORE_PLANE_ABOVE_ME_LIMIT_DEFAULT,
 #endif
-#ifdef USE_SERIALRX_CRSF
+#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP)
     .snr_alarm = SETTING_OSD_SNR_ALARM_DEFAULT,
     .crsf_lq_format = SETTING_OSD_CRSF_LQ_FORMAT_DEFAULT,
     .link_quality_alarm = SETTING_OSD_LINK_QUALITY_ALARM_DEFAULT,
@@ -4044,6 +4176,8 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
     .system_msg_display_time = SETTING_OSD_SYSTEM_MSG_DISPLAY_TIME_DEFAULT,
     .units = SETTING_OSD_UNITS_DEFAULT,
     .main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT,
+    .decimals_altitude = SETTING_OSD_DECIMALS_ALTITUDE_DEFAULT,
+    .decimals_distance = SETTING_OSD_DECIMALS_DISTANCE_DEFAULT,
     .use_pilot_logo = SETTING_OSD_USE_PILOT_LOGO_DEFAULT,
     .inav_to_pilot_logo_spacing = SETTING_OSD_INAV_TO_PILOT_LOGO_SPACING_DEFAULT,
     .arm_screen_display_time = SETTING_OSD_ARM_SCREEN_DISPLAY_TIME_DEFAULT,
@@ -4125,11 +4259,15 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
     osdLayoutsConfig->item_pos[0][OSD_PILOT_LOGO] = OSD_POS(20, 3);
     osdLayoutsConfig->item_pos[0][OSD_VTX_CHANNEL] = OSD_POS(8, 6);
 
-#ifdef USE_SERIALRX_CRSF
-    osdLayoutsConfig->item_pos[0][OSD_CRSF_RSSI_DBM] = OSD_POS(23, 12);
-    osdLayoutsConfig->item_pos[0][OSD_CRSF_LQ] = OSD_POS(23, 11);
-    osdLayoutsConfig->item_pos[0][OSD_CRSF_SNR_DB] = OSD_POS(24, 9);
-    osdLayoutsConfig->item_pos[0][OSD_CRSF_TX_POWER] = OSD_POS(24, 10);
+#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP)
+    osdLayoutsConfig->item_pos[0][OSD_RSSI_DBM] = OSD_POS(23, 12);
+    osdLayoutsConfig->item_pos[0][OSD_LQ_UPLINK] = OSD_POS(23, 10);
+    osdLayoutsConfig->item_pos[0][OSD_LQ_DOWNLINK] = OSD_POS(23, 11);
+    osdLayoutsConfig->item_pos[0][OSD_SNR_DB] = OSD_POS(24, 9);
+    osdLayoutsConfig->item_pos[0][OSD_TX_POWER_UPLINK] = OSD_POS(24, 10);
+    osdLayoutsConfig->item_pos[0][OSD_RX_POWER_DOWNLINK] = OSD_POS(24, 11);
+    osdLayoutsConfig->item_pos[0][OSD_RX_BAND] = OSD_POS(24, 12);
+    osdLayoutsConfig->item_pos[0][OSD_RX_MODE] = OSD_POS(24, 13);
 #endif
 
     osdLayoutsConfig->item_pos[0][OSD_ONTIME] = OSD_POS(23, 8);
@@ -4264,18 +4402,11 @@ uint8_t drawLogos(bool singular, uint8_t row) {
     bool usePilotLogo = (osdConfig()->use_pilot_logo && osdDisplayIsHD());
     bool useINAVLogo = (singular && !usePilotLogo) || !singular;
 
-#ifndef DISABLE_MSP_DJI_COMPAT   // IF DJICOMPAT is in use, the pilot logo cannot be used, due to font issues.
-    if (isDJICompatibleVideoSystem(osdConfig())) {
-        usePilotLogo = false;
-        useINAVLogo = false;
-    }
-#endif
-
     uint8_t logoSpacing = osdConfig()->inav_to_pilot_logo_spacing;
 
     if (logoSpacing > 0 && ((osdDisplayPort->cols % 2) != (logoSpacing % 2))) {
         logoSpacing++; // Add extra 1 character space between logos, if the odd/even of the OSD cols doesn't match the odd/even of the logo spacing
-}
+    }
 
     // Draw Logo(s)
     if (usePilotLogo && !singular) {
@@ -4564,7 +4695,7 @@ static void osdResetStats(void)
     stats.max_3D_speed = 0;
     stats.max_air_speed = 0;
     stats.min_voltage = 12000;
-    stats.min_rssi = 99;
+    stats.min_rssi = 100;
     stats.min_lq = 300;
     stats.min_rssi_dbm = 0;
     stats.max_altitude = 0;
@@ -4789,7 +4920,7 @@ uint8_t drawStat_UsedEnergy(uint8_t col, uint8_t row, uint8_t statValX)
         strcat(buff, "/");
         osdFormatCentiNumber(preBuff, getMWhDrawn() / 10, 0, 2, 0, 3, false);
         strcat(buff, osdFormatTrimWhiteSpace(preBuff));
-        tfp_sprintf(buff + strlen(buff), "%s%c", buff, SYM_WH);
+        tfp_sprintf(buff + strlen(buff), "%c", SYM_WH);
     }
     displayWrite(osdDisplayPort, statValX, row++, buff);
 
@@ -4851,7 +4982,7 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b
                         osdFormatCentiNumber(buff, (int32_t)((getMWhDrawn() - stats.flightStartMWh) * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, digits, false);
                         strcat(outBuff, osdFormatTrimWhiteSpace(buff));
                         strcat(outBuff, "/");
-                        osdFormatCentiNumber(buff + strlen(buff), (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, digits, false);
+                        osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, digits, false);
                         strcat(outBuff, osdFormatTrimWhiteSpace(buff));
                     } else {
                         strcat(outBuff, "---/---");
@@ -5203,65 +5334,92 @@ static void osdShowHDArmScreen(void)
     dateTime_t dt;
     char        buf[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)];
     char        buf2[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)];
-    char craftNameBuf[MAX_NAME_LENGTH];
+    char        craftNameBuf[MAX_NAME_LENGTH];
     char        versionBuf[osdDisplayPort->cols];
     uint8_t     safehomeRow     = 0;
-    uint8_t     armScreenRow    = 2; // Start at row 2
+    uint8_t     armScreenRow    = 1;
+
+    bool        showPilotOrCraftName = false;
 
     armScreenRow = drawLogos(false, armScreenRow);
     armScreenRow++;
 
+    memset(buf2, '\0', sizeof(buf2));
+    if (!osdConfig()->use_pilot_logo && strlen(systemConfig()->pilotName) > 0) {
+        osdFormatPilotName(buf2);
+        showPilotOrCraftName = true;
+    }
+
+    memset(craftNameBuf, '\0', sizeof(craftNameBuf));
     if (strlen(systemConfig()->craftName) > 0) {
         osdFormatCraftName(craftNameBuf);
-        strcpy(buf2, "ARMED!");
-        tfp_sprintf(buf, "%s - %s", craftNameBuf, buf2);
-    } else {
-        strcpy(buf, "ARMED!");
+        if (strlen(buf2) > 0) {
+            strcat(buf2, " : ");
+        }
+        showPilotOrCraftName = true;
     }
+
+    if (showPilotOrCraftName) {
+        tfp_sprintf(buf, "%s%s: ! ARMED !", buf2, craftNameBuf);
+    } else {
+        strcpy(buf, " ! ARMED !");
+    }
+
     displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
+    memset(buf, '\0', sizeof(buf));
+    memset(buf2, '\0', sizeof(buf2));
+
 #if defined(USE_GPS)
 #if defined (USE_SAFE_HOME)
     if (posControl.safehomeState.distance) {
         safehomeRow = armScreenRow;
-        armScreenRow++;
+        armScreenRow +=2;
     }
 #endif // USE_SAFE_HOME
 #endif // USE_GPS
-    armScreenRow++;
 
     if (posControl.waypointListValid && posControl.waypointCount > 0) {
 #ifdef USE_MULTI_MISSION
         tfp_sprintf(buf, "MISSION %u/%u (%u WP)", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, posControl.waypointCount);
         displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
+        memset(buf, '\0', sizeof(buf));
 #else
         strcpy(buf, "*MISSION LOADED*");
         displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
+        memset(buf, '\0', sizeof(buf));
 #endif
     }
-    armScreenRow++;
 
 #if defined(USE_GPS)
     if (feature(FEATURE_GPS)) {
         if (STATE(GPS_FIX_HOME)) {
-            if (osdConfig()->osd_home_position_arm_screen){
-                osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat);
-                osdFormatCoordinate(buf2, SYM_LON, GPS_home.lon);
-                uint8_t gap = 1;
-                uint8_t col = strlen(buf) + strlen(buf2) + gap;
+            if (osdConfig()->osd_home_position_arm_screen) {
+                // Show pluscode if enabled on any OSD layout. Otherwise show GNSS cordinates.
+                if (osdElementEnabled(OSD_PLUS_CODE, false)) {
+                    int digits = osdConfig()->plus_code_digits;
+                    olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf));
+                    tfp_sprintf(buf2, "+CODE: %s%c", buf, '\0');
+                    displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf2)) / 2, armScreenRow++, buf2);
+                    memset(buf, '\0', sizeof(buf));
+                    memset(buf2, '\0', sizeof(buf2));
+                } else {
+                    osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat);
+                    osdFormatCoordinate(buf2, SYM_LON, GPS_home.lon);
+                    uint8_t gap = 1;
+                    uint8_t col = strlen(buf) + strlen(buf2) + gap;
 
-                if ((osdDisplayPort->cols %2) != (col %2)) {
-                    gap++;
-                    col++;
+                    if ((osdDisplayPort->cols %2) != (col %2)) {
+                        gap++;
+                        col++;
+                    }
+
+                    col = (osdDisplayPort->cols - col) / 2;
+
+                    displayWrite(osdDisplayPort, col, armScreenRow, buf);
+                    displayWrite(osdDisplayPort, col + strlen(buf) + gap, armScreenRow++, buf2);
+                    memset(buf, '\0', sizeof(buf));
+                    memset(buf2, '\0', sizeof(buf2));
                 }
-
-                col = (osdDisplayPort->cols - col) / 2;
-
-                displayWrite(osdDisplayPort, col, armScreenRow, buf);
-                displayWrite(osdDisplayPort, col + strlen(buf) + gap, armScreenRow++, buf2);
-
-                int digits = osdConfig()->plus_code_digits;
-                olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf));
-                displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
             }
 
 #if defined (USE_SAFE_HOME)
@@ -5275,19 +5433,23 @@ static void osdShowHDArmScreen(void)
                 textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT;
                 // write this message below the ARMED message to make it obvious
                 displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, safehomeRow, buf, elemAttr);
+                memset(buf, '\0', sizeof(buf));
+                memset(buf2, '\0', sizeof(buf2));
             }
 #endif
         } else {
             strcpy(buf, "!NO HOME POSITION!");
             displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
+            memset(buf, '\0', sizeof(buf));
+            armScreenRow++;
         }
-        armScreenRow++;
     }
 #endif
 
     if (rtcGetDateTimeLocal(&dt)) {
         tfp_sprintf(buf, "%04u-%02u-%02u  %02u:%02u:%02u", dt.year, dt.month, dt.day, dt.hours, dt.minutes, dt.seconds);
         displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
+        memset(buf, '\0', sizeof(buf));
         armScreenRow++;
     }
 
@@ -5308,45 +5470,79 @@ static void osdShowSDArmScreen(void)
     char        buf2[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)];
     char        craftNameBuf[MAX_NAME_LENGTH];
     char        versionBuf[osdDisplayPort->cols];
-    // We need 12 visible rows, start row never < first fully visible row 1
-    uint8_t     armScreenRow = osdDisplayPort->rows > 13 ? (osdDisplayPort->rows - 12) / 2 : 1;
+    uint8_t     armScreenRow = 1;
     uint8_t     safehomeRow = 0;
+    bool        showPilotOrCraftName = false;
 
-    strcpy(buf, "ARMED!");
+    strcpy(buf, "! ARMED !");
     displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
-    safehomeRow = armScreenRow;
-    armScreenRow++;
+    memset(buf, '\0', sizeof(buf));
+#if defined(USE_GPS)
+#if defined (USE_SAFE_HOME)
+    if (posControl.safehomeState.distance) {
+        safehomeRow = armScreenRow;
+        armScreenRow += 2;
+    }
+#endif
+#endif
 
+    memset(buf2, '\0', sizeof(buf2));
+    if (strlen(systemConfig()->pilotName) > 0) {
+        osdFormatPilotName(buf2);
+        showPilotOrCraftName = true;
+    }
+
+    memset(craftNameBuf, '\0', sizeof(craftNameBuf));
     if (strlen(systemConfig()->craftName) > 0) {
         osdFormatCraftName(craftNameBuf);
-        displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(systemConfig()->craftName)) / 2, armScreenRow++, craftNameBuf );
+        if (strlen(buf2) > 0) {
+            strcat(buf2, " : ");
+        }
+        showPilotOrCraftName = true;
+    }
+
+    if (showPilotOrCraftName) {
+        tfp_sprintf(buf, "%s%s", buf2, craftNameBuf);
+        displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf );
+        memset(buf, '\0', sizeof(buf));
+        memset(buf2, '\0', sizeof(buf2));
+        armScreenRow++;
     }
 
     if (posControl.waypointListValid && posControl.waypointCount > 0) {
 #ifdef USE_MULTI_MISSION
         tfp_sprintf(buf, "MISSION %u/%u (%u WP)", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, posControl.waypointCount);
-        displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow, buf);
+        displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
+        memset(buf, '\0', sizeof(buf));
 #else
         strcpy(buf, "*MISSION LOADED*");
-        displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow, buf);
+        displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
+        memset(buf, '\0', sizeof(buf));
 #endif
     }
-    armScreenRow++;
 
 #if defined(USE_GPS)
     if (feature(FEATURE_GPS)) {
         if (STATE(GPS_FIX_HOME)) {
             if (osdConfig()->osd_home_position_arm_screen) {
-                osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat);
-                osdFormatCoordinate(buf2, SYM_LON, GPS_home.lon);
+                // Show pluscode if enabled on any OSD layout. Otherwise show GNSS cordinates.
+                if (osdElementEnabled(OSD_PLUS_CODE, false)) {
+                    int digits = osdConfig()->plus_code_digits;
+                    olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf));
+                    tfp_sprintf(buf2, "+CODE: %s%c", buf, '\0');
+                    displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf2)) / 2, armScreenRow++, buf2);
+                    memset(buf, '\0', sizeof(buf));
+                    memset(buf2, '\0', sizeof(buf2));
+                } else {
+                    osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat);
+                    osdFormatCoordinate(buf2, SYM_LON, GPS_home.lon);
 
-                uint8_t gpsStartCol = (osdDisplayPort->cols - (strlen(buf) + strlen(buf2) + 2)) / 2;
-                displayWrite(osdDisplayPort, gpsStartCol, armScreenRow, buf);
-                displayWrite(osdDisplayPort, gpsStartCol + strlen(buf) + 2, armScreenRow++, buf2);
-
-                int digits = osdConfig()->plus_code_digits;
-                olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf));
-                displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
+                    uint8_t gpsStartCol = (osdDisplayPort->cols - (strlen(buf) + strlen(buf2) + 2)) / 2;
+                    displayWrite(osdDisplayPort, gpsStartCol, armScreenRow, buf);
+                    displayWrite(osdDisplayPort, gpsStartCol + strlen(buf) + 2, armScreenRow++, buf2);
+                    memset(buf, '\0', sizeof(buf));
+                    memset(buf2, '\0', sizeof(buf2));
+                }
             }
 
 #if defined (USE_SAFE_HOME)
@@ -5357,22 +5553,25 @@ static void osdShowSDArmScreen(void)
                     osdFormatDistanceStr(buf2, posControl.safehomeState.distance);
                     tfp_sprintf(buf, "%c SAFEHOME %u @ %s", SYM_HOME, posControl.safehomeState.index, buf2);
                 }
-                textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT;
                 // write this message below the ARMED message to make it obvious
-                displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, safehomeRow, buf, elemAttr);
+                displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, safehomeRow, buf);
+                memset(buf, '\0', sizeof(buf));
+                memset(buf2, '\0', sizeof(buf2));
             }
 #endif
         } else {
             strcpy(buf, "!NO HOME POSITION!");
             displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
+            memset(buf, '\0', sizeof(buf));
+            armScreenRow++;
         }
-        armScreenRow++;
     }
 #endif
 
     if (rtcGetDateTimeLocal(&dt)) {
         tfp_sprintf(buf, "%04u-%02u-%02u  %02u:%02u:%02u", dt.year, dt.month, dt.day, dt.hours, dt.minutes, dt.seconds);
         displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
+        memset(buf, '\0', sizeof(buf));
         armScreenRow++;
     }
 
@@ -5779,7 +5978,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                 } else if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) {
                     // Countdown display for remaining Waypoints
                     char buf[6];
-                    osdFormatDistanceSymbol(buf, posControl.wpDistance, 0);
+                    osdFormatDistanceSymbol(buf, posControl.wpDistance, 0, 3);
                     tfp_sprintf(messageBuf, "TO WP %u/%u (%s)", getGeoWaypointNumber(posControl.activeWaypointIndex), posControl.geoWaypointCount, buf);
                     messages[messageCount++] = messageBuf;
                 } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) {
@@ -5808,6 +6007,12 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                     messages[messageCount++] = safehomeMessage;
                 }
 #endif
+
+#ifdef USE_GEOZONE
+                if (geozone.avoidInRTHInProgress) {
+                    messages[messageCount++] = OSD_MSG_AVOID_ZONES_RTH;
+                }
+#endif
                 if (FLIGHT_MODE(FAILSAFE_MODE)) {   // In FS mode while armed
                     if (NAV_Status.state == MW_NAV_STATE_LAND_SETTLE && posControl.landingDelay > 0) {
                         uint16_t remainingHoldSec = MS2S(posControl.landingDelay - millis());
@@ -5832,6 +6037,64 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
             } else if (STATE(LANDING_DETECTED)) {
                 messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED);
             } else {
+#ifdef USE_GEOZONE
+            char buf[12], buf1[12];
+            switch (geozone.messageState) {
+                case GEOZONE_MESSAGE_STATE_NFZ:
+                    messages[messageCount++] = OSD_MSG_NFZ;
+                    break;
+                case GEOZONE_MESSAGE_STATE_LEAVING_FZ:
+                    osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3);
+                    tfp_sprintf(messageBuf, OSD_MSG_LEAVING_FZ, buf);
+                    messages[messageCount++] = messageBuf;
+                    break;
+                case GEOZONE_MESSAGE_STATE_OUTSIDE_FZ:
+                    messages[messageCount++] = OSD_MSG_OUTSIDE_FZ;
+                    break;
+                case GEOZONE_MESSAGE_STATE_ENTERING_NFZ:
+                osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3);
+                    if (geozone.zoneInfo == INT32_MAX) {
+                        tfp_sprintf(buf1, "%s%c", "INF", SYM_ALT_M);
+                    } else {
+                        osdFormatAltitudeSymbol(buf1, geozone.zoneInfo);
+                    }
+                    tfp_sprintf(messageBuf, OSD_MSG_ENTERING_NFZ, buf, buf1);
+                    messages[messageCount++] = messageBuf;
+                    break;
+                case GEOZONE_MESSAGE_STATE_AVOIDING_FB:
+                    messages[messageCount++] = OSD_MSG_AVOIDING_FB;
+                    if (!posControl.sendTo.lockSticks) {
+                        messages[messageCount++] = OSD_MSG_MOVE_STICKS;
+                    }
+                    break;
+                case GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE:
+                    messages[messageCount++] = OSD_MSG_RETURN_TO_ZONE;
+                    if (!posControl.sendTo.lockSticks) {
+                        messages[messageCount++] = OSD_MSG_MOVE_STICKS;
+                    }
+                    break;
+                case GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH:
+                    messages[messageCount++] = OSD_MSG_AVOIDING_ALT_BREACH;
+                    if (!posControl.sendTo.lockSticks) {
+                        messages[messageCount++] = OSD_MSG_MOVE_STICKS;
+                    }
+                    break;
+                case GEOZONE_MESSAGE_STATE_FLYOUT_NFZ:
+                    messages[messageCount++] = OSD_MSG_FLYOUT_NFZ;
+                    if (!posControl.sendTo.lockSticks) {
+                        messages[messageCount++] = OSD_MSG_MOVE_STICKS;
+                    }
+                    break;
+                case GEOZONE_MESSAGE_STATE_POS_HOLD:
+                    messages[messageCount++] = OSD_MSG_AVOIDING_FB;
+                    if (!geozone.sticksLocked) {
+                        messages[messageCount++] = OSD_MSG_MOVE_STICKS;
+                    }
+                    break;
+                case GEOZONE_MESSAGE_STATE_NONE:
+                    break;
+            }
+#endif
                 /* Messages shown only when Failsafe, WP, RTH or Emergency Landing not active and landed state inactive */
                 /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL */
                 if (STATE(AIRPLANE)) {      /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL */
@@ -5870,6 +6133,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                             messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_PITCH);
                         }
                     }
+
                 } else if (STATE(MULTIROTOR)) {     /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */
                     if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
                         if (posControl.cruise.multicopterSpeed >= 50.0f) {

src/main/io/osd.h

@@ -132,6 +132,19 @@
 #define OSD_MSG_LOITERING_SAFEHOME  "LOITERING AROUND SAFEHOME"
 #endif
 
+#if defined(USE_GEOZONE)
+#define OSD_MSG_NFZ                 "NO FLY ZONE"
+#define OSD_MSG_LEAVING_FZ          "LEAVING FZ IN %s"
+#define OSD_MSG_OUTSIDE_FZ          "OUTSIDE FZ"
+#define OSD_MSG_ENTERING_NFZ        "ENTERING NFZ IN %s %s"
+#define OSD_MSG_AVOIDING_FB         "AVOIDING FENCE BREACH"
+#define OSD_MSG_RETURN_TO_ZONE      "RETURN TO FZ"
+#define OSD_MSG_FLYOUT_NFZ          "FLY OUT NFZ"
+#define OSD_MSG_AVOIDING_ALT_BREACH "REACHED ZONE ALTITUDE LIMIT"
+#define OSD_MSG_AVOID_ZONES_RTH     "AVOIDING NO FLY ZONES"
+#define OSD_MSG_GEOZONE_ACTION      "PERFORM ACTION IN %s %s"
+#endif
+
 typedef enum {
     OSD_RSSI_VALUE,
     OSD_MAIN_BATT_VOLTAGE,
@@ -242,10 +255,10 @@ typedef enum {
     OSD_ESC_RPM,
     OSD_ESC_TEMPERATURE,
     OSD_AZIMUTH,
-    OSD_CRSF_RSSI_DBM,
-    OSD_CRSF_LQ,
-    OSD_CRSF_SNR_DB,
-    OSD_CRSF_TX_POWER,
+    OSD_RSSI_DBM,
+    OSD_LQ_UPLINK,
+    OSD_SNR_DB,
+    OSD_TX_POWER_UPLINK,
     OSD_GVAR_0,
     OSD_GVAR_1,
     OSD_GVAR_2,
@@ -291,7 +304,15 @@ typedef enum {
     OSD_CUSTOM_ELEMENT_5,
     OSD_CUSTOM_ELEMENT_6,
     OSD_CUSTOM_ELEMENT_7,
-    OSD_CUSTOM_ELEMENT_8, // 158
+    OSD_CUSTOM_ELEMENT_8,
+    OSD_LQ_DOWNLINK,
+    OSD_RX_POWER_DOWNLINK, // 160
+    OSD_RX_BAND,
+    OSD_RX_MODE,
+    OSD_COURSE_TO_FENCE,
+    OSD_H_DIST_TO_FENCE,
+    OSD_V_DIST_TO_FENCE,
+    OSD_NAV_FW_ALT_CONTROL_RESPONSE,
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;
 
@@ -369,7 +390,7 @@ typedef struct osdConfig_s {
     float           gforce_alarm;
     float           gforce_axis_alarm_min;
     float           gforce_axis_alarm_max;
-#ifdef USE_SERIALRX_CRSF
+#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP)
     int8_t          snr_alarm;                          //CRSF SNR alarm in dB
     int8_t          link_quality_alarm;
     int16_t         rssi_dbm_alarm;                     // in dBm
@@ -394,6 +415,8 @@ typedef struct osdConfig_s {
 
     // Preferences
     uint8_t         main_voltage_decimals;
+    uint8_t         decimals_altitude;
+    uint8_t         decimals_distance;
     uint8_t         ahi_reverse_roll;
     uint8_t         ahi_max_pitch;
     uint8_t         crosshairs_style;                   // from osd_crosshairs_style_e
@@ -466,12 +489,17 @@ typedef struct osdConfig_s {
 #ifndef DISABLE_MSP_DJI_COMPAT
     bool            highlight_djis_missing_characters;  // If enabled, show question marks where there is no character in DJI's font to represent an OSD element symbol
 #endif
- #ifdef USE_ADSB
+    bool            enable_broken_o4_workaround;        // If enabled, override STATUS/STATUS_EX messages to work around DJI's broken O4 air unit MSP DisplayPort implementation
+#ifdef USE_ADSB
     uint16_t adsb_distance_warning;                     // in metres
     uint16_t adsb_distance_alert;                       // in metres
     uint16_t adsb_ignore_plane_above_me_limit;          // in metres
- #endif
+#endif
     uint8_t  radar_peers_display_time;                  // in seconds
+#ifdef USE_GEOZONE
+    uint8_t geozoneDistanceWarning;                     // Distance to fence or action
+    bool geozoneDistanceType;                            // Shows a countdown timer or distance to fence/action
+#endif
 } osdConfig_t;
 
 PG_DECLARE(osdConfig_t, osdConfig);

src/main/io/osd/custom_elements.c

@@ -98,6 +98,11 @@ uint8_t customElementDrawPart(char *buff, uint8_t customElementIndex, uint8_t cu
             osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -99, 99) * (int32_t) 10), 1, 1, 0, 3, false);
             return 3;
         }
+        case CUSTOM_ELEMENT_TYPE_GV_FLOAT_1_2:
+        {
+            osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -999, 999)), 1, 2, 0, 4, false);
+            return 4;
+        }
         case CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_1:
         {
             osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -999, 999) * (int32_t) 10), 1, 1, 0, 4, false);
@@ -154,6 +159,11 @@ uint8_t customElementDrawPart(char *buff, uint8_t customElementIndex, uint8_t cu
             osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -99, 99) * (int32_t) 10), 1, 1, 0, 3, false);
             return 3;
         }
+        case CUSTOM_ELEMENT_TYPE_LC_FLOAT_1_2:
+        {
+            osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -999, 999)), 1, 2, 0, 4, false);
+            return 4;
+        }
         case CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_1:
         {
             osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -999, 999) * (int32_t) 10), 1, 1, 0, 4, false);

src/main/io/osd/custom_elements.h

@@ -35,22 +35,25 @@ typedef enum {
     CUSTOM_ELEMENT_TYPE_GV_4            = 8,
     CUSTOM_ELEMENT_TYPE_GV_5            = 9,
     CUSTOM_ELEMENT_TYPE_GV_FLOAT_1_1    = 10,
-    CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_1    = 11,
-    CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_2    = 12,
-    CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_1    = 13,
-    CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_2    = 14,
-    CUSTOM_ELEMENT_TYPE_GV_FLOAT_4_1    = 15,
-    CUSTOM_ELEMENT_TYPE_LC_1            = 16,
-    CUSTOM_ELEMENT_TYPE_LC_2            = 17,
-    CUSTOM_ELEMENT_TYPE_LC_3            = 18,
-    CUSTOM_ELEMENT_TYPE_LC_4            = 19,
-    CUSTOM_ELEMENT_TYPE_LC_5            = 20,
-    CUSTOM_ELEMENT_TYPE_LC_FLOAT_1_1    = 21,
-    CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_1    = 22,
-    CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_2    = 23,
-    CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_1    = 24,
-    CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_2    = 25,
-    CUSTOM_ELEMENT_TYPE_LC_FLOAT_4_1    = 26,
+    CUSTOM_ELEMENT_TYPE_GV_FLOAT_1_2    = 11,
+    CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_1    = 12,
+    CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_2    = 13,
+    CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_1    = 14,
+    CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_2    = 15,
+    CUSTOM_ELEMENT_TYPE_GV_FLOAT_4_1    = 16,
+    CUSTOM_ELEMENT_TYPE_LC_1            = 17,
+    CUSTOM_ELEMENT_TYPE_LC_2            = 18,
+    CUSTOM_ELEMENT_TYPE_LC_3            = 19,
+    CUSTOM_ELEMENT_TYPE_LC_4            = 20,
+    CUSTOM_ELEMENT_TYPE_LC_5            = 21,
+    CUSTOM_ELEMENT_TYPE_LC_FLOAT_1_1    = 22,
+    CUSTOM_ELEMENT_TYPE_LC_FLOAT_1_2    = 23,
+    CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_1    = 24,
+    CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_2    = 25,
+    CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_1    = 26,
+    CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_2    = 27,
+    CUSTOM_ELEMENT_TYPE_LC_FLOAT_4_1    = 28,
+    CUSTOM_ELEMENT_TYPE_END             // Must be last
 } osdCustomElementType_e;
 
 typedef enum {

src/main/io/osd_dji_hd.c

@@ -89,13 +89,6 @@
 
 #if defined(USE_DJI_HD_OSD)
 
-#define DJI_MSP_BAUDRATE                    115200
-
-#define DJI_ARMING_DISABLE_FLAGS_COUNT      25
-#define DJI_OSD_WARNING_COUNT               16
-#define DJI_OSD_TIMER_COUNT                 2
-#define DJI_OSD_FLAGS_OSD_FEATURE           (1 << 0)
-#define EFFICIENCY_UPDATE_INTERVAL          (5 * 1000)
 
 #define RC_RX_LINK_LOST_MSG "!RC RX LINK LOST!"
 
@@ -269,7 +262,7 @@ void djiOsdSerialInit(void)
     }
 }
 
-static void djiPackBoxModeBitmask(boxBitmask_t * flightModeBitmask)
+void djiPackBoxModeBitmask(boxBitmask_t * flightModeBitmask)
 {
     memset(flightModeBitmask, 0, sizeof(boxBitmask_t));
 
@@ -311,7 +304,7 @@ static void djiPackBoxModeBitmask(boxBitmask_t * flightModeBitmask)
     }
 }
 
-static uint32_t djiPackArmingDisabledFlags(void)
+uint32_t djiPackArmingDisabledFlags(void)
 {
     // TODO: Map INAV arming disabled flags to DJI/BF ones
     // https://github.com/betaflight/betaflight/blob/c6e5882dd91fa20d246b8f8af10cf6c92876bc3d/src/main/fc/runtime_config.h#L42
@@ -525,6 +518,8 @@ static char * osdArmingDisabledReasonMessage(void)
         case ARMING_DISABLED_DSHOT_BEEPER:
             return OSD_MESSAGE_STR("MOTOR BEEPER ACTIVE");
             // Cases without message
+        case ARMING_DISABLED_GEOZONE:
+            return OSD_MESSAGE_STR("NO FLY ZONE");
         case ARMING_DISABLED_LANDING_DETECTED:
             FALLTHROUGH;
         case ARMING_DISABLED_CMS_MENU:
@@ -956,6 +951,9 @@ static void osdDJIAdjustmentMessage(char *buff, uint8_t adjustmentFunction)
         case ADJUSTMENT_VEL_Z_D:
             tfp_sprintf(buff, "VZD %3d", pidBankMutable()->pid[PID_VEL_Z].D);
             break;
+        case ADJUSTMENT_NAV_FW_ALT_CONTROL_RESPONSE:
+            tfp_sprintf(buff, "ACR %3d", pidProfileMutable()->fwAltControlResponseFactor);
+            break;
         case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
             tfp_sprintf(buff, "MTDPA %4d", navConfigMutable()->fw.minThrottleDownPitchAngle);
             break;

src/main/io/osd_dji_hd.h

@@ -30,6 +30,7 @@
 #include "msp/msp_serial.h"
 
 #include "config/parameter_group.h"
+#include "fc/rc_modes.h"
 
 #if defined(USE_DJI_HD_OSD)
 
@@ -66,6 +67,14 @@
 #define DJI_ALTERNATING_DURATION_LONG   (djiOsdConfig()->craftNameAlternatingDuration * 100)
 #define DJI_ALTERNATING_DURATION_SHORT  1000
 
+#define DJI_MSP_BAUDRATE                    115200
+
+#define DJI_ARMING_DISABLE_FLAGS_COUNT      25
+#define DJI_OSD_WARNING_COUNT               16
+#define DJI_OSD_TIMER_COUNT                 2
+#define DJI_OSD_FLAGS_OSD_FEATURE           (1 << 0)
+#define EFFICIENCY_UPDATE_INTERVAL          (5 * 1000)
+
 enum djiOsdTempSource_e {
     DJI_OSD_TEMP_ESC    = 0,
     DJI_OSD_TEMP_CORE   = 1,
@@ -95,4 +104,7 @@ PG_DECLARE(djiOsdConfig_t, djiOsdConfig);
 void djiOsdSerialInit(void);
 void djiOsdSerialProcess(void);
 
+uint32_t djiPackArmingDisabledFlags(void);
+void djiPackBoxModeBitmask(boxBitmask_t * flightModeBitmask);
+
 #endif

src/main/io/vtx_msp.c

@@ -17,157 +17,102 @@
  *
  * If not, see <http://www.gnu.org/licenses/>.
  */
+/* Created by geoffsim */
 
-/* Created by phobos- */
-
-#include <stdbool.h>
 #include <stdint.h>
-#include <stdlib.h>
-#include <math.h>
-#include <ctype.h>
 #include <string.h>
+#include <stdbool.h>
 
 #include "platform.h"
 
 #if defined(USE_VTX_MSP) && defined(USE_VTX_CONTROL) && defined(USE_VTX_COMMON)
 
-#include "build/debug.h"
-
-//#include "cms/cms_menu_vtx_msp.h"
-#include "common/crc.h"
 #include "common/log.h"
-#include "config/feature.h"
-
+#include "common/crc.h"
 #include "drivers/vtx_common.h"
-//#include "drivers/vtx_table.h"
-
-#include "fc/runtime_config.h"
-#include "flight/failsafe.h"
-
-#include "io/serial.h"
-#include "io/vtx_msp.h"
-#include "io/vtx_control.h"
-#include "io/vtx_string.h"
-#include "io/vtx_smartaudio.h"
-#include "io/vtx.h"
-#include "io/displayport_msp_osd.h"
-
 #include "msp/msp_protocol.h"
-#include "msp/msp_serial.h"
-#include "msp/msp.h"
-
-//#include "pg/vtx_table.h"
-#include "fc/settings.h"
-
-#include "rx/crsf.h"
-//#include "rx/crsf_protocol.h"
 #include "rx/rx.h"
+#include "rx/crsf.h"
+#include "telemetry/crsf.h"
+#include "vtx.h"
+#include "displayport_msp_osd.h"
+#include "vtx_string.h"
+#include "vtx_msp.h"
 
-#include "telemetry/msp_shared.h"
+#define VTX_MSP_MIN_BAND           (1)
+#define VTX_MSP_MAX_BAND           (VTX_MSP_MIN_BAND + VTX_MSP_BAND_COUNT - 1)
+#define VTX_MSP_MIN_CHANNEL        (1)
+#define VTX_MSP_MAX_CHANNEL        (VTX_MSP_MIN_CHANNEL + VTX_MSP_CHANNEL_COUNT -1)
 
-//static uint16_t mspConfFreq = 0;
-static uint8_t mspConfBand = SETTING_VTX_BAND_DEFAULT;
-static uint8_t mspConfChannel = SETTING_VTX_CHANNEL_DEFAULT;
-//static uint16_t mspConfPower = 0;
-static uint16_t mspConfPowerIndex = SETTING_VTX_POWER_DEFAULT;
-static uint8_t mspConfPitMode = 0;
-static bool mspVtxConfigChanged = false;
-static timeUs_t mspVtxLastTimeUs = 0;
-static bool prevLowPowerDisarmedState = false;
+#define VTX_UPDATE_REQ_NONE         0x00
+#define VTX_UPDATE_REQ_FREQUENCY    0x01
+#define VTX_UPDATE_REQ_POWER        0x02
+#define VTX_UPDATE_REQ_PIT_MODE     0x04
 
-static const vtxVTable_t mspVTable; // forward
-static vtxDevice_t vtxMsp = {
-    .vTable = &mspVTable,
-    .capability.bandCount = VTX_MSP_TABLE_MAX_BANDS,
-    .capability.channelCount = VTX_MSP_TABLE_MAX_CHANNELS,
-    .capability.powerCount = VTX_MSP_TABLE_MAX_POWER_LEVELS,
-    .capability.bandNames = (char **)vtx58BandNames,
-    .capability.channelNames = (char **)vtx58ChannelNames,
-    .capability.powerNames = (char**)saPowerNames
+typedef struct {
+    bool ready;
+    uint8_t timeouts;
+    uint8_t updateReqMask;
+    bool crsfTelemetryEnabled;
 
+    struct {
+        uint8_t     band;
+        uint8_t     channel;
+        uint16_t    freq;
+        uint8_t     power;
+        uint8_t     powerIndex;
+        uint8_t     pitMode;
+    } request;
+;
+} vtxProtoState_t;
+
+const char * const vtxMspBandNames[VTX_MSP_BAND_COUNT + 1] = {
+    "-----", "A 2.4", "B 2.4", "E 2.4", "F 2.4", "R 2.4"
 };
 
-STATIC_UNIT_TESTED mspVtxStatus_e mspVtxStatus = MSP_VTX_STATUS_OFFLINE;
-static uint8_t mspVtxPortIdentifier = 255;
+const char * vtxMspBandLetters = "-ABEFR";
 
-#define MSP_VTX_REQUEST_PERIOD_US (200 * 1000) // 200ms
+const char * const vtxMspChannelNames[VTX_MSP_CHANNEL_COUNT + 1] = {
+    "-", "1", "2", "3", "4", "5", "6", "7", "8"
+};
 
-static bool isCrsfPortConfig(const serialPortConfig_t *portConfig)
+const char * const vtxMspPowerNames[VTX_MSP_POWER_COUNT + 1] = {
+    "0", "25", "200", "500", "MAX"
+};
+
+const unsigned vtxMspPowerTable[VTX_MSP_POWER_COUNT + 1] = {
+    0, 25, 200, 500, 1000
+};
+
+static serialPortIdentifier_e mspVtxPortIdentifier;
+static vtxProtoState_t vtxState;
+
+static vtxDevType_e vtxMspGetDeviceType(const vtxDevice_t *);
+static bool vtxMspIsReady(const vtxDevice_t *);
+static vtxDevice_t vtxMsp;
+
+static void prepareMspFrame(vtxDevice_t *vtxDevice, uint8_t *mspFrame)
 {
-    return portConfig->functionMask & FUNCTION_RX_SERIAL && portConfig->functionMask & FUNCTION_VTX_MSP && rxConfig()->serialrx_provider == SERIALRX_CRSF;
-}
-
-static bool isLowPowerDisarmed(void)
-{
-    return (!ARMING_FLAG(ARMED) && !failsafeIsActive() &&
-        (vtxSettingsConfig()->lowPowerDisarm == VTX_LOW_POWER_DISARM_ALWAYS ||
-        (vtxSettingsConfig()->lowPowerDisarm == VTX_LOW_POWER_DISARM_UNTIL_FIRST_ARM && !ARMING_FLAG(WAS_EVER_ARMED))));
-}
-
-bool isVtxConfigValid(const vtxConfig_t *cfg)
-{
-    LOG_DEBUG(VTX, "msp isVtxConfigValid\r\n");
-    for (int i = 0; i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; ++i) {
-
-        if (cfg->vtxChannelActivationConditions[i].band || 
-            (cfg->vtxChannelActivationConditions[i].range.startStep && cfg->vtxChannelActivationConditions[i].range.endStep) ||
-            cfg->vtxChannelActivationConditions[i].auxChannelIndex ||
-            cfg->vtxChannelActivationConditions[i].channel) {
-                return true;
-        }
-    }
-
-    LOG_DEBUG(VTX, "msp Invalid Config!\r\n");
-    return false;
-}
-
-
-void setMspVtxDeviceStatusReady(const int descriptor)
-{
-    LOG_DEBUG(VTX, "msp setMspVtxDeviceStatusReady\r\n");
-    UNUSED(descriptor);
-
-    mspVtxStatus = MSP_VTX_STATUS_READY;
-    mspVtxConfigChanged = true;
-}
-
-
-void prepareMspFrame(uint8_t *mspFrame)
-{
-    LOG_DEBUG(VTX, "msp PrepareMspFrame\r\n");
-/*
-HDZERO parsing
-    fc_band_rx = msp_rx_buf[1];
-    fc_channel_rx = msp_rx_buf[2];
-    fc_pwr_rx = msp_rx_buf[3];
-    fc_pit_rx = msp_rx_buf[4];
-    fc_lp_rx = msp_rx_buf[8];
-*/
-
-    uint8_t pitmode = 0;
-    vtxCommonGetPitMode(&vtxMsp, &pitmode);
-
-    mspFrame[0] = VTXDEV_MSP,
-    mspFrame[1] = vtxSettingsConfig()->band;
-    mspFrame[2] = vtxSettingsConfig()->channel;
-    mspFrame[3] = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power; // index based
-    mspFrame[4] = pitmode;
-    mspFrame[5] = 0; // Freq_L 
-    mspFrame[6] = 0; // Freq_H
-    mspFrame[7] = (mspVtxStatus == MSP_VTX_STATUS_READY) ? 1 : 0;
-    mspFrame[8] = isLowPowerDisarmed();
-    mspFrame[9] = 0; // Pitmode freq Low
-    mspFrame[10] = 0; // pitmod freq High
+    // Send an MSP_VTX_V2 frame to the VTX
+    mspFrame[0]  = vtxMspGetDeviceType(vtxDevice);
+    mspFrame[1]  = vtxState.request.band;
+    mspFrame[2]  = vtxState.request.channel;
+    mspFrame[3]  = vtxState.request.powerIndex;
+    mspFrame[4]  = vtxState.request.pitMode;
+    mspFrame[5]  = 0; // Freq_L 
+    mspFrame[6]  = 0; // Freq_H
+    mspFrame[7]  = vtxMspIsReady(vtxDevice);
+    mspFrame[8]  = vtxSettingsConfig()->lowPowerDisarm;
+    mspFrame[9]  = 0; // pitmode freq Low
+    mspFrame[10] = 0; // pitmode freq High
     mspFrame[11] = 0; // 1 if using vtx table
-    mspFrame[12] = 0; // vtx table bands or 0
-    mspFrame[13] = 0; // vtx table channels or 0
-    mspFrame[14] = 0; // vtx table power levels or 0
+    mspFrame[12] = vtxMsp.capability.bandCount; // bands or 0
+    mspFrame[13] = vtxMsp.capability.channelCount; // channels or 0
+    mspFrame[14] = vtxMsp.capability.powerCount; // power levels or 0
 } 
 
 static void mspCrsfPush(const uint8_t mspCommand, const uint8_t *mspFrame, const uint8_t mspFrameSize)
 {
-
-    LOG_DEBUG(VTX, "msp CrsfPush\r\n");
 #ifndef USE_TELEMETRY_CRSF
     UNUSED(mspCommand);
     UNUSED(mspFrame);
@@ -201,285 +146,144 @@ static void mspCrsfPush(const uint8_t mspCommand, const uint8_t *mspFrame, const
     crc8_dvb_s2_sbuf_append(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length
     sbufSwitchToReader(dst, crsfFrame);
 
-    crsfRxSendTelemetryData(); //give the FC a chance to send outstanding telemetry
+    crsfRxSendTelemetryData(); // give the FC a chance to send outstanding telemetry
     crsfRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
     crsfRxSendTelemetryData();
 #endif
 }
 
-static uint16_t packetCounter = 0;
-
-static bool isVtxConfigChanged(void)
-{
-    if(mspVtxStatus == MSP_VTX_STATUS_READY) {
-        if (mspVtxConfigChanged == true)
-                return true;
-
-        if (isLowPowerDisarmed() != prevLowPowerDisarmedState) {
-                LOG_DEBUG(VTX, "msp vtx config changed (lower power disarm 2)\r\n");
-                mspVtxConfigChanged = true;
-                prevLowPowerDisarmedState = isLowPowerDisarmed();
-        }
-
-        if (mspConfPowerIndex != vtxSettingsConfig()->power) {
-                LOG_DEBUG(VTX, "msp vtx config changed (power 2)\r\n");
-                mspVtxConfigChanged = true;
-        }
-
-        if (mspConfBand != vtxSettingsConfig()->band || mspConfChannel != vtxSettingsConfig()->channel) {
-                LOG_DEBUG(VTX, "msp vtx config changed (band and channel 2)\r\n");
-                mspVtxConfigChanged = true;
-        }
-
-        return mspVtxConfigChanged;
-    }
-
-    return false;
-}
-
 static void vtxMspProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
 {
     UNUSED(vtxDevice);
 
-    const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP_OSD);
-    uint8_t frame[15];
+    uint8_t mspFrame[15];
 
-    switch (mspVtxStatus) {
-    case MSP_VTX_STATUS_OFFLINE:
-        LOG_DEBUG(VTX, "msp MspProcess: OFFLINE\r\n");
-        // wait for MSP communication from the VTX
-#ifdef USE_CMS
-        //mspCmsUpdateStatusString();
-#endif
-        break;
-    case MSP_VTX_STATUS_READY:
-        LOG_DEBUG(VTX, "msp MspProcess: READY\r\n");
-        // send an update if stuff has changed with 200ms period
-        if ((isVtxConfigChanged()) && cmp32(currentTimeUs, mspVtxLastTimeUs) >= MSP_VTX_REQUEST_PERIOD_US) {
-
-            LOG_DEBUG(VTX, "msp-vtx: vtxInfo Changed\r\n");
-            prepareMspFrame(frame);
-
-            if (isCrsfPortConfig(portConfig)) {
-                mspCrsfPush(MSP_VTX_CONFIG, frame, sizeof(frame));
-            } else {
-                mspPort_t *port = getMspOsdPort();
-                if(port != NULL && port->port) {
-                    LOG_DEBUG(VTX, "msp-vtx: mspSerialPushPort\r\n");
-                    int sent = mspSerialPushPort(MSP_VTX_CONFIG, frame, sizeof(frame), port, MSP_V2_NATIVE);
-                    if (sent <= 0) {
-                        break;
-                    }
-                }
+    mspPort_t *mspPort = getMspOsdPort();
+    unsigned lastActivity = (currentTimeUs/1000) - mspPort->lastActivityMs;    
+    if (lastActivity > VTX_MSP_TIMEOUT) {
+        if (vtxState.timeouts++ > 3) {
+            if (vtxState.ready) {
+                vtxState.ready = false;
             }
-            packetCounter++;
-            mspVtxLastTimeUs = currentTimeUs;
-            mspVtxConfigChanged = false;
-
-#ifdef USE_CMS
-            //mspCmsUpdateStatusString();
-#endif
         }
-        break;
-    default:
-        mspVtxStatus = MSP_VTX_STATUS_OFFLINE;
-        break;
+    } else { // active
+        if (!vtxState.ready) {
+            vtxState.ready = true;
+        }
     }
 
-#if 0
-    DEBUG_SET(DEBUG_VTX_MSP, 0, packetCounter);
-    DEBUG_SET(DEBUG_VTX_MSP, 1, isCrsfPortConfig(portConfig));
-    DEBUG_SET(DEBUG_VTX_MSP, 2, isLowPowerDisarmed());
-#if defined(USE_MSP_OVER_TELEMETRY)
-    DEBUG_SET(DEBUG_VTX_MSP, 3, isCrsfPortConfig(portConfig) ? getMspTelemetryDescriptor() : getMspSerialPortDescriptor(mspVtxPortIdentifier));
-#else
-    DEBUG_SET(DEBUG_VTX_MSP, 3, getMspSerialPortDescriptor(mspVtxPortIdentifier));
-#endif
-#endif
+    if (vtxState.ready) {
+        if (vtxState.updateReqMask != VTX_UPDATE_REQ_NONE) {
+            prepareMspFrame(vtxDevice, mspFrame);
+            if (vtxState.crsfTelemetryEnabled) { // keep ELRS LUA up to date ?
+                mspCrsfPush(MSP_VTX_CONFIG, mspFrame, sizeof(mspFrame));
+            }
+
+            int sent = mspSerialPushPort(MSP_VTX_CONFIG, mspFrame, sizeof(mspFrame), mspPort, MSP_V2_NATIVE);
+            if (sent > 0) {
+                vtxState.updateReqMask = VTX_UPDATE_REQ_NONE;
+            }
+        }   
+    }
 }
 
 static vtxDevType_e vtxMspGetDeviceType(const vtxDevice_t *vtxDevice)
 {
-    //LOG_DEBUG(VTX, "msp GetDeviceType\r\n");
     UNUSED(vtxDevice);
     return VTXDEV_MSP;
 }
 
 static bool vtxMspIsReady(const vtxDevice_t *vtxDevice)
 {
-    //LOG_DEBUG(VTX, "msp vtxIsReady: %s\r\n", (vtxDevice != NULL && mspVtxStatus == MSP_VTX_STATUS_READY) ? "Y": "N");
-    return vtxDevice != NULL && mspVtxStatus == MSP_VTX_STATUS_READY;
+    return vtxDevice != NULL && mspVtxPortIdentifier >=0 && vtxState.ready;
 }
 
 static void vtxMspSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
 {
-    LOG_DEBUG(VTX, "msp SetBandAndChannel\r\n");
     UNUSED(vtxDevice);
-    if (band != mspConfBand || channel != mspConfChannel) {
-        LOG_DEBUG(VTX, "msp vtx config changed (band and channel)\r\n");
-        mspVtxConfigChanged = true;
+
+    if (band < VTX_MSP_MIN_BAND || band > VTX_MSP_MAX_BAND || channel < VTX_MSP_MIN_CHANNEL || channel > VTX_MSP_MAX_CHANNEL) {
+        return;
     }
-    mspConfBand = band;
-    mspConfChannel = channel;
+
+    vtxState.request.band = band;
+    vtxState.request.channel = channel;
+    vtxState.request.freq = vtx58_Bandchan2Freq(band, channel);
+    vtxState.updateReqMask |= VTX_UPDATE_REQ_FREQUENCY;
 }
 
 static void vtxMspSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
 {
-    LOG_DEBUG(VTX, "msp SetPowerByIndex\r\n");
     UNUSED(vtxDevice);
 
-    if (index > 0 && (index < VTX_MSP_TABLE_MAX_POWER_LEVELS))
-    {
-        if (index != mspConfPowerIndex)
-        {
-            LOG_DEBUG(VTX, "msp vtx config changed (power by index)\r\n");
-            mspVtxConfigChanged = true;
-        }
-        mspConfPowerIndex = index;
-    }
+    vtxState.request.power = vtxMspPowerTable[index];
+    vtxState.request.powerIndex = index;
+    vtxState.updateReqMask |= VTX_UPDATE_REQ_POWER;
 }
 
-static void vtxMspSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
-{
-    LOG_DEBUG(VTX, "msp SetPitMode\r\n");
-    UNUSED(vtxDevice);
-    if (onoff != mspConfPitMode) {
-        LOG_DEBUG(VTX, "msp vtx config changed (pitmode)\r\n");
-        mspVtxConfigChanged = true;
-    }
-    mspConfPitMode = onoff;
-}
-
-#if 0
-static void vtxMspSetFreq(vtxDevice_t *vtxDevice, uint16_t freq)
+static void vtxMspSetPitMode(vtxDevice_t *vtxDevice, uint8_t onOff)
 {
     UNUSED(vtxDevice);
-    if (freq != mspConfFreq) {
-        mspVtxConfigChanged = true;
-    }
-    mspConfFreq = freq;
+
+    vtxState.request.pitMode = onOff;
+    vtxState.updateReqMask |= VTX_UPDATE_REQ_PIT_MODE;
 }
-#endif
-
-
-
 
 static bool vtxMspGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
 {
-    if (!vtxMspIsReady(vtxDevice)) {
-        return false;
-    }
-
-    *pBand = vtxSettingsConfig()->band;
-    *pChannel = vtxSettingsConfig()->channel;
-
-    //LOG_DEBUG(VTX, "msp GetBandAndChannel: %02x:%02x\r\n", vtxSettingsConfig()->band, vtxSettingsConfig()->channel);
+    UNUSED(vtxDevice);
 
+    *pBand = vtxState.request.band; 
+    *pChannel = vtxState.request.channel;
     return true;
 }
 
 static bool vtxMspGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
 {
-    if (!vtxMspIsReady(vtxDevice)) {
-        return false;
-    }
+    UNUSED(vtxDevice);
 
-    uint8_t power = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power;
-    // Special case, power not set
-    if (power > VTX_MSP_TABLE_MAX_POWER_LEVELS) {
-        *pIndex = 0;
-        //LOG_DEBUG(VTX, "msp GetPowerIndex: %u\r\n", *pIndex);
-        return true;
-    }
+    *pIndex = vtxState.request.powerIndex;
+    return true;
+}
 
-    *pIndex = power;
+static bool vtxMspGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff)
+{
+    UNUSED(vtxDevice);
 
-    //LOG_DEBUG(VTX, "msp GetPowerIndex: %u\r\n", *pIndex);
+    *pOnOff = vtxState.request.pitMode;
     return true;
 }
 
 static bool vtxMspGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
 {
-    LOG_DEBUG(VTX, "msp GetFreq\r\n");
-    if (!vtxMspIsReady(vtxDevice)) {
-        return false;
-    }
+    UNUSED(vtxDevice);
 
-    *pFreq = 5800;
+    *pFreq = vtxState.request.freq;
     return true;
 }
 
 static bool vtxMspGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw)
 {
-    LOG_DEBUG(VTX, "msp GetPower\r\n");
-    uint8_t powerIndex;
+    UNUSED(vtxDevice);
 
-    if (!vtxMspGetPowerIndex(vtxDevice, &powerIndex)) {
-        return false;
-    }
-
-
-    *pIndex = powerIndex;
-    *pPowerMw = *pIndex;
+    *pIndex = vtxState.request.powerIndex;
+    *pPowerMw = vtxState.request.power;
     return true;
 }
 
-static bool vtxMspGetOsdInfo(const  vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo)
+static bool vtxMspGetOsdInfo(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t *pOsdInfo)
 {
-    LOG_DEBUG(VTX, "msp GetOsdInfo\r\n");
-    uint8_t powerIndex;
-    uint16_t powerMw;
-    uint16_t freq;
-    uint8_t band, channel;
+    UNUSED(vtxDevice);
 
-    if (!vtxMspGetBandAndChannel(vtxDevice, &band, &channel)) {
-        return false;
-    }
-
-    if (!vtxMspGetFreq(vtxDevice, &freq)) {
-        return false;
-    }
-
-    if (!vtxMspGetPower(vtxDevice, &powerIndex, &powerMw)) {
-        return false;
-    }
-
-    pOsdInfo->band = band;
-    pOsdInfo->channel = channel;
-    pOsdInfo->frequency = freq;
-    pOsdInfo->powerIndex = powerIndex;
-    pOsdInfo->powerMilliwatt = powerMw;
-    pOsdInfo->bandLetter = vtx58BandNames[band][0];
-    pOsdInfo->bandName = vtx58BandNames[band];
-    pOsdInfo->channelName = vtx58ChannelNames[channel];
-    pOsdInfo->powerIndexLetter = '0' + powerIndex;
-    return true;
-}
-
-
-bool vtxMspInit(void)
-{
-    LOG_DEBUG(VTX, "msp %s\r\n", __FUNCTION__);
-    // don't bother setting up this device if we don't have MSP vtx enabled
-    // Port is shared with MSP_OSD
-    const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP_OSD);
-    if (!portConfig) {
-        return false;
-    }
-
-    mspVtxPortIdentifier = portConfig->identifier;
-  
-    // XXX Effect of USE_VTX_COMMON should be reviewed, as following call to vtxInit will do nothing if vtxCommonSetDevice is not called.
-#if defined(USE_VTX_COMMON)
-    vtxCommonSetDevice(&vtxMsp);
-#endif
-
-    mspConfBand = vtxSettingsConfig()->band;
-    mspConfChannel = vtxSettingsConfig()->channel;
-    mspConfPowerIndex = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power; // index based
-    vtxCommonGetPitMode(&vtxMsp, &mspConfPitMode);
-
-    vtxInit();
+    pOsdInfo->band = vtxState.request.band;
+    pOsdInfo->channel = vtxState.request.channel;
+    pOsdInfo->frequency = vtxState.request.freq;
+    pOsdInfo->powerIndex = vtxState.request.powerIndex;
+    pOsdInfo->powerMilliwatt = vtxState.request.power;
+    pOsdInfo->bandName = vtxMspBandNames[vtxState.request.band];
+    pOsdInfo->bandLetter = vtxMspBandLetters[vtxState.request.band];
+    pOsdInfo->channelName = vtxMspChannelNames[vtxState.request.channel];
+    pOsdInfo->powerIndexLetter = '0' + vtxState.request.powerIndex;
 
     return true;
 }
@@ -491,211 +295,48 @@ static const vtxVTable_t mspVTable = {
     .setBandAndChannel = vtxMspSetBandAndChannel,
     .setPowerByIndex = vtxMspSetPowerByIndex,
     .setPitMode = vtxMspSetPitMode,
-    //.setFrequency = vtxMspSetFreq,
     .getBandAndChannel = vtxMspGetBandAndChannel,
     .getPowerIndex = vtxMspGetPowerIndex,
+    .getPitMode = vtxMspGetPitMode,
     .getFrequency = vtxMspGetFreq,
-    //.getStatus = vtxMspGetStatus,
     .getPower = vtxMspGetPower,
-    //.serializeCustomDeviceStatus = NULL,
-    .getOsdInfo = vtxMspGetOsdInfo,
+    .getOsdInfo = vtxMspGetOsdInfo
 };
 
-static mspResult_e mspVtxProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
+static vtxDevice_t vtxMsp = {
+    .vTable = &mspVTable,
+    .capability.bandCount = VTX_MSP_MAX_BAND,
+    .capability.channelCount = VTX_MSP_MAX_CHANNEL,
+    .capability.powerCount = VTX_MSP_POWER_COUNT,
+    .capability.bandNames = (char **)vtxMspBandNames,
+    .capability.channelNames = (char **)vtxMspChannelNames,
+    .capability.powerNames = (char **)vtxMspPowerNames
+};
+
+bool vtxMspInit(void)
 {
-    //LOG_DEBUG(VTX, "msp VTX_MSP_PROCESS\r\n");
-    UNUSED(mspPostProcessFn);
+    const serialPortConfig_t *portConfig;
     
-    sbuf_t *dst = &reply->buf;
-    sbuf_t *src = &cmd->buf;
-
-    const unsigned int dataSize = sbufBytesRemaining(src);
-    UNUSED(dst);
-    UNUSED(src);
-
-    // Start initializing the reply message
-    reply->cmd = cmd->cmd;
-    reply->result = MSP_RESULT_ACK;
-
-    vtxDevice_t *vtxDevice = vtxCommonDevice();
-    if (!vtxDevice || vtxCommonGetDeviceType(vtxDevice) != VTXDEV_MSP) {
-        LOG_DEBUG(VTX, "msp wrong vtx\r\n");
-        return MSP_RESULT_ERROR;
+    // Shares MSP_OSD port
+    portConfig = findSerialPortConfig(FUNCTION_VTX_MSP);
+    if (!portConfig) {
+        return false;
     }
 
-    switch (cmd->cmd)
-    {
-        case MSP_VTXTABLE_BAND:
-        {
-            LOG_DEBUG(VTX, "msp MSP_VTXTABLE_BAND\r\n");
-            uint8_t deviceType = vtxCommonGetDeviceType(vtxDevice);
-            if (deviceType == VTXDEV_MSP)
-            {
-                /*
-                char bandName[MSP_VTX_TABLE_BAND_NAME_LENGTH + 1];
-                memset(bandName, 0, MSP_VTX_TABLE_BAND_NAME_LENGTH + 1);
-                uint16_t frequencies[MSP_VTX_TABLE_MAX_CHANNELS];
-                const uint8_t band = sbufReadU8(src);
-                const uint8_t bandNameLength = sbufReadU8(src);
-                for (int i = 0; i < bandNameLength; i++) {
-                    const char nameChar = sbufReadU8(src);
-                    if (i < MSP_VTX_TABLE_BAND_NAME_LENGTH) {
-                        bandName[i] = toupper(nameChar);
-                    }
-                }
-                const char bandLetter = toupper(sbufReadU8(src));
-                const bool isFactoryBand = (bool)sbufReadU8(src);
-                const uint8_t channelCount = sbufReadU8(src);
-                for (int i = 0; i < channelCount; i++)
-                {
-                    const uint16_t frequency = sbufReadU16(src);
-                    if (i < vtxTableConfig()->channels)
-                    {
-                        frequencies[i] = frequency;
-                    }
-                }
-                */
+    portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
 
-                setMspVtxDeviceStatusReady(1);
-            }
-            break;
-        }
-        case MSP_VTXTABLE_POWERLEVEL:
-        {
-            LOG_DEBUG(VTX, "msp MSP_VTXTABLE_POWERLEVEL\r\n");
+    vtxState.ready = false;
+    vtxState.timeouts = 0;
+    vtxState.updateReqMask = VTX_UPDATE_REQ_NONE;
+    vtxState.crsfTelemetryEnabled = crsfRxIsActive();
+    vtxState.request.band =  vtxSettingsConfig()->band;
+    vtxState.request.channel = vtxSettingsConfig()->channel;
+    vtxState.request.freq = vtx58_Bandchan2Freq(vtxState.request.band, vtxState.request.channel);
+    vtxState.request.power = vtxSettingsConfig()->power;
+    vtxState.request.pitMode = 0;
+    vtxCommonSetDevice(&vtxMsp);
 
-                /*
-                char powerLevelLabel[VTX_TABLE_POWER_LABEL_LENGTH + 1];
-                memset(powerLevelLabel, 0, VTX_TABLE_POWER_LABEL_LENGTH + 1);
-                const uint8_t powerLevel = sbufReadU8(src);
-                const uint16_t powerValue = sbufReadU16(src);
-                const uint8_t powerLevelLabelLength = sbufReadU8(src);
-                for (int i = 0; i < powerLevelLabelLength; i++)
-                {
-                    const char labelChar = sbufReadU8(src);
-                    if (i < VTX_TABLE_POWER_LABEL_LENGTH)
-                    {
-                        powerLevelLabel[i] = toupper(labelChar);
-                    }
-                }
-                */
-                setMspVtxDeviceStatusReady(1);
-        }
-        break;
-        case MSP_VTX_CONFIG:
-        {
-            LOG_DEBUG(VTX, "msp MSP_VTX_CONFIG received\r\n");
-            LOG_DEBUG(VTX, "msp MSP_VTX_CONFIG VTXDEV_MSP\r\n");
-            uint8_t pitmode = 0;
-            vtxCommonGetPitMode(vtxDevice, &pitmode);
-
-            // VTXDEV_MSP,
-            sbufWriteU8(dst, VTXDEV_MSP);
-            // band;
-            sbufWriteU8(dst, vtxSettingsConfig()->band);
-            // channel;
-            sbufWriteU8(dst, vtxSettingsConfig()->channel);
-            // power; // index based
-            sbufWriteU8(dst, vtxSettingsConfig()->power);
-            // pit mode;
-            // Freq_L
-            sbufWriteU8(dst, 0);
-            // Freq_H
-            sbufWriteU8(dst, 0);
-            // vtx status
-            sbufWriteU8(dst, 1);
-            // lowPowerDisarm
-
-            sbufWriteU8(dst, vtxSettingsConfig()->lowPowerDisarm);
-            // Pitmode freq Low
-            sbufWriteU8(dst, 0);
-            // pitmod freq High
-            sbufWriteU8(dst, 0);
-            // 1 if using vtx table
-            sbufWriteU8(dst, 0);
-            // vtx table bands or 0
-            sbufWriteU8(dst, 0);
-            // vtx table channels or 0
-            sbufWriteU8(dst, 0);
-
-            setMspVtxDeviceStatusReady(1);
-            break;
-        }
-        case MSP_SET_VTX_CONFIG:
-            LOG_DEBUG(VTX, "msp MSP_SET_VTX_CONFIG\r\n");
-            if (dataSize == 15)
-            {
-                if (vtxCommonGetDeviceType(vtxDevice) != VTXDEV_UNKNOWN)
-                {
-                    for (int i = 0; i < 15; ++i)
-                    {
-                        uint8_t data = sbufReadU8(src);
-                        switch (i)
-                        {
-                        case 1:
-                            vtxSettingsConfigMutable()->band = data;
-                            break;
-                        case 2:
-                            vtxSettingsConfigMutable()->channel = data;
-                            break;
-                        case 3:
-                            vtxSettingsConfigMutable()->power = data;
-                            break;
-                        case 4:
-                            vtxCommonSetPitMode(vtxDevice, data);
-                            break;
-                        case 7:
-                            // vtx ready
-                            break;
-                        case 8:
-                            vtxSettingsConfigMutable()->lowPowerDisarm = data;
-                            break;
-                        }
-                    }
-                }
-
-                setMspVtxDeviceStatusReady(1);
-                break;
-            }
-            LOG_DEBUG(VTX, "msp MSP_RESULT_ERROR\r\n");
-            return MSP_RESULT_ERROR;
-        default:
-            // debug[1]++;
-            // debug[2] = cmd->cmd;
-            if(cmd->cmd != MSP_STATUS && cmd->cmd != MSP_STATUS_EX && cmd->cmd != MSP_RC) {
-                LOG_DEBUG(VTX, "msp default: %02x\r\n", cmd->cmd);
-            }
-            reply->result = MSP_RESULT_ERROR;
-            break;
-    }
-
-    // Process DONT_REPLY flag
-    if (cmd->flags & MSP_FLAG_DONT_REPLY)
-    {
-        reply->result = MSP_RESULT_NO_REPLY;
-    }
-
-    return reply->result;
+    return true;
 }
 
-void mspVtxSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn)
-{
-    UNUSED(mspProcessCommandFn);
-    // Check if VTX is ready
-    /*
-    if (mspVtxStatus != MSP_VTX_STATUS_READY) {
-        LOG_DEBUG(VTX, "msp VTX NOT READY, skipping\r\n");
-        return;
-    }
-    */
-
-    mspPort_t *port = getMspOsdPort();
-
-    if(port) {
-        mspSerialProcessOnePort(port, MSP_SKIP_NON_MSP_DATA, mspVtxProcessMspCommand);
-    }
-
-}
-
-
 #endif

src/main/io/vtx_msp.h

@@ -17,37 +17,17 @@
  *
  * If not, see <http://www.gnu.org/licenses/>.
  */
+/* Created by geoffsim */
 
-#pragma once
+#ifndef _VTX_MSP_H
+#define _VTX_MSP_H
 
-#include <stdint.h>
-
-#include "build/build_config.h"
-
-#include "msp/msp_protocol.h"
-#include "msp/msp_serial.h"
-
-typedef enum {
-    // Offline - device hasn't responded yet
-    MSP_VTX_STATUS_OFFLINE = 0,
-    MSP_VTX_STATUS_READY,
-} mspVtxStatus_e;
-
-typedef struct mspPowerTable_s {
-    int mW;         // rfpower
-    int16_t dbi;    // valueV1
-} mspPowerTable_t;
-
-#define VTX_MSP_TABLE_MAX_BANDS             5 // default freq table has 5 bands
-#define VTX_MSP_TABLE_MAX_CHANNELS          8 // and eight channels
-#define VTX_MSP_TABLE_MAX_POWER_LEVELS      5 //max of VTX_TRAMP_POWER_COUNT, VTX_SMARTAUDIO_POWER_COUNT and VTX_RTC6705_POWER_COUNT
-#define VTX_MSP_TABLE_CHANNEL_NAME_LENGTH   1
-#define VTX_MSP_TABLE_BAND_NAME_LENGTH      8
-#define VTX_MSP_TABLE_POWER_LABEL_LENGTH    3
 
+#define VTX_MSP_TIMEOUT         250 // ms
+#define VTX_MSP_BAND_COUNT      5
+#define VTX_MSP_CHANNEL_COUNT   8
+#define VTX_MSP_POWER_COUNT     4
 
 bool vtxMspInit(void);
-void setMspVtxDeviceStatusReady(const int descriptor);
-void prepareMspFrame(uint8_t *mspFrame);
 
-void mspVtxSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn);
+#endif // _VTX_MSP_H

src/main/msc/usbd_storage_sd_spi.c

@@ -213,8 +213,9 @@ static int8_t STORAGE_Read (uint8_t lun,
 	UNUSED(lun);
 	LED1_ON;
 	for (int i = 0; i < blk_len; i++) {
-    while (sdcard_readBlock(blk_addr + i, buf + (512 * i), NULL, 0) == 0);
-    while (sdcard_poll() == 0);
+		while (sdcard_readBlock(blk_addr + i, buf + (512 * i), NULL, 0) == 0)
+			sdcard_poll();
+		while (sdcard_poll() == 0);
 	}
 	LED1_OFF;
 	return 0;

src/main/msc/usbd_storage_sdio.c

@@ -1,289 +0,0 @@
-/**
-  ******************************************************************************
-  * @file    usbd_storage_template.c
-  * @author  MCD Application Team
-  * @version V1.2.0
-  * @date    09-November-2015
-  * @brief   Memory management layer
-  ******************************************************************************
-  * @attention
-  *
-  * <h2><center>&copy; COPYRIGHT 2015 STMicroelectronics</center></h2>
-  *
-  * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
-  * You may not use this file except in compliance with the License.
-  * You may obtain a copy of the License at:
-  *
-  *        http://www.st.com/software_license_agreement_liberty_v2
-  *
-  * Unless required by applicable law or agreed to in writing, software
-  * distributed under the License is distributed on an "AS IS" BASIS,
-  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-  * See the License for the specific language governing permissions and
-  * limitations under the License.
-  *
-  ******************************************************************************
-  */
-
-
-/* Includes ------------------------------------------------------------------*/
-#include <stdio.h>
-#include <stdbool.h>
-
-#include "platform.h"
-
-#include "drivers/sdmmc_sdio.h"
-#include "drivers/light_led.h"
-#include "drivers/io.h"
-#include "common/utils.h"
-
-#ifdef USE_HAL_DRIVER
-#include "usbd_msc.h"
-#else
-#include "usbd_msc_mem.h"
-#include "usbd_msc_core.h"
-#endif
-
-#include "usbd_storage.h"
-
-/* Private typedef -----------------------------------------------------------*/
-/* Private define ------------------------------------------------------------*/
-/* Private macro -------------------------------------------------------------*/
-/* Private variables ---------------------------------------------------------*/
-/* Private function prototypes -----------------------------------------------*/
-/* Extern function prototypes ------------------------------------------------*/
-/* Private functions ---------------------------------------------------------*/
-
-/* USB NVIC Priority has to be lower than both DMA and SDIO priority,
- * otherwise SDIO won't be able to preempt USB.
- */
-
-#define STORAGE_LUN_NBR                  1
-#define STORAGE_BLK_NBR                  0x10000
-#define STORAGE_BLK_SIZ                  0x200
-
-static int8_t STORAGE_Init (uint8_t lun);
-
-#ifdef USE_HAL_DRIVER
-static int8_t STORAGE_GetCapacity (uint8_t lun,
-                           uint32_t *block_num,
-                           uint16_t *block_size);
-#else
-static int8_t STORAGE_GetCapacity (uint8_t lun,
-                           uint32_t *block_num,
-                           uint32_t *block_size);
-#endif
-
-static int8_t  STORAGE_IsReady (uint8_t lun);
-
-static int8_t  STORAGE_IsWriteProtected (uint8_t lun);
-
-static int8_t STORAGE_Read (uint8_t lun,
-                        uint8_t *buf,
-                        uint32_t blk_addr,
-                        uint16_t blk_len);
-
-static int8_t STORAGE_Write (uint8_t lun,
-                        uint8_t *buf,
-                        uint32_t blk_addr,
-                        uint16_t blk_len);
-
-static int8_t STORAGE_GetMaxLun (void);
-
-/* USB Mass storage Standard Inquiry Data */
-static uint8_t  STORAGE_Inquirydata[] = {//36
-
-  /* LUN 0 */
-  0x00,
-  0x80,
-  0x02,
-  0x02,
-#ifdef USE_HAL_DRIVER
-  (STANDARD_INQUIRY_DATA_LEN - 5),
-#else
-  (USBD_STD_INQUIRY_LENGTH - 5),
-#endif
-  0x00,
-  0x00,
-  0x00,
-  'S', 'T', 'M', ' ', ' ', ' ', ' ', ' ', /* Manufacturer : 8 bytes */
-  'P', 'r', 'o', 'd', 'u', 't', ' ', ' ', /* Product      : 16 Bytes */
-  ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ',
-  '0', '.', '0' ,'1',                     /* Version      : 4 Bytes */
-};
-
-#ifdef USE_HAL_DRIVER
-USBD_StorageTypeDef USBD_MSC_MICRO_SDIO_fops =
-{
-  STORAGE_Init,
-  STORAGE_GetCapacity,
-  STORAGE_IsReady,
-  STORAGE_IsWriteProtected,
-  STORAGE_Read,
-  STORAGE_Write,
-  STORAGE_GetMaxLun,
-  (int8_t*)STORAGE_Inquirydata,
-};
-#else
-USBD_STORAGE_cb_TypeDef USBD_MSC_MICRO_SDIO_fops =
-{
-  STORAGE_Init,
-  STORAGE_GetCapacity,
-  STORAGE_IsReady,
-  STORAGE_IsWriteProtected,
-  STORAGE_Read,
-  STORAGE_Write,
-  STORAGE_GetMaxLun,
-  (int8_t*)STORAGE_Inquirydata,
-};
-#endif
-
-/*******************************************************************************
-* Function Name  : Read_Memory
-* Description    : Handle the Read operation from the microSD card.
-* Input          : None.
-* Output         : None.
-* Return         : None.
-*******************************************************************************/
-static int8_t STORAGE_Init (uint8_t lun)
-{
-	//Initialize SD_DET
-#ifdef SDCARD_DETECT_PIN
-	const IO_t sd_det = IOGetByTag(IO_TAG(SDCARD_DETECT_PIN));
-	IOInit(sd_det, OWNER_SDCARD_DETECT, 0);
-	IOConfigGPIO(sd_det, IOCFG_IPU);
-#endif
-
-	UNUSED(lun);
-	LED0_OFF;
-#if defined(STM32H7) // H7 uses IDMA
-	SD_Initialize_LL(0);
-#else
-	SD_Initialize_LL(SDIO_DMA);
-#endif
-	if (SD_Init() != 0) return 1;
-	LED0_ON;
-	return 0;
-}
-
-/*******************************************************************************
-* Function Name  : Read_Memory
-* Description    : Handle the Read operation from the STORAGE card.
-* Input          : None.
-* Output         : None.
-* Return         : None.
-*******************************************************************************/
-#ifdef USE_HAL_DRIVER
-static int8_t STORAGE_GetCapacity (uint8_t lun, uint32_t *block_num, uint16_t *block_size)
-#else
-static int8_t STORAGE_GetCapacity (uint8_t lun, uint32_t *block_num, uint32_t *block_size)
-#endif
-{
-	UNUSED(lun);
-	if (SD_IsDetected() == 0) {
-		return -1;
-	}
-	SD_GetCardInfo();
-
-	*block_num = SD_CardInfo.CardCapacity;
-	*block_size = 512;
-	return (0);
-}
-
-/*******************************************************************************
-* Function Name  : Read_Memory
-* Description    : Handle the Read operation from the STORAGE card.
-* Input          : None.
-* Output         : None.
-* Return         : None.
-*******************************************************************************/
-static int8_t  STORAGE_IsReady (uint8_t lun)
-{
-	UNUSED(lun);
-	int8_t ret = -1;
-	if (SD_GetState() == true && SD_IsDetected() == SD_PRESENT) {
-        ret = 0;
-	}
-	return ret;
-}
-
-/*******************************************************************************
-* Function Name  : Read_Memory
-* Description    : Handle the Read operation from the STORAGE card.
-* Input          : None.
-* Output         : None.
-* Return         : None.
-*******************************************************************************/
-static int8_t  STORAGE_IsWriteProtected (uint8_t lun)
-{
-  UNUSED(lun);
-  return  0;
-}
-
-/*******************************************************************************
-* Function Name  : Read_Memory
-* Description    : Handle the Read operation from the STORAGE card.
-* Input          : None.
-* Output         : None.
-* Return         : None.
-*******************************************************************************/
-static int8_t STORAGE_Read (uint8_t lun,
-                 uint8_t *buf,
-                 uint32_t blk_addr,
-                 uint16_t blk_len)
-{
-	UNUSED(lun);
-	if (SD_IsDetected() == 0) {
-		return -1;
-	}
-	LED1_ON;
-	//buf should be 32bit aligned, but usually is so we don't do byte alignment
-	if (SD_ReadBlocks_DMA(blk_addr, (uint32_t*) buf, 512, blk_len) == 0) {
-		while (SD_CheckRead());
-		while(SD_GetState() == false);
-		LED1_OFF;
-		return 0;
-	}
-	LED1_OFF;
-	return -1;
-}
-/*******************************************************************************
-* Function Name  : Write_Memory
-* Description    : Handle the Write operation to the STORAGE card.
-* Input          : None.
-* Output         : None.
-* Return         : None.
-*******************************************************************************/
-static int8_t STORAGE_Write (uint8_t lun,
-                  uint8_t *buf,
-                  uint32_t blk_addr,
-                  uint16_t blk_len)
-{
-	UNUSED(lun);
-	if (SD_IsDetected() == 0) {
-		return -1;
-	}
-	LED1_ON;
-	//buf should be 32bit aligned, but usually is so we don't do byte alignment
-	if (SD_WriteBlocks_DMA(blk_addr, (uint32_t*) buf, 512, blk_len) == 0) {
-		while (SD_CheckWrite());
-		while(SD_GetState() == false);
-		LED1_OFF;
-		return 0;
-	}
-	LED1_OFF;
-	return -1;
-}
-/*******************************************************************************
-* Function Name  : Write_Memory
-* Description    : Handle the Write operation to the STORAGE card.
-* Input          : None.
-* Output         : None.
-* Return         : None.
-*******************************************************************************/
-static int8_t STORAGE_GetMaxLun (void)
-{
-  return (STORAGE_LUN_NBR - 1);
-}
-
-/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

src/main/msp/msp.h

@@ -46,6 +46,7 @@ typedef struct mspPacket_s {
 
 typedef enum {
     MSP_FLAG_DONT_REPLY           = (1 << 0),
+    MSP_FLAG_ILMI                 = (1 << 1), // "In-Line Message identifier"
 } mspFlags_e;
 
 struct serialPort_s;

src/main/msp/msp_protocol_v2_common.h

@@ -15,22 +15,25 @@
  * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#define MSP2_COMMON_TZ                  0x1001  //out message       Gets the TZ offset for the local time (returns: minutes(i16))
-#define MSP2_COMMON_SET_TZ              0x1002  //in message        Sets the TZ offset for the local time (args: minutes(i16))
-#define MSP2_COMMON_SETTING             0x1003  //in/out message    Returns the value for a setting
-#define MSP2_COMMON_SET_SETTING         0x1004  //in message        Sets the value for a setting
+#define MSP2_COMMON_TZ                      0x1001  //out message       Gets the TZ offset for the local time (returns: minutes(i16))
+#define MSP2_COMMON_SET_TZ                  0x1002  //in message        Sets the TZ offset for the local time (args: minutes(i16))
+#define MSP2_COMMON_SETTING                 0x1003  //in/out message    Returns the value for a setting
+#define MSP2_COMMON_SET_SETTING             0x1004  //in message        Sets the value for a setting
     
-#define MSP2_COMMON_MOTOR_MIXER         0x1005
-#define MSP2_COMMON_SET_MOTOR_MIXER     0x1006
+#define MSP2_COMMON_MOTOR_MIXER             0x1005
+#define MSP2_COMMON_SET_MOTOR_MIXER         0x1006
     
-#define MSP2_COMMON_SETTING_INFO        0x1007  //in/out message    Returns info about a setting (PG, type, flags, min/max, etc..).
-#define MSP2_COMMON_PG_LIST             0x1008  //in/out message    Returns a list of the PG ids used by the settings
+#define MSP2_COMMON_SETTING_INFO            0x1007  //in/out message    Returns info about a setting (PG, type, flags, min/max, etc..).
+#define MSP2_COMMON_PG_LIST                 0x1008  //in/out message    Returns a list of the PG ids used by the settings
     
-#define MSP2_COMMON_SERIAL_CONFIG       0x1009
-#define MSP2_COMMON_SET_SERIAL_CONFIG   0x100A
+#define MSP2_COMMON_SERIAL_CONFIG           0x1009
+#define MSP2_COMMON_SET_SERIAL_CONFIG       0x100A
     
 // radar commands   
-#define MSP2_COMMON_SET_RADAR_POS       0x100B //SET radar position information
-#define MSP2_COMMON_SET_RADAR_ITD       0x100C //SET radar information to display
+#define MSP2_COMMON_SET_RADAR_POS           0x100B //SET radar position information
+#define MSP2_COMMON_SET_RADAR_ITD           0x100C //SET radar information to display
 
-#define MSP2_BETAFLIGHT_BIND            0x3000
+#define MSP2_COMMON_SET_MSP_RC_LINK_STATS   0x100D //in message        Sets the MSP RC stats
+#define MSP2_COMMON_SET_MSP_RC_INFO         0x100E //in message        Sets the MSP RC info
+
+#define MSP2_BETAFLIGHT_BIND                0x3000

src/main/msp/msp_protocol_v2_inav.h

@@ -89,6 +89,7 @@
 #define MSP2_INAV_LOGIC_CONDITIONS_SINGLE       0x203B
 
 #define MSP2_INAV_ESC_RPM                       0x2040
+#define MSP2_INAV_ESC_TELEM                     0x2041
 
 #define MSP2_INAV_LED_STRIP_CONFIG_EX           0x2048
 #define MSP2_INAV_SET_LED_STRIP_CONFIG_EX       0x2049
@@ -114,3 +115,8 @@
 
 #define MSP2_INAV_SERVO_CONFIG                  0x2200
 #define MSP2_INAV_SET_SERVO_CONFIG              0x2201
+
+#define MSP2_INAV_GEOZONE                      0x2210
+#define MSP2_INAV_SET_GEOZONE                  0x2211
+#define MSP2_INAV_GEOZONE_VERTEX               0x2212
+#define MSP2_INAV_SET_GEOZONE_VERTEX           0x2213

src/main/navigation/navigation.c

@@ -356,6 +356,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(naviga
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FINISHED(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(navigationFSMState_t previousState);
 #endif
+#ifdef USE_GEOZONE
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_INITALIZE(navigationFSMState_t previousState);
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_IN_PROGRESS(navigationFSMState_t previousState);
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_FINISHED(navigationFSMState_t previousState);
+#endif
 
 static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
     /** Idle state ******************************************************/
@@ -377,6 +382,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
             [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]          = NAV_STATE_COURSE_HOLD_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_CRUISE]               = NAV_STATE_CRUISE_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_MIXERAT]              = NAV_STATE_MIXERAT_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_SEND_TO]              = NAV_STATE_SEND_TO_INITALIZE,
         }
     },
 
@@ -449,6 +455,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]          = NAV_STATE_COURSE_HOLD_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_CRUISE]               = NAV_STATE_CRUISE_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_SEND_TO]              = NAV_STATE_SEND_TO_INITALIZE,
         }
     },
     /** CRUISE_HOLD mode ************************************************/
@@ -485,6 +492,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
             [NAV_FSM_EVENT_SWITCH_TO_RTH]                  = NAV_STATE_RTH_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT]             = NAV_STATE_WAYPOINT_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_SEND_TO]              = NAV_STATE_SEND_TO_INITALIZE,
         }
     },
 
@@ -507,6 +515,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
             [NAV_FSM_EVENT_SWITCH_TO_RTH]                  = NAV_STATE_RTH_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT]             = NAV_STATE_WAYPOINT_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_SEND_TO]              = NAV_STATE_SEND_TO_INITALIZE,
         }
     },
 
@@ -544,6 +553,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
             [NAV_FSM_EVENT_SWITCH_TO_RTH]                  = NAV_STATE_RTH_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT]             = NAV_STATE_WAYPOINT_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_SEND_TO]              = NAV_STATE_SEND_TO_INITALIZE,
         }
     },
 
@@ -566,6 +576,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
             [NAV_FSM_EVENT_SWITCH_TO_RTH]                  = NAV_STATE_RTH_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT]             = NAV_STATE_WAYPOINT_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_SEND_TO]              = NAV_STATE_SEND_TO_INITALIZE,
         }
     },
 
@@ -637,15 +648,16 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .mwState = MW_NAV_STATE_RTH_ENROUTE,
         .mwError = MW_NAV_ERROR_NONE,
         .onEvent = {
-            [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_RTH_HEAD_HOME,           // re-process the state
-            [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING,
-            [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
-            [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
-            [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]           = NAV_STATE_POSHOLD_3D_INITIALIZE,
-            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
-            [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]          = NAV_STATE_COURSE_HOLD_INITIALIZE,
-            [NAV_FSM_EVENT_SWITCH_TO_CRUISE]               = NAV_STATE_CRUISE_INITIALIZE,
-            [NAV_FSM_EVENT_SWITCH_TO_MIXERAT]              = NAV_STATE_MIXERAT_INITIALIZE,
+            [NAV_FSM_EVENT_TIMEOUT]                             = NAV_STATE_RTH_HEAD_HOME,           // re-process the state
+            [NAV_FSM_EVENT_SUCCESS]                             = NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING,
+            [NAV_FSM_EVENT_SWITCH_TO_IDLE]                      = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]                   = NAV_STATE_ALTHOLD_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]                = NAV_STATE_POSHOLD_3D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]         = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]               = NAV_STATE_COURSE_HOLD_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE]                    = NAV_STATE_CRUISE_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_MIXERAT]                   = NAV_STATE_MIXERAT_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE]  = NAV_STATE_RTH_INITIALIZE,
         }
     },
 
@@ -995,6 +1007,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
             [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_IDLE,
             [NAV_FSM_EVENT_ERROR]                          = NAV_STATE_IDLE,
             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_SWITCH_TO_RTH]                  = NAV_STATE_RTH_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
         }
     },
 
@@ -1178,6 +1192,63 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         }
     },
 #endif
+
+#ifdef USE_GEOZONE
+    [NAV_STATE_SEND_TO_INITALIZE] = {
+        .persistentId = NAV_PERSISTENT_ID_SEND_TO_INITALIZE,
+        .onEntry = navOnEnteringState_NAV_STATE_SEND_TO_INITALIZE,
+        .timeoutMs = 0,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
+        .mapToFlightModes = NAV_SEND_TO,
+        .mwState = MW_NAV_STATE_NONE,
+        .mwError = MW_NAV_ERROR_NONE,
+        .onEvent = {
+            [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_SEND_TO_IN_PROGESS,
+            [NAV_FSM_EVENT_ERROR]                          = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
+        }
+    },
+
+    [NAV_STATE_SEND_TO_IN_PROGESS] = {
+        .persistentId = NAV_PERSISTENT_ID_SEND_TO_IN_PROGRES,
+        .onEntry = navOnEnteringState_NAV_STATE_SEND_TO_IN_PROGRESS,
+        .timeoutMs = 10,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
+        .mapToFlightModes = NAV_SEND_TO,
+        .mwState = MW_NAV_STATE_NONE,
+        .mwError = MW_NAV_ERROR_NONE,
+        .onEvent = {
+            [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_SEND_TO_IN_PROGESS,   // re-process the state
+            [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_SEND_TO_FINISHED,
+            [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]           = NAV_STATE_POSHOLD_3D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_RTH]                  = NAV_STATE_RTH_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]          = NAV_STATE_COURSE_HOLD_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE]               = NAV_STATE_CRUISE_INITIALIZE,
+        }
+    },
+     [NAV_STATE_SEND_TO_FINISHED] = {
+        .persistentId = NAV_PERSISTENT_ID_SEND_TO_FINISHED,
+        .onEntry = navOnEnteringState_NAV_STATE_SEND_TO_FINISHED,
+        .timeoutMs = 0,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
+        .mapToFlightModes = NAV_SEND_TO,
+        .mwState = MW_NAV_STATE_NONE,
+        .mwError = MW_NAV_ERROR_NONE,
+        .onEvent = {
+            [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_SEND_TO_FINISHED,   // re-process the state
+            [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]           = NAV_STATE_POSHOLD_3D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_RTH]                  = NAV_STATE_RTH_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]          = NAV_STATE_COURSE_HOLD_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE]               = NAV_STATE_CRUISE_INITIALIZE,
+        }
+    },
+#endif
 };
 
 static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state)
@@ -1444,6 +1515,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
     // If we have valid position sensor or configured to ignore it's loss at initial stage - continue
     if ((posControl.flags.estPosStatus >= EST_USABLE) || navConfig()->general.flags.rth_climb_ignore_emerg) {
         // Prepare controllers
+#ifdef USE_GEOZONE
+        geozoneResetRTH();
+        geozoneSetupRTH();
+#endif
         resetPositionController();
         resetAltitudeController(false);     // Make sure surface tracking is not enabled - RTH uses global altitude, not AGL
         setupAltitudeController();
@@ -1610,22 +1685,55 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
 
     // If we have position sensor - continue home
     if ((posControl.flags.estPosStatus >= EST_USABLE)) {
-        fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL);
-
-        if (isWaypointReached(tmpHomePos, &posControl.activeWaypoint.bearing)) {
-            // Successfully reached position target - update XYZ-position
-            setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
-
-            posControl.landingDelay = 0;
-
-            if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive)
-                posControl.rthState.rthLinearDescentActive = false;
-
-            return NAV_FSM_EVENT_SUCCESS;       // NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING
-        } else {
-            setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY);
+#ifdef USE_GEOZONE
+        // Check for NFZ in our way
+        int8_t wpCount = geozoneCheckForNFZAtCourse(true);
+        if (wpCount > 0) {
+            calculateAndSetActiveWaypointToLocalPosition(geozoneGetCurrentRthAvoidWaypoint());
             return NAV_FSM_EVENT_NONE;
+        } else if (geozone.avoidInRTHInProgress) {
+            if (isWaypointReached(geozoneGetCurrentRthAvoidWaypoint(), &posControl.activeWaypoint.bearing)) {
+                if (geoZoneIsLastRthWaypoint()) {
+                    // Already at Home?
+                    fpVector3_t *tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL);
+                    if (isWaypointReached(tmpHomePos, &posControl.activeWaypoint.bearing)) {
+                        setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
+                        return NAV_FSM_EVENT_SUCCESS;
+                    }
+
+                    posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
+                    return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE;
+                } else {
+                    geozoneAdvanceRthAvoidWaypoint();
+                    calculateAndSetActiveWaypointToLocalPosition(geozoneGetCurrentRthAvoidWaypoint());
+                    return NAV_FSM_EVENT_NONE;
+                }
+            }
+            setDesiredPosition(geozoneGetCurrentRthAvoidWaypoint(), 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
+            return NAV_FSM_EVENT_NONE;
+        } else if (wpCount < 0 && geoZoneConfig()->noWayHomeAction == NO_WAY_HOME_ACTION_EMRG_LAND) {
+            return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
+        } else {
+#endif
+            fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL);
+
+            if (isWaypointReached(tmpHomePos, &posControl.activeWaypoint.bearing)) {
+                // Successfully reached position target - update XYZ-position
+                setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
+
+                posControl.landingDelay = 0;
+
+                if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive)
+                    posControl.rthState.rthLinearDescentActive = false;
+
+                return NAV_FSM_EVENT_SUCCESS;       // NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING
+            } else {
+                setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY);
+                return NAV_FSM_EVENT_NONE;
+            }
+#ifdef USE_GEOZONE
         }
+#endif
     }
     /* Position sensor failure timeout - land */
     else if (checkForPositionSensorTimeout()) {
@@ -1797,6 +1905,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation
         updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, 0, ROC_TO_ALT_CONSTANT);  // FIXME
     }
 
+#ifdef USE_GEOZONE
+    geozoneResetRTH();
+#endif
+
     // Prevent I-terms growing when already landed
     pidResetErrorAccumulators();
     return NAV_FSM_EVENT_NONE;
@@ -2452,6 +2564,64 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(naviga
 }
 #endif
 
+#ifdef USE_GEOZONE
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_INITALIZE(navigationFSMState_t previousState)
+{
+    UNUSED(previousState);
+
+    if (checkForPositionSensorTimeout()) {
+        return NAV_FSM_EVENT_SWITCH_TO_IDLE;
+    }
+
+    resetPositionController();
+    resetAltitudeController(false);
+
+     if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY)) {
+        setDesiredPosition(&posControl.sendTo.targetPos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
+    } else {
+        setDesiredPosition(&posControl.sendTo.targetPos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
+    }
+
+    posControl.sendTo.startTime = millis();
+    return NAV_FSM_EVENT_SUCCESS;
+}
+
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_IN_PROGRESS(navigationFSMState_t previousState)
+{
+    UNUSED(previousState);
+
+    // "Send to" is designed to kick in even user is making inputs, lock sticks for a short time to avoid the mode ends immediately
+    if (posControl.sendTo.lockSticks && millis() - posControl.sendTo.startTime > posControl.sendTo.lockStickTime) {
+        posControl.sendTo.lockSticks = false;
+    }
+
+    if (!posControl.sendTo.lockSticks && areSticksDeflected()) {
+        abortSendTo();
+        return NAV_FSM_EVENT_SWITCH_TO_IDLE;
+    }
+
+    if (calculateDistanceToDestination(&posControl.sendTo.targetPos) <= posControl.sendTo.targetRange ) {
+        if (posControl.sendTo.altitudeTargetRange > 0) {
+            if ((getEstimatedActualPosition(Z) > posControl.sendTo.targetPos.z - posControl.sendTo.altitudeTargetRange) && (getEstimatedActualPosition(Z) < posControl.sendTo.targetPos.z +  posControl.sendTo.altitudeTargetRange)) {
+                return NAV_FSM_EVENT_SUCCESS;
+            } else {
+                return NAV_FSM_EVENT_NONE;
+            }
+        }
+        return NAV_FSM_EVENT_SUCCESS;
+    }
+    return NAV_FSM_EVENT_NONE;
+}
+
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_FINISHED(navigationFSMState_t previousState)
+{
+    UNUSED(previousState);
+    posControl.sendTo.lockSticks = false;
+    posControl.flags.sendToActive = false;
+    return NAV_FSM_EVENT_NONE;
+}
+#endif
+
 static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState)
 {
     navigationFSMState_t previousState;
@@ -2539,6 +2709,11 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
     switch (mode) {
         case RTH_HOME_ENROUTE_INITIAL:
             posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthInitialAltitude;
+#ifdef USE_GEOZONE
+            if (geozone.currentzoneMaxAltitude > 0) {
+                posControl.rthState.homeTmpWaypoint.z = MIN(geozone.currentzoneMaxAltitude, posControl.rthState.homeTmpWaypoint.z);
+            }
+#endif
             break;
 
         case RTH_HOME_ENROUTE_PROPORTIONAL:
@@ -2552,6 +2727,11 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
                     posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude;
                 }
             }
+#ifdef USE_GEOZONE
+            if (geozone.distanceVertToNearestZone < 0 && ABS(geozone.distanceVertToNearestZone) < geoZoneConfig()->safeAltitudeDistance) {
+                posControl.rthState.homeTmpWaypoint.z += geoZoneConfig()->safeAltitudeDistance;
+            }
+#endif
             break;
 
         case RTH_HOME_ENROUTE_FINAL:
@@ -2757,6 +2937,17 @@ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void)
     return posControl.flags.isTerrainFollowEnabled ? &posControl.actualState.agl : &posControl.actualState.abs;
 }
 
+/*-----------------------------------------------------------
+ * Calculates 2D distance between two points
+ *-----------------------------------------------------------*/
+float calculateDistance2(const fpVector2_t* startPos, const fpVector2_t* destinationPos)
+{
+	const float deltaX = destinationPos->x - startPos->x;
+	const float deltaY = destinationPos->y - startPos->y;
+
+	return calc_length_pythagorean_2D(deltaX, deltaY);
+}
+
 /*-----------------------------------------------------------
  * Calculates distance and bearing to destination point
  *-----------------------------------------------------------*/
@@ -2944,6 +3135,11 @@ static void updateDesiredRTHAltitude(void)
         posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
         posControl.rthState.rthFinalAltitude = posControl.actualState.abs.pos.z;
     }
+#if defined(USE_GEOZONE)
+    if (geozone.homeHasMaxAltitue) {
+        posControl.rthState.rthFinalAltitude = MIN(posControl.rthState.rthFinalAltitude, geozone.maxHomeAltitude);
+    }
+#endif
 }
 
 /*-----------------------------------------------------------
@@ -3154,6 +3350,9 @@ void updateHomePosition(void)
                     setHome = true;
                     break;
             }
+#ifdef USE_GEOZONE
+            geozoneUpdateMaxHomeAltitude();
+#endif
         }
     }
     else {
@@ -3410,7 +3609,15 @@ bool isProbablyStillFlying(void)
  *-----------------------------------------------------------*/
 float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
 {
+
     const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding();
+
+#ifdef USE_GEOZONE
+    if (!emergLandingIsActive && geozone.nearestHorZoneHasAction && ((geozone.currentzoneMaxAltitude != 0 && navGetCurrentActualPositionAndVelocity()->pos.z >= geozone.currentzoneMaxAltitude && posControl.desiredState.climbRateDemand > 0) ||
+        (geozone.currentzoneMinAltitude != 0 && navGetCurrentActualPositionAndVelocity()->pos.z <= geozone.currentzoneMinAltitude && posControl.desiredState.climbRateDemand < 0 ))) {
+        return 0.0f;
+    }
+#endif
     float maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_auto_climb_rate : navConfig()->fw.max_auto_climb_rate;
 
     if (posControl.flags.rocToAltMode == ROC_TO_ALT_CONSTANT) {
@@ -4050,6 +4257,16 @@ static void processNavigationRCAdjustments(void)
     /* Process pilot's RC input. Disable all pilot's input when in FAILSAFE_MODE */
     navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
 
+#ifdef USE_GEOZONE
+    if (geozone.sticksLocked) {
+        posControl.flags.isAdjustingAltitude = false;
+        posControl.flags.isAdjustingPosition = false;
+        posControl.flags.isAdjustingHeading = false;
+
+        return;
+    }
+#endif
+
     if (FLIGHT_MODE(FAILSAFE_MODE)) {
         if (STATE(MULTIROTOR) && navStateFlags & NAV_RC_POS) {
             resetMulticopterBrakingMode();
@@ -4096,6 +4313,10 @@ void applyWaypointNavigationAndAltitudeHold(void)
         // Reset RTH trackback
         resetRthTrackBack();
 
+#ifdef USE_GEOZONE
+        posControl.flags.sendToActive = false;
+#endif
+
         return;
     }
 
@@ -4145,7 +4366,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
 void switchNavigationFlightModes(void)
 {
     const flightModeFlags_e enabledNavFlightModes = navGetMappedFlightModes(posControl.navState);
-    const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_FW_AUTOLAND | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE) & (~enabledNavFlightModes);
+    const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_FW_AUTOLAND | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE | NAV_SEND_TO) & (~enabledNavFlightModes);
     DISABLE_FLIGHT_MODE(disabledFlightModes);
     ENABLE_FLIGHT_MODE(enabledNavFlightModes);
 }
@@ -4296,6 +4517,17 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
             return NAV_FSM_EVENT_SWITCH_TO_RTH;
         }
 
+#ifdef USE_GEOZONE
+        if (posControl.flags.sendToActive) {
+            return NAV_FSM_EVENT_SWITCH_TO_SEND_TO;
+        }
+
+
+        if (posControl.flags.forcedPosholdActive) {
+            return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D;
+        }
+#endif
+
         /* WP mission activation control:
          * canActivateWaypoint & waypointWasActivated are used to prevent WP mission
          * auto restarting after interruption by Manual or RTH modes.
@@ -4728,8 +4960,8 @@ void navigationUsePIDs(void)
 
     navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f,
                                         (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 100.0f,
-                                        (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 100.0f,
-                                        0.0f,
+                                        (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 300.0f,
+                                        (float)pidProfile()->bank_fw.pid[PID_POS_Z].FF / 100.0f,
                                         NAV_DTERM_CUT_HZ,
                                         0.0f
     );
@@ -4769,6 +5001,14 @@ void navigationInit(void)
 #ifdef USE_MULTI_MISSION
     posControl.multiMissionCount = 0;
 #endif
+
+#ifdef USE_GEOZONE
+    posControl.flags.sendToActive = false;
+    posControl.sendTo.lockSticks = false;
+    posControl.sendTo.lockStickTime = 0;
+    posControl.sendTo.startTime = 0;
+    posControl.sendTo.targetRange = 0;
+#endif
     /* Set initial surface invalid */
     posControl.actualState.surfaceMin = -1.0f;
 
@@ -4851,6 +5091,40 @@ rthState_e getStateOfForcedRTH(void)
     }
 }
 
+
+#ifdef USE_GEOZONE
+// "Send to" is not to intended to be activated by user, only by external event
+void activateSendTo(void)
+{
+    if (!geozone.avoidInRTHInProgress) {
+        abortFixedWingLaunch();
+        posControl.flags.sendToActive = true;
+        navProcessFSMEvents(selectNavEventFromBoxModeInput());
+    }
+}
+
+void abortSendTo(void)
+{
+    posControl.flags.sendToActive = false;
+    navProcessFSMEvents(selectNavEventFromBoxModeInput());
+}
+
+void activateForcedPosHold(void)
+{
+    if (!geozone.avoidInRTHInProgress) {
+        abortFixedWingLaunch();
+        posControl.flags.forcedPosholdActive = true;
+        navProcessFSMEvents(selectNavEventFromBoxModeInput());
+    }
+}
+
+void abortForcedPosHold(void)
+{
+    posControl.flags.forcedPosholdActive = false;
+    navProcessFSMEvents(selectNavEventFromBoxModeInput());
+}
+#endif
+
 /*-----------------------------------------------------------
  * Ability to execute Emergency Landing on external event
  *-----------------------------------------------------------*/

src/main/navigation/navigation.h

@@ -116,6 +116,123 @@ void resetFwAutolandApproach(int8_t idx);
 
 #endif
 
+#if defined(USE_GEOZONE)
+
+#ifndef USE_GPS
+    #error "Geozone needs GPS support"
+#endif
+
+typedef enum {
+    GEOZONE_MESSAGE_STATE_NONE,
+    GEOZONE_MESSAGE_STATE_NFZ,
+    GEOZONE_MESSAGE_STATE_LEAVING_FZ,
+    GEOZONE_MESSAGE_STATE_OUTSIDE_FZ,
+    GEOZONE_MESSAGE_STATE_ENTERING_NFZ,
+    GEOZONE_MESSAGE_STATE_AVOIDING_FB,
+    GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE,
+    GEOZONE_MESSAGE_STATE_FLYOUT_NFZ,
+    GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH,
+    GEOZONE_MESSAGE_STATE_POS_HOLD
+} geozoneMessageState_e;
+
+enum fenceAction_e {
+    GEOFENCE_ACTION_NONE,
+    GEOFENCE_ACTION_AVOID,
+    GEOFENCE_ACTION_POS_HOLD,
+    GEOFENCE_ACTION_RTH,
+};
+
+enum noWayHomeAction {
+    NO_WAY_HOME_ACTION_RTH,
+    NO_WAY_HOME_ACTION_EMRG_LAND,
+};
+
+#define GEOZONE_SHAPE_CIRCULAR 0
+#define GEOZONE_SHAPE_POLYGON  1
+
+#define GEOZONE_TYPE_EXCLUSIVE 0
+#define GEOZONE_TYPE_INCLUSIVE 1
+
+typedef struct geoZoneConfig_s
+{
+    uint8_t shape;
+    uint8_t type;
+    int32_t minAltitude;
+    int32_t maxAltitude;
+    bool isSealevelRef;
+    uint8_t fenceAction;
+    uint8_t vertexCount;
+} geoZoneConfig_t;
+
+typedef struct geozone_config_s
+{
+    uint32_t fenceDetectionDistance;
+    uint16_t avoidAltitudeRange;
+    uint16_t safeAltitudeDistance;
+    bool nearestSafeHomeAsInclusivZone;
+    uint8_t safeHomeFenceAction;
+    uint32_t copterFenceStopDistance;
+    uint8_t noWayHomeAction;
+} geozone_config_t;
+
+typedef struct vertexConfig_s
+{
+    int8_t zoneId;
+    uint8_t idx;
+    int32_t lat;
+    int32_t lon;
+} vertexConfig_t;
+
+PG_DECLARE(geozone_config_t, geoZoneConfig);
+PG_DECLARE_ARRAY(geoZoneConfig_t, MAX_GEOZONES_IN_CONFIG, geoZonesConfig);
+PG_DECLARE_ARRAY(vertexConfig_t, MAX_VERTICES_IN_CONFIG, geoZoneVertices);
+
+typedef struct geozone_s {
+    bool insideFz;
+    bool insideNfz;
+    uint32_t distanceToZoneBorder3d;
+    int32_t vertDistanceToZoneBorder;
+    geozoneMessageState_e messageState;
+    int32_t directionToNearestZone;
+    int32_t distanceHorToNearestZone;
+    int32_t distanceVertToNearestZone;
+    int32_t zoneInfo;
+    int32_t currentzoneMaxAltitude; 
+    int32_t currentzoneMinAltitude;
+    bool nearestHorZoneHasAction;
+    bool sticksLocked;
+    int8_t loiterDir;
+    bool avoidInRTHInProgress;
+    int32_t maxHomeAltitude;
+    bool homeHasMaxAltitue;
+} geozone_t;
+
+extern geozone_t geozone;
+
+bool geozoneSetVertex(uint8_t zoneId, uint8_t vertexId, int32_t lat, int32_t lon);
+int8_t geozoneGetVertexIdx(uint8_t zoneId, uint8_t vertexId);
+bool isGeozoneActive(void);
+uint8_t geozoneGetUsedVerticesCount(void);
+void geozoneReset(int8_t idx);
+void geozoneResetVertices(int8_t zoneId, int16_t idx);
+void geozoneUpdate(timeUs_t curentTimeUs);
+bool geozoneIsBlockingArming(void);
+void geozoneAdvanceRthAvoidWaypoint(void);
+int8_t geozoneCheckForNFZAtCourse(bool isRTH);
+bool geoZoneIsLastRthWaypoint(void);
+fpVector3_t *geozoneGetCurrentRthAvoidWaypoint(void);
+void geozoneSetupRTH(void);
+void geozoneResetRTH(void);
+void geozoneUpdateMaxHomeAltitude(void);
+uint32_t geozoneGetDetectionDistance(void);
+
+void activateSendTo(void);
+void abortSendTo(void);
+void activateForcedPosHold(void);
+void abortForcedPosHold(void);
+
+#endif
+
 #ifndef NAV_MAX_WAYPOINTS
 #define NAV_MAX_WAYPOINTS 15
 #endif
@@ -231,35 +348,37 @@ typedef enum {
 
 typedef struct positionEstimationConfig_s {
     uint8_t automatic_mag_declination;
-    uint8_t reset_altitude_type; // from nav_reset_type_e
-    uint8_t reset_home_type; // nav_reset_type_e
-    uint8_t gravity_calibration_tolerance;    // Tolerance of gravity calibration (cm/s/s)
+    uint8_t reset_altitude_type;            // from nav_reset_type_e
+    uint8_t reset_home_type;                // nav_reset_type_e
+    uint8_t gravity_calibration_tolerance;  // Tolerance of gravity calibration (cm/s/s)
     uint8_t allow_dead_reckoning;
 
     uint16_t max_surface_altitude;
 
-    float w_z_baro_p;   // Weight (cutoff frequency) for barometer altitude measurements
+    float w_z_baro_p;           // Weight (cutoff frequency) for barometer altitude measurements
+    float w_z_baro_v;           // Weight (cutoff frequency) for barometer climb rate measurements
 
-    float w_z_surface_p;  // Weight (cutoff frequency) for surface altitude measurements
-    float w_z_surface_v;  // Weight (cutoff frequency) for surface velocity measurements
+    float w_z_surface_p;        // Weight (cutoff frequency) for surface altitude measurements
+    float w_z_surface_v;        // Weight (cutoff frequency) for surface velocity measurements
 
-    float w_z_gps_p;    // GPS altitude data is very noisy and should be used only on airplanes
-    float w_z_gps_v;    // Weight (cutoff frequency) for GPS climb rate measurements
+    float w_z_gps_p;            // GPS altitude data is very noisy and should be used only on airplanes
+    float w_z_gps_v;            // Weight (cutoff frequency) for GPS climb rate measurements
 
-    float w_xy_gps_p;   // Weight (cutoff frequency) for GPS position measurements
-    float w_xy_gps_v;   // Weight (cutoff frequency) for GPS velocity measurements
+    float w_xy_gps_p;           // Weight (cutoff frequency) for GPS position measurements
+    float w_xy_gps_v;           // Weight (cutoff frequency) for GPS velocity measurements
 
     float w_xy_flow_p;
     float w_xy_flow_v;
 
-    float w_z_res_v;    // When velocity sources lost slowly decrease estimated velocity with this weight
+    float w_z_res_v;            // When velocity sources lost slowly decrease estimated velocity with this weight
     float w_xy_res_v;
 
-    float w_acc_bias;   // Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable.
+    float w_acc_bias;           // Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable.
 
-    float max_eph_epv;  // Max estimated position error acceptable for estimation (cm)
-    float baro_epv;     // Baro position error
+    float max_eph_epv;          // Max estimated position error acceptable for estimation (cm)
+    float baro_epv;             // Baro position error
 
+    uint8_t default_alt_sensor; // default altitude sensor source
 #ifdef USE_GPS_FIX_ESTIMATION
     uint8_t allow_gps_fix_estimation;
 #endif

src/main/navigation/navigation_fixedwing.c

@@ -250,6 +250,12 @@ static int8_t loiterDirection(void) {
         dir *= -1;
     }
 
+#ifdef USE_GEOZONE
+    if (geozone.loiterDir != 0) {
+        dir = geozone.loiterDir;
+    }
+#endif
+
     return dir;
 }
 

src/main/navigation/navigation_fw_launch.c

@@ -613,6 +613,7 @@ uint8_t fixedWingLaunchStatus(void)
 void abortFixedWingLaunch(void)
 {
     setCurrentState(FW_LAUNCH_STATE_ABORTED, 0);
+    DISABLE_FLIGHT_MODE(NAV_LAUNCH_MODE);
 }
 
 const char * fixedWingLaunchStateMessage(void)

src/main/navigation/navigation_pos_estimator.c

@@ -56,7 +56,7 @@
 navigationPosEstimator_t posEstimator;
 static float initialBaroAltitudeOffset = 0.0f;
 
-PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 7);
+PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 8);
 
 PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
         // Inertial position estimator parameters
@@ -69,6 +69,7 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
         .max_surface_altitude = SETTING_INAV_MAX_SURFACE_ALTITUDE_DEFAULT,
 
         .w_z_baro_p = SETTING_INAV_W_Z_BARO_P_DEFAULT,
+        .w_z_baro_v = SETTING_INAV_W_Z_BARO_V_DEFAULT,
 
         .w_z_surface_p = SETTING_INAV_W_Z_SURFACE_P_DEFAULT,
         .w_z_surface_v = SETTING_INAV_W_Z_SURFACE_V_DEFAULT,
@@ -89,6 +90,8 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
 
         .max_eph_epv = SETTING_INAV_MAX_EPH_EPV_DEFAULT,
         .baro_epv = SETTING_INAV_BARO_EPV_DEFAULT,
+
+        .default_alt_sensor = SETTING_INAV_DEFAULT_ALT_SENSOR_DEFAULT,
 #ifdef USE_GPS_FIX_ESTIMATION
         .allow_gps_fix_estimation = SETTING_INAV_ALLOW_GPS_FIX_ESTIMATION_DEFAULT
 #endif
@@ -342,6 +345,7 @@ static bool gravityCalibrationComplete(void)
 
     return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration);
 }
+
 #define ACC_VIB_FACTOR_S 1.0f
 #define ACC_VIB_FACTOR_E 3.0f
 static void updateIMUEstimationWeight(const float dt)
@@ -423,7 +427,6 @@ static void updateIMUTopic(timeUs_t currentTimeUs)
             posEstimator.imu.calibratedGravityCMSS = gyroConfig()->gravity_cmss_cal;
         }
 
-        /* If calibration is incomplete - report zero acceleration */
         if (gravityCalibrationComplete()) {
 #ifdef USE_SIMULATOR
             if (ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) {
@@ -432,7 +435,7 @@ static void updateIMUTopic(timeUs_t currentTimeUs)
 #endif
             posEstimator.imu.accelNEU.z -= posEstimator.imu.calibratedGravityCMSS;
         }
-        else {
+        else {      // If calibration is incomplete - report zero acceleration
             posEstimator.imu.accelNEU.x = 0.0f;
             posEstimator.imu.accelNEU.y = 0.0f;
             posEstimator.imu.accelNEU.z = 0.0f;
@@ -476,14 +479,16 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
     /* Figure out if we have valid position data from our data sources */
     uint32_t newFlags = 0;
 
+    const float max_eph_epv = positionEstimationConfig()->max_eph_epv;
+
     if ((sensors(SENSOR_GPS)
 #ifdef USE_GPS_FIX_ESTIMATION
             || STATE(GPS_ESTIMATED_FIX)
 #endif
         ) && posControl.gpsOrigin.valid &&
         ((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) &&
-        (posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv)) {
-        if (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv) {
+        (posEstimator.gps.eph < max_eph_epv)) {
+        if (posEstimator.gps.epv < max_eph_epv) {
             newFlags |= EST_GPS_XY_VALID | EST_GPS_Z_VALID;
         }
         else {
@@ -503,11 +508,11 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
         newFlags |= EST_FLOW_VALID;
     }
 
-    if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) {
+    if (posEstimator.est.eph < max_eph_epv) {
         newFlags |= EST_XY_VALID;
     }
 
-    if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
+    if (posEstimator.est.epv < max_eph_epv) {
         newFlags |= EST_Z_VALID;
     }
 
@@ -521,7 +526,9 @@ static void estimationPredict(estimationContext_t * ctx)
     if ((ctx->newFlags & EST_Z_VALID)) {
         posEstimator.est.pos.z += posEstimator.est.vel.z * ctx->dt;
         posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f;
-        posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt;
+        if (ARMING_FLAG(WAS_EVER_ARMED)) {   // Hold at zero until first armed
+            posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt;
+        }
     }
 
     /* Prediction step: XY-axis */
@@ -552,20 +559,28 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
     DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount());            // Clip count
 
     bool correctOK = false;
+    const uint8_t defaultAltitudeSource = positionEstimationConfig()->default_alt_sensor;
+    float wGps = defaultAltitudeSource == ALTITUDE_SOURCE_BARO_ONLY && ctx->newFlags & EST_BARO_VALID ? 0.0f : 1.0f;
+    float wBaro = defaultAltitudeSource == ALTITUDE_SOURCE_GPS_ONLY && ctx->newFlags & EST_GPS_Z_VALID ? 0.0f : 1.0f;
 
-    float wBaro = 0.0f;
-    if (ctx->newFlags & EST_BARO_VALID) {
-        // Ignore baro if difference is too big, baro is probably wrong
-        const float gpsBaroResidual = ctx->newFlags & EST_GPS_Z_VALID ? fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt) : 0.0f;
+    if (wBaro && ctx->newFlags & EST_BARO_VALID && wGps && ctx->newFlags & EST_GPS_Z_VALID) {
+        const float gpsBaroResidual = fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt);
 
-        // Fade out the baro to prevent sudden jump
-        const float start_epv = positionEstimationConfig()->max_eph_epv;
-        const float end_epv = positionEstimationConfig()->max_eph_epv * 2.0f;
+        // Fade out the non default sensor to prevent sudden jump
+        uint16_t residualErrorEpvLimit = defaultAltitudeSource == ALTITUDE_SOURCE_BARO ? 2 * positionEstimationConfig()->baro_epv : positionEstimationConfig()->max_eph_epv;
+        const float start_epv = residualErrorEpvLimit;
+        const float end_epv = residualErrorEpvLimit * 2.0f;
+
+        // Calculate residual gps/baro sensor weighting based on assumed default altitude source = GPS
         wBaro = scaleRangef(constrainf(gpsBaroResidual, start_epv, end_epv), start_epv, end_epv, 1.0f, 0.0f);
+
+        if (defaultAltitudeSource == ALTITUDE_SOURCE_BARO) {    // flip residual sensor weighting if default = BARO
+            wGps = wBaro;
+            wBaro = 1.0f;
+        }
     }
 
-    // Always use Baro if no GPS otherwise only use if accuracy OK compared to GPS
-    if (wBaro > 0.01f) {
+    if (ctx->newFlags & EST_BARO_VALID && wBaro) {
         timeUs_t currentTimeUs = micros();
 
         if (!ARMING_FLAG(ARMED)) {
@@ -588,21 +603,25 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
                                              ((ctx->newFlags & EST_BARO_VALID) && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt));
 
         // Altitude
-        const float baroAltResidual = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z;
-        ctx->estPosCorr.z += wBaro * baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt;
-        ctx->estVelCorr.z += wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt;
+        const float baroAltResidual = wBaro * ((isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z);
+        const float baroVelZResidual = isAirCushionEffectDetected ? 0.0f : wBaro * (posEstimator.baro.baroAltRate - posEstimator.est.vel.z);
+        const float w_z_baro_p = positionEstimationConfig()->w_z_baro_p;
 
-        ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p);
+        ctx->estPosCorr.z += baroAltResidual * w_z_baro_p * ctx->dt;
+        ctx->estVelCorr.z += baroAltResidual * sq(w_z_baro_p) * ctx->dt;
+        ctx->estVelCorr.z += baroVelZResidual * positionEstimationConfig()->w_z_baro_v * ctx->dt;
+
+        ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, w_z_baro_p);
 
         // Accelerometer bias
         if (!isAirCushionEffectDetected) {
-            ctx->accBiasCorr.z -= wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p);
+            ctx->accBiasCorr.z -= baroAltResidual * sq(w_z_baro_p);
         }
 
-        correctOK = true;
+        correctOK = ARMING_FLAG(WAS_EVER_ARMED);    // No correction until first armed
     }
 
-    if (ctx->newFlags & EST_GPS_Z_VALID) {
+    if (ctx->newFlags & EST_GPS_Z_VALID && wGps) {
         // Reset current estimate to GPS altitude if estimate not valid
         if (!(ctx->newFlags & EST_Z_VALID)) {
             ctx->estPosCorr.z += posEstimator.gps.pos.z - posEstimator.est.pos.z;
@@ -611,19 +630,20 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
         }
         else {
             // Altitude
-            const float gpsAltResudual = posEstimator.gps.pos.z - posEstimator.est.pos.z;
-            const float gpsVelZResudual = posEstimator.gps.vel.z - posEstimator.est.vel.z;
+            const float gpsAltResidual = wGps * (posEstimator.gps.pos.z - posEstimator.est.pos.z);
+            const float gpsVelZResidual = wGps * (posEstimator.gps.vel.z - posEstimator.est.vel.z);
+            const float w_z_gps_p = positionEstimationConfig()->w_z_gps_p;
 
-            ctx->estPosCorr.z += gpsAltResudual * positionEstimationConfig()->w_z_gps_p * ctx->dt;
-            ctx->estVelCorr.z += gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt;
-            ctx->estVelCorr.z += gpsVelZResudual * positionEstimationConfig()->w_z_gps_v * ctx->dt;
-            ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, fabsf(gpsAltResudual)), positionEstimationConfig()->w_z_gps_p);
+            ctx->estPosCorr.z += gpsAltResidual * w_z_gps_p * ctx->dt;
+            ctx->estVelCorr.z += gpsAltResidual * sq(w_z_gps_p) * ctx->dt;
+            ctx->estVelCorr.z += gpsVelZResidual * positionEstimationConfig()->w_z_gps_v * ctx->dt;
+            ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, fabsf(gpsAltResidual)), w_z_gps_p);
 
             // Accelerometer bias
-            ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p);
+            ctx->accBiasCorr.z -= gpsAltResidual * sq(w_z_gps_p);
         }
 
-        correctOK = true;
+        correctOK = ARMING_FLAG(WAS_EVER_ARMED);    // No correction until first armed
     }
 
     return correctOK;
@@ -700,21 +720,23 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
 {
     estimationContext_t ctx;
 
+    const float max_eph_epv = positionEstimationConfig()->max_eph_epv;
+
     /* Calculate dT */
     ctx.dt = US2S(currentTimeUs - posEstimator.est.lastUpdateTime);
     posEstimator.est.lastUpdateTime = currentTimeUs;
 
     /* If IMU is not ready we can't estimate anything */
     if (!isImuReady()) {
-        posEstimator.est.eph = positionEstimationConfig()->max_eph_epv + 0.001f;
-        posEstimator.est.epv = positionEstimationConfig()->max_eph_epv + 0.001f;
+        posEstimator.est.eph = max_eph_epv + 0.001f;
+        posEstimator.est.epv = max_eph_epv + 0.001f;
         posEstimator.flags = 0;
         return;
     }
 
     /* Calculate new EPH and EPV for the case we didn't update postion */
-    ctx.newEPH = posEstimator.est.eph * ((posEstimator.est.eph <= positionEstimationConfig()->max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
-    ctx.newEPV = posEstimator.est.epv * ((posEstimator.est.epv <= positionEstimationConfig()->max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
+    ctx.newEPH = posEstimator.est.eph * ((posEstimator.est.eph <= max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
+    ctx.newEPV = posEstimator.est.epv * ((posEstimator.est.epv <= max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
     ctx.newFlags = calculateCurrentValidityFlags(currentTimeUs);
     vectorZero(&ctx.estPosCorr);
     vectorZero(&ctx.estVelCorr);
@@ -737,12 +759,12 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
         estimationCalculateCorrection_XY_FLOW(&ctx);
 
     // If we can't apply correction or accuracy is off the charts - decay velocity to zero
-    if (!estXYCorrectOk || ctx.newEPH > positionEstimationConfig()->max_eph_epv) {
+    if (!estXYCorrectOk || ctx.newEPH > max_eph_epv) {
         ctx.estVelCorr.x = (0.0f - posEstimator.est.vel.x) * positionEstimationConfig()->w_xy_res_v * ctx.dt;
         ctx.estVelCorr.y = (0.0f - posEstimator.est.vel.y) * positionEstimationConfig()->w_xy_res_v * ctx.dt;
     }
 
-    if (!estZCorrectOk || ctx.newEPV > positionEstimationConfig()->max_eph_epv) {
+    if (!estZCorrectOk || ctx.newEPV > max_eph_epv) {
         ctx.estVelCorr.z = (0.0f - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_res_v * ctx.dt;
     }
     // Boost the corrections based on accWeight
@@ -754,16 +776,17 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
     vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr);
 
     /* Correct accelerometer bias */
-    if (positionEstimationConfig()->w_acc_bias > 0.0f) {
+    const float w_acc_bias = positionEstimationConfig()->w_acc_bias;
+    if (w_acc_bias > 0.0f) {
         const float accelBiasCorrMagnitudeSq = sq(ctx.accBiasCorr.x) + sq(ctx.accBiasCorr.y) + sq(ctx.accBiasCorr.z);
         if (accelBiasCorrMagnitudeSq < sq(INAV_ACC_BIAS_ACCEPTANCE_VALUE)) {
             /* transform error vector from NEU frame to body frame */
             imuTransformVectorEarthToBody(&ctx.accBiasCorr);
 
             /* Correct accel bias */
-            posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * positionEstimationConfig()->w_acc_bias * ctx.dt;
-            posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * positionEstimationConfig()->w_acc_bias * ctx.dt;
-            posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * positionEstimationConfig()->w_acc_bias * ctx.dt;
+            posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * w_acc_bias * ctx.dt;
+            posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * w_acc_bias * ctx.dt;
+            posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * w_acc_bias * ctx.dt;
         }
     }
 

src/main/navigation/navigation_pos_estimator_private.h

@@ -146,6 +146,13 @@ typedef enum {
     EST_Z_VALID                 = (1 << 6),
 } navPositionEstimationFlags_e;
 
+typedef enum {
+    ALTITUDE_SOURCE_GPS,
+    ALTITUDE_SOURCE_BARO,
+    ALTITUDE_SOURCE_GPS_ONLY,
+    ALTITUDE_SOURCE_BARO_ONLY,
+} navDefaultAltitudeSensor_e;
+
 typedef struct {
     timeUs_t    baroGroundTimeout;
     float       baroGroundAlt;

src/main/navigation/navigation_private.h

@@ -113,6 +113,11 @@ typedef struct navigationFlags_s {
     bool rthTrackbackActive;                // Activation status of RTH trackback
     bool wpTurnSmoothingActive;             // Activation status WP turn smoothing
     bool manualEmergLandActive;             // Activation status of manual emergency landing
+
+#ifdef USE_GEOZONE
+    bool sendToActive;
+    bool forcedPosholdActive;
+#endif
 } navigationFlags_t;
 
 typedef struct {
@@ -160,6 +165,7 @@ typedef enum {
     NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ,
     NAV_FSM_EVENT_SWITCH_TO_MIXERAT,
     NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING,
+    NAV_FSM_EVENT_SWITCH_TO_SEND_TO,
 
     NAV_FSM_EVENT_STATE_SPECIFIC_1,             // State-specific event
     NAV_FSM_EVENT_STATE_SPECIFIC_2,             // State-specific event
@@ -245,6 +251,10 @@ typedef enum {
     NAV_PERSISTENT_ID_FW_LANDING_FLARE                          = 46,
     NAV_PERSISTENT_ID_FW_LANDING_ABORT                          = 47,
     NAV_PERSISTENT_ID_FW_LANDING_FINISHED                       = 48,
+
+    NAV_PERSISTENT_ID_SEND_TO_INITALIZE                         = 49,
+    NAV_PERSISTENT_ID_SEND_TO_IN_PROGRES                        = 50,
+    NAV_PERSISTENT_ID_SEND_TO_FINISHED                          = 51
 } navigationPersistentId_e;
 
 typedef enum {
@@ -304,6 +314,10 @@ typedef enum {
     NAV_STATE_MIXERAT_IN_PROGRESS,
     NAV_STATE_MIXERAT_ABORT,
 
+    NAV_STATE_SEND_TO_INITALIZE,
+    NAV_STATE_SEND_TO_IN_PROGESS,
+    NAV_STATE_SEND_TO_FINISHED,
+
     NAV_STATE_COUNT,
 } navigationFSMState_t;
 
@@ -406,6 +420,17 @@ typedef enum {
     RTH_HOME_FINAL_LAND,            // Home position and altitude
 } rthTargetMode_e;
 
+#ifdef USE_GEOZONE
+typedef struct navSendTo_s {
+    fpVector3_t targetPos;
+    uint16_t altitudeTargetRange;   // 0 for only "2D" 
+    uint32_t targetRange;
+    bool lockSticks;
+    uint32_t lockStickTime;
+    timeMs_t startTime;
+} navSendTo_t;
+#endif
+
 typedef struct {
     fpVector3_t nearestSafeHome;    // The nearestSafeHome found during arming
     uint32_t    distance;           // distance to the nearest safehome
@@ -478,6 +503,10 @@ typedef struct {
     fwLandState_t fwLandState;
 #endif
 
+#ifdef USE_GEOZONE
+    navSendTo_t                  sendTo; // Used for Geozones
+#endif
+
     /* Internals & statistics */
     int16_t                     rcAdjustment[4];
     float                       totalTripDistance;
@@ -502,7 +531,9 @@ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void);
 
 bool isThrustFacingDownwards(void);
 uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos);
+void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t distance);
 int32_t calculateBearingToDestination(const fpVector3_t * destinationPos);
+float calculateDistance2(const fpVector2_t* startPos, const fpVector2_t* destinationPos);
 
 bool isLandingDetected(void);
 void resetLandingDetector(void);

src/main/programming/logic_condition.c

@@ -294,48 +294,53 @@ static int logicConditionCompute(
             return true;
             break;
 #endif
-        case LOGIC_CONDITION_SET_VTX_POWER_LEVEL:
-#if defined(USE_VTX_CONTROL)
-#if(defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP))
-            if (
-                logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] != operandA &&
-                vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)
-            ) {
-                logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount);
-                vtxSettingsConfigMutable()->power = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL];
-                return logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL];
-            } else {
-                return false;
-            }
-            break;
-#else
-            return false;
-#endif
 
+#if defined(USE_VTX_CONTROL)
+        case LOGIC_CONDITION_SET_VTX_POWER_LEVEL:
+        {
+            uint8_t newPower = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL];
+            if ((newPower != operandA || newPower != vtxSettingsConfig()->power) && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) {
+                newPower = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount);
+                logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = newPower;
+                if (newPower != vtxSettingsConfig()->power) {
+                    vtxCommonSetPowerByIndex(vtxCommonDevice(), newPower); // Force setting if modified elsewhere
+                }
+                vtxSettingsConfigMutable()->power = newPower;
+                return newPower;
+            }
+            return false;
+            break;
+        }
         case LOGIC_CONDITION_SET_VTX_BAND:
-            if (
-                logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] != operandA &&
-                vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)
-            ) {
-                logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] = constrain(operandA, VTX_SETTINGS_MIN_BAND, VTX_SETTINGS_MAX_BAND);
-                vtxSettingsConfigMutable()->band = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND];
-                return logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND];
-            } else {
-                return false;
+        {
+            uint8_t newBand = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND];
+            if ((newBand != operandA  || newBand != vtxSettingsConfig()->band) && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) {
+                newBand = constrain(operandA, VTX_SETTINGS_MIN_BAND, vtxDeviceCapability.bandCount);
+                logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] = newBand;
+                if (newBand != vtxSettingsConfig()->band) {
+                    vtxCommonSetBandAndChannel(vtxCommonDevice(), newBand, vtxSettingsConfig()->channel);
+                }
+                vtxSettingsConfigMutable()->band = newBand;
+                return newBand;
             }
+            return false;
             break;
+        }
         case LOGIC_CONDITION_SET_VTX_CHANNEL:
-            if (
-                logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] != operandA &&
-                vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)
-            ) {
-                logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] = constrain(operandA, VTX_SETTINGS_MIN_CHANNEL, VTX_SETTINGS_MAX_CHANNEL);
-                vtxSettingsConfigMutable()->channel = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL];
-                return logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL];
-            } else {
-                return false;
+        {
+            uint8_t newChannel = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL];
+            if ((newChannel != operandA  || newChannel != vtxSettingsConfig()->channel) && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) {
+                newChannel = constrain(operandA, VTX_SETTINGS_MIN_CHANNEL, vtxDeviceCapability.channelCount);
+                logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] = newChannel;
+                if (newChannel != vtxSettingsConfig()->channel) {
+                    vtxCommonSetBandAndChannel(vtxCommonDevice(), vtxSettingsConfig()->band, newChannel);
+                }
+                vtxSettingsConfigMutable()->channel = newChannel;
+                return newChannel;
             }
+            return false;
             break;
+        }
 #endif
         case LOGIC_CONDITION_INVERT_ROLL:
             LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_ROLL);
@@ -796,20 +801,36 @@ static int logicConditionGetFlightOperandValue(int operand) {
             return constrain(calc_length_pythagorean_2D(GPS_distanceToHome, getEstimatedActualPosition(Z) / 100.0f), 0, INT32_MAX);
             break;
 
-        case LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ:
-        #ifdef USE_SERIALRX_CRSF
+        case LOGIC_CONDITION_OPERAND_FLIGHT_LQ_UPLINK:
+#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP)
             return rxLinkStatistics.uplinkLQ;
-        #else
+#else
             return 0;
-        #endif
+#endif
             break;
 
-        case LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR:
-        #ifdef USE_SERIALRX_CRSF
-            return rxLinkStatistics.uplinkSNR;
-        #else
+        case LOGIC_CONDITION_OPERAND_FLIGHT_UPLINK_RSSI_DBM:
+#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP)
+            return rxLinkStatistics.uplinkRSSI;
+#else
             return 0;
-        #endif
+#endif        
+            break;
+
+case LOGIC_CONDITION_OPERAND_FLIGHT_LQ_DOWNLINK:
+#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP)
+            return rxLinkStatistics.downlinkLQ;
+#else
+            return 0;
+#endif
+            break;
+
+        case LOGIC_CONDITION_OPERAND_FLIGHT_SNR:
+#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP)
+            return rxLinkStatistics.uplinkSNR;
+#else
+            return 0;
+#endif
             break;
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE: // int

src/main/programming/logic_condition.h

@@ -129,8 +129,8 @@ typedef enum {
     LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH,                        // 26
     LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW,                          // 27
     LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE,                        // 28
-    LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ,                                 // 29
-    LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR,                                // 39
+    LOGIC_CONDITION_OPERAND_FLIGHT_LQ_UPLINK,                               // 29
+    LOGIC_CONDITION_OPERAND_FLIGHT_SNR,                                     // 39
     LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID, // 0/1                        // 31
     LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS,                           // 32
     LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE, //int                    // 33
@@ -144,6 +144,8 @@ typedef enum {
     LOGIC_CONDITION_OPERAND_FLIGHT_FW_LAND_STATE,                           // 41
     LOGIC_CONDITION_OPERAND_FLIGHT_BATT_PROFILE, // int                     // 42
     LOGIC_CONDITION_OPERAND_FLIGHT_FLOWN_LOITER_RADIUS,                     // 43
+    LOGIC_CONDITION_OPERAND_FLIGHT_LQ_DOWNLINK,                             // 44
+    LOGIC_CONDITION_OPERAND_FLIGHT_UPLINK_RSSI_DBM,                         // 45
 } logicFlightOperands_e;
 
 typedef enum {

src/main/rx/rx.c

@@ -572,6 +572,18 @@ static void setRSSIValue(uint16_t rssiValue, rssiSource_e source, bool filtered)
     rssi = constrain(scaleRange(rssi, rssiMin, rssiMax, 0, RSSI_MAX_VALUE), 0, RSSI_MAX_VALUE);
 }
 
+void setRSSIFromMSP_RC(uint8_t newMspRssi)
+{
+    if (activeRssiSource == RSSI_SOURCE_NONE && (rxConfig()->rssi_source == RSSI_SOURCE_MSP || rxConfig()->rssi_source == RSSI_SOURCE_AUTO)) {
+        activeRssiSource = RSSI_SOURCE_MSP;
+    }
+
+    if (activeRssiSource == RSSI_SOURCE_MSP) {
+        rssi = constrain(scaleRange(constrain(newMspRssi, 0, 100), 0, 100, 0, RSSI_MAX_VALUE), 0, RSSI_MAX_VALUE);
+        lastMspRssiUpdateUs = micros();
+    }
+}
+
 void setRSSIFromMSP(uint8_t newMspRssi)
 {
     if (activeRssiSource == RSSI_SOURCE_NONE && (rxConfig()->rssi_source == RSSI_SOURCE_MSP || rxConfig()->rssi_source == RSSI_SOURCE_AUTO)) {

src/main/rx/rx.h

@@ -181,12 +181,16 @@ typedef enum {
 } rssiSource_e;
 
 typedef struct rxLinkStatistics_s {
-    int16_t uplinkRSSI;     // RSSI value in dBm
-    uint8_t uplinkLQ;       // A protocol specific measure of the link quality in [0..100]
-    int8_t uplinkSNR;       // The SNR of the uplink in dB
-    uint8_t rfMode;         // A protocol specific measure of the transmission bandwidth [2 = 150Hz, 1 = 50Hz, 0 = 4Hz]
-    uint16_t uplinkTXPower; // power in mW
-    uint8_t activeAntenna;
+    int16_t     uplinkRSSI;         // RSSI value in dBm
+    uint8_t     uplinkLQ;           // A protocol specific measure of the link quality in [0..100]
+    uint8_t     downlinkLQ;         // A protocol specific measure of the link quality in [0..100]
+    int8_t      uplinkSNR;          // The SNR of the uplink in dB
+    uint8_t     rfMode;             // A protocol specific measure of the transmission bandwidth [2 = 150Hz, 1 = 50Hz, 0 = 4Hz]
+    uint16_t    uplinkTXPower;      // power in mW
+    uint16_t    downlinkTXPower;    // power in mW
+    uint8_t     activeAntenna;
+    char        band[4];
+    char        mode[6];
 } rxLinkStatistics_t;
 
 typedef uint16_t (*rcReadRawDataFnPtr)(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data
@@ -212,6 +216,7 @@ bool isRxPulseValid(uint16_t pulseDuration);
 uint8_t calculateChannelRemapping(const uint8_t *channelMap, uint8_t channelMapEntryCount, uint8_t channelToRemap);
 void parseRcChannels(const char *input);
 
+void setRSSIFromMSP_RC(uint8_t newMspRssi);
 void setRSSIFromMSP(uint8_t newMspRssi);
 void updateRSSI(timeUs_t currentTimeUs);
 // Returns RSSI in [0, RSSI_MAX_VALUE] range.

src/main/scheduler/scheduler.h

@@ -137,6 +137,10 @@ typedef enum {
     TASK_TELEMETRY_SBUS2,
 #endif
 
+#if defined (USE_GEOZONE) && defined(USE_GPS)
+    TASK_GEOZONE,
+#endif
+
     /* Count of real tasks */
     TASK_COUNT,
 

src/main/sensors/battery.c

@@ -351,16 +351,23 @@ static void checkBatteryCapacityState(void)
 
 void batteryUpdate(timeUs_t timeDelta)
 {
+    static timeUs_t batteryConnectedTime = 0;
     /* battery has just been connected*/
     if (batteryState == BATTERY_NOT_PRESENT && vbat > VBATT_PRESENT_THRESHOLD) {
+        if(batteryConnectedTime == 0) {
+            batteryConnectedTime = micros();
+            return;
+        }
+
+        /* wait for VBatt to stabilise then we can calc number of cells
+        (using the filtered value takes a long time to ramp up)
+        Blocking can cause issues with some ESCs */
+        if((micros() - batteryConnectedTime) < VBATT_STABLE_DELAY) {
+            return;
+        }
 
         /* Actual battery state is calculated below, this is really BATTERY_PRESENT */
         batteryState = BATTERY_OK;
-        /* wait for VBatt to stabilise then we can calc number of cells
-        (using the filtered value takes a long time to ramp up)
-        We only do this on the ground so don't care if we do block, not
-        worse than original code anyway*/
-        delay(VBATT_STABLE_DELAY);
         updateBatteryVoltage(timeDelta, true);
 
         int8_t detectedProfileIndex = -1;
@@ -396,6 +403,7 @@ void batteryUpdate(timeUs_t timeDelta)
         /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of VBATT_PRESENT_THRESHOLD */
         if (batteryState != BATTERY_NOT_PRESENT && vbat <= VBATT_PRESENT_THRESHOLD) {
             batteryState = BATTERY_NOT_PRESENT;
+            batteryConnectedTime = 0;
             batteryCellCount = 0;
             batteryWarningVoltage = 0;
             batteryCriticalVoltage = 0;

src/main/target/AETH743Basic/CMakeLists.txt

@@ -0,0 +1 @@
+target_stm32h743xi(AETH743Basic)

src/main/target/AETH743Basic/config.c

@@ -0,0 +1,36 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+
+#include "platform.h"
+
+#include "fc/fc_msp_box.h"
+#include "fc/config.h"
+
+#include "io/piniobox.h"
+#include "io/serial.h"
+
+
+void targetConfiguration(void)
+{
+    pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1;
+    pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2;
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART7)].functionMask = FUNCTION_MSP;
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART7)].msp_baudrateIndex = BAUD_115200;
+    beeperConfigMutable()->pwmMode = true;
+}

src/main/target/AETH743Basic/target.c

@@ -0,0 +1,53 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * INAV is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+
+#include "platform.h"
+
+#include "drivers/bus.h"
+#include "drivers/io.h"
+#include "drivers/pwm_mapping.h"
+#include "drivers/timer.h"
+#include "drivers/pinio.h"
+#include "drivers/sensor.h"
+
+BUSDEV_REGISTER_SPI_TAG(busdev_icm42688_0, DEVHW_ICM42605, ICM42688_0_SPI_BUS, ICM42688_0_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42688_0_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_icm42688_1, DEVHW_ICM42605, ICM42688_1_SPI_BUS, ICM42688_1_CS_PIN, NONE, 2, DEVFLAGS_NONE, IMU_ICM42688_1_ALIGN);
+
+timerHardware_t timerHardware[] = {
+    DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0),   // S1
+    DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 1),   // S2
+
+    DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 2),   // S3  
+    DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 3),   // S4
+    DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 4),   // S5
+    DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 5),   // S6
+
+    DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 6),   // S7
+    DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 7),   // S8
+    DEF_TIM(TIM4, CH3, PD14, TIM_USE_OUTPUT_AUTO, 0, 0),   // S9
+    DEF_TIM(TIM4, CH4, PD15, TIM_USE_OUTPUT_AUTO, 0, 0),   // S10 DMA_NONE
+
+    DEF_TIM(TIM15, CH1, PE5, TIM_USE_OUTPUT_AUTO, 0, 0),   // S11
+    DEF_TIM(TIM15, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0),   // S12 DMA_NONE
+
+    DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9),    // LED_2812
+    DEF_TIM(TIM2, CH1, PA15, TIM_USE_BEEPER, 0, 0),  // BEEPER PWM
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

src/main/target/AETH743Basic/target.h

@@ -0,0 +1,181 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * INAV is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "H743"
+
+#define USBD_PRODUCT_STRING     "AETH743Basic"
+
+#define USE_TARGET_CONFIG
+
+#define LED0                    PE3
+#define LED1                    PE4
+
+#define BEEPER                  PA15
+#define BEEPER_INVERTED
+#define BEEPER_PWM_FREQUENCY    2000
+
+// *************** IMU generic ***********************
+#define USE_DUAL_GYRO
+#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
+#define USE_SPI
+#define USE_IMU_ICM42605
+
+// *************** SPI1 IMU0 ICM42688 ****************
+#define USE_SPI_DEVICE_1
+#define SPI1_SCK_PIN            PA5
+#define SPI1_MISO_PIN           PA6
+#define SPI1_MOSI_PIN           PD7
+
+#define IMU_ICM42688_0_ALIGN       CW0_DEG_FLIP
+#define ICM42688_0_SPI_BUS         BUS_SPI1
+#define ICM42688_0_CS_PIN          PC15
+
+// *************** SPI4 IMU1 ICM42688 **************
+#define USE_SPI_DEVICE_4
+#define SPI4_SCK_PIN            PE12
+#define SPI4_MISO_PIN           PE13
+#define SPI4_MOSI_PIN           PE14
+
+#define IMU_ICM42688_1_ALIGN      CW90_DEG_FLIP
+#define ICM42688_1_SPI_BUS        BUS_SPI4
+#define ICM42688_1_CS_PIN         PC13
+
+// *************** SPI2 OSD ***********************
+#define USE_SPI_DEVICE_2
+#define SPI2_SCK_PIN            PB13
+#define SPI2_MISO_PIN           PB14
+#define SPI2_MOSI_PIN           PB15
+
+#define USE_MAX7456
+#define MAX7456_SPI_BUS         BUS_SPI2
+#define MAX7456_CS_PIN          PB12
+
+// *************** I2C /Baro/Mag *********************
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL                PB6
+#define I2C1_SDA                PB7
+
+#define USE_I2C_DEVICE_2
+#define I2C2_SCL                PB10
+#define I2C2_SDA                PB11
+
+#define USE_BARO
+#define BARO_I2C_BUS            BUS_I2C2
+#define USE_BARO_SPL06
+
+#define USE_MAG
+#define MAG_I2C_BUS             BUS_I2C1
+#define USE_MAG_ALL
+
+#define TEMPERATURE_I2C_BUS     BUS_I2C2
+#define PITOT_I2C_BUS           BUS_I2C2
+
+#define USE_RANGEFINDER
+#define RANGEFINDER_I2C_BUS     BUS_I2C1
+
+// *************** UART *****************************
+#define USE_VCP
+
+#define USE_UART1
+#define UART1_TX_PIN            PA9
+#define UART1_RX_PIN            PA10
+
+#define USE_UART2
+#define UART2_TX_PIN            PD5
+#define UART2_RX_PIN            PD6
+
+#define USE_UART3
+#define UART3_TX_PIN            PD8
+#define UART3_RX_PIN            PD9
+
+#define USE_UART4
+#define UART4_TX_PIN            PB9
+#define UART4_RX_PIN            PB8
+
+#define USE_UART6
+#define UART6_TX_PIN            PC6
+#define UART6_RX_PIN            PC7
+
+#define USE_UART7
+#define UART7_TX_PIN            PE8
+#define UART7_RX_PIN            PE7
+
+#define USE_UART8
+#define UART8_TX_PIN            PE1
+#define UART8_RX_PIN            PE0
+
+#define SERIAL_PORT_COUNT       8
+
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER       SERIALRX_CRSF
+#define SERIALRX_UART           SERIAL_PORT_USART6
+
+#define GPS_UART                SERIAL_PORT_USART2
+
+// *************** SDIO SD BLACKBOX*******************
+#define USE_SDCARD
+#define USE_SDCARD_SDIO
+#define SDCARD_SDIO_DEVICE      SDIODEV_1
+#define SDCARD_SDIO_4BIT
+
+#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
+
+// *************** ADC *****************************
+#define USE_ADC
+#define ADC_INSTANCE                ADC1
+
+#define ADC_CHANNEL_1_PIN           PC0  //ADC123 VBAT1
+#define ADC_CHANNEL_2_PIN           PC1  //ADC123 CURR1
+#define ADC_CHANNEL_3_PIN           PC5  //ADC12  RSSI
+#define ADC_CHANNEL_4_PIN           PC4  //ADC12  AirS
+#define ADC_CHANNEL_5_PIN           PA4  //ADC12  VB2
+#define ADC_CHANNEL_6_PIN           PA7  //ADC12  CU2
+
+#define VBAT_ADC_CHANNEL            ADC_CHN_1
+#define CURRENT_METER_ADC_CHANNEL   ADC_CHN_2
+#define RSSI_ADC_CHANNEL            ADC_CHN_3
+#define AIRSPEED_ADC_CHANNEL        ADC_CHN_4
+
+// *************** PINIO ***************************
+#define USE_PINIO
+#define USE_PINIOBOX
+#define PINIO1_PIN                  PD10  // 9Vsw
+#define PINIO2_PIN                  PD11  // Camera switcher
+
+// *************** LEDSTRIP ************************
+#define USE_LED_STRIP
+#define WS2811_PIN                  PA8
+
+#define DEFAULT_FEATURES            (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX)
+#define CURRENT_METER_SCALE         160
+
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+
+#define TARGET_IO_PORTA (0xffff & ~(BIT(14) | BIT(13)))
+#define TARGET_IO_PORTB 0xffff
+#define TARGET_IO_PORTC 0xffff
+#define TARGET_IO_PORTD 0xffff
+#define TARGET_IO_PORTE 0xffff
+
+#define MAX_PWM_OUTPUT_PORTS        15
+#define USE_DSHOT
+#define USE_ESC_SENSOR
+

src/main/target/GEPRC_TAKER_H743/CMakeLists.txt

@@ -0,0 +1 @@
+target_stm32h743xi(GEPRC_TAKER_H743)

src/main/target/GEPRC_TAKER_H743/config.c

@@ -0,0 +1,63 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * INAV is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <platform.h>
+
+#include "common/axis.h"
+
+#include "config/config_master.h"
+#include "config/feature.h"
+
+#include "drivers/sensor.h"
+#include "drivers/pwm_esc_detect.h"
+#include "drivers/pwm_output.h"
+#include "drivers/serial.h"
+
+#include "fc/rc_controls.h"
+
+#include "flight/failsafe.h"
+#include "flight/mixer.h"
+#include "flight/pid.h"
+
+#include "rx/rx.h"
+
+#include "io/serial.h"
+
+#include "sensors/battery.h"
+#include "sensors/sensors.h"
+
+#include "telemetry/telemetry.h"
+
+#include "fc/fc_msp_box.h"
+
+#include "io/piniobox.h"
+
+
+#define BLUETOOTH_MSP_BAUDRATE      BAUD_115200
+
+void targetConfiguration(void)
+{
+    pinioBoxConfigMutable()->permanentId[0] = BOXARM;
+    pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER1;
+
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART3)].functionMask = FUNCTION_MSP;
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART3)].msp_baudrateIndex = BLUETOOTH_MSP_BAUDRATE;
+}
+

src/main/target/GEPRC_TAKER_H743/target.c

@@ -0,0 +1,52 @@
+/*
+* This file is part of INAV Project.
+*
+* INAV is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* INAV is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <stdint.h>
+
+#include "platform.h"
+
+#include "drivers/bus.h"
+#include "drivers/io.h"
+#include "drivers/pwm_mapping.h"
+#include "drivers/timer.h"
+#include "drivers/sensor.h"
+
+
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000,  DEVHW_MPU6000,  IMU1_SPI_BUS,   IMU1_CS_PIN,   NONE,   0,  DEVFLAGS_NONE,  IMU1_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000_2,  DEVHW_MPU6000, IMU2_SPI_BUS,   IMU2_CS_PIN,   NONE,   1,  DEVFLAGS_NONE,  IMU2_ALIGN);
+
+BUSDEV_REGISTER_SPI_TAG(busdev_icm42688,  DEVHW_MPU6000,  IMU1_SPI_BUS,   IMU1_CS_PIN,   NONE,   0,  DEVFLAGS_NONE,  IMU1_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_icm42688_2, DEVHW_ICM42605, IMU2_SPI_BUS,   IMU2_CS_PIN,   NONE,   1,  DEVFLAGS_NONE,  IMU2_ALIGN);
+
+
+
+timerHardware_t timerHardware[] = {
+
+    DEF_TIM(TIM3, CH3, PB0,  TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM3, CH4, PB1,  TIM_USE_OUTPUT_AUTO, 0, 1),
+    DEF_TIM(TIM3, CH2, PB5,  TIM_USE_OUTPUT_AUTO, 0, 2),
+    DEF_TIM(TIM3, CH1, PB4,  TIM_USE_OUTPUT_AUTO, 0, 3),
+
+    DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 4),
+    DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 5),
+    DEF_TIM(TIM8, CH3, PC8,  TIM_USE_OUTPUT_AUTO, 0, 6),
+    DEF_TIM(TIM8, CH4, PC9,  TIM_USE_OUTPUT_AUTO, 0, 7),
+
+    DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED,                         0, 8),
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

src/main/target/GEPRC_TAKER_H743/target.h

@@ -0,0 +1,196 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * INAV is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "GEPR"
+#define USBD_PRODUCT_STRING     "GEPRC_TAKER_H743"
+
+#define USE_TARGET_CONFIG
+
+#define LED0                    PC13
+
+#define BEEPER                  PD2
+#define BEEPER_INVERTED
+
+
+//**************** SPI BUS *************************
+#define USE_SPI
+
+#define USE_SPI_DEVICE_1
+#define SPI1_SCK_PIN            PA5
+#define SPI1_MISO_PIN           PA6
+#define SPI1_MOSI_PIN           PA7
+
+#define USE_SPI_DEVICE_2
+#define SPI2_SCK_PIN            PB13
+#define SPI2_MISO_PIN           PB14
+#define SPI2_MOSI_PIN           PB15
+
+#define USE_SPI_DEVICE_3
+#define SPI3_SCK_PIN            PC10
+#define SPI3_MISO_PIN   	    PC11
+#define SPI3_MOSI_PIN   	    PC12
+
+#define USE_SPI_DEVICE_4
+#define SPI4_SCK_PIN            PE2
+#define SPI4_MISO_PIN           PE5
+#define SPI4_MOSI_PIN           PE6
+
+
+
+// *************** Gyro & ACC **********************
+
+#define USE_DUAL_GYRO
+#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
+#define USE_IMU_MPU6000
+#define USE_IMU_ICM42605
+
+
+// *****IMU1 MPU6000 & ICM42688 ON  SPI1 **************
+#define IMU1_ALIGN              CW180_DEG
+#define IMU1_SPI_BUS            BUS_SPI1
+#define IMU1_CS_PIN             PA4
+
+
+
+// *****IMU2 MPU6000 & ICM42688 ON  SPI2 **************
+#define IMU2_ALIGN              CW0_DEG
+#define IMU2_SPI_BUS            BUS_SPI2
+#define IMU2_CS_PIN             PB12
+
+
+// *************** I2C/Baro/Mag *********************
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL                PB8
+#define I2C1_SDA                PB9
+
+
+#define USE_BARO
+#define BARO_I2C_BUS            BUS_I2C1
+#define USE_BARO_BMP280
+#define USE_BARO_DPS310
+#define USE_BARO_MS5611
+
+#define USE_MAG
+#define MAG_I2C_BUS             BUS_I2C1
+#define USE_MAG_ALL
+
+#define TEMPERATURE_I2C_BUS     BUS_I2C1
+
+#define PITOT_I2C_BUS           BUS_I2C1
+
+#define USE_RANGEFINDER
+#define RANGEFINDER_I2C_BUS     BUS_I2C1
+#define BNO055_I2C_BUS          BUS_I2C1
+
+// *************** FLASH **************************
+#define USE_SDCARD
+#define USE_SDCARD_SPI
+#define SDCARD_SPI_BUS          BUS_SPI3
+#define SDCARD_CS_PIN           PA15
+
+#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
+
+
+// ************PINIO to disable BT*****************
+#define USE_PINIO
+#define USE_PINIOBOX
+#define PINIO1_PIN                  PE13  
+#define PINIO1_FLAGS                PINIO_FLAGS_INVERTED
+
+
+// ************PINIO to other
+#define PINIO2_PIN                  PC14               //Enable vsw
+
+
+// *************** OSD *****************************
+#define USE_MAX7456
+#define MAX7456_SPI_BUS         BUS_SPI4
+#define MAX7456_CS_PIN          PE4
+
+// *************** UART *****************************
+#define USE_VCP
+
+#define USE_UART1
+#define UART1_RX_PIN            PA10
+#define UART1_TX_PIN            PA9
+
+#define USE_UART2
+#define UART2_RX_PIN            PA3
+#define UART2_TX_PIN            PA2
+
+#define USE_UART3
+#define UART3_RX_PIN            PB11
+#define UART3_TX_PIN            PB10
+
+#define USE_UART4
+#define UART4_RX_PIN            PA1
+#define UART4_TX_PIN            PA0
+
+#define USE_UART6
+#define UART6_RX_PIN            PC7
+#define UART6_TX_PIN            PC6
+
+#define USE_UART7
+#define UART7_RX_PIN            PE7
+#define UART7_TX_PIN            PE8
+
+#define USE_UART8
+#define UART8_RX_PIN            PE0
+#define UART8_TX_PIN            PE1
+
+#define SERIAL_PORT_COUNT       8
+
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER       SERIALRX_SBUS
+#define SERIALRX_UART           SERIAL_PORT_USART2
+
+// *************** ADC *****************************
+#define USE_ADC
+#define ADC_INSTANCE                ADC3
+// #define ADC1_DMA_STREAM             DMA2_Stream0
+#define ADC_CHANNEL_1_PIN           PC3
+#define ADC_CHANNEL_2_PIN           PC5
+#define ADC_CHANNEL_3_PIN           PC2
+
+#define VBAT_ADC_CHANNEL            ADC_CHN_1
+#define RSSI_ADC_CHANNEL            ADC_CHN_2
+#define CURRENT_METER_ADC_CHANNEL   ADC_CHN_3
+
+#define VBAT_SCALE_DEFAULT              1120
+
+//****************************************************
+
+#define DEFAULT_FEATURES            (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX)
+
+#define USE_LED_STRIP
+#define WS2811_PIN                  PA8
+
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+
+#define TARGET_IO_PORTA 0xffff
+#define TARGET_IO_PORTB 0xffff
+#define TARGET_IO_PORTC 0xffff
+#define TARGET_IO_PORTD 0xffff
+#define TARGET_IO_PORTE 0xffff
+
+#define MAX_PWM_OUTPUT_PORTS        8
+
+#define USE_DSHOT
+#define USE_ESC_SENSOR

src/main/target/HGLRCF405V2/CMakeLists.txt

@@ -0,0 +1 @@
+target_stm32f405xg(HGLRCF405V2)

src/main/target/HGLRCF405V2/target.c

@@ -0,0 +1,39 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * INAV is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+#include <platform.h>
+
+#include "drivers/io.h"
+#include "drivers/timer.h"
+
+timerHardware_t timerHardware[] = {
+    DEF_TIM(TIM5, CH4, PA3,   TIM_USE_PPM        , 0, 1), // PPM IN
+
+    DEF_TIM(TIM8, CH1, PC6,   TIM_USE_OUTPUT_AUTO, 0, 1), // S1
+    DEF_TIM(TIM8, CH2, PC7,   TIM_USE_OUTPUT_AUTO, 0, 1), // S2
+    DEF_TIM(TIM8, CH3, PC8,   TIM_USE_OUTPUT_AUTO, 0, 1), // S3
+    DEF_TIM(TIM8, CH4, PC9,   TIM_USE_OUTPUT_AUTO, 0, 0), // S4
+    DEF_TIM(TIM2, CH1, PA15,  TIM_USE_OUTPUT_AUTO, 0, 0), // S5
+    DEF_TIM(TIM1, CH1, PA8,   TIM_USE_OUTPUT_AUTO, 0, 1), // S6
+    DEF_TIM(TIM2, CH3, PB10,  TIM_USE_OUTPUT_AUTO, 0, 0), // S7
+    DEF_TIM(TIM2, CH4, PB11,  TIM_USE_OUTPUT_AUTO, 0, 0), // S8
+
+    DEF_TIM(TIM3, CH4, PB1,  TIM_USE_LED, 0, 0), // LED_STRIP
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);