diff --git a/.github/workflows/cygwin_build.yml b/.github/workflows/cygwin_build.yml
index 0d601df2fa..3f5d35eb78 100644
--- a/.github/workflows/cygwin_build.yml
+++ b/.github/workflows/cygwin_build.yml
@@ -10,7 +10,7 @@ jobs:
     runs-on: 'windows-latest'
 
     steps:
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
         with:
           submodules: 'recursive'
       - uses: cygwin/cygwin-install-action@master
@@ -31,13 +31,13 @@ jobs:
 
       - name: Check build files
         id: check_files
-        uses: andstor/file-existence-action@v1
+        uses: andstor/file-existence-action@v2
         with:
           files: "artifacts/*"
-          allow_failure: true
+          fail: true
 
       - name: Archive build
-        uses: actions/upload-artifact@v2
+        uses: actions/upload-artifact@v3
         with:
            name: binaries
            path: artifacts
diff --git a/.github/workflows/macos_build.yml b/.github/workflows/macos_build.yml
index e142a796ac..6b648caa96 100644
--- a/.github/workflows/macos_build.yml
+++ b/.github/workflows/macos_build.yml
@@ -17,7 +17,7 @@ jobs:
         ]
 
     steps:
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
         with:
           submodules: 'recursive'
       - name: Install Prerequisites
@@ -33,9 +33,9 @@ jobs:
         id: ccache_cache_timestamp
         run: |
           NOW=$(date -u +"%F-%T")
-          echo "::set-output name=timestamp::${NOW}"
+          echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v2
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{matrix.config}}-${{steps.ccache_cache_timestamp.outputs.timestamp}}
diff --git a/.github/workflows/test_ccache.yml b/.github/workflows/test_ccache.yml
index dd5d970d5b..3f3ba047df 100644
--- a/.github/workflows/test_ccache.yml
+++ b/.github/workflows/test_ccache.yml
@@ -21,7 +21,7 @@ jobs:
         gcc: [10]
     steps:
       # git checkout the PR
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
         with:
           submodules: 'recursive'
       - name: ccache test
diff --git a/.github/workflows/test_chibios.yml b/.github/workflows/test_chibios.yml
index faf560472c..d6085ac5cb 100644
--- a/.github/workflows/test_chibios.yml
+++ b/.github/workflows/test_chibios.yml
@@ -59,7 +59,7 @@ jobs:
 
     steps:
       # git checkout the PR
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
         with:
           submodules: 'recursive'
       # Put ccache into github cache for faster build
@@ -67,9 +67,9 @@ jobs:
         id: ccache_cache_timestamp
         run: |
           NOW=$(date -u +"%F-%T")
-          echo "::set-output name=timestamp::${NOW}"
+          echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v2
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{matrix.config}}-${{ matrix.toolchain }}-${{ matrix.gcc }}-${{steps.ccache_cache_timestamp.outputs.timestamp}}
diff --git a/.github/workflows/test_coverage.yml b/.github/workflows/test_coverage.yml
index b700fa830e..ed9bc392ef 100644
--- a/.github/workflows/test_coverage.yml
+++ b/.github/workflows/test_coverage.yml
@@ -29,7 +29,7 @@ jobs:
         ]
     steps:
       # git checkout the PR
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
         with:
           submodules: 'recursive'
       # Put ccache into github cache for faster build
@@ -37,9 +37,9 @@ jobs:
         id: ccache_cache_timestamp
         run: |
           NOW=$(date -u +"%F-%T")
-          echo "::set-output name=timestamp::${NOW}"
+          echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v2
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}}
diff --git a/.github/workflows/test_environment.yml b/.github/workflows/test_environment.yml
index ee34b50aaf..8d7ba5c87d 100644
--- a/.github/workflows/test_environment.yml
+++ b/.github/workflows/test_environment.yml
@@ -62,7 +62,7 @@ jobs:
           esac
 
       # git checkout the PR
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
         with:
             submodules: 'recursive'
       - name: test install environment ${{matrix.os}}.${{matrix.name}}
diff --git a/.github/workflows/test_linux_sbc.yml b/.github/workflows/test_linux_sbc.yml
index e36a28fc3d..bb13d48dc7 100644
--- a/.github/workflows/test_linux_sbc.yml
+++ b/.github/workflows/test_linux_sbc.yml
@@ -43,7 +43,7 @@ jobs:
 
     steps:
       # git checkout the PR
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
         with:
           submodules: 'recursive'
       # Put ccache into github cache for faster build
@@ -51,9 +51,9 @@ jobs:
         id: ccache_cache_timestamp
         run: |
           NOW=$(date -u +"%F-%T")
-          echo "::set-output name=timestamp::${NOW}"
+          echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v2
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{matrix.config}}-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}}
diff --git a/.github/workflows/test_replay.yml b/.github/workflows/test_replay.yml
index 7ecae879f4..5923527b1b 100644
--- a/.github/workflows/test_replay.yml
+++ b/.github/workflows/test_replay.yml
@@ -24,7 +24,7 @@ jobs:
         ]
     steps:
       # git checkout the PR
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
         with:
           submodules: 'recursive'
       # Put ccache into github cache for faster build
@@ -32,9 +32,9 @@ jobs:
         id: ccache_cache_timestamp
         run: |
           NOW=$(date -u +"%F-%T")
-          echo "::set-output name=timestamp::${NOW}"
+          echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v2
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -55,7 +55,7 @@ jobs:
           Tools/scripts/build_ci.sh
 
       - name: Archive buildlog artifacts
-        uses: actions/upload-artifact@v2
+        uses: actions/upload-artifact@v3
         if: failure()
         with:
            name: fail-${{ matrix.toolchain }}-${{matrix.config}}
diff --git a/.github/workflows/test_scripting_docs.yml b/.github/workflows/test_scripting_docs.yml
new file mode 100644
index 0000000000..a281bc459e
--- /dev/null
+++ b/.github/workflows/test_scripting_docs.yml
@@ -0,0 +1,45 @@
+name: test scripting docs
+
+on:
+  push:
+    paths: # only run for scripting changes
+      - 'libraries/AP_Scripting/tests/docs_check.py'
+      - 'libraries/AP_Scripting/generator/**'
+
+  pull_request:
+    paths: # only run for scripting changes
+      - 'libraries/AP_Scripting/tests/docs_check.py'
+      - 'libraries/AP_Scripting/generator/**'
+
+  workflow_dispatch:
+
+concurrency:
+  group: ci-${{github.workflow}}-${{ github.ref }}
+  cancel-in-progress: true
+
+jobs:
+  test-scripting-docs:
+    runs-on: ubuntu-20.04
+    container: ardupilot/ardupilot-dev-base:latest
+    steps:
+      # git checkout the PR
+      - uses: actions/checkout@v3
+        with:
+          submodules: 'recursive'
+
+      - name: copy docs
+        run: |
+          PATH="/github/home/.local/bin:$PATH"
+          mv "libraries/AP_Scripting/docs/docs.lua" "libraries/AP_Scripting/docs/current_docs.lua"
+
+      - name: build sitl # we don't really need to build the full code, just trigger docs re-gen with --scripting-docs, timeout after 10 seconds
+        shell: bash
+        run: |
+          PATH="/github/home/.local/bin:$PATH"
+          ./waf configure --board sitl
+          timeout 10 ./waf antennatracker --scripting-docs || code=$?; if [[ $code -ne 124 && $code -ne 0 ]]; then exit $code; fi
+
+      - name: run compare
+        run: |
+          PATH="/github/home/.local/bin:$PATH"
+          python ./libraries/AP_Scripting/tests/docs_check.py "./libraries/AP_Scripting/docs/docs.lua" "./libraries/AP_Scripting/docs/current_docs.lua"
diff --git a/.github/workflows/test_sitl_copter.yml b/.github/workflows/test_sitl_copter.yml
index 28c72a64f0..271cb375d0 100644
--- a/.github/workflows/test_sitl_copter.yml
+++ b/.github/workflows/test_sitl_copter.yml
@@ -21,7 +21,7 @@ jobs:
         ]
     steps:
       # git checkout the PR
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
         with:
           submodules: 'recursive'
       # Put ccache into github cache for faster build
@@ -29,9 +29,9 @@ jobs:
         id: ccache_cache_timestamp
         run: |
           NOW=$(date -u +"%F-%T")
-          echo "::set-output name=timestamp::${NOW}"
+          echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v2
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -73,7 +73,7 @@ jobs:
 
     steps:
       # git checkout the PR
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
         with:
           submodules: 'recursive'
       # Put ccache into github cache for faster build
@@ -81,9 +81,9 @@ jobs:
         id: ccache_cache_timestamp
         run: |
           NOW=$(date -u +"%F-%T")
-          echo "::set-output name=timestamp::${NOW}"
+          echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v2
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -100,7 +100,7 @@ jobs:
           Tools/scripts/build_ci.sh
 
       - name: Archive buildlog artifacts
-        uses: actions/upload-artifact@v2
+        uses: actions/upload-artifact@v3
         if: failure()
         with:
            name: fail-${{matrix.config}}
@@ -114,7 +114,7 @@ jobs:
            retention-days: 14
 
       - name: Archive .bin artifacts
-        uses: actions/upload-artifact@v2
+        uses: actions/upload-artifact@v3
         with:
            name: BIN-${{matrix.config}}
            path: /__w/ardupilot/ardupilot/logs
@@ -127,7 +127,7 @@ jobs:
       options: --privileged --cap-add=SYS_PTRACE --security-opt apparmor=unconfined --security-opt seccomp=unconfined
     steps:
       # git checkout the PR
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
         with:
           submodules: 'recursive'
       # Put ccache into github cache for faster build
@@ -135,9 +135,9 @@ jobs:
         id: ccache_cache_timestamp
         run: |
           NOW=$(date -u +"%F-%T")
-          echo "::set-output name=timestamp::${NOW}"
+          echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v2
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -167,7 +167,7 @@ jobs:
 
     steps:
       # git checkout the PR
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
         with:
           submodules: 'recursive'
       # Put ccache into github cache for faster build
@@ -175,9 +175,9 @@ jobs:
         id: ccache_cache_timestamp
         run: |
           NOW=$(date -u +"%F-%T")
-          echo "::set-output name=timestamp::${NOW}"
+          echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v2
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -194,7 +194,7 @@ jobs:
           Tools/scripts/build_ci.sh
 
       - name: Archive buildlog artifacts
-        uses: actions/upload-artifact@v2
+        uses: actions/upload-artifact@v3
         if: failure()
         with:
            name: fail-${{matrix.config}}
@@ -208,7 +208,7 @@ jobs:
            retention-days: 14
 
       - name: Archive .bin artifacts
-        uses: actions/upload-artifact@v2
+        uses: actions/upload-artifact@v3
         with:
            name: BIN-${{matrix.config}}
            path: /__w/ardupilot/ardupilot/logs
diff --git a/.github/workflows/test_sitl_periph.yml b/.github/workflows/test_sitl_periph.yml
index 8a517adf87..b236cfcc28 100644
--- a/.github/workflows/test_sitl_periph.yml
+++ b/.github/workflows/test_sitl_periph.yml
@@ -14,7 +14,7 @@ jobs:
     container: ardupilot/ardupilot-dev-base:latest
     steps:
       # git checkout the PR
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
         with:
           submodules: 'recursive'
       # Put ccache into github cache for faster build
@@ -22,9 +22,9 @@ jobs:
         id: ccache_cache_timestamp
         run: |
           NOW=$(date -u +"%F-%T")
-          echo "::set-output name=timestamp::${NOW}"
+          echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v2
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -67,7 +67,7 @@ jobs:
 
     steps:
       # git checkout the PR
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
         with:
           submodules: 'recursive'
       # Put ccache into github cache for faster build
@@ -75,9 +75,9 @@ jobs:
         id: ccache_cache_timestamp
         run: |
           NOW=$(date -u +"%F-%T")
-          echo "::set-output name=timestamp::${NOW}"
+          echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v2
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}}
diff --git a/.github/workflows/test_sitl_plane.yml b/.github/workflows/test_sitl_plane.yml
index 3640bc4e90..e1903669b8 100644
--- a/.github/workflows/test_sitl_plane.yml
+++ b/.github/workflows/test_sitl_plane.yml
@@ -21,7 +21,7 @@ jobs:
         ]
     steps:
       # git checkout the PR
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
         with:
           submodules: 'recursive'
       # Put ccache into github cache for faster build
@@ -29,9 +29,9 @@ jobs:
         id: ccache_cache_timestamp
         run: |
           NOW=$(date -u +"%F-%T")
-          echo "::set-output name=timestamp::${NOW}"
+          echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v2
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -68,7 +68,7 @@ jobs:
 
     steps:
       # git checkout the PR
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
         with:
           submodules: 'recursive'
       # Put ccache into github cache for faster build
@@ -76,9 +76,9 @@ jobs:
         id: ccache_cache_timestamp
         run: |
           NOW=$(date -u +"%F-%T")
-          echo "::set-output name=timestamp::${NOW}"
+          echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v2
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -95,7 +95,7 @@ jobs:
           Tools/scripts/build_ci.sh
 
       - name: Archive buildlog artifacts
-        uses: actions/upload-artifact@v2
+        uses: actions/upload-artifact@v3
         if: failure()
         with:
            name: fail-${{matrix.config}}
@@ -109,7 +109,7 @@ jobs:
            retention-days: 14
 
       - name: Archive .bin artifacts
-        uses: actions/upload-artifact@v2
+        uses: actions/upload-artifact@v3
         with:
            name: BIN-${{matrix.config}}
            path: /__w/ardupilot/ardupilot/logs
diff --git a/.github/workflows/test_sitl_rover.yml b/.github/workflows/test_sitl_rover.yml
index f14ffcf919..98751a4c6a 100644
--- a/.github/workflows/test_sitl_rover.yml
+++ b/.github/workflows/test_sitl_rover.yml
@@ -21,7 +21,7 @@ jobs:
         ]
     steps:
       # git checkout the PR
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
         with:
           submodules: 'recursive'
       # Put ccache into github cache for faster build
@@ -29,9 +29,9 @@ jobs:
         id: ccache_cache_timestamp
         run: |
           NOW=$(date -u +"%F-%T")
-          echo "::set-output name=timestamp::${NOW}"
+          echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v2
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -69,7 +69,7 @@ jobs:
 
     steps:
       # git checkout the PR
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
         with:
           submodules: 'recursive'
       # Put ccache into github cache for faster build
@@ -77,9 +77,9 @@ jobs:
         id: ccache_cache_timestamp
         run: |
           NOW=$(date -u +"%F-%T")
-          echo "::set-output name=timestamp::${NOW}"
+          echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v2
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -96,7 +96,7 @@ jobs:
           Tools/scripts/build_ci.sh
 
       - name: Archive buildlog artifacts
-        uses: actions/upload-artifact@v2
+        uses: actions/upload-artifact@v3
         if: failure()
         with:
            name: fail-${{matrix.config}}
@@ -110,7 +110,7 @@ jobs:
            retention-days: 14
 
       - name: Archive .bin artifacts
-        uses: actions/upload-artifact@v2
+        uses: actions/upload-artifact@v3
         with:
            name: BIN-${{matrix.config}}
            path: /__w/ardupilot/ardupilot/logs
diff --git a/.github/workflows/test_sitl_sub.yml b/.github/workflows/test_sitl_sub.yml
index 39c25c28b5..c5685ce3e0 100644
--- a/.github/workflows/test_sitl_sub.yml
+++ b/.github/workflows/test_sitl_sub.yml
@@ -21,7 +21,7 @@ jobs:
         ]
     steps:
       # git checkout the PR
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
         with:
           submodules: 'recursive'
       # Put ccache into github cache for faster build
@@ -29,9 +29,9 @@ jobs:
         id: ccache_cache_timestamp
         run: |
           NOW=$(date -u +"%F-%T")
-          echo "::set-output name=timestamp::${NOW}"
+          echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v2
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -67,7 +67,7 @@ jobs:
 
     steps:
       # git checkout the PR
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
         with:
           submodules: 'recursive'
       # Put ccache into github cache for faster build
@@ -75,9 +75,9 @@ jobs:
         id: ccache_cache_timestamp
         run: |
           NOW=$(date -u +"%F-%T")
-          echo "::set-output name=timestamp::${NOW}"
+          echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v2
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -94,7 +94,7 @@ jobs:
           Tools/scripts/build_ci.sh
 
       - name: Archive buildlog artifacts
-        uses: actions/upload-artifact@v2
+        uses: actions/upload-artifact@v3
         if: failure()
         with:
            name: fail-${{matrix.config}}
@@ -108,7 +108,7 @@ jobs:
            retention-days: 14
 
       - name: Archive .bin artifacts
-        uses: actions/upload-artifact@v2
+        uses: actions/upload-artifact@v3
         with:
            name: BIN-${{matrix.config}}
            path: /__w/ardupilot/ardupilot/logs
diff --git a/.github/workflows/test_sitl_tracker.yml b/.github/workflows/test_sitl_tracker.yml
index 8c6abec961..3ded077b4d 100644
--- a/.github/workflows/test_sitl_tracker.yml
+++ b/.github/workflows/test_sitl_tracker.yml
@@ -21,7 +21,7 @@ jobs:
         ]
     steps:
       # git checkout the PR
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
         with:
           submodules: 'recursive'
       # Put ccache into github cache for faster build
@@ -29,9 +29,9 @@ jobs:
         id: ccache_cache_timestamp
         run: |
           NOW=$(date -u +"%F-%T")
-          echo "::set-output name=timestamp::${NOW}"
+          echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v2
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -67,7 +67,7 @@ jobs:
 
     steps:
       # git checkout the PR
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
         with:
           submodules: 'recursive'
       # Put ccache into github cache for faster build
@@ -75,9 +75,9 @@ jobs:
         id: ccache_cache_timestamp
         run: |
           NOW=$(date -u +"%F-%T")
-          echo "::set-output name=timestamp::${NOW}"
+          echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v2
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -94,7 +94,7 @@ jobs:
           Tools/scripts/build_ci.sh
 
       - name: Archive buildlog artifacts
-        uses: actions/upload-artifact@v2
+        uses: actions/upload-artifact@v3
         if: failure()
         with:
            name: fail-${{matrix.config}}
@@ -108,7 +108,7 @@ jobs:
            retention-days: 14
 
       - name: Archive .bin artifacts
-        uses: actions/upload-artifact@v2
+        uses: actions/upload-artifact@v3
         with:
            name: BIN-${{matrix.config}}
            path: /__w/ardupilot/ardupilot/logs
diff --git a/.github/workflows/test_size.yml b/.github/workflows/test_size.yml
index ed38175a1f..e576606291 100644
--- a/.github/workflows/test_size.yml
+++ b/.github/workflows/test_size.yml
@@ -26,7 +26,7 @@ jobs:
             f103-GPS  # see special code for Periph below (3 places!)
         ]
     steps:
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
         with:
           ref: ${{ github.event.pull_request.base.ref }}
           path: base_branch
@@ -36,13 +36,16 @@ jobs:
         id: ccache_cache_timestamp
         run: |
           NOW=$(date -u +"%F-%T")
-          echo "::set-output name=timestamp::${NOW}"
+          echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v2
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}}
           restore-keys: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-  # restore ccache from either previous build on this branch or on base branch
+      - name: setup ccache
+        run: |
+          . base_branch/.github/workflows/ccache.env
       - name: Build ${{ github.event.pull_request.base.ref }} ${{matrix.config}} ${{ matrix.toolchain }}
         env:
           CI_BUILD_TARGET: ${{matrix.config}}
@@ -77,7 +80,7 @@ jobs:
 
           echo [`date`] Built ${{ github.event.pull_request.base.ref }} with no versions
 
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
         with:
           fetch-depth: 0
           path: 'pr'
@@ -163,7 +166,7 @@ jobs:
           zip -r elf_diff.zip elf_diff
 
       - name: Archive elf_diff output
-        uses: actions/upload-artifact@v2
+        uses: actions/upload-artifact@v3
         with:
            name: ELF_DIFF_${{matrix.config}}
            path: elf_diff.zip
diff --git a/.github/workflows/test_unit_tests.yml b/.github/workflows/test_unit_tests.yml
index eba8083a9d..0dad924a80 100644
--- a/.github/workflows/test_unit_tests.yml
+++ b/.github/workflows/test_unit_tests.yml
@@ -29,7 +29,7 @@ jobs:
         ]
     steps:
       # git checkout the PR
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
         with:
           submodules: 'recursive'
       # Put ccache into github cache for faster build
@@ -37,9 +37,9 @@ jobs:
         id: ccache_cache_timestamp
         run: |
           NOW=$(date -u +"%F-%T")
-          echo "::set-output name=timestamp::${NOW}"
+          echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v2
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -60,7 +60,7 @@ jobs:
           Tools/scripts/build_ci.sh
 
       - name: Archive buildlog artifacts
-        uses: actions/upload-artifact@v2
+        uses: actions/upload-artifact@v3
         if: failure()
         with:
            name: fail-${{ matrix.toolchain }}-${{matrix.config}}
diff --git a/ArduPlane/Ottano/eeprom-base.bin b/ArduPlane/Ottano/eeprom-base.bin
new file mode 100644
index 0000000000..0f3ef9b007
Binary files /dev/null and b/ArduPlane/Ottano/eeprom-base.bin differ
diff --git a/ArduPlane/Ottano/go.sh b/ArduPlane/Ottano/go.sh
new file mode 100755
index 0000000000..bbdddf097a
--- /dev/null
+++ b/ArduPlane/Ottano/go.sh
@@ -0,0 +1,8 @@
+#!/bin/bash
+
+[ -f eeprom.bin ] || {
+    echo "Creating eeprom.bin"
+    cp eeprom-base.bin eeprom.bin
+}
+
+nice ../../Tools/autotest/sim_vehicle.py -D -G -f quadplane-ice --console --map -L SpringValley3 --aircraft test "$*" -A --uartF=sim:megasquirt
diff --git a/ArduPlane/Ottano/test/mavinit.scr b/ArduPlane/Ottano/test/mavinit.scr
new file mode 100644
index 0000000000..dd42b4a07c
--- /dev/null
+++ b/ArduPlane/Ottano/test/mavinit.scr
@@ -0,0 +1,5 @@
+console add RPM "RPM: %.0f" RPM.rpm1
+console add vrpm "VRPM: %.0f %.0f %.0f %.0f" ESC_TELEMETRY_1_TO_4.rpm[0],ESC_TELEMETRY_1_TO_4.rpm[1],ESC_TELEMETRY_1_TO_4.rpm[2],ESC_TELEMETRY_1_TO_4.rpm[3]
+console add CHead "CHead: %.0f %.0f" EFI_STATUS.cylinder_head_temperature,EFI_STATUS.barometric_pressure
+console add Ign "Ign: %.0f" RC_CHANNELS.chan7_raw
+console add Aspd "Aspd %.1f %.1f" VFR_HUD.airspeed,NAMED_VALUE_FLOAT["AS2"].value 5
diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index ebecaaff3d..2b050518e6 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -2364,7 +2364,7 @@ void QuadPlane::vtol_position_controller(void)
         const float aspeed_threshold = MAX(plane.aparm.airspeed_min-2, assist_speed);
 
         // run fixed wing navigation
-        plane.nav_controller->update_waypoint(plane.current_loc, loc);
+        plane.nav_controller->update_waypoint(plane.auto_state.crosstrack ? plane.prev_WP_loc : plane.current_loc, loc);
 
         // use TECS for throttle
         SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.TECS_controller.get_throttle_demand());
@@ -2431,10 +2431,11 @@ void QuadPlane::vtol_position_controller(void)
         const uint32_t min_airbrake_ms = 1000;
         if (poscontrol.get_state() == QPOS_AIRBRAKE &&
             poscontrol.time_since_state_start_ms() > min_airbrake_ms &&
-            (aspeed < aspeed_threshold ||
-             fabsf(degrees(closing_vel.angle(desired_closing_vel))) > 60 ||
-             closing_speed > MAX(desired_closing_speed*1.2, desired_closing_speed+2) ||
-             labs(plane.ahrs.roll_sensor - plane.nav_roll_cd) > attitude_error_threshold_cd ||
+            (aspeed < aspeed_threshold || // too low airspeed
+             fabsf(degrees(closing_vel.angle(desired_closing_vel))) > 60 || // wrong direction
+             closing_speed > MAX(desired_closing_speed*1.2, desired_closing_speed+2) || // too fast
+             closing_speed < desired_closing_speed*0.5 || // too slow ground speed
+             labs(plane.ahrs.roll_sensor - plane.nav_roll_cd) > attitude_error_threshold_cd || // bad attitude
              labs(plane.ahrs.pitch_sensor - plane.nav_pitch_cd) > attitude_error_threshold_cd)) {
             gcs().send_text(MAV_SEVERITY_INFO,"VTOL position1 v=%.1f d=%.1f h=%.1f dc=%.1f",
                             (double)groundspeed,
@@ -2446,6 +2447,18 @@ void QuadPlane::vtol_position_controller(void)
 
             // switch to vfwd for throttle control
             vel_forward.integrator = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
+
+            // adjust the initial forward throttle based on our desired and actual closing speed
+            // this allows for significant initial forward throttle
+            // when we have a strong headwind, but low throttle in the usual case where
+            // we want to slow down ready for POSITION2
+            vel_forward.integrator = linear_interpolate(0, vel_forward.integrator,
+                                                        closing_speed,
+                                                        1.2*desired_closing_speed, 0.5*desired_closing_speed);
+
+            // limit our initial forward throttle in POSITION1 to be 0.5 of cruise throttle
+            vel_forward.integrator = constrain_float(vel_forward.integrator, 0, plane.aparm.throttle_cruise*0.5);
+            
             vel_forward.last_ms = now_ms;
         }
 
@@ -3144,7 +3157,8 @@ bool QuadPlane::do_vtol_land(const AP_Mission::Mission_Command& cmd)
     plane.crash_state.is_crashed = false;
     
     // also update nav_controller for status output
-    plane.nav_controller->update_waypoint(plane.current_loc, plane.next_WP_loc);
+    plane.nav_controller->update_waypoint(plane.auto_state.crosstrack ? plane.prev_WP_loc : plane.current_loc,
+                                          plane.next_WP_loc);
 
     poscontrol_init_approach();
     return true;
@@ -3988,6 +4002,10 @@ Vector2f QuadPlane::landing_desired_closing_velocity()
     float tecs_land_airspeed = plane.TECS_controller.get_land_airspeed();
     if (is_positive(tecs_land_airspeed)) {
         land_speed = tecs_land_airspeed;
+    } else {
+        // use half way between min airspeed and cruise if
+        // TECS_LAND_AIRSPEED not set
+        land_speed = 0.5*(land_speed+plane.aparm.airspeed_min);
     }
     target_speed = MIN(target_speed, eas2tas * land_speed);
 
@@ -4029,12 +4047,16 @@ float QuadPlane::get_land_airspeed(void)
         return approach_speed;
     }
 
+    if (qstate == QPOS_AIRBRAKE) {
+        // during airbraking ask TECS to slow us to stall speed
+        return plane.aparm.airspeed_min;
+    }
+    
     // calculate speed based on landing desired velocity
     Vector2f vel = landing_desired_closing_velocity();
-    const Vector3f wind = plane.ahrs.wind_estimate();
+    const Vector2f wind = plane.ahrs.wind_estimate().xy();
     const float eas2tas = plane.ahrs.get_EAS2TAS();
-    vel.x -= wind.x;
-    vel.y -= wind.y;
+    vel -= wind;
     vel /= eas2tas;
     return vel.length();
 }
diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt
index 87864143e1..b31562f5b8 100644
--- a/Tools/AP_Bootloader/board_types.txt
+++ b/Tools/AP_Bootloader/board_types.txt
@@ -194,3 +194,15 @@ AP_HW_SWBOOMBOARD_PERIPH             1403
 AP_HW_CubeOrange_ODID               10140
 AP_HW_Pixhawk6_ODID                 10053
 
+
+# IDs 5000-5100 reserved for Carbonix
+AP_HW_CarbonixCubeOrange             5000
+AP_HW_CarbonixF405_M1                5001
+AP_HW_CarbonixF405_M2                5002
+AP_HW_CarbonixF405_M3                5003
+AP_HW_CarbonixF405_M4                5004
+AP_HW_CarbonixF405_M5                5005
+AP_HW_CarbonixF405_LWing             5006
+AP_HW_CarbonixF405_RWing             5007
+AP_HW_CarbonixF405_LTail             5008
+AP_HW_CarbonixF405_RTail             5009
\ No newline at end of file
diff --git a/Tools/AP_Periph/AP_ESC_APDHVPro.cpp b/Tools/AP_Periph/AP_ESC_APDHVPro.cpp
new file mode 100644
index 0000000000..709673d1ed
--- /dev/null
+++ b/Tools/AP_Periph/AP_ESC_APDHVPro.cpp
@@ -0,0 +1,130 @@
+
+#include "AP_Periph.h"
+#include "AP_ESC_APDHVPro.h"
+
+#ifdef HAL_PERIPH_ENABLE_ESC_APDHVPRO200
+
+extern const AP_HAL::HAL &hal;
+
+AP_ESC_APDHVPro::AP_ESC_APDHVPro(void){
+    
+}
+
+void AP_ESC_APDHVPro::init(AP_HAL::UARTDriver *_uart) {
+
+    port = _uart;
+    port->begin(115200, 128, 256);
+}
+
+bool AP_ESC_APDHVPro::update() {
+    bool status = false;
+
+    if (!port) {
+        return status;
+    }
+
+    uint32_t size = port->available();
+    if (size >= ESC_PACKET_SIZE) {
+
+        if (size > ESC_PACKET_SIZE * 2) {
+            port->discard_input();
+        }
+        else {
+            int bytes_read = port->read(max_buffer, size);
+
+            if (read_ESC_telemetry_data(bytes_read)) {
+                status = parse_ESC_telemetry_data();
+                if (status) {
+                    last_read_ms = AP_HAL::native_millis();
+                } 
+            }
+        }
+    }
+
+    return status;
+}
+
+bool AP_ESC_APDHVPro::read_ESC_telemetry_data(uint32_t bytes_read) {
+    bool status = false;
+
+    if (bytes_read < ESC_PACKET_SIZE)
+        return status;
+
+    for (int i = 0; i < ESC_PACKET_SIZE; i++) {
+        raw_buffer[i] = max_buffer[i];
+    }
+
+    status = true;
+
+    return status;
+}
+
+
+bool AP_ESC_APDHVPro::parse_ESC_telemetry_data() {
+    bool status = false;
+    float temperature = 0.0;
+
+    int check_fletch = check_flectcher16();
+    decoded.checksum = (uint16_t)((raw_buffer[19] << 8) | raw_buffer[18]);
+
+    // Currently, checksum code is not mandated
+    if (check_fletch == (int)decoded.checksum) {
+        status = true;
+
+        decoded.voltage = (float)(((((raw_buffer[1] << 8) | raw_buffer[0]) / 100.0) - HVPRO_CALIB_INTCPT) / HVPRO_CALIB_SLOPE);
+
+        float temp = (float)((raw_buffer[3] << 8) | raw_buffer[2]);
+        if (temp > 0)
+        {
+            float rntc = (TEMPERATURE_MAX_RESOLUTION / temp) - 1;
+
+            rntc = SERIESRESISTOR / rntc;
+
+            temperature = rntc / (float)NOMINAL_RESISTANCE;
+            temperature = (float)logF(temperature);
+            temperature /= BCOEFFICIENT;
+
+            temperature += (float)1.0 / ((float)NOMINAL_TEMPERATURE + (float)273.15);
+            temperature = (temperature > 0) ? (float)1.0 / temperature : 0;
+            temperature -= (float)273.15;
+        }
+
+        // filter bad values
+        if (temperature < 0 || temperature > 200) {
+            temperature = 0;
+        }
+
+        decoded.temperature = ((float)trunc(temperature * 100) / 100) + 273.15; // Temperature
+
+        float temp_current = (short)((raw_buffer[5] << 8) | raw_buffer[4]);
+        temp_current /= CURRENT_COEFFICIENT;
+        decoded.bus_current = (temp_current);
+        decoded.reserved1 = (uint16_t)((raw_buffer[7] << 8) | raw_buffer[6]);
+        decoded.rpm = ((uint32_t)((raw_buffer[11] << 24) | (raw_buffer[10] << 16) | (raw_buffer[9] << 8) | raw_buffer[8])) / POLEPAIRS;
+        decoded.input_duty = (uint16_t)((raw_buffer[13] << 8) | raw_buffer[12]) / 10;
+        decoded.motor_duty = (uint16_t)((raw_buffer[15] << 8) | raw_buffer[14]) / 10;
+        decoded.status = raw_buffer[16];
+        decoded.reserved2 = raw_buffer[17];
+    }
+
+    return status;
+}
+
+
+int AP_ESC_APDHVPro::check_flectcher16() {
+    int fCCRC16;
+    int i;
+    int c0 = 0;
+    int c1 = 0;
+
+    // Calculate checksum intermediate bytesUInt16
+    for (i = 0; i < 18; i++) { //Check only first 18 bytes, skip crc bytes
+        c0 = (int)(c0 + ((int)raw_buffer[i])) % 255;
+        c1 = (int)(c1 + c0) % 255;
+    }
+    // Assemble the 16-bit checksum value
+    fCCRC16 = ( c1 << 8 ) | c0;
+    return (int)fCCRC16;
+}
+
+#endif // HAL_PERIPH_ENABLE_ESC_APDHVPRO200
\ No newline at end of file
diff --git a/Tools/AP_Periph/AP_ESC_APDHVPro.h b/Tools/AP_Periph/AP_ESC_APDHVPro.h
new file mode 100644
index 0000000000..24b6db36d5
--- /dev/null
+++ b/Tools/AP_Periph/AP_ESC_APDHVPro.h
@@ -0,0 +1,69 @@
+
+#pragma once
+
+// #include "AP_ESC.h"
+#include <AP_HAL/AP_HAL.h>
+
+#ifdef HAL_PERIPH_ENABLE_ESC_APDHVPRO200
+
+#define SINGLE_LOOP_MAX_BYTES 500
+#define BUFFER_LOOP_SIZE 44
+#define ESC_PACKET_SIZE 22
+
+#define POLECOUNT 42
+#define POLEPAIRS (POLECOUNT / 2)
+#define CURRENT_COEFFICIENT 12.5
+
+#define SERIESRESISTOR 10000
+#define NOMINAL_RESISTANCE 10000
+#define NOMINAL_TEMPERATURE 25
+#define BCOEFFICIENT 3455
+
+#define TEMPERATURE_MAX_RESOLUTION 4096
+
+#define HVPRO_CALIB_SLOPE 0.9309
+#define HVPRO_CALIB_INTCPT 2.2297
+
+typedef struct {
+    float voltage;
+    float temperature;
+    float bus_current;
+    uint16_t reserved1;
+    uint32_t rpm;
+    uint16_t input_duty;
+    uint16_t motor_duty;
+    uint8_t  status;
+    uint8_t  reserved2;
+    uint16_t checksum;
+
+}APD_struct;
+
+
+class AP_ESC_APDHVPro {
+public:
+    // constructor
+    AP_ESC_APDHVPro();
+
+    void init(AP_HAL::UARTDriver *uart);
+
+    // update the structure
+    bool update();
+
+    APD_struct decoded;
+
+private:
+    // AP_ESC *_frontend;
+
+    AP_HAL::UARTDriver *port;
+
+    uint32_t last_read_ms;
+
+    uint8_t max_buffer[SINGLE_LOOP_MAX_BYTES];
+    uint8_t raw_buffer[ESC_PACKET_SIZE];
+
+    bool read_ESC_telemetry_data(uint32_t);
+    bool parse_ESC_telemetry_data();
+    int check_flectcher16();
+};
+
+#endif // HAL_PERIPH_ENABLE_ESC_APDHVPRO200
\ No newline at end of file
diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp
index 4d228cdab3..a09b4d846c 100644
--- a/Tools/AP_Periph/AP_Periph.cpp
+++ b/Tools/AP_Periph/AP_Periph.cpp
@@ -24,6 +24,7 @@
 #include <AP_HAL/AP_HAL_Boards.h>
 #include "AP_Periph.h"
 #include <stdio.h>
+#include <dronecan_msgs.h>
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
 #include <AP_HAL_ChibiOS/hwdef/common/stm32_util.h>
@@ -222,6 +223,10 @@ void AP_Periph_FW::init()
     hwesc_telem.init(hal.serial(3));
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_ESC_APDHVPRO200
+    APD_ESC_Telem.init(hal.serial(1));
+#endif
+
 #ifdef HAL_PERIPH_ENABLE_MSP
     if (g.msp_port >= 0) {
         msp_init(hal.serial(g.msp_port));
@@ -235,6 +240,11 @@ void AP_Periph_FW::init()
 #if AP_SCRIPTING_ENABLED
     scripting.init();
 #endif
+    // custom code carbonix
+    char *ap_periph_version;
+    asprintf(&ap_periph_version, "CPN Start (%s)", THISFIRMWARE);
+    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s", ap_periph_version);
+    ap_periph_version = nullptr;
     start_ms = AP_HAL::native_millis();
 }
 
@@ -386,7 +396,19 @@ void AP_Periph_FW::update()
         last_error_ms = now;
         can_printf("IERR 0x%x %u", unsigned(ierr.errors()), unsigned(ierr.last_error_line()));
     }
-
+//this will only monitor Arming signal
+#if HAL_PERIPH_ARM_MONITORING_ENABLE
+    static uint32_t last_arm_check_ms;
+    if (now - last_arm_check_ms > g.disarm_delay){
+        last_arm_check_ms = now;
+        if(periph.arm_update_status){    
+            periph.arm_update_status = false;
+        }
+        else{
+            hal.util->set_soft_armed(UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_STATUS_DISARMED);
+        }
+    }
+#endif
 #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && CH_DBG_ENABLE_STACK_CHECK == TRUE
     static uint32_t last_debug_ms;
     if ((g.debug&(1<<DEBUG_SHOW_STACK)) && now - last_debug_ms > 5000) {
diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h
index 85f356659d..3504bc2daf 100644
--- a/Tools/AP_Periph/AP_Periph.h
+++ b/Tools/AP_Periph/AP_Periph.h
@@ -21,6 +21,8 @@
 #include <AP_Scripting/AP_Scripting.h>
 #include <AP_HAL/CANIface.h>
 #include <AP_Stats/AP_Stats.h>
+#include "version.h"
+#include "AP_ESC_APDHVPro.h"
 
 #if HAL_GCS_ENABLED
 #include "GCS_MAVLink.h"
@@ -206,6 +208,11 @@ class AP_Periph_FW {
     uint32_t efi_update_ms;
 #endif
     
+#ifdef HAL_PERIPH_ENABLE_ESC_APDHVPRO200
+    AP_ESC_APDHVPro APD_ESC_Telem;
+    void APD_ESC_Telem_update();
+#endif
+
 #ifdef HAL_PERIPH_ENABLE_RC_OUT
 #if HAL_WITH_ESC_TELEM
     AP_ESC_Telem esc_telem;
@@ -216,6 +223,9 @@ class AP_Periph_FW {
     SRV_Channels servo_channels;
     bool rcout_has_new_data_to_update;
 
+    uint32_t last_esc_raw_command_ms;
+    uint8_t  last_esc_num_channels;
+
     void rcout_init();
     void rcout_init_1Hz();
     void rcout_esc(int16_t *rc, uint8_t num_channels);
@@ -256,6 +266,9 @@ class AP_Periph_FW {
 
 #if HAL_GCS_ENABLED
     GCS_Periph _gcs;
+#endif
+#if HAL_PERIPH_ARM_MONITORING_ENABLE
+    bool arm_update_status;
 #endif
     // setup the var_info table
     AP_Param param_loader{var_info};
diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp
index 394a77a6fe..902770976b 100644
--- a/Tools/AP_Periph/Parameters.cpp
+++ b/Tools/AP_Periph/Parameters.cpp
@@ -336,7 +336,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GSCALAR(hardpoint_rate, "HARDPOINT_RATE", 100),
 #endif
 
-#ifdef HAL_PERIPH_ENABLE_HWESC
+#if defined(HAL_PERIPH_ENABLE_HWESC) || defined(HAL_PERIPH_ENABLE_ESC_APDHVPRO200)
     // @Param: ESC_NUMBER
     // @DisplayName: ESC number
     // @Description: This is the ESC number to report as in UAVCAN ESC telemetry feedback packets.
@@ -359,6 +359,14 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     // @RebootRequired: True
     GSCALAR(esc_pwm_type, "ESC_PWM_TYPE",     0),
 
+    // @Param: ESC_CMD_TIMO
+    // @DisplayName: ESC Command Timeout
+    // @Description: This is the duration (ms) with which to hold the last driven ESC command before timing out and zeroing the ESC outputs. To disable zeroing of outputs in event of CAN loss, use 0. Use values greater than the expected duration between two CAN frames to ensure Periph is not starved of ESC Raw Commands.
+    // @Range: 0 10000
+    // @Units: ms
+    // @User: Advanced
+    GSCALAR(esc_command_timeout_ms, "ESC_CMD_TIMO",     200),
+
 #if HAL_WITH_ESC_TELEM && !HAL_GCS_ENABLED
     // @Param: ESC_TELEM_PORT
     // @DisplayName: ESC Telemetry Serial Port
@@ -451,6 +459,16 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GOBJECT(efi, "EFI", AP_EFI),
 #endif
 
+#if HAL_PERIPH_ARM_MONITORING_ENABLE
+     // @Param: DISARM_DELAY
+    // @DisplayName: ARM Command Timeout
+    // @Description: This is the duration (ms) with which if not received a signal will disarm the system default to 2 sec as ARM packet is set to 1 Hz
+    // @Range: 0 10000
+    // @Units: ms
+    // @User: Advanced
+    GSCALAR(disarm_delay, "DISARM_DELAY", 2000),
+    
+#endif
     AP_VAREND
 };
 
diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h
index 092b4106ad..76347dca60 100644
--- a/Tools/AP_Periph/Parameters.h
+++ b/Tools/AP_Periph/Parameters.h
@@ -59,6 +59,8 @@ class Parameters {
         k_param_efi,
         k_param_efi_port,
         k_param_efi_baudrate,
+        k_param_disarm_delay,
+        k_param_esc_command_timeout_ms,
     };
 
     AP_Int16 format_version;
@@ -98,7 +100,7 @@ class Parameters {
     AP_Int8 hardpoint_rate;
 #endif
 
-#ifdef HAL_PERIPH_ENABLE_HWESC
+#if defined(HAL_PERIPH_ENABLE_HWESC) || defined(HAL_PERIPH_ENABLE_ESC_APDHVPRO200)
     AP_Int8 esc_number;
 #endif
 
@@ -115,6 +117,7 @@ class Parameters {
 
 #ifdef HAL_PERIPH_ENABLE_RC_OUT
     AP_Int8 esc_pwm_type;
+    AP_Int16 esc_command_timeout_ms;
 #if HAL_WITH_ESC_TELEM && !HAL_GCS_ENABLED
     AP_Int8 esc_telem_port;
 #endif
@@ -143,6 +146,11 @@ class Parameters {
 #else
     static constexpr uint8_t can_fdmode = 0;
 #endif
+
+#if HAL_PERIPH_ARM_MONITORING_ENABLE
+    AP_Int32 disarm_delay;
+#endif
+
     Parameters() {}
 };
 
diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp
index d7759bda54..42e871fdc4 100644
--- a/Tools/AP_Periph/can.cpp
+++ b/Tools/AP_Periph/can.cpp
@@ -607,6 +607,9 @@ static void handle_arming_status(CanardInstance* ins, CanardRxTransfer* transfer
         return;
     }
     hal.util->set_soft_armed(req.status == UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_STATUS_FULLY_ARMED);
+#if HAL_PERIPH_ARM_MONITORING_ENABLE
+    periph.arm_update_status = true;
+#endif
 }
 
 #ifdef HAL_PERIPH_ENABLE_GPS
@@ -762,6 +765,10 @@ static void handle_esc_rawcommand(CanardInstance* ins, CanardRxTransfer* transfe
         return;
     }
     periph.rcout_esc(cmd.cmd.data, cmd.cmd.len);
+
+    // Update internal copy for disabling output to ESC when CAN packets are lost
+    periph.last_esc_num_channels = cmd.cmd.len;
+    periph.last_esc_raw_command_ms = AP_HAL::millis();
 }
 
 static void handle_act_command(CanardInstance* ins, CanardRxTransfer* transfer)
@@ -1605,6 +1612,31 @@ void AP_Periph_FW::hwesc_telem_update()
 }
 #endif // HAL_PERIPH_ENABLE_HWESC
 
+#ifdef HAL_PERIPH_ENABLE_ESC_APDHVPRO200
+void AP_Periph_FW::APD_ESC_Telem_update() {
+    if (!APD_ESC_Telem.update()) {
+        return;
+    }
+    
+    uavcan_equipment_esc_Status pkt {};
+    pkt.esc_index = g.esc_number;
+    pkt.voltage = APD_ESC_Telem.decoded.voltage;
+    pkt.current = APD_ESC_Telem.decoded.bus_current;
+    pkt.temperature = APD_ESC_Telem.decoded.temperature;
+    pkt.rpm = APD_ESC_Telem.decoded.rpm;
+    pkt.error_count = APD_ESC_Telem.decoded.status;
+    // pkt.power_rating_pct = APD_ESC_Telem.decoded.
+
+    uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE] {};
+    uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer, !periph.canfdout());
+    canard_broadcast(UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE,
+                    UAVCAN_EQUIPMENT_ESC_STATUS_ID,
+                    CANARD_TRANSFER_PRIORITY_LOW,
+                    &buffer[0],
+                    total_size);
+}
+#endif // HAL_PERIPH_ENABLE_ESC_APDHVPRO200
+
 #ifdef HAL_PERIPH_ENABLE_RC_OUT
 #if HAL_WITH_ESC_TELEM
 /*
@@ -1726,7 +1758,11 @@ void AP_Periph_FW::can_update()
     #ifdef HAL_PERIPH_ENABLE_EFI
         can_efi_update();
     #endif
+    #ifdef HAL_PERIPH_ENABLE_ESC_APDHVPRO200
+        APD_ESC_Telem_update();
+    #endif
     }
+
     const uint32_t now_us = AP_HAL::micros();
     while ((AP_HAL::micros() - now_us) < 1000) {
         hal.scheduler->delay_microseconds(HAL_PERIPH_LOOP_DELAY_US);
diff --git a/Tools/AP_Periph/hwing_esc.cpp b/Tools/AP_Periph/hwing_esc.cpp
index a79862f3cf..9ae4331a77 100644
--- a/Tools/AP_Periph/hwing_esc.cpp
+++ b/Tools/AP_Periph/hwing_esc.cpp
@@ -135,7 +135,7 @@ bool HWESC_Telem::parse_packet(void)
     decoded.current = int16_t(be16toh(pkt.current)) * 0.01;
     decoded.mos_temperature = temperature_decode(pkt.mos_temperature);
     decoded.cap_temperature = temperature_decode(pkt.cap_temperature);
-    decoded.status = be16toh(pkt.status);
+    decoded.status = be32toh(pkt.status);
     if (decoded.status != 0) {
         decoded.error_count++;
     }
diff --git a/Tools/AP_Periph/hwing_esc.h b/Tools/AP_Periph/hwing_esc.h
index bfb53645f6..dd3d758db6 100644
--- a/Tools/AP_Periph/hwing_esc.h
+++ b/Tools/AP_Periph/hwing_esc.h
@@ -27,7 +27,7 @@ class HWESC_Telem {
         float current;
         uint8_t mos_temperature;
         uint8_t cap_temperature;
-        uint16_t status;
+        uint32_t error_status;
         uint32_t error_count;
     };
 
diff --git a/Tools/AP_Periph/rc_out.cpp b/Tools/AP_Periph/rc_out.cpp
index 2b105184a6..f260744369 100644
--- a/Tools/AP_Periph/rc_out.cpp
+++ b/Tools/AP_Periph/rc_out.cpp
@@ -131,6 +131,19 @@ void AP_Periph_FW::rcout_handle_safety_state(uint8_t safety_state)
 
 void AP_Periph_FW::rcout_update()
 {
+     uint32_t now_ms = AP_HAL::millis();
+
+    const uint16_t esc_timeout_ms = g.esc_command_timeout_ms >= 0 ? g.esc_command_timeout_ms : 0; // Don't allow negative timeouts!
+    const bool has_esc_rawcommand_timed_out = esc_timeout_ms != 0 && ((now_ms - last_esc_raw_command_ms) >= esc_timeout_ms);
+    if (last_esc_num_channels > 0 && has_esc_rawcommand_timed_out) {
+        // If we've seen ESCs previously, and a timeout has occurred, then zero the outputs
+        int16_t esc_output[last_esc_num_channels] {};
+        rcout_esc(esc_output, last_esc_num_channels);
+
+        // register that the output has been changed
+        rcout_has_new_data_to_update = true;
+    }
+
     if (!rcout_has_new_data_to_update) {
         return;
     }
@@ -141,7 +154,6 @@ void AP_Periph_FW::rcout_update()
     SRV_Channels::output_ch_all();
     SRV_Channels::push();
 #if HAL_WITH_ESC_TELEM
-    uint32_t now_ms = AP_HAL::millis();
     if (now_ms - last_esc_telem_update_ms >= 20) {
         last_esc_telem_update_ms = now_ms;
         esc_telem_update();
diff --git a/Tools/AP_Periph/version.h b/Tools/AP_Periph/version.h
index 4e3f59b496..3a907a7ea6 100644
--- a/Tools/AP_Periph/version.h
+++ b/Tools/AP_Periph/version.h
@@ -1,16 +1,19 @@
 #pragma once
 
 #include <GCS_MAVLink/GCS_MAVLink.h>
+#ifndef __AP_PERIPH_FW_VERSION__
+#define __AP_PERIPH_FW_VERSION__
 
-#define THISFIRMWARE "AP_Periph V1.5dev"
+#define THISFIRMWARE "Ottano Carbopilot V4.2.1 OV3-404"
 
 // the following line is parsed by the autotest scripts
 #define FIRMWARE_VERSION 1,5,0,FIRMWARE_VERSION_TYPE_DEV
 
+#ifndef FW_MAJOR
 #define FW_MAJOR 1
 #define FW_MINOR 5
 #define FW_PATCH 0
 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV
+#endif
 
-
-
+#endif
diff --git a/Tools/Carbonix_scripts/carbonix_waf_build.sh b/Tools/Carbonix_scripts/carbonix_waf_build.sh
new file mode 100755
index 0000000000..1c68adee41
--- /dev/null
+++ b/Tools/Carbonix_scripts/carbonix_waf_build.sh
@@ -0,0 +1,97 @@
+#!/bin/bash
+
+
+# Check unstaged changes
+if [[ $(git status --porcelain) ]]; then
+    echo "There are unstaged changes in the repository."
+    exit 1
+fi
+
+# Check outdated submodules
+# git submodule update --remote --quiet
+# outdated_submodules=$(git submodule status | grep -e '-[[:alnum:]]' | awk '{print $2}')
+# if [[ -n $outdated_submodules ]]; then
+#     echo "The following submodules are not updated:"
+#     echo "$outdated_submodules"
+#     exit 1
+# fi
+
+echo "Running distclean..."
+./waf distclean
+
+# ArduPlane with CubeOrange
+echo "Compiling ArduPlane for CarbonixCubeOrange..."
+./waf configure --board CarbonixCubeOrange
+./waf plane
+./waf configure --board CarbonixCubeOrange --bootloader
+./waf bootloader
+
+echo "Compiling ArduPlane for CubeOrange..."
+./waf configure --board CubeOrange
+./waf plane
+./waf configure --board CubeOrange --bootloader
+./waf bootloader
+
+echo "Compiling AP_Periph for CarbonixF405..."
+./waf configure --board CarbonixF405
+./waf AP_Periph
+./waf configure --board CarbonixF405 --bootloader
+./waf bootloader
+
+echo "Compiling AP_Periph for CarbonixF405_M1..."
+./waf configure --board CarbonixF405-M1
+./waf AP_Periph
+./waf configure --board CarbonixF405-M1 --bootloader
+./waf bootloader
+
+echo "Compiling AP_Periph for CarbonixF405_M2..."
+./waf configure --board CarbonixF405-M2
+./waf AP_Periph
+./waf configure --board CarbonixF405-M2 --bootloader
+./waf bootloader
+
+echo "Compiling AP_Periph for CarbonixF405_M3..."
+./waf configure --board CarbonixF405-M3
+./waf AP_Periph
+./waf configure --board CarbonixF405-M3 --bootloader
+./waf bootloader
+
+echo "Compiling AP_Periph for CarbonixF405_M4..."
+./waf configure --board CarbonixF405-M4
+./waf AP_Periph
+./waf configure --board CarbonixF405-M4 --bootloader
+./waf bootloader
+
+echo "Compiling AP_Periph for CarbonixF405_M5..."
+./waf configure --board CarbonixF405-M5
+./waf AP_Periph
+./waf configure --board CarbonixF405-M5 --bootloader
+./waf bootloader
+
+echo "Compiling AP_Periph for CarbonixF405_LWing..."
+./waf configure --board CarbonixF405-LWing
+./waf AP_Periph
+./waf configure --board CarbonixF405-LWing --bootloader
+./waf bootloader
+
+echo "Compiling AP_Periph for CarbonixF405_RWing..."
+./waf configure --board CarbonixF405-RWing
+./waf AP_Periph
+./waf configure --board CarbonixF405-RWing --bootloader
+./waf bootloader
+
+echo "Compiling AP_Periph for CarbonixF405_RTail..."
+./waf configure --board CarbonixF405-RTail
+./waf AP_Periph
+./waf configure --board CarbonixF405-RTail --bootloader
+./waf bootloader
+
+echo "Compiling AP_Periph for CarbonixF405_RTail..."
+./waf configure --board CarbonixF405-LTail
+./waf AP_Periph
+./waf configure --board CarbonixF405-LTail --bootloader
+./waf bootloader
+
+# echo "Compiling SITL for CarbonixF405..."
+# ./waf configure --board sitl
+# ./Tools/autotest/sim_vehicle.py --console -f quadplane --vehicle=ArduPlane
\ No newline at end of file
diff --git a/Tools/autotest/locations.txt b/Tools/autotest/locations.txt
index 541a099634..7b00117b03 100644
--- a/Tools/autotest/locations.txt
+++ b/Tools/autotest/locations.txt
@@ -56,6 +56,7 @@ Elvenes=68.871422,17.986690,17,256
 Kawachi=35.879129,140.339683,7,0
 SpringValley=-35.280252,149.005821,597.3,5
 SpringValley2=-35.28240059,149.00542037,582,10
+SpringValley3=-35.28240515,149.00716878,579,12.6
 Pyramid=29.9764,31.1339,0,0
 AAMEastField=39.842288,-105.212928,1809,106
 HachinoheMine=40.4539496,141.5419051,56,270
diff --git a/Tools/scripts/decode_devid.py b/Tools/scripts/decode_devid.py
index 15b574306b..b71babe29c 100755
--- a/Tools/scripts/decode_devid.py
+++ b/Tools/scripts/decode_devid.py
@@ -64,6 +64,8 @@ def num(s):
     0x12 : "DEVTYPE_RM3100",
     0x13 : "DEVTYPE_MMC5883",
     0x14 : "DEVTYPE_AK09918",
+    0x15 : "DEVTYPE_AK09915", 
+    0x16 : "DEVTYPE_QMC5883P",
 }
 
 imu_types = {
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp
index e125bbd0cc..4fa46ecb0b 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp
+++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp
@@ -16,6 +16,7 @@
 #include "AP_BattMonitor_INA2xx.h"
 #include "AP_BattMonitor_LTC2946.h"
 #include "AP_BattMonitor_Torqeedo.h"
+#include "AP_BattMonitor_AD7091R5.h"
 #include "AP_BattMonitor_FuelLevel_Analog.h"
 
 #include <AP_HAL/AP_HAL.h>
@@ -323,6 +324,11 @@ AP_BattMonitor::init()
             case Type::Torqeedo:
                 drivers[instance] = new AP_BattMonitor_Torqeedo(*this, state[instance], _params[instance]);
                 break;
+#endif
+#if AP_BATTMON_AD7091R5_ENABLED
+            case Type::AD7091R5:
+                drivers[instance] = new AP_BattMonitor_AD7091R5(*this, state[instance], _params[instance]);
+                break;
 #endif
             case Type::NONE:
             default:
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h
index f4eaa68518..085d676c11 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor.h
+++ b/libraries/AP_BattMonitor/AP_BattMonitor.h
@@ -57,6 +57,7 @@ class AP_BattMonitor_INA2XX;
 class AP_BattMonitor_LTC2946;
 class AP_BattMonitor_Torqeedo;
 class AP_BattMonitor_FuelLevel_Analog;
+class AP_BattMonitor_AD7091R5;
 
 class AP_BattMonitor
 {
@@ -76,7 +77,7 @@ class AP_BattMonitor
     friend class AP_BattMonitor_LTC2946;
 
     friend class AP_BattMonitor_Torqeedo;
-    friend class AP_BattMonitor_FuelLevel_Analog;
+    friend class AP_BattMonitor_AD7091R5;
 
 public:
 
@@ -112,6 +113,7 @@ class AP_BattMonitor
         LTC2946                    = 22,
         Torqeedo                   = 23,
         FuelLevel_Analog           = 24,
+        AD7091R5                   = 28,
     };
 
     FUNCTOR_TYPEDEF(battery_failsafe_handler_fn_t, void, const char *, const int8_t);
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.cpp
new file mode 100644
index 0000000000..8f09bf9beb
--- /dev/null
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.cpp
@@ -0,0 +1,236 @@
+#include "AP_BattMonitor_AD7091R5.h"
+
+/**
+ * @brief You can use it to Read Current and voltage of 1-3 batteries from a ADC extender IC over I2C.
+ * AD7091R5 is a ADC extender and we are using it to read current and voltage of multiple batteries.
+ * Examples of Pin combination:
+ *  1)Pin 50 = Voltage 51,52,53 =  Current. For 3 battery combination Voltage will be same accross.
+ *  2)Pin 50,51 = Voltage and Current Battery 1 -   Pin 52,53 = Voltage and Current Battery 2
+ * Only the First instance of Battery Monitor will be reading the values from IC over I2C.
+ * Make sure you understand the method of calculation used in this driver before using it.
+ * e.g. using pin 1 on IC to read voltage of 2 batteries and pin 2 and 3 to read current from individual battery.
+ * Pin number represents 50 = pin 1, 51 = pin 2 and so on 52, 53
+ * BATT2_Monitor  = 24 ,  BATT3_Monitor  = 24
+ * BATT2_VOLT_PIN = 50 ,  BATT3_VOLT_PIN = 50
+ * BATT2_CURR_PIN = 51 ,  BATT3_CURR_PIN = 52
+ *
+ *
+ */
+
+#if AP_BATTMON_AD7091R5_ENABLED
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Common/AP_Common.h>
+#include <AP_Math/AP_Math.h>
+
+//macro defines
+#define AD7091R5_I2C_ADDR        0x2F // A0 and A1 tied to GND
+#define AD7091R5_I2C_BUS         0
+#define AD7091R5_RESET           0x02
+#define AD7091R5_RESULT_ADDR     0x00
+#define AD7091R5_CHAN_ADDR       0x01
+#define AD7091R5_CONF_ADDR       0x02
+#define AD7091R5_CH_ID(x)        ((x >> 5) & 0x03)
+#define AD7091R5_RES_MASK        0x0F
+#define AD7091R5_REF             3.3f
+#define AD7091R5_RESOLUTION      (float)4096
+#define AD7091R5_PERIOD_USEC     100000
+#define AD7091R5_BASE_PIN        50
+
+
+extern const AP_HAL::HAL& hal;
+const AP_Param::GroupInfo AP_BattMonitor_AD7091R5::var_info[] = {
+
+    // @Param: VOLT_PIN
+    // @DisplayName: Battery Voltage sensing pin on the AD7091R5 Ic
+    // @Description: Sets the analog input pin that should be used for voltage monitoring on AD7091R5.
+    // @Values: -1:Disabled
+    // @User: Standard
+    // @RebootRequired: True
+    AP_GROUPINFO("VOLT_PIN", 30, AP_BattMonitor_AD7091R5, _volt_pin, 0),
+
+    // @Param: CURR_PIN
+    // @DisplayName: Battery Current sensing pin
+    // @Description: Sets the analog input pin that should be used for Current monitoring on AD7091R5.
+    // @Values: -1:Disabled
+    // @User: Standard
+    // @RebootRequired: True
+    AP_GROUPINFO("CURR_PIN", 31, AP_BattMonitor_AD7091R5, _curr_pin, 0),
+
+    // @Param: VOLT_MULT
+    // @DisplayName: Voltage Multiplier
+    // @Description: Used to convert the voltage of the voltage sensing pin (@PREFIX@VOLT_PIN) to the actual battery's voltage (pin_voltage * VOLT_MULT).
+    // @User: Advanced
+    AP_GROUPINFO("VOLT_MULT", 32, AP_BattMonitor_AD7091R5, _volt_multiplier, 0),
+
+    // @Param: AMP_PERVLT
+    // @DisplayName: Amps per volt
+    // @Description: Number of amps that a 1V reading on the current sensor corresponds to.
+    // @Units: A/V
+    // @User: Standard
+    AP_GROUPINFO("AMP_PERVLT", 33, AP_BattMonitor_AD7091R5, _curr_amp_per_volt, 0),
+
+    // @Param: AMP_OFFSET
+    // @DisplayName: AMP offset
+    // @Description: Voltage offset at zero current on current sensor
+    // @Units: V
+    // @User: Standard
+    AP_GROUPINFO("AMP_OFFSET", 34, AP_BattMonitor_AD7091R5, _curr_amp_offset, 0),
+
+    // @Param: VLT_OFFSET
+    // @DisplayName: Volage offset
+    // @Description: Voltage offset on voltage pin. This allows for an offset due to a diode. This voltage is subtracted before the scaling is applied
+    // @Units: V
+    // @User: Advanced
+    AP_GROUPINFO("VLT_OFFSET", 35, AP_BattMonitor_AD7091R5, _volt_offset, 0),
+
+    // Param indexes must be less than 10 to avoid conflict with other battery monitor param tables loaded by pointer
+
+    AP_GROUPEND
+};
+
+
+//Variable initialised to read from first instance.
+AP_BattMonitor_AD7091R5::AnalogData AP_BattMonitor_AD7091R5::_analog_data[AD7091R5_NO_OF_CHANNELS] = {0};
+bool AP_BattMonitor_AD7091R5::_first = true;
+bool AP_BattMonitor_AD7091R5::_health = false;
+
+/**
+ * @brief Construct a new ap battmonitor ad7091r5::ap battmonitor ad7091r5 object
+ *
+ * @param mon
+ * @param mon_state
+ * @param params
+ */
+AP_BattMonitor_AD7091R5::AP_BattMonitor_AD7091R5(AP_BattMonitor &mon,
+        AP_BattMonitor::BattMonitor_State &mon_state,
+        AP_BattMonitor_Params &params) :
+    AP_BattMonitor_Backend(mon, mon_state, params)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+    _state.var_info = var_info;
+}
+
+/**
+ * @brief probe and initialize the sensor and register call back
+ *
+ */
+void AP_BattMonitor_AD7091R5::init()
+{
+    // voltage and current pins from params and check if there are in range
+    if (_volt_pin.get() >= AD7091R5_BASE_PIN && _volt_pin.get() <= AD7091R5_BASE_PIN + AD7091R5_NO_OF_CHANNELS && 
+    _curr_pin.get() >= AD7091R5_BASE_PIN && _curr_pin.get() <= AD7091R5_BASE_PIN + AD7091R5_NO_OF_CHANNELS) {
+        volt_buff_pt = _volt_pin.get() - AD7091R5_BASE_PIN;
+        curr_buff_pt = _curr_pin.get() - AD7091R5_BASE_PIN;
+    }
+    else{
+        return; //pin values are out of range
+    }
+
+    // only the first instance read the i2c device
+    if (_first) {
+        _first = false;
+        // probe i2c device
+        _dev = hal.i2c_mgr->get_device(AD7091R5_I2C_BUS, AD7091R5_I2C_ADDR);
+
+        if (_dev) {
+            WITH_SEMAPHORE(_dev->get_semaphore());
+            _dev->set_retries(10); // lots of retries during probe
+            //Reset and config device
+            if (_initialize()) {
+                _dev->set_retries(2); // drop to 2 retries for runtime
+                _dev->register_periodic_callback(AD7091R5_PERIOD_USEC, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_AD7091R5::_read_adc, void));
+            }
+        }
+    }
+}
+
+/**
+ * @brief read - read the voltage and curren
+ *
+ */
+void AP_BattMonitor_AD7091R5::read()
+{
+
+    WITH_SEMAPHORE(sem);
+    //copy global health status to all instances
+    _state.healthy = _health;
+
+    //return if system not healthy
+    if (!_state.healthy) {
+        return;
+    }
+
+    //voltage conversion
+    _state.voltage = (_data_to_volt(_analog_data[volt_buff_pt].data) - _volt_offset) * _volt_multiplier;
+
+    //current amps conversion
+    _state.current_amps = (_data_to_volt(_analog_data[curr_buff_pt].data) - _curr_amp_offset) * _curr_amp_per_volt;
+
+    // calculate time since last current read
+    uint32_t tnow = AP_HAL::micros();
+    uint32_t dt_us = tnow - _state.last_time_micros;
+
+    // update total current drawn since startup
+    update_consumed(_state, dt_us);
+
+    // record time
+    _state.last_time_micros = tnow;
+}
+
+/**
+ * @brief read all four channels and store the results
+ *
+ */
+void AP_BattMonitor_AD7091R5::_read_adc()
+{
+    uint8_t data[AD7091R5_NO_OF_CHANNELS*2];
+    //reset and reconfigure IC if health status is not good.
+    if (!_state.healthy) {
+        _initialize();
+    }
+    //read value
+    bool ret = _dev->transfer(nullptr, 0, data, sizeof(data));
+    WITH_SEMAPHORE(sem);
+    if (ret) {
+        for (int i=0; i<AD7091R5_NO_OF_CHANNELS; i++) {
+            uint8_t chan = AD7091R5_CH_ID(data[2*i]);
+            _analog_data[chan].data = ((uint16_t)(data[2*i]&AD7091R5_RES_MASK)<<8) | data[2*i+1];
+        }
+        _health = true;
+    } else {
+        _health = false;
+    }
+}
+
+/**
+ * @brief config the adc
+ *
+ * @return true
+ * @return false
+ */
+bool AP_BattMonitor_AD7091R5::_initialize()
+{
+    //reset the device
+    uint8_t data[3] = {AD7091R5_CONF_ADDR, AD7091R5_CONF_CMD | AD7091R5_RESET, AD7091R5_CONF_PDOWN0};
+
+    if(_dev->transfer(data, sizeof(data), nullptr, 0)){
+        //command mode, use external 3.3 reference, all channels enabled, set address pointer register to read the adc results
+        uint8_t data_2[6] = {AD7091R5_CONF_ADDR, AD7091R5_CONF_CMD, AD7091R5_CONF_PDOWN0, AD7091R5_CHAN_ADDR, AD7091R5_CHAN_ALL, AD7091R5_RESULT_ADDR};
+        return _dev->transfer(data_2, sizeof(data_2), nullptr, 0);
+    }
+    return false;
+}
+
+/**
+ * @brief convert binary reading to volts
+ *
+ * @param data
+ * @return float
+ */
+float AP_BattMonitor_AD7091R5::_data_to_volt(uint32_t data)
+{
+    return (AD7091R5_REF/AD7091R5_RESOLUTION)*data;
+}
+
+#endif // AP_BATTMON_AD7091R5_ENABLED
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.h b/libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.h
new file mode 100644
index 0000000000..eab1f6d2f0
--- /dev/null
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.h
@@ -0,0 +1,75 @@
+#pragma once
+
+#include "AP_BattMonitor_Backend.h"
+
+#ifndef AP_BATTMON_AD7091R5_ENABLED
+#define AP_BATTMON_AD7091R5_ENABLED (BOARD_FLASH_SIZE > 1024)
+#endif
+
+#if AP_BATTMON_AD7091R5_ENABLED
+
+#include <AP_HAL/utility/OwnPtr.h>
+#include <AP_HAL/I2CDevice.h>
+
+#define AD7091R5_NO_OF_CHANNELS  4
+#define AD7091R5_CONF_CMD        0x04
+#define AD7091R5_CHAN_ALL        0x0F
+#define AD7091R5_CONF_PDOWN0     0x00
+#define AD7091R5_CONF_PDOWN2     0x02
+#define AD7091R5_CONF_PDOWN3     0x03
+#define AD7091R5_CONF_PDOWN_MASK 0x03
+
+class AP_BattMonitor_AD7091R5 : public AP_BattMonitor_Backend
+{
+public:
+    // Constructor
+    AP_BattMonitor_AD7091R5(AP_BattMonitor &mon,
+                            AP_BattMonitor::BattMonitor_State &mon_state,
+                            AP_BattMonitor_Params &params);
+
+    // Read the battery voltage and current.  Should be called at 10hz
+    void read() override;
+    void init(void) override;
+
+    // returns true if battery monitor provides consumed energy info
+    bool has_consumed_energy() const override
+    {
+        return has_current();
+    }
+
+    // returns true if battery monitor provides current info
+    bool has_current() const override
+    {
+        return true;
+    }
+
+    static const struct AP_Param::GroupInfo var_info[];
+
+private:
+    void _read_adc();
+    bool _initialize();
+    float _data_to_volt(uint32_t data);
+
+    static struct AnalogData {
+        uint32_t data;
+    } _analog_data[AD7091R5_NO_OF_CHANNELS];
+    static bool _first;
+    static bool _health;
+
+    HAL_Semaphore sem; // semaphore for access to shared frontend data
+    AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
+    uint8_t volt_buff_pt;
+    uint8_t curr_buff_pt;
+
+protected:
+
+    // Parameters
+    AP_Float _volt_multiplier;          // voltage on volt pin multiplied by this to calculate battery voltage
+    AP_Float _curr_amp_per_volt;        // voltage on current pin multiplied by this to calculate current in amps
+    AP_Float _curr_amp_offset;          // offset voltage that is subtracted from current pin before conversion to amps
+    AP_Float _volt_offset;              // offset voltage that is subtracted from voltage pin before conversion
+    AP_Int8  _volt_pin;                 // board pin used to measure battery voltage
+    AP_Int8  _curr_pin;                 // board pin used to measure battery current
+};
+
+#endif // AP_BATTMON_AD7091R5_ENABLED
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp
index 87b9a2a541..6c4f60b146 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp
@@ -13,7 +13,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = {
     // @Param: MONITOR
     // @DisplayName: Battery monitoring
     // @Description: Controls enabling monitoring of the battery's voltage and current
-    // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Generic,8:DroneCAN-BatteryInfo,9:ESC,10:Sum Of Selected Monitors,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6,15:NeoDesign,16:SMBus-Maxell,17:Generator-Elec,18:Generator-Fuel,19:Rotoye,20:MPPT,21:INA2XX,22:LTC2946,23:Torqeedo,24:FuelLevelAnalog
+    // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Generic,8:DroneCAN-BatteryInfo,9:ESC,10:Sum Of Selected Monitors,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6,15:NeoDesign,16:SMBus-Maxell,17:Generator-Elec,18:Generator-Fuel,19:Rotoye,20:MPPT,21:INA2XX,22:LTC2946,23:Torqeedo,24:FuelLevelAnalog,25:AD7091R5
     // @User: Standard
     // @RebootRequired: True
     AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, int8_t(AP_BattMonitor::Type::NONE), AP_PARAM_FLAG_ENABLE),
diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp
index e4067c7062..de84764728 100644
--- a/libraries/AP_Compass/AP_Compass.cpp
+++ b/libraries/AP_Compass/AP_Compass.cpp
@@ -21,6 +21,7 @@
 #include "AP_Compass_LIS3MDL.h"
 #include "AP_Compass_AK09916.h"
 #include "AP_Compass_QMC5883L.h"
+#include "AP_Compass_QMC5883P.h"
 #if HAL_ENABLE_LIBUAVCAN_DRIVERS
 #include "AP_Compass_UAVCAN.h"
 #endif
@@ -1094,6 +1095,24 @@ void Compass::_probe_external_i2c_compasses(void)
     }
 #endif
 
+#if !defined(HAL_DISABLE_I2C_MAGS_BY_DEFAULT) || defined(HAL_USE_I2C_MAG_QMC5883P)
+    //external i2c bus
+    FOREACH_I2C_EXTERNAL(i) {
+        ADD_BACKEND(DRIVER_QMC5883P, AP_Compass_QMC5883P::probe(GET_I2C_DEVICE(i, HAL_COMPASS_QMC5883P_I2C_ADDR),
+                    true, HAL_COMPASS_QMC5883P_ORIENTATION_EXTERNAL));
+    }
+
+    // internal i2c bus
+    if (all_external) {
+        // only probe QMC5883P on internal if we are treating internals as externals
+        FOREACH_I2C_INTERNAL(i) {
+            ADD_BACKEND(DRIVER_QMC5883P, AP_Compass_QMC5883P::probe(GET_I2C_DEVICE(i, HAL_COMPASS_QMC5883P_I2C_ADDR),
+                        all_external,
+                        all_external?HAL_COMPASS_QMC5883P_ORIENTATION_EXTERNAL:HAL_COMPASS_QMC5883P_ORIENTATION_INTERNAL));
+        }
+    }
+#endif
+
 #ifndef HAL_BUILD_AP_PERIPH
     // AK09916 on ICM20948
     FOREACH_I2C_EXTERNAL(i) {
diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h
index d2040c1434..c7cc075338 100644
--- a/libraries/AP_Compass/AP_Compass.h
+++ b/libraries/AP_Compass/AP_Compass.h
@@ -432,6 +432,7 @@ friend class AP_Compass_Backend;
         DRIVER_MSP      =17,
         DRIVER_SERIAL   =18,
         DRIVER_MMC5XX3  =19,
+        DRIVER_QMC5883P =20,
     };
 
     bool _driver_enabled(enum DriverType driver_type);
diff --git a/libraries/AP_Compass/AP_Compass_Backend.h b/libraries/AP_Compass/AP_Compass_Backend.h
index 8636f98258..e014978ea5 100644
--- a/libraries/AP_Compass/AP_Compass_Backend.h
+++ b/libraries/AP_Compass/AP_Compass_Backend.h
@@ -68,6 +68,7 @@ class AP_Compass_Backend
         DEVTYPE_MMC5983 = 0x13,
         DEVTYPE_AK09918 = 0x14,
         DEVTYPE_AK09915 = 0x15,
+        DEVTYPE_QMC5883P = 0x16,
     };
 
 #if HAL_MSP_COMPASS_ENABLED
diff --git a/libraries/AP_Compass/AP_Compass_QMC5883P.cpp b/libraries/AP_Compass/AP_Compass_QMC5883P.cpp
new file mode 100644
index 0000000000..6fe1a718ca
--- /dev/null
+++ b/libraries/AP_Compass/AP_Compass_QMC5883P.cpp
@@ -0,0 +1,218 @@
+/*
+ * This file 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.
+ *
+ * This file 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * Driver by Lokesh Ramina, Jan 2022
+ */
+#include "AP_Compass_QMC5883P.h"
+
+#include <stdio.h>
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Math/AP_Math.h>
+
+
+//Register Address
+#define QMC5883P_REG_ID                 0x00
+#define QMC5883P_REG_DATA_OUTPUT_X      0x01
+#define QMC5883P_REG_DATA_OUTPUT_Z_MSB  0x06
+#define QMC5883P_REG_STATUS             0x09
+#define QMC5883P_REG_CONF1              0x0A
+#define QMC5883P_REG_CONF2              0x0B
+
+#define QMC5883P_ID_VAL 0x80
+
+//Register values
+// Sensor operation modes
+#define QMC5883P_MODE_SUSPEND    0x00
+#define QMC5883P_MODE_NORMAL     0x01
+#define QMC5883P_MODE_SINGLE     0x02
+#define QMC5883P_MODE_CONTINUOUS 0x03
+
+// ODR data output rates for 5883L
+#define QMC5883P_ODR_10HZ  (0x00 << 2)
+#define QMC5883P_ODR_50HZ  (0x01 << 2)
+#define QMC5883P_ODR_100HZ (0x02 << 2)
+#define QMC5883P_ODR_200HZ (0x03 << 2)
+
+// Over sampling Ratio OSR1
+#define QMC5883P_OSR1_8 (0x00 << 4)
+#define QMC5883P_OSR1_4 (0x01 << 4)
+#define QMC5883P_OSR1_2	(0x02 << 4)
+#define QMC5883P_OSR1_1	(0x03 << 4)
+
+// Down sampling Rate OSR2
+#define QMC5883P_OSR2_8	0x08
+
+//RNG
+#define QMC5883P_RNG_30G (0x00 << 2)
+#define QMC5883P_RNG_12G (0x01 << 2)
+#define QMC5883P_RNG_8G  (0x10 << 2)
+#define QMC5883P_RNG_2G  (0x11 << 2)
+
+#define QMC5883P_SET_XYZ_SIGN 0x29
+
+//Reset
+#define QMC5883P_RST 0x80
+
+//Status Val
+#define QMC5883P_STATUS_DATA_READY 0x01
+
+#ifndef DEBUG
+#define DEBUG 0
+#endif
+
+extern const AP_HAL::HAL &hal;
+
+AP_Compass_Backend *AP_Compass_QMC5883P::probe(AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
+        bool force_external,
+        enum Rotation rotation)
+{
+    if (!dev) {
+        return nullptr;
+    }
+    AP_Compass_QMC5883P *sensor = new AP_Compass_QMC5883P(std::move(dev),force_external,rotation);
+    if (!sensor || !sensor->init()) {
+        delete sensor;
+        return nullptr;
+    }
+    return sensor;
+}
+
+AP_Compass_QMC5883P::AP_Compass_QMC5883P(AP_HAL::OwnPtr<AP_HAL::Device> dev,
+        bool force_external,
+        enum Rotation rotation)
+    : _dev(std::move(dev))
+    , _rotation(rotation)
+    , _force_external(force_external)
+{
+}
+
+bool AP_Compass_QMC5883P::init()
+{
+    _dev->get_semaphore()->take_blocking();
+
+    _dev->set_retries(10);
+#if DEBUG
+    _dump_registers();
+#endif
+    if (!_check_whoami()) {
+        goto fail;
+    }
+    //As mentioned in the Datasheet 7.2 to do continues mode 0x29 will set sign for X,Y,Z
+    if (!_dev->write_register(QMC5883P_REG_DATA_OUTPUT_Z_MSB, QMC5883P_SET_XYZ_SIGN)||
+        !_dev->write_register(QMC5883P_REG_CONF1,
+                              QMC5883P_MODE_CONTINUOUS|
+                              QMC5883P_ODR_100HZ|
+                              QMC5883P_OSR1_8|
+                              QMC5883P_OSR2_8)||
+        !_dev->write_register(QMC5883P_REG_CONF2,QMC5883P_OSR2_8)) {
+        goto fail;
+    }
+
+    // lower retries for run
+    _dev->set_retries(3);
+
+    _dev->get_semaphore()->give();
+
+    //register compass instance
+    _dev->set_device_type(DEVTYPE_QMC5883P);
+    if (!register_compass(_dev->get_bus_id(), _instance)) {
+        return false;
+    }
+    set_dev_id(_instance, _dev->get_bus_id());
+
+    printf("%s found on bus %u id %u address 0x%02x\n", name,
+           _dev->bus_num(), _dev->get_bus_id(), _dev->get_bus_address());
+
+    set_rotation(_instance, _rotation);
+
+    if (_force_external) {
+        set_external(_instance, true);
+    }
+
+    //Enable 100HZ
+    _dev->register_periodic_callback(10000,
+                                     FUNCTOR_BIND_MEMBER(&AP_Compass_QMC5883P::timer, void));
+
+    return true;
+
+fail:
+    _dev->get_semaphore()->give();
+    return false;
+}
+bool AP_Compass_QMC5883P::_check_whoami()
+{
+    uint8_t whoami;
+    if (!_dev->read_registers(QMC5883P_REG_ID, &whoami,1)||
+        whoami != QMC5883P_ID_VAL) {
+        return false;
+    }
+    return true;
+}
+
+void AP_Compass_QMC5883P::timer()
+{
+    struct PACKED {
+        int16_t rx;
+        int16_t ry;
+        int16_t rz;
+    } buffer;
+
+    const float range_scale = 1000.0f / 3000.0f;
+
+    uint8_t status;
+    if (!_dev->read_registers(QMC5883P_REG_STATUS,&status,1)) {
+        return;
+    }
+    //new data is ready
+    if (!(status & QMC5883P_STATUS_DATA_READY)) {
+        return;
+    }
+
+    if (!_dev->read_registers(QMC5883P_REG_DATA_OUTPUT_X, (uint8_t *) &buffer, sizeof(buffer))) {
+        return ;
+    }
+
+    auto x = buffer.rx;
+    auto y = buffer.ry;
+    auto z = buffer.rz;
+
+#if DEBUG
+    printf("mag.x:%d\n",x);
+    printf("mag.y:%d\n",y);
+    printf("mag.z:%d\n",z);
+#endif
+
+    Vector3f field = Vector3f{x * range_scale, y * range_scale, z * range_scale };
+
+    accumulate_sample(field, _instance, 20);
+}
+
+void AP_Compass_QMC5883P::read()
+{
+    drain_accumulated_samples(_instance);
+}
+
+void AP_Compass_QMC5883P::_dump_registers()
+{
+    printf("QMC5883P registers dump\n");
+    for (uint8_t reg = QMC5883P_REG_DATA_OUTPUT_X; reg <= 0x30; reg++) {
+        uint8_t v;
+        _dev->read_registers(reg,&v,1);
+        printf("%02x:%02x ", (unsigned)reg, (unsigned)v);
+        if ((reg - ( QMC5883P_REG_DATA_OUTPUT_X-1)) % 16 == 0) {
+            printf("\n");
+        }
+    }
+}
diff --git a/libraries/AP_Compass/AP_Compass_QMC5883P.h b/libraries/AP_Compass/AP_Compass_QMC5883P.h
new file mode 100644
index 0000000000..a6b0d31773
--- /dev/null
+++ b/libraries/AP_Compass/AP_Compass_QMC5883P.h
@@ -0,0 +1,68 @@
+/*
+ * This file 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.
+ *
+ * This file 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * Driver by Lokesh Ramina, Jan 2022
+ */
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/I2CDevice.h>
+#include <AP_Math/AP_Math.h>
+
+
+#include "AP_Compass.h"
+#include "AP_Compass_Backend.h"
+
+#ifndef HAL_COMPASS_QMC5883P_I2C_ADDR
+#define HAL_COMPASS_QMC5883P_I2C_ADDR 0x2C
+#endif
+
+/*
+  setup default orientations
+ */
+#ifndef HAL_COMPASS_QMC5883P_ORIENTATION_EXTERNAL
+#define HAL_COMPASS_QMC5883P_ORIENTATION_EXTERNAL ROTATION_ROLL_180
+#endif
+
+#ifndef HAL_COMPASS_QMC5883P_ORIENTATION_INTERNAL
+#define HAL_COMPASS_QMC5883P_ORIENTATION_INTERNAL ROTATION_ROLL_180_YAW_270
+#endif
+
+class AP_Compass_QMC5883P : public AP_Compass_Backend
+{
+public:
+    static AP_Compass_Backend *probe(AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
+                                     bool force_external,
+                                     enum Rotation rotation);
+
+    void read() override;
+
+    static constexpr const char *name = "QMC5883P";
+
+private:
+    AP_Compass_QMC5883P(AP_HAL::OwnPtr<AP_HAL::Device> dev,
+                        bool force_external,
+                        enum Rotation rotation);
+
+    void _dump_registers();
+    bool _check_whoami();
+    void timer();
+    bool init();
+
+    AP_HAL::OwnPtr<AP_HAL::Device> _dev;
+
+    enum Rotation _rotation;
+    uint8_t _instance;
+    bool _force_external:1;
+};
\ No newline at end of file
diff --git a/libraries/AP_EFI/AP_EFI.cpp b/libraries/AP_EFI/AP_EFI.cpp
index 3911cd0178..15d95698ab 100644
--- a/libraries/AP_EFI/AP_EFI.cpp
+++ b/libraries/AP_EFI/AP_EFI.cpp
@@ -23,7 +23,7 @@
 #include "AP_EFI_DroneCAN.h"
 #include "AP_EFI_Currawong_ECU.h"
 #include "AP_EFI_Scripting.h"
-
+#include "AP_EFI_Serial_Hirth.h"
 #include <AP_Logger/AP_Logger.h>
 
 #if HAL_MAX_CAN_PROTOCOL_DRIVERS
@@ -64,6 +64,91 @@ const AP_Param::GroupInfo AP_EFI::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("_FUEL_DENS", 4, AP_EFI, ecu_fuel_density, 0),
 
+    // @Param: _THTL_FO
+    // @DisplayName: Throttle value - First Order 
+    // @Description:  First Order Polynomial. (=1, if throttle is first order polynomial trendline)
+    // @Range: -1 1
+    // @User: Advanced
+    // @RebootRequired: True
+    AP_GROUPINFO("_THTL_FOP", 5, AP_EFI, throttle_firstorder, 1),
+
+    // @Param: _THTL_SOP
+    // @DisplayName: Throttle value - Second Order 
+    // @Description:  Second Order Polynomial. (=0, if throttle is first order polynomial trendline)
+    // @Range: -1 1
+    // @User: Advanced
+    // @RebootRequired: True
+    AP_GROUPINFO("_THTL_SOP", 6, AP_EFI, throttle_secondorder, 0),
+
+    // @Param: _THTL_TOP
+    // @DisplayName: Throttle value - First Order 
+    // @Description:  Third Order Polynomial. (=0, if throttle is first order polynomial trendline)
+    // @Range: -1 1
+    // @User: Advanced
+    // @RebootRequired: True
+    AP_GROUPINFO("_THTL_TOP", 7, AP_EFI, throttle_thirdorder, 0),
+
+    // @Param: _THTL_OFF
+    // @DisplayName: EFI throttle linearization offset
+    // @Description: Offset for throttle linearization 
+    // @Range: 0 100
+    // @User: Advanced
+    // @RebootRequired: True
+    AP_GROUPINFO("_THTL_OFF", 8, AP_EFI, throttle_offset, 10),
+
+    // @Param: _THROTTLE_IDLE
+    // @DisplayName: EFI IDLE Throttle value
+    // @Description:  This is the offset value. Ensure ICE_IDLE_PCT=0 for this functionality to work.
+    // @Range: 0 100
+    // @User: Advanced
+    // @RebootRequired: True
+    AP_GROUPINFO("_THTL_IDLE", 50, AP_EFI, throttle_idle, 15),
+
+    // @Param: _THROTTLE_MAX
+    // @DisplayName: EFI Max Throttle value
+    // @Description: Throttle max cap. Throttle scaling is derived from _THROTTLE_MAX and _THROTTLE_IDLE.
+    // @Range: 0 100
+    // @Increment: 0.1
+    // @User: Advanced
+    // @RebootRequired: True
+    AP_GROUPINFO("_THTL_MAX", 51, AP_EFI, throttle_max, 70),
+
+    // @Param: _EFCR_SLP
+    // @DisplayName: ECU Fuel Consumption Rate factor
+    // @Description: ECU FCR gradient/factor. Must be used along with _EFCR_OFT
+    // @Range: 0 1000
+    // @Increment: 0.1
+    // @User: Advanced
+    // @RebootRequired: True
+    AP_GROUPINFO("_EFCR_SLP", 52, AP_EFI, ecu_fcr_slope, 1),
+
+    // @Param: _EFCR_OFT
+    // @DisplayName: ECU Fuel Consumption Rate Offset
+    // @Description: ECU FCR intercept/offset. Must be used along with _EFCR_SLP
+    // @Range: 0 1000
+    // @Increment: 0.1
+    // @User: Advanced
+    // @RebootRequired: True
+    AP_GROUPINFO("_EFCR_OFT", 53, AP_EFI, ecu_fcr_offset, 0),
+
+    // @Param: _EFCR_AVG
+    // @DisplayName: ECU Fuel Consumption Rate Average count
+    // @Description: Averages _EFCR_AVG consecutive reading
+    // @Range: 0 100
+    // @Increment: 1
+    // @User: Advanced
+    // @RebootRequired: True
+    AP_GROUPINFO("_EFCR_AVG", 54, AP_EFI, ecu_fcr_average_count, 1),
+
+    // @Param: _FUEL_VOL
+    // @DisplayName: Full Fuel Volume / Capacity
+    // @Description: Full fuel volume in ml
+    // @Range: 0 65535
+    // @Increment: 1
+    // @User: Advanced
+    // @RebootRequired: True
+    AP_GROUPINFO("_FUEL_VOL", 55, AP_EFI, fuel_volume_in_ml, 1),
+
     AP_GROUPEND
 };
 
@@ -112,10 +197,15 @@ void AP_EFI::init(void)
         backend = new AP_EFI_Scripting(*this);
 #endif
         break;
+    case Type::Hirth:
+        backend = new AP_EFI_Serial_Hirth(*this);
+        break;
     default:
         GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Unknown EFI type");
         break;
     }
+
+    lua_fuel_consumed = 0;
 }
 
 // Ask all backends to update the frontend
@@ -123,6 +213,7 @@ void AP_EFI::update()
 {
     if (backend) {
         backend->update();
+        state.estimated_consumed_fuel_volume_cm3 = lua_fuel_consumed;
 #if HAL_LOGGING_ENABLED
         log_status();
 #endif
@@ -173,7 +264,8 @@ void AP_EFI::log_status(void)
                        float(state.oil_pressure),
                        float(state.oil_temperature),
                        float(state.fuel_pressure),
-                       float(state.fuel_consumption_rate_cm3pm),
+                       float(state.fuel_consumption_rate_raw),
+                       //float(state.fuel_consumption_rate_cm3pm),
                        float(state.estimated_consumed_fuel_volume_cm3),
                        uint8_t(state.throttle_position_percent),
                        uint8_t(state.ecu_index));
@@ -194,23 +286,62 @@ void AP_EFI::log_status(void)
 // @Field: SPU: Spark plug usage
 // @Field: IDX: Index of the publishing ECU
     AP::logger().WriteStreaming("EFI2",
-                       "TimeUS,Healthy,ES,GE,CSE,TS,FPS,OPS,DS,MS,DebS,SPU,IDX",
-                       "s------------",
-                       "F------------",
-                       "QBBBBBBBBBBBB",
-                       AP_HAL::micros64(),
-                       uint8_t(is_healthy()),
-                       uint8_t(state.engine_state),
-                       uint8_t(state.general_error),
-                       uint8_t(state.crankshaft_sensor_status),
-                       uint8_t(state.temperature_status),
-                       uint8_t(state.fuel_pressure_status),
-                       uint8_t(state.oil_pressure_status),
-                       uint8_t(state.detonation_status),
-                       uint8_t(state.misfire_status),
-                       uint8_t(state.debris_status),
-                       uint8_t(state.spark_plug_usage),
-                       uint8_t(state.ecu_index));
+                    "TimeUS,H,ES,SF,ETS,ATS,APS,TS,LCt,CT1_E,CT2_E,FR,FT,FA,IDX",
+                    "s--------------",
+                    "F--------------",
+                    "QBBBBBBBBBBfffB",
+                    AP_HAL::micros64(),
+                    uint8_t(is_healthy()),
+                    uint8_t(state.engine_state),
+                    uint8_t(state.save_in_flash),
+                    uint8_t(state.engine_temperature_sensor_status),
+                    uint8_t(state.air_temperature_sensor_status),
+                    uint8_t(state.air_pressure_sensor_status),
+                    uint8_t(state.throttle_sensor_status),
+                    uint8_t(state.no_of_log_data),
+                    uint8_t(state.CHT_1_error_excess_temperature_status),
+                    uint8_t(state.CHT_2_error_excess_temperature_status),
+                    float(state.fuel_consumption_rate_raw),
+                    float(state.total_fuel_consumed),
+                    float(state.fuel_consumption_rate_average),
+                    uint8_t(state.ecu_index));
+
+    AP::logger().WriteStreaming("EFI3",
+                    "TimeUS,E1_E,E2_E,C1_T,C2_T,E1_T,E2_T,k_th,thr_f,a_t,e_t,IDX",
+                    "s--OOOOOOOO-",
+                    "F--00000000-",
+                    "QBBffffffffB",
+                    AP_HAL::micros64(),
+                    uint8_t(state.EGT_1_error_excess_temperature_status),
+                    uint8_t(state.EGT_2_error_excess_temperature_status),
+                    float(state.cht1_temp),
+                    float(state.cht2_temp),
+                    float(state.egt1_temp),
+                    float(state.egt2_temp),
+                    float(state.k_throttle),
+                    float(state.thr_pos),
+                    float(state.air_temp),
+                    float(state.eng_temp),
+                    uint8_t(state.ecu_index));
+
+    AP::logger().WriteStreaming("EFI4",
+                    "TimeUS,BVOL,crc,uptime,lpc,ack,pkt,at,a1,a2,a3,IDX",
+                    "sv----------",
+                    "F-----------",
+                    "QfIIIIIIIIIB",
+                    AP_HAL::micros64(),
+                    float(state.battery_voltage),
+                    uint32_t(state.crc_fail_cnt),
+                    uint32_t(state.uptime),
+                    uint32_t(state.loop_cnt),
+                    uint32_t(state.ack_fail_cnt),
+                    uint32_t(state.packet_sent),
+                    uint32_t(state.ack_thr),
+                    uint32_t(state.ack_s1),
+                    uint32_t(state.ack_s2),
+                    uint32_t(state.ack_s3),
+                    uint8_t(state.ecu_index));
+
 
 // @LoggerMessage: ECYL
 // @Description: EFI per-cylinder information
@@ -228,12 +359,12 @@ void AP_EFI::log_status(void)
                                 "F-0C0000",
                                 "QBfffffB",
                                 AP_HAL::micros64(),
-                                0,
-                                state.cylinder_status.ignition_timing_deg,
-                                state.cylinder_status.injection_time_ms,
-                                state.cylinder_status.cylinder_head_temperature,
-                                state.cylinder_status.exhaust_gas_temperature,
-                                state.cylinder_status.lambda_coefficient,
+                           0,
+                           state.cylinder_status.ignition_timing_deg,
+                           state.cylinder_status.injection_time_ms,
+                           state.cylinder_status.cylinder_head_temperature,
+                           state.cylinder_status.exhaust_gas_temperature,
+                           state.cylinder_status.lambda_coefficient,
                                 state.ecu_index);
 }
 #endif // LOGGING_ENABLED
@@ -246,6 +377,8 @@ void AP_EFI::send_mavlink_status(mavlink_channel_t chan)
     if (!backend) {
         return;
     }
+    // Adding in new requested ECU telemetry fields for Hirth on 18/1/23 for live monitoring
+    // EGT and CGT variables are already in Celcius from Hirth
     mavlink_msg_efi_status_send(
         chan,
         AP_EFI::is_healthy(),
@@ -253,20 +386,20 @@ void AP_EFI::send_mavlink_status(mavlink_channel_t chan)
         state.engine_speed_rpm,
         state.estimated_consumed_fuel_volume_cm3,
         state.fuel_consumption_rate_cm3pm,
-        state.engine_load_percent,
-        state.throttle_position_percent,
-        state.spark_dwell_time_ms,
-        state.atmospheric_pressure_kpa,
-        state.intake_manifold_pressure_kpa,
-        KELVIN_TO_C(state.intake_manifold_temperature),
-        KELVIN_TO_C(state.cylinder_status.cylinder_head_temperature),
+        state.egt2_temp,        //was state.engine_load_percent, //EGT2
+        state.throttle_position_percent, //throttle position
+        state.spark_dwell_time_ms, //TBD
+        state.cht2_temp, //was barometric pressure/state.atmospheric_pressure_kpa, //CHT2
+        state.converted_map, //was state.intake_manifold_pressure_kpa
+        state.air_temp, //was KELVIN_TO_C(state.intake_manifold_temperature),
+        state.cht1_temp, //KELVIN_TO_C(state.cylinder_status[0].cylinder_head_temperature), //CHT1
         state.cylinder_status.ignition_timing_deg,
         state.cylinder_status.injection_time_ms,
-        KELVIN_TO_C(state.cylinder_status.exhaust_gas_temperature),
-        state.throttle_out,
-        state.pt_compensation,
+        state.egt1_temp, //EGT1
+        state.thr_pos, //throttle_out from 0 - 100
+        float(state.engine_state),//pt_compensation
         state.ignition_voltage
-        );
+        ); 
 }
 
 // get a copy of state structure
@@ -284,4 +417,3 @@ AP_EFI *EFI()
 }
 
 #endif // HAL_EFI_ENABLED
-
diff --git a/libraries/AP_EFI/AP_EFI.h b/libraries/AP_EFI/AP_EFI.h
index 1fefdd5972..919a45b758 100644
--- a/libraries/AP_EFI/AP_EFI.h
+++ b/libraries/AP_EFI/AP_EFI.h
@@ -58,6 +58,14 @@ class AP_EFI {
     // Returns the RPM
     uint32_t get_rpm() const { return state.engine_speed_rpm; }
 
+    // for debugging pupose, this value is updated using lua script (which in turn can obtain it from a serial connector)
+    bool set_fuel_consumed(float fuel_consumed) {
+        bool status = true; // send true by default. no health check logic implemented
+        lua_fuel_consumed = fuel_consumed;
+        // state.estimated_consumed_fuel_volume_cm3 = fuel_consumed;
+        return status;
+    }
+
     // returns enabled state of EFI
     bool enabled() const { return type != Type::NONE; }
 
@@ -84,8 +92,11 @@ class AP_EFI {
         DroneCAN = 5,
         CurrawongECU = 6,
         SCRIPTING  = 7,
+        Hirth = 8,
     };
 
+    float lua_fuel_consumed;
+
     static AP_EFI *get_singleton(void) {
         return singleton;
     }
@@ -104,6 +115,20 @@ class AP_EFI {
     AP_Float coef2;
 
     AP_Float ecu_fuel_density;
+    AP_Float throttle_idle;
+    AP_Float throttle_max;
+
+    // polynomial config variables for throttle linearization  
+    AP_Float throttle_firstorder;
+    AP_Float throttle_secondorder;
+    AP_Float throttle_thirdorder;
+    AP_Float throttle_offset;
+
+    // ECU fuel consumption calculation
+    AP_Float ecu_fcr_slope;
+    AP_Float ecu_fcr_offset;
+    AP_Int16 ecu_fcr_average_count;
+    AP_Int16 fuel_volume_in_ml;
 
     EFI_State state;
 
@@ -126,4 +151,4 @@ namespace AP {
     AP_EFI *EFI();
 };
 
-#endif // HAL_EFI_ENABLED
+#endif // HAL_EFI_ENABLED
\ No newline at end of file
diff --git a/libraries/AP_EFI/AP_EFI_Backend.cpp b/libraries/AP_EFI/AP_EFI_Backend.cpp
index 8eff55e857..a2de0d94d8 100644
--- a/libraries/AP_EFI/AP_EFI_Backend.cpp
+++ b/libraries/AP_EFI/AP_EFI_Backend.cpp
@@ -42,6 +42,49 @@ float AP_EFI_Backend::get_coef2(void) const
     return frontend.coef2;
 }
 
+float AP_EFI_Backend::get_throttle_firstorder(void) const
+{
+    return frontend.throttle_firstorder;
+}
+
+float AP_EFI_Backend::get_throttle_secondorder(void) const
+{
+    return frontend.throttle_secondorder;
+}
+
+float AP_EFI_Backend::get_throttle_thirdorder(void) const
+{
+    return frontend.throttle_thirdorder;
+}
+
+float AP_EFI_Backend::get_throttle_offset(void) const
+{
+    return frontend.throttle_offset;
+}
+
+float AP_EFI_Backend::get_ecu_fcr_slope(void) const
+{
+    return frontend.ecu_fcr_slope;
+}
+
+float AP_EFI_Backend::get_ecu_fcr_offset(void) const
+{
+    return frontend.ecu_fcr_offset;
+}
+
+int16_t AP_EFI_Backend::get_ecu_fcr_average_count(void) const
+{
+    if (frontend.ecu_fcr_average_count >= 255){
+        return 255;
+    }
+    return frontend.ecu_fcr_average_count;
+}
+
+int16_t AP_EFI_Backend::get_fuel_volume_in_ml(void) const
+{
+    return frontend.fuel_volume_in_ml;
+}
+
 HAL_Semaphore &AP_EFI_Backend::get_sem(void)
 {
     return frontend.sem;
diff --git a/libraries/AP_EFI/AP_EFI_Backend.h b/libraries/AP_EFI/AP_EFI_Backend.h
index 9506c7ff09..35ef016342 100644
--- a/libraries/AP_EFI/AP_EFI_Backend.h
+++ b/libraries/AP_EFI/AP_EFI_Backend.h
@@ -46,7 +46,16 @@ class AP_EFI_Backend {
     float get_coef1(void) const;
     float get_coef2(void) const;
     float get_ecu_fuel_density(void) const;
-
+    float get_throttle_idle(void) const;
+    float get_throttle_max(void) const;
+    float get_throttle_firstorder(void) const;
+    float get_throttle_secondorder(void) const;
+    float get_throttle_thirdorder(void) const;
+    float get_throttle_offset(void) const;
+    float get_ecu_fcr_slope(void) const;
+    float get_ecu_fcr_offset(void) const;
+    int16_t get_ecu_fcr_average_count(void) const;
+    int16_t get_fuel_volume_in_ml(void) const;
     HAL_Semaphore &get_sem(void);
 
 private:
diff --git a/libraries/AP_EFI/AP_EFI_Serial_Hirth.cpp b/libraries/AP_EFI/AP_EFI_Serial_Hirth.cpp
new file mode 100644
index 0000000000..d71ee3a22c
--- /dev/null
+++ b/libraries/AP_EFI/AP_EFI_Serial_Hirth.cpp
@@ -0,0 +1,385 @@
+/*
+   This program 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.
+
+   This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#include <AP_HAL/AP_HAL.h>
+
+#include "AP_EFI_Serial_Hirth.h"
+
+#if HAL_EFI_ENABLED
+#include <AP_SerialManager/AP_SerialManager.h>
+#include <SRV_Channel/SRV_Channel.h>
+#include <AP_ICEngine/AP_ICEngine.h>
+
+
+extern const AP_HAL::HAL& hal;
+
+/**
+ * @brief Constructor with port initialization
+ * 
+ * @param _frontend 
+ */
+AP_EFI_Serial_Hirth::AP_EFI_Serial_Hirth(AP_EFI &_frontend) : AP_EFI_Backend(_frontend) {
+    port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_EFI, 0);
+    get_throttle_polynomial();
+    fuel_avg_config = get_ecu_fcr_average_count();
+    fuel_avg_count = 0;
+}
+
+/**
+ * @brief initializes the polynomials 
+ * 
+ */
+void AP_EFI_Serial_Hirth::get_throttle_polynomial() {
+    throttle_fop = get_throttle_firstorder();
+    throttle_sop = get_throttle_secondorder();
+    throttle_top = get_throttle_thirdorder();
+    throttle_offset = get_throttle_offset();
+}
+
+/**
+ * @brief checks for response from or makes requests to Hirth ECU periodically
+ * 
+ */
+void AP_EFI_Serial_Hirth::update() {
+    bool status = false;
+    uint32_t now = AP_HAL::millis();
+
+    //log the delay in loop interval
+    internal_state.loop_cnt = now - last_loop_ms;
+    last_loop_ms = now;
+
+    if ((port != nullptr)) {
+
+        //waiting for response
+        if (waiting_response) {
+
+            // reset request if not response for SERIAL_WAIT_TIMEOUT-ms
+            if (now - last_response_ms > SERIAL_WAIT_TIMEOUT) {
+                waiting_response = false;
+                last_response_ms = now;
+
+                port->discard_input();
+
+                internal_state.ack_fail_cnt++;
+                if(data_send == 1) {internal_state.ack_s1++;}
+                if(data_send == 2) {internal_state.ack_s2++;}
+                if(data_send == 3) {internal_state.ack_s3++;}
+                if(data_send == 5) {internal_state.ack_thr++;}
+            }
+            else{
+
+                num_bytes = port->available();
+
+                // if already requested
+                if (num_bytes >= expected_bytes) {
+                    
+                    // read data from buffer
+                    uint8_t computed_checksum = 0;
+                    computed_checksum += res_data.quantity = port->read();
+                    computed_checksum += res_data.code = port->read();
+
+                    if (res_data.code == req_data.code) {
+                        for (int i = 0; i < (res_data.quantity - QUANTITY_REQUEST_STATUS); i++) {
+                            computed_checksum += raw_data[i] = port->read();
+                        }
+                    }
+
+                    res_data.checksum = port->read();
+                    computed_checksum = computed_checksum % BYTE_RANGE_MAX;
+                    if (res_data.checksum != (BYTE_RANGE_MAX - computed_checksum)) {
+                        internal_state.crc_fail_cnt++;
+                        port->discard_input();
+                    }
+                    else {
+                        internal_state.uptime = now - last_uptime;
+                        last_uptime = now;
+                        internal_state.last_updated_ms = now;
+                        decode_data();
+                        copy_to_frontend();
+                    }
+
+                    waiting_response = false;
+                }
+            }
+        }
+        
+        // sending cmd
+        if(!waiting_response) {
+
+            // get new throttle value
+            new_throttle = (uint16_t)SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
+
+            // check for change or timeout for throttle value
+            if ((new_throttle != old_throttle) || (now - last_req_send_throttle > 500)) {
+                // if new throttle value, send throttle request
+                // also send new throttle value, only when ARMED
+                bool allow_throttle = hal.util->get_soft_armed();
+                if (!allow_throttle) {
+                    const auto *ice = AP::ice();
+                    if (ice != nullptr) {
+                        allow_throttle = ice->allow_throttle_disarmed();
+                    }
+                }
+                if (allow_throttle) {
+                    status = send_target_values(new_throttle);
+                }
+                else{
+                    status = send_target_values(0);
+                }
+
+                old_throttle = new_throttle;
+                internal_state.k_throttle = new_throttle;
+                last_req_send_throttle = now;
+            }
+            else {
+                // request Status request, if no throttle commands
+                status = send_request_status();
+            }      
+
+            if (status == true) {
+                waiting_response = true;
+                last_response_ms = now;
+            }
+        }
+    }
+}
+
+
+/**
+ * @brief updates the current quantity that will be expected
+ * 
+ */
+void AP_EFI_Serial_Hirth::get_quantity() {
+    switch (req_data.code)
+    {
+    case CODE_REQUEST_STATUS_1:
+        expected_bytes = QUANTITY_RESPONSE_STATUS_1;
+        break;
+    case CODE_REQUEST_STATUS_2:
+        expected_bytes = QUANTITY_RESPONSE_STATUS_2;
+        break;
+    case CODE_REQUEST_STATUS_3:
+        expected_bytes = QUANTITY_RESPONSE_STATUS_3;
+        break;
+    case CODE_SET_VALUE:
+        expected_bytes = QUANTITY_ACK_SET_VALUES;
+        break;
+    }
+}
+
+
+/**
+ * @brief sends the new throttle command to Hirth ECU
+ * 
+ * @param thr - new throttle value given by SRV_Channel::k_throttle
+ * @return true - if success
+ * @return false - currently not implemented
+ */
+bool AP_EFI_Serial_Hirth::send_target_values(uint16_t thr) {
+    int idx = 0;
+    uint8_t computed_checksum = 0;
+
+    // clear buffer 
+    for (size_t i = 0; i < BYTE_RANGE_MAX; i++)
+    {
+        raw_data[i] = 0;
+    }
+    
+    // throttle value is computed as a polynomial defined by the config parameters
+    // to disable linearization, set throttle_top=0, throttle_sop=0, throttle_fop=1, throttle_offset=0
+    uint16_t throttle = ((throttle_top * thr * thr * thr) 
+                        + (throttle_sop * thr * thr) 
+                        + (throttle_fop * thr) 
+                        + throttle_offset) * THROTTLE_POSITION_FACTOR;
+
+    // set Quantity + Code + "20 bytes of records to set" + Checksum
+    computed_checksum += raw_data[idx++] = req_data.quantity = QUANTITY_SET_VALUE;
+    computed_checksum += raw_data[idx++] = req_data.code = CODE_SET_VALUE;
+    computed_checksum += raw_data[idx++] = throttle & 0xFF;
+    computed_checksum += raw_data[idx++] = (throttle >> 0x08) & 0xFF;
+
+    for (; idx < QUANTITY_SET_VALUE - 2; idx++) {
+        // 0 has no impact on checksum
+        raw_data[idx] = 0;
+    }
+    //checksum calculation
+    computed_checksum = computed_checksum % BYTE_RANGE_MAX;
+    raw_data[QUANTITY_SET_VALUE - 1] = (BYTE_RANGE_MAX - computed_checksum);
+
+    //debug
+    internal_state.packet_sent = 5;
+    data_send = 5;
+    
+    expected_bytes = QUANTITY_ACK_SET_VALUES;
+    //write data
+    port->write(raw_data, QUANTITY_SET_VALUE);
+
+    return true;
+}
+
+
+/**
+ * @brief cyclically sends different Status requests to Hirth ECU
+ * 
+ * @return true - when successful
+ * @return false  - not implemented
+ */
+bool AP_EFI_Serial_Hirth::send_request_status() {
+
+    bool status = false;
+
+    switch (req_data.code)
+    {
+    case CODE_REQUEST_STATUS_1:
+        req_data.quantity = QUANTITY_REQUEST_STATUS;
+        req_data.code = CODE_REQUEST_STATUS_2;
+        req_data.checksum = CHECKSUM_REQUEST_STATUS_2;
+        expected_bytes = QUANTITY_RESPONSE_STATUS_2;
+        internal_state.packet_sent = 2;
+        data_send =2;
+        break;
+    case CODE_REQUEST_STATUS_2:
+        req_data.quantity = QUANTITY_REQUEST_STATUS;
+        req_data.code = CODE_REQUEST_STATUS_3;
+        req_data.checksum = CHECKSUM_REQUEST_STATUS_3;
+        expected_bytes = QUANTITY_RESPONSE_STATUS_3;
+        internal_state.packet_sent = 3;
+        data_send =3;
+        break;
+    case CODE_REQUEST_STATUS_3:
+        req_data.quantity = QUANTITY_REQUEST_STATUS;
+        req_data.code = CODE_REQUEST_STATUS_1;
+        req_data.checksum = CHECKSUM_REQUEST_STATUS_1;
+        expected_bytes = QUANTITY_RESPONSE_STATUS_1;
+        internal_state.packet_sent = 1;
+        data_send =1;
+        break;
+    default:
+        req_data.quantity = QUANTITY_REQUEST_STATUS;
+        req_data.code = CODE_REQUEST_STATUS_1;
+        req_data.checksum = CHECKSUM_REQUEST_STATUS_1;
+        expected_bytes = QUANTITY_RESPONSE_STATUS_1;
+        internal_state.packet_sent = 1;
+        data_send =1;
+        break;
+    }
+    //memcpy(&raw_data, 0, sizeof(raw_data));
+    raw_data[0] = req_data.quantity;
+    raw_data[1] = req_data.code;
+    raw_data[2] = req_data.checksum;
+
+    port->write(raw_data, QUANTITY_REQUEST_STATUS);
+
+    status = true;
+
+    return status;
+}
+
+
+/**
+ * @brief parses the response from Hirth ECU and updates the internal state instance
+ * 
+ */
+void AP_EFI_Serial_Hirth::decode_data() {
+    const uint32_t now_temp = AP_HAL::millis();
+
+    int engine_status = 0;
+
+    int excess_temp_error = 0;
+
+    switch (res_data.code)
+    {
+    case CODE_REQUEST_STATUS_1:
+        engine_status = (raw_data[8] | raw_data[9] << 0x08);
+
+        //EFI2 Log
+        internal_state.engine_state = (Engine_State)engine_status;
+        internal_state.save_in_flash = raw_data[1] | raw_data[2] << 0x08;
+        internal_state.engine_temperature_sensor_status = raw_data[82] & 0x01;
+        internal_state.air_temperature_sensor_status = raw_data[82] & 0x02;
+        internal_state.air_pressure_sensor_status = raw_data[82] & 0x04;
+        internal_state.throttle_sensor_status = raw_data[82] & 0x08;
+        internal_state.thr_pos = (raw_data[72] | raw_data[73] << 0x08);
+        internal_state.air_temp = (raw_data[78] | raw_data[79] << 0x08);
+        internal_state.eng_temp = (raw_data[74] | raw_data[75] << 0x08);
+        //add in ADC voltage of MAP sensor > convert to MAP in kPa
+        internal_state.converted_map = (raw_data[50] | raw_data[51] << 0x08) * ADC_CALIBRATION * MAP_HPA_PER_VOLT_FACTOR * HPA_TO_KPA;
+        internal_state.battery_voltage = (raw_data[76] | raw_data[77] << 0x08) * VOLTAGE_RESOLUTION;
+
+        //EFI1 Log
+        internal_state.engine_speed_rpm = (raw_data[10] | raw_data[11] << 0x08);
+        internal_state.cylinder_status.injection_time_ms = ((raw_data[32] | raw_data[33] << 0x08)) * INJECTION_TIME_RESOLUTION;
+        internal_state.cylinder_status.ignition_timing_deg = (raw_data[34] | raw_data[35] << 0x08);
+        //air temperature
+        internal_state.cylinder_status.cylinder_head_temperature = (raw_data[78] | raw_data[79] << 0x08) + KELVIN_CONVERSION_CONSTANT;
+        //engine temperature
+        internal_state.cylinder_status.exhaust_gas_temperature = (raw_data[74] | raw_data[75] << 0x08) + KELVIN_CONVERSION_CONSTANT;
+        internal_state.crankshaft_sensor_status = ((raw_data[82] & CRANK_SHAFT_SENSOR_OK) == CRANK_SHAFT_SENSOR_OK) ? Crankshaft_Sensor_Status::OK : Crankshaft_Sensor_Status::ERROR;     
+
+        internal_state.intake_manifold_temperature = internal_state.air_temp;
+
+        break;
+
+    case CODE_REQUEST_STATUS_2:
+
+        fuel_consumption_rate_raw = (raw_data[52] | raw_data[53] << 0x08) / FUEL_CONSUMPTION_RESOLUTION;
+        internal_state.fuel_consumption_rate_raw = get_avg_fuel_consumption_rate(fuel_consumption_rate_raw);
+        internal_state.fuel_consumption_rate_cm3pm = (fuel_consumption_rate_raw * get_ecu_fcr_slope()) + get_ecu_fcr_offset();
+
+        if (last_fuel_integration_ms != 0) {
+            internal_state.estimated_consumed_fuel_volume_cm3 += internal_state.fuel_consumption_rate_cm3pm * (now_temp - last_fuel_integration_ms)/60000.0f;
+        }
+        last_fuel_integration_ms = now_temp;
+
+        total_fuel_consumed = total_fuel_consumed + internal_state.fuel_consumption_rate_cm3pm;
+        internal_state.total_fuel_consumed = total_fuel_consumed;
+        internal_state.throttle_position_percent = (raw_data[62] | raw_data[63] << 0x08) / 10;
+        break;
+
+    case CODE_REQUEST_STATUS_3: // TBD - internal state addition
+        excess_temp_error = (raw_data[46] | raw_data[47] << 0x08);
+
+        //EFI2 Log
+        internal_state.CHT_1_error_excess_temperature_status = (excess_temp_error & 0x01) || (excess_temp_error & 0x02) || (excess_temp_error & 0x04);
+        internal_state.CHT_2_error_excess_temperature_status = (excess_temp_error & 0x08) || (excess_temp_error & 0x10) || (excess_temp_error & 0x20);
+        internal_state.EGT_1_error_excess_temperature_status = (excess_temp_error & 0x40) || (excess_temp_error & 0x80) || (excess_temp_error & 0x100);
+        internal_state.EGT_2_error_excess_temperature_status = (excess_temp_error & 0x200) || (excess_temp_error & 0x400) || (excess_temp_error & 0x800);
+        internal_state.cht1_temp = (raw_data[16] | raw_data[17] << 0x08);
+        internal_state.cht2_temp = (raw_data[18] | raw_data[19] << 0x08);
+        internal_state.egt1_temp = (raw_data[20] | raw_data[21] << 0x08);
+        internal_state.egt2_temp = (raw_data[22] | raw_data[23] << 0x08);
+        break;
+        
+    // case CODE_SET_VALUE:
+    //     // Do nothing for now
+    //     break;
+    }
+}
+
+
+float AP_EFI_Serial_Hirth::get_avg_fuel_consumption_rate(float fuel_consumed) {
+    fuel_avg_count = (fuel_avg_count) % fuel_avg_config;
+    float avg_fuel_consumed = 0;
+
+    instance_fuel_reading[fuel_avg_count++] = fuel_consumed;
+    for (int i = 0; i < fuel_avg_config; i++) {
+        avg_fuel_consumed += instance_fuel_reading[i];
+    }
+    
+    return (avg_fuel_consumed / fuel_avg_config);
+}
+
+#endif // HAL_EFI_ENABLED
diff --git a/libraries/AP_EFI/AP_EFI_Serial_Hirth.h b/libraries/AP_EFI/AP_EFI_Serial_Hirth.h
new file mode 100644
index 0000000000..55d807e374
--- /dev/null
+++ b/libraries/AP_EFI/AP_EFI_Serial_Hirth.h
@@ -0,0 +1,145 @@
+/*
+   This program 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.
+
+   This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#pragma once
+
+
+#include "AP_EFI.h"
+#include "AP_EFI_Backend.h"
+
+
+#define HIRTH_MAX_PKT_SIZE 100
+#define HIRTH_MAX_RAW_PKT_SIZE 103
+
+#define BYTE_RANGE_MAX 256
+
+#define SERIAL_WAIT_DURATION 100
+#define SERIAL_WAIT_TIMEOUT 100
+
+#define ENGINE_RUNNING 4
+#define THROTTLE_POSITION_FACTOR 10
+#define CRANK_SHAFT_SENSOR_OK 0x0F
+#define INJECTION_TIME_RESOLUTION 0.8
+#define FUEL_CONSUMPTION_RESOLUTION 10.0
+#define THROTTLE_POSITION_RESOLUTION 0.1
+#define KELVIN_CONVERSION_CONSTANT 273.5
+#define VOLTAGE_RESOLUTION 0.0049       /* 5/1024 */
+#define ADC_CALIBRATION 5.0/1024.0
+#define MAP_HPA_PER_VOLT_FACTOR 248.2673
+#define HPA_TO_KPA 0.1
+#define TPS_SCALE 0.70
+
+const uint8_t QUANTITY_REQUEST_STATUS    = 0x03;
+const uint8_t QUANTITY_SET_VALUE         = 0x17;
+
+const uint8_t CODE_REQUEST_STATUS_1      = 0x04;
+const uint8_t CODE_REQUEST_STATUS_2      = 0x0B;
+const uint8_t CODE_REQUEST_STATUS_3      = 0x0D;
+const uint8_t CODE_SET_VALUE             = 0xC9;
+
+const uint8_t CHECKSUM_REQUEST_STATUS_1  = 0xF9;
+const uint8_t CHECKSUM_REQUEST_STATUS_2  = 0xF2;
+const uint8_t CHECKSUM_REQUEST_STATUS_3  = 0xF0;
+const uint8_t CHECKSUM_SET_VALUE         = 0x34;
+
+const uint8_t QUANTITY_RESPONSE_STATUS_1 = 0x57;
+const uint8_t QUANTITY_RESPONSE_STATUS_2 = 0x65;
+const uint8_t QUANTITY_RESPONSE_STATUS_3 = 0x67;
+const uint8_t QUANTITY_ACK_SET_VALUES    = 0x03;
+
+
+
+/*!
+ * Data mapping between rawBytes and Telemetry packets
+ */
+typedef struct
+{
+    uint8_t quantity;
+    uint8_t code;
+    uint8_t checksum;
+} data_set_t;
+
+
+/*!
+ * class definition for Hirth 4103 ECU
+ */
+class AP_EFI_Serial_Hirth: public AP_EFI_Backend
+{
+public:
+    AP_EFI_Serial_Hirth(AP_EFI &_frontend);
+
+    void update() override;
+
+    void decode_data();
+
+    bool send_request_status();
+
+    bool send_target_values(uint16_t);
+
+    void get_quantity();
+
+    float get_avg_fuel_consumption_rate(float fuel_consumed);
+
+    void get_throttle_polynomial();
+
+private:
+    // serial port instance
+    AP_HAL::UARTDriver *port;
+
+    // periodic refresh 
+    uint32_t last_response_ms;
+    uint32_t last_loop_ms;
+    uint32_t last_uptime;
+    uint32_t last_req_send_throttle;
+
+    // raw bytes - max size
+    uint8_t raw_data[BYTE_RANGE_MAX];
+
+    // request and response data 
+    data_set_t req_data;
+    data_set_t res_data;
+
+    // number of bytes 
+    uint32_t num_bytes;
+
+    // TRUE - Request is sent; waiting for response
+    // FALSE - Response is already received
+    bool waiting_response;
+
+    // Expected bytes from response
+    uint8_t expected_bytes;
+
+    // Throttle values
+    uint16_t new_throttle;
+    uint16_t old_throttle;
+
+    float fuel_consumption_rate_average;
+    float fuel_consumption_rate_raw;
+    float fuel_consumption_rate;
+    float total_fuel_consumed;
+    uint32_t last_fuel_integration_ms;
+    float instance_fuel_reading[BYTE_RANGE_MAX];
+    uint8_t fuel_avg_count;
+    uint8_t fuel_avg_config;
+
+
+    float throttle_fop;
+    float throttle_sop;
+    float throttle_top;
+    float throttle_offset;
+
+    uint8_t data_send;
+};
diff --git a/libraries/AP_EFI/AP_EFI_State.h b/libraries/AP_EFI/AP_EFI_State.h
index 6b0abc93d2..c4f24e1272 100644
--- a/libraries/AP_EFI/AP_EFI_State.h
+++ b/libraries/AP_EFI/AP_EFI_State.h
@@ -33,8 +33,9 @@
 enum class Engine_State : uint8_t {
     STOPPED  = 0,
     STARTING = 1,
-    RUNNING  = 2,
-    FAULT    = 3
+    FAULT    = 2,
+    STARTING_WARM_UP = 3,
+    RUNNING  = 4
 };
 
 enum class Crankshaft_Sensor_Status : uint8_t {
@@ -206,4 +207,38 @@ struct EFI_State {
 
     // PT compensation
     float pt_compensation;
+    
+    //custom parameters CX
+    uint8_t save_in_flash;
+    uint8_t engine_temperature_sensor_status;
+    uint8_t air_temperature_sensor_status;
+    uint8_t air_pressure_sensor_status;
+    uint8_t throttle_sensor_status;
+    uint8_t no_of_log_data;
+    uint8_t CHT_1_error_excess_temperature_status;
+    uint8_t CHT_2_error_excess_temperature_status;
+    uint8_t EGT_1_error_excess_temperature_status;
+    uint8_t EGT_2_error_excess_temperature_status;
+    float cht1_temp;
+    float cht2_temp;
+    float egt1_temp;
+    float egt2_temp;
+    float k_throttle;
+    float thr_pos;
+    float air_temp;
+    float eng_temp;
+    float battery_voltage;
+    float converted_map;
+    uint32_t crc_fail_cnt;
+    uint32_t uptime;
+    uint32_t loop_cnt;
+    uint32_t ack_fail_cnt;
+    uint32_t packet_sent;
+    uint32_t ack_thr;
+    uint32_t ack_s1;
+    uint32_t ack_s2;
+    uint32_t ack_s3;
+    float fuel_consumption_rate_average;
+    float fuel_consumption_rate_raw;
+    float total_fuel_consumed;
 };
diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
index b0ee54c5d9..8eab14fc37 100644
--- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
+++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
@@ -435,6 +435,9 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem
     if (data_mask & AP_ESC_Telem_Backend::TelemetryType::USAGE) {
         _telem_data[esc_index].usage_s = new_data.usage_s;
     }
+    if (data_mask & AP_ESC_Telem_Backend::TelemetryType::ERROR_STATUS) {
+        _telem_data[esc_index].error_status = new_data.error_status;
+    }
 
     _telem_data[esc_index].count++;
     _telem_data[esc_index].types |= data_mask;
@@ -504,7 +507,8 @@ void AP_ESC_Telem::update()
                     esc_temp    : _telem_data[i].temperature_cdeg,
                     current_tot : _telem_data[i].consumption_mah,
                     motor_temp  : _telem_data[i].motor_temp_cdeg,
-                    error_rate  : _rpm_data[i].error_rate
+                    error_rate  : _rpm_data[i].error_rate,
+                    error_status: _telem_data[i].error_status
                 };
                 AP::logger().WriteBlock(&pkt, sizeof(pkt));
                 _last_telem_log_ms[i] = _telem_data[i].last_update_ms;
diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h
index 769807230f..7bd7610a99 100644
--- a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h
+++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h
@@ -25,6 +25,7 @@ class AP_ESC_Telem_Backend {
         uint32_t last_update_ms;    // last update time in milliseconds, determines whether active
         uint16_t types;             // telemetry types present
         uint16_t count;             // number of times updated
+        uint32_t error_status;      // error status
     };
 
     struct RpmData {
@@ -41,7 +42,8 @@ class AP_ESC_Telem_Backend {
         VOLTAGE     = 1 << 2,
         CURRENT     = 1 << 3,
         CONSUMPTION = 1 << 4,
-        USAGE       = 1 << 5
+        USAGE       = 1 << 5,
+        ERROR_STATUS= 1 << 6
     };
 
 
diff --git a/libraries/AP_ESC_Telem/LogStructure.h b/libraries/AP_ESC_Telem/LogStructure.h
index 89d0578875..91fe179905 100644
--- a/libraries/AP_ESC_Telem/LogStructure.h
+++ b/libraries/AP_ESC_Telem/LogStructure.h
@@ -17,6 +17,7 @@
 // @Field: CTot: current consumed total mAh
 // @Field: MotTemp: measured motor temperature in centi-degrees C
 // @Field: Err: error rate
+// @Field: error_status: error status
 struct PACKED log_Esc {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -29,8 +30,9 @@ struct PACKED log_Esc {
     float current_tot;
     int16_t motor_temp;
     float error_rate;
+    uint32_t error_status;
 };
 
 #define LOG_STRUCTURE_FROM_ESC_TELEM \
     { LOG_ESC_MSG, sizeof(log_Esc), \
-      "ESC",  "QBeeffcfcf", "TimeUS,Instance,RPM,RawRPM,Volt,Curr,Temp,CTot,MotTemp,Err", "s#qqvAOaO%", "F-BB--BCB-" , true },
+      "ESC",  "QBeeffcfcfI", "TimeUS,Instance,RPM,RawRPM,Volt,Curr,Temp,CTot,MotTemp,Err,ErrS", "s#qqvAOaO%-", "F-BB--BCB--" , true },
diff --git a/libraries/AP_HAL/CANIface.h b/libraries/AP_HAL/CANIface.h
index 30a5564e3e..cd17c42b91 100644
--- a/libraries/AP_HAL/CANIface.h
+++ b/libraries/AP_HAL/CANIface.h
@@ -220,9 +220,30 @@ class AP_HAL::CANIface
         return 0;
     }
 
+    typedef struct {
+        uint32_t tx_requests;
+        uint32_t tx_rejected;
+        uint32_t tx_overflow;
+        uint32_t tx_success;
+        uint32_t tx_timedout;
+        uint32_t tx_abort;
+        uint32_t rx_received;
+        uint32_t rx_overflow;
+        uint32_t rx_errors;
+        uint32_t num_busoff_err;
+    } bus_stats_t;
+
+#if !defined(HAL_BOOTLOADER_BUILD)
     //Get status info of the interface
     virtual void get_stats(ExpandingString &str) {}
 
+    /*
+      return bus statistics for logging
+      return nullptr if no statistics available
+     */
+    virtual const bus_stats_t *get_statistics(void) const { return nullptr; };
+#endif
+
     // return true if busoff was detected and not cleared
     virtual bool is_busoff() const
     {
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/defaults.parm
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/hwdef-bl.dat
new file mode 100644
index 0000000000..06557e5b42
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/hwdef-bl.dat
@@ -0,0 +1,16 @@
+# hw definition file for processing by chibios_hwdef.py
+# for H743 bootloader
+
+include ../CubeOrange/hwdef-bl.dat
+
+undef USB_STRING_PRODUCT
+undef USB_STRING_MANUFACTURER
+undef APJ_BOARD_ID
+undef USB_PRODUCT
+
+# USB setup
+USB_PRODUCT 0x1056
+USB_STRING_MANUFACTURER "CubePilot"
+USB_STRING_PRODUCT "Carbonix-bl"
+
+APJ_BOARD_ID 5000
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/hwdef.dat
new file mode 100644
index 0000000000..c86431b5f4
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/hwdef.dat
@@ -0,0 +1,15 @@
+# hw definition file for processing by chibios_hwdef.py
+
+include ../CubeOrange/hwdef.dat
+
+undef USB_STRING_PRODUCT
+undef USB_STRING_MANUFACTURER
+undef APJ_BOARD_ID
+undef USB_PRODUCT
+
+# USB setup
+USB_PRODUCT 0x1057
+USB_STRING_MANUFACTURER "CubePilot"
+USB_STRING_PRODUCT "Carbonix"
+
+APJ_BOARD_ID 5000
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/scripts/CX_LED_driver.lua b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/scripts/CX_LED_driver.lua
new file mode 100644
index 0000000000..c532fa1bf9
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/scripts/CX_LED_driver.lua
@@ -0,0 +1,50 @@
+
+-- LED is switched on using motor function
+local MOTOR_LED_FUNCTION = 94
+
+-- Ensure that the out2_function min = 200 and max = 10000
+local MOTOR_LED_PWM_VAL_TRIM  = 5000
+local MOTOR_LED_PWM_VAL_MIN  = 200
+local MOTOR_LED_PWM_VAL_MAX  = 10000
+local MOTOR_LED_PWM_VAL_OFF  = 0
+
+-- Arming check interval
+local LED_CYCLE_TIME = 2000
+
+
+local LED_servo = SRV_Channels:find_channel(MOTOR_LED_FUNCTION)
+
+
+gcs:send_text(0, "LED -- BEGIN")
+
+
+-- LED is switched ON using a PWM signal configured as MOTOR_LED_FUNCTION
+-- This is done to overcome the design limitations on CPN
+local function switch_LED(LED_val)
+
+    if (LED_servo ~= null) then
+        SRV_Channels:set_output_pwm_chan_timeout(LED_servo, LED_val, LED_CYCLE_TIME)
+    end
+
+    return check_arming, LED_CYCLE_TIME
+end
+
+
+
+-- Function to continuously check if the vehicle is armed or not
+-- ARMED -> LED_ON
+-- DISARMED -> LED_OFF 
+local function check_arming()
+    vehicle_armed = arming:is_armed()
+    
+    if vehicle_armed == true then
+        switch_LED(MOTOR_LED_PWM_VAL_MAX)
+    else
+        switch_LED(MOTOR_LED_PWM_VAL_MIN)
+    end
+
+    return check_arming, LED_CYCLE_TIME
+end
+
+return check_arming(), 10000
+
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LTail/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LTail/defaults.parm
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LTail/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LTail/hwdef-bl.dat
new file mode 100644
index 0000000000..35b51d805b
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LTail/hwdef-bl.dat
@@ -0,0 +1,8 @@
+include ../CarbonixF405/hwdef-bl.dat
+
+undef CAN_APP_NODE_NAME
+undef APJ_BOARD_ID
+
+APJ_BOARD_ID 5008
+
+define CAN_APP_NODE_NAME "CPN-27-Left-Tail"
\ No newline at end of file
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LTail/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LTail/hwdef.dat
new file mode 100644
index 0000000000..b5d05539f0
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LTail/hwdef.dat
@@ -0,0 +1,9 @@
+include ../CarbonixF405/hwdef.dat
+
+undef CAN_APP_NODE_NAME
+undef APJ_BOARD_ID
+
+APJ_BOARD_ID 5008
+
+define CAN_APP_NODE_NAME "CPN-27-Left-Tail"
+
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LWing/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LWing/defaults.parm
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LWing/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LWing/hwdef-bl.dat
new file mode 100644
index 0000000000..aca49879e6
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LWing/hwdef-bl.dat
@@ -0,0 +1,8 @@
+include ../CarbonixF405/hwdef-bl.dat
+
+undef CAN_APP_NODE_NAME
+undef APJ_BOARD_ID
+
+APJ_BOARD_ID 5006
+
+define CAN_APP_NODE_NAME "CPN-26-Left-Wing"
\ No newline at end of file
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LWing/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LWing/hwdef.dat
new file mode 100644
index 0000000000..1381083d32
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LWing/hwdef.dat
@@ -0,0 +1,9 @@
+include ../CarbonixF405/hwdef.dat
+
+undef CAN_APP_NODE_NAME
+undef APJ_BOARD_ID
+
+APJ_BOARD_ID 5006
+
+define CAN_APP_NODE_NAME "CPN-26-Left-Wing"
+
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M1/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M1/defaults.parm
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M1/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M1/hwdef-bl.dat
new file mode 100644
index 0000000000..b932184550
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M1/hwdef-bl.dat
@@ -0,0 +1,8 @@
+include ../CarbonixF405/hwdef-bl.dat
+
+undef CAN_APP_NODE_NAME
+undef APJ_BOARD_ID
+
+APJ_BOARD_ID 5001
+
+define CAN_APP_NODE_NAME "CPN-11-Motor-1"
\ No newline at end of file
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M1/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M1/hwdef.dat
new file mode 100644
index 0000000000..975f7179fa
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M1/hwdef.dat
@@ -0,0 +1,9 @@
+include ../CarbonixF405/hwdef.dat
+
+undef CAN_APP_NODE_NAME
+undef APJ_BOARD_ID
+
+APJ_BOARD_ID 5001
+
+define CAN_APP_NODE_NAME "CPN-11-Motor-1"
+
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M2/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M2/defaults.parm
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M2/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M2/hwdef-bl.dat
new file mode 100644
index 0000000000..ae045b604b
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M2/hwdef-bl.dat
@@ -0,0 +1,8 @@
+include ../CarbonixF405/hwdef-bl.dat
+
+undef CAN_APP_NODE_NAME
+undef APJ_BOARD_ID
+
+APJ_BOARD_ID 5002
+
+define CAN_APP_NODE_NAME "CPN-12-Motor-2"
\ No newline at end of file
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M2/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M2/hwdef.dat
new file mode 100644
index 0000000000..dcc67fc75a
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M2/hwdef.dat
@@ -0,0 +1,9 @@
+include ../CarbonixF405/hwdef.dat
+
+undef CAN_APP_NODE_NAME
+undef APJ_BOARD_ID
+
+APJ_BOARD_ID 5002
+
+define CAN_APP_NODE_NAME "CPN-12-Motor-2"
+
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M3/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M3/defaults.parm
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M3/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M3/hwdef-bl.dat
new file mode 100644
index 0000000000..e9432facd3
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M3/hwdef-bl.dat
@@ -0,0 +1,8 @@
+include ../CarbonixF405/hwdef-bl.dat
+
+undef CAN_APP_NODE_NAME
+undef APJ_BOARD_ID
+
+APJ_BOARD_ID 5003
+
+define CAN_APP_NODE_NAME "CPN-23-Motor-3"
\ No newline at end of file
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M3/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M3/hwdef.dat
new file mode 100644
index 0000000000..d118a02233
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M3/hwdef.dat
@@ -0,0 +1,8 @@
+include ../CarbonixF405/hwdef.dat
+
+undef CAN_APP_NODE_NAME
+undef APJ_BOARD_ID
+
+APJ_BOARD_ID 5003
+
+define CAN_APP_NODE_NAME "CPN-23-Motor-3"
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M4/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M4/defaults.parm
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M4/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M4/hwdef-bl.dat
new file mode 100644
index 0000000000..1f195d2039
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M4/hwdef-bl.dat
@@ -0,0 +1,8 @@
+include ../CarbonixF405/hwdef-bl.dat
+
+undef CAN_APP_NODE_NAME
+undef APJ_BOARD_ID
+
+APJ_BOARD_ID 5004
+
+define CAN_APP_NODE_NAME "CPN-24-Motor-4"
\ No newline at end of file
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M4/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M4/hwdef.dat
new file mode 100644
index 0000000000..3fb1b9d2d8
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M4/hwdef.dat
@@ -0,0 +1,8 @@
+include ../CarbonixF405/hwdef.dat
+
+undef CAN_APP_NODE_NAME
+undef APJ_BOARD_ID
+
+APJ_BOARD_ID 5004
+
+define CAN_APP_NODE_NAME "CPN-24-Motor-4"
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M5/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M5/defaults.parm
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M5/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M5/hwdef-bl.dat
new file mode 100644
index 0000000000..024b0630bc
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M5/hwdef-bl.dat
@@ -0,0 +1,8 @@
+include ../CarbonixF405/hwdef-bl.dat
+
+undef CAN_APP_NODE_NAME
+undef APJ_BOARD_ID
+
+APJ_BOARD_ID 5005
+
+define CAN_APP_NODE_NAME "CPN-15-Pusher"
\ No newline at end of file
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M5/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M5/hwdef.dat
new file mode 100644
index 0000000000..1e35c45bcb
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M5/hwdef.dat
@@ -0,0 +1,8 @@
+include ../CarbonixF405/hwdef.dat
+
+undef CAN_APP_NODE_NAME
+undef APJ_BOARD_ID
+
+APJ_BOARD_ID 5005
+
+define CAN_APP_NODE_NAME "CPN-15-Pusher"
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RTail/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RTail/defaults.parm
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RTail/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RTail/hwdef-bl.dat
new file mode 100644
index 0000000000..eae4bf756c
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RTail/hwdef-bl.dat
@@ -0,0 +1,8 @@
+include ../CarbonixF405/hwdef-bl.dat
+
+undef CAN_APP_NODE_NAME
+undef APJ_BOARD_ID
+
+APJ_BOARD_ID 5009
+
+define CAN_APP_NODE_NAME "CPN-17-Right-Tail"
\ No newline at end of file
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RTail/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RTail/hwdef.dat
new file mode 100644
index 0000000000..35138a6454
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RTail/hwdef.dat
@@ -0,0 +1,9 @@
+include ../CarbonixF405/hwdef.dat
+
+undef CAN_APP_NODE_NAME
+undef APJ_BOARD_ID
+
+APJ_BOARD_ID 5009
+
+define CAN_APP_NODE_NAME "CPN-17-Right-Tail"
+
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RWing/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RWing/defaults.parm
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RWing/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RWing/hwdef-bl.dat
new file mode 100644
index 0000000000..28fafd40d3
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RWing/hwdef-bl.dat
@@ -0,0 +1,8 @@
+include ../CarbonixF405/hwdef-bl.dat
+
+undef CAN_APP_NODE_NAME
+undef APJ_BOARD_ID
+
+APJ_BOARD_ID 5007
+
+define CAN_APP_NODE_NAME "CPN-16-Right-Wing"
\ No newline at end of file
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RWing/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RWing/hwdef.dat
new file mode 100644
index 0000000000..a12fc21f2c
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RWing/hwdef.dat
@@ -0,0 +1,9 @@
+include ../CarbonixF405/hwdef.dat
+
+undef CAN_APP_NODE_NAME
+undef APJ_BOARD_ID
+
+APJ_BOARD_ID 5007
+
+define CAN_APP_NODE_NAME "CPN-16-Right-Wing"
+
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef-bl.dat
index c9b65a069f..e048f428f4 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef-bl.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef-bl.dat
@@ -15,8 +15,8 @@ APJ_BOARD_ID 1064
 # setup build for a peripheral firmware
 env AP_PERIPH 1
 
-# crystal frequency set to 0 to use internal clock
-OSCILLATOR_HZ 0
+# crystal frequency set to 0 to use internal clock; currently configured for external crystal 12MHZ
+OSCILLATOR_HZ 12000000
 
 # assume 1024K flash part
 FLASH_SIZE_KB 1024
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat
index 011bf96f83..3b5649331a 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat
@@ -16,8 +16,8 @@ APJ_BOARD_ID 1064
 # setup build for a peripheral firmware
 env AP_PERIPH 1
 
-# crystal frequency set to 0 to use internal clock
-OSCILLATOR_HZ 0
+# crystal frequency set to 12MHz to use internal clock
+OSCILLATOR_HZ 12000000
 
 #MCU F405 Flash 1024
 FLASH_SIZE_KB 1024
@@ -124,22 +124,30 @@ define HAL_PERIPH_ENABLE_RC_OUT
 # enable ESC control
 define HAL_SUPPORT_RCOUT_SERIAL 1
 define HAL_WITH_ESC_TELEM 1
+define HAL_PERIPH_ENABLE_ESC_APDHVPRO200
 
 # enable GPS
 define HAL_PERIPH_ENABLE_GPS
 define HAL_PERIPH_GPS_PORT_DEFAULT 2
-#define HAL_PERIPH_ENABLE_NOTIFY
+define HAL_PERIPH_ENABLE_NOTIFY
 #define HAL_PERIPH_ENABLE_NCP5623_LED_WITHOUT_NOTIFY
 
 # default ADSB off by setting 0 baudrate
-define HAL_PERIPH_ENABLE_ADSB
-define HAL_PERIPH_ADSB_PORT_DEFAULT 3
-define HAL_PERIPH_ADSB_BAUD_DEFAULT 57600
+#define HAL_PERIPH_ENABLE_ADSB
+#define HAL_PERIPH_ADSB_PORT_DEFAULT 3
+#define HAL_PERIPH_ADSB_BAUD_DEFAULT 57600
+
+define HAL_PERIPH_ARM_MONITORING_ENABLE 1
 
 BARO MS56XX I2C:0:0x76
-COMPASS MMC5XX3 I2C:0:0x30 false ROTATION_NONE
+COMPASS QMC5883P I2C:0:0x2C false ROTATION_YAW_180
 
+#GPS Compass Enable
+define HAL_PROBE_EXTERNAL_I2C_COMPASSES
+COMPASS IST8310 I2C:ALL_EXTERNAL:0x0E true ROTATION_NONE
 
+# enable Rangefinder
+define HAL_PERIPH_ENABLE_RANGEFINDER 1
 
 
 
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat
index 1b5ff08dc7..07437488ba 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat
@@ -127,6 +127,7 @@ define HAL_PERIPH_ENABLE_RC_OUT
 # enable ESC control
 define HAL_SUPPORT_RCOUT_SERIAL 1
 define HAL_WITH_ESC_TELEM 1
+define HAL_PERIPH_ENABLE_ESC_APDHVPRO200
 
 # enable GPS
 define HAL_PERIPH_ENABLE_GPS
@@ -139,9 +140,14 @@ define HAL_PERIPH_ENABLE_ADSB
 define HAL_PERIPH_ADSB_PORT_DEFAULT 3
 define HAL_PERIPH_ADSB_BAUD_DEFAULT 57600
 
+define HAL_PERIPH_ARM_MONITORING_ENABLE 1
+
 BARO MS56XX I2C:0:0x76
-COMPASS MMC5XX3 I2C:0:0x30 false ROTATION_NONE
+COMPASS QMC5883P I2C:0:0x2C false ROTATION_YAW_180
 
+#GPS Compass Enable
+define HAL_PROBE_EXTERNAL_I2C_COMPASSES
+COMPASS IST8310 I2C:ALL_EXTERNAL:0x0E true ROTATION_NONE
 
 
 
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/scripts/CX_LED_driver.lua b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/scripts/CX_LED_driver.lua
new file mode 100644
index 0000000000..c532fa1bf9
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/scripts/CX_LED_driver.lua
@@ -0,0 +1,50 @@
+
+-- LED is switched on using motor function
+local MOTOR_LED_FUNCTION = 94
+
+-- Ensure that the out2_function min = 200 and max = 10000
+local MOTOR_LED_PWM_VAL_TRIM  = 5000
+local MOTOR_LED_PWM_VAL_MIN  = 200
+local MOTOR_LED_PWM_VAL_MAX  = 10000
+local MOTOR_LED_PWM_VAL_OFF  = 0
+
+-- Arming check interval
+local LED_CYCLE_TIME = 2000
+
+
+local LED_servo = SRV_Channels:find_channel(MOTOR_LED_FUNCTION)
+
+
+gcs:send_text(0, "LED -- BEGIN")
+
+
+-- LED is switched ON using a PWM signal configured as MOTOR_LED_FUNCTION
+-- This is done to overcome the design limitations on CPN
+local function switch_LED(LED_val)
+
+    if (LED_servo ~= null) then
+        SRV_Channels:set_output_pwm_chan_timeout(LED_servo, LED_val, LED_CYCLE_TIME)
+    end
+
+    return check_arming, LED_CYCLE_TIME
+end
+
+
+
+-- Function to continuously check if the vehicle is armed or not
+-- ARMED -> LED_ON
+-- DISARMED -> LED_OFF 
+local function check_arming()
+    vehicle_armed = arming:is_armed()
+    
+    if vehicle_armed == true then
+        switch_LED(MOTOR_LED_PWM_VAL_MAX)
+    else
+        switch_LED(MOTOR_LED_PWM_VAL_MIN)
+    end
+
+    return check_arming, LED_CYCLE_TIME
+end
+
+return check_arming(), 10000
+
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py
index f3acfb703b..8d220b0559 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py
+++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py
@@ -2867,6 +2867,10 @@ def add_apperiph_defaults(f):
 #define AP_STATS_ENABLED 0
 #endif
 
+#ifndef HAL_PERIPH_ARM_MONITORING_ENABLE
+#define HAL_PERIPH_ARM_MONITORING_ENABLE 0
+#endif
+
 /*
  * GPS Backends - we selectively turn backends on.
  *   Note also that f103-GPS explicitly disables some of these backends.
@@ -2877,6 +2881,7 @@ def add_apperiph_defaults(f):
 #define AP_GPS_ERB_ENABLED 0
 #endif
 
+
 #ifndef AP_GPS_GSOF_ENABLED
 #define AP_GPS_GSOF_ENABLED defined(HAL_PERIPH_ENABLE_GPS)
 #endif
diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp
index ca03aaf360..4039681b0e 100644
--- a/libraries/AP_HAL_SITL/SITL_State.cpp
+++ b/libraries/AP_HAL_SITL/SITL_State.cpp
@@ -382,6 +382,12 @@ SITL::SerialDevice *SITL_State::create_serial_sim(const char *name, const char *
         }
         efi_ms = new SITL::EFI_MegaSquirt();
         return efi_ms;
+    } else if (streq(name, "hirth")) {
+        if (efi_hirth != nullptr) {
+            AP_HAL::panic("Only one hirth at a time");
+        }
+        efi_hirth = new SITL::EFI_Hirth();
+        return efi_hirth;
     } else if (streq(name, "VectorNav")) {
         if (vectornav != nullptr) {
             AP_HAL::panic("Only one VectorNav at a time");
@@ -622,7 +628,9 @@ void SITL_State::_fdm_input_local(void)
     if (efi_ms != nullptr) {
         efi_ms->update();
     }
-
+    if (efi_hirth != nullptr) {
+        efi_hirth->update();
+    }
     if (frsky_d != nullptr) {
         frsky_d->update();
     }
diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h
index 872c33f460..b4bf4c399a 100644
--- a/libraries/AP_HAL_SITL/SITL_State.h
+++ b/libraries/AP_HAL_SITL/SITL_State.h
@@ -48,6 +48,7 @@
 #include <SITL/SIM_LORD.h>
 #include <SITL/SIM_AIS.h>
 #include <SITL/SIM_GPS.h>
+#include <SITL/SIM_EFI_Hirth.h>
 
 #include <SITL/SIM_Frsky_D.h>
 #include <SITL/SIM_CRSF.h>
@@ -287,6 +288,9 @@ class HALSITL::SITL_State {
     // simulated EFI MegaSquirt device:
     SITL::EFI_MegaSquirt *efi_ms;
 
+    // simulated EFI Hirth device:
+    SITL::EFI_Hirth *efi_hirth;
+
     // output socket for flightgear viewing
     SocketAPM fg_socket{true};
     
diff --git a/libraries/AP_ICEngine/AP_ICEngine.cpp b/libraries/AP_ICEngine/AP_ICEngine.cpp
old mode 100644
new mode 100755
index 6e36b3c133..e02d8941e8
--- a/libraries/AP_ICEngine/AP_ICEngine.cpp
+++ b/libraries/AP_ICEngine/AP_ICEngine.cpp
@@ -24,6 +24,7 @@
 #include <AP_Notify/AP_Notify.h>
 #include <RC_Channel/RC_Channel.h>
 #include <AP_RPM/AP_RPM.h>
+#include <AP_Relay/AP_Relay.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -154,9 +155,22 @@ const AP_Param::GroupInfo AP_ICEngine::var_info[] = {
     // @Units: RPM
     AP_GROUPINFO("REDLINE_RPM", 17, AP_ICEngine, redline_rpm, 0),
 
+    // @Param: IGNITION_RLY
+    // @DisplayName: Ignition relay channel
+    // @Description: This is a a relay channel to use for ignition control
+    // @User: Standard
+    // @Values: 0:None, 1:Relay1,2:Relay2,3:Relay3,4:Relay4
+    AP_GROUPINFO("IGNITION_RLY", 18, AP_ICEngine, ignition_relay, 0),
+
     AP_GROUPEND
 };
 
+#define TCA9554_I2C_BUS      1
+#define TCA9554_I2C_ADDR     0x20
+#define TCA9554_OUTPUT       0x01  // Output Port register address. Outgoing logic levels
+#define TCA9554_OUT_DEFAULT  0x30  // 0011 0000
+#define TCA9554_CONF         0x03  // Configuration Port register address [0 = Output]
+#define TCA9554_PINS         0xC2  // Set all used ports to outputs = 1100 0010
 
 // constructor
 AP_ICEngine::AP_ICEngine(const AP_RPM &_rpm) :
@@ -182,6 +196,13 @@ void AP_ICEngine::update(void)
         return;
     }
 
+    if (i2c_state == I2C_UNINITIALIZED) {
+        i2c_state = I2C_FAILED;
+        if (TCA9554_init()) {
+            i2c_state = I2C_SUCCESS;
+        }
+    }
+
     uint16_t cvalue = 1500;
     RC_Channel *c = rc().channel(start_chan-1);
     if (c != nullptr && rc().has_valid_input()) {
@@ -336,6 +357,8 @@ void AP_ICEngine::update(void)
     case ICE_OFF:
         SRV_Channels::set_output_pwm(SRV_Channel::k_ignition, pwm_ignition_off);
         SRV_Channels::set_output_pwm(SRV_Channel::k_starter,  pwm_starter_off);
+        control_ign_str(IGN_OFF_STR_OFF);
+        ignition_relay_set(false);
         starter_start_time_ms = 0;
         break;
 
@@ -343,11 +366,21 @@ void AP_ICEngine::update(void)
     case ICE_START_DELAY:
         SRV_Channels::set_output_pwm(SRV_Channel::k_ignition, pwm_ignition_on);
         SRV_Channels::set_output_pwm(SRV_Channel::k_starter,  pwm_starter_off);
+        control_ign_str(IGN_ON_STR_OFF);
+        ignition_relay_set(false);
         break;
 
     case ICE_STARTING:
         SRV_Channels::set_output_pwm(SRV_Channel::k_ignition, pwm_ignition_on);
         SRV_Channels::set_output_pwm(SRV_Channel::k_starter,  pwm_starter_on);
+        if (!(hal.util->get_soft_armed() || allow_throttle_disarmed())) {
+            control_ign_str(IGN_ON_STR_OFF);
+            ignition_relay_set(false);
+        } else {
+            control_ign_str(IGN_ON_STR_ON_DIR_ON);
+            ignition_relay_set(true);
+        }
+        
         if (starter_start_time_ms == 0) {
             starter_start_time_ms = now;
         }
@@ -357,6 +390,8 @@ void AP_ICEngine::update(void)
     case ICE_RUNNING:
         SRV_Channels::set_output_pwm(SRV_Channel::k_ignition, pwm_ignition_on);
         SRV_Channels::set_output_pwm(SRV_Channel::k_starter,  pwm_starter_off);
+        control_ign_str(IGN_ON_STR_OFF);
+        ignition_relay_set(true);
         starter_start_time_ms = 0;
         break;
     }
@@ -537,6 +572,79 @@ void AP_ICEngine::update_idle_governor(int8_t &min_throttle)
     min_throttle = roundf(idle_governor_integrator);
 }
 
+bool AP_ICEngine::TCA9554_init()
+{
+    dev_TCA9554 = std::move(hal.i2c_mgr->get_device(TCA9554_I2C_BUS, TCA9554_I2C_ADDR));
+    if (!dev_TCA9554) {
+        return false;
+    }
+    WITH_SEMAPHORE(dev_TCA9554->get_semaphore());
+
+    dev_TCA9554->set_retries(10);
+
+    // set outputs
+    bool ret = dev_TCA9554->write_register(TCA9554_OUTPUT, TCA9554_OUT_DEFAULT);
+    if (!ret) {
+        return false;
+    }
+    ret = dev_TCA9554->write_register(TCA9554_CONF, TCA9554_PINS);
+    if (!ret) {
+        return false;
+    }
+    TCA9554_set(IGN_OFF_STR_OFF);
+    dev_TCA9554->set_retries(1);
+    return true;
+}
+
+void AP_ICEngine::TCA9554_set(TCA9554_state_t value)
+{
+    if (value != TCA9554_state) {
+        TCA9554_state = value;
+        WITH_SEMAPHORE(dev_TCA9554->get_semaphore());
+        // set outputs and status leds
+        //dev_TCA9554->write_register(TCA9554_OUTPUT, (~(value<<2) & 0x0C) | value);
+        dev_TCA9554->write_register(TCA9554_OUTPUT, (~(value<<2) & 0x0C) | value);
+        //0011 0010
+        //1100 1000 & 0000 1100
+        //0000 1000 OR 0011 0010
+        //0011 1010
+    }
+}
+
+void AP_ICEngine::control_ign_str(TCA9554_state_t value)
+{
+    if (i2c_state == I2C_SUCCESS)
+    {
+        TCA9554_set(value);
+    }
+    else
+    {
+    	//Leave for now
+    }
+}
+
+/*
+  control relay for ICE ignition
+ */
+void AP_ICEngine::ignition_relay_set(bool on)
+{
+    if (ignition_relay > 0) {
+        auto *relay = AP::relay();
+        if (relay != nullptr) {
+            if (on) {
+                relay->on(ignition_relay-1);
+            } else {
+                relay->off(ignition_relay-1);
+            }
+        }
+    }
+}
+
+bool AP_ICEngine::allow_throttle_disarmed() const
+{
+    return option_set(Options::THROTTLE_WHILE_DISARMED) &&
+        hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED;
+}
 
 // singleton instance. Should only ever be set in the constructor.
 AP_ICEngine *AP_ICEngine::_singleton;
diff --git a/libraries/AP_ICEngine/AP_ICEngine.h b/libraries/AP_ICEngine/AP_ICEngine.h
old mode 100644
new mode 100755
index f5a1818f22..815587b7b6
--- a/libraries/AP_ICEngine/AP_ICEngine.h
+++ b/libraries/AP_ICEngine/AP_ICEngine.h
@@ -19,6 +19,10 @@
 #pragma once
 
 #include "AP_ICEngine_config.h"
+#include <AP_HAL/AP_HAL.h>
+#include <AP_RPM/AP_RPM.h>
+#include <AP_Param/AP_Param.h>
+#include <AP_HAL/I2CDevice.h>
 
 #if AP_ICENGINE_ENABLED
 
@@ -57,9 +61,44 @@ class AP_ICEngine {
 
     static AP_ICEngine *get_singleton() { return _singleton; }
 
+    bool allow_throttle_disarmed() const;
+
 private:
     static AP_ICEngine *_singleton;
 
+    /*
+     * TCA9554 output register mapping for PMB Rev E
+     * P0 = PMU_EN - PMU output ON/OFF  (CN6 pin 2)
+     * P1 = ECU_EN - Unused (previously Engine Kill Switch)
+     * P2 = I2C_P2 - Unused
+     * P3 = LED (active low)
+     * P4 = PMU_START - Crank Direction (CN6 pin 5)
+     * P5 = PMU_ARM  - Crank Signal (CN6 pin 6)
+     * P6 = PMU_STAT_IN - Unused
+     * P7 = PMU_STAT - Unused
+     */
+
+    enum TCA9554_state_t {
+        IGN_OFF_STR_OFF = 0x30,	 		//output register - 0011 0000
+        IGN_ON_STR_OFF = 0x30,		 	//output register - 0011 0000
+		IGN_ON_STR_ON_DIR_ON = 0x11, 	//output register - 0001 0001
+    } TCA9554_state = IGN_OFF_STR_OFF;
+
+    enum i2c_init_t {
+        I2C_UNINITIALIZED = 0,
+        I2C_FAILED = 1,
+        I2C_SUCCESS = 2
+    } i2c_state = I2C_UNINITIALIZED;
+
+    bool TCA9554_init();
+
+    void TCA9554_set(TCA9554_state_t value);
+
+    void control_ign_str(TCA9554_state_t value);
+    void ignition_relay_set(bool on);
+
+    AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev_TCA9554;
+
     const class AP_RPM &rpm;
 
     enum ICE_State state;
@@ -116,6 +155,9 @@ class AP_ICEngine {
     // Idle Controller Slew Rate
     AP_Float idle_slew;
     
+    // relay number for ignition
+    AP_Int8 ignition_relay;
+    
     // height when we enter ICE_START_HEIGHT_DELAY
     float initial_height;
 
diff --git a/libraries/AP_Math/crc.cpp b/libraries/AP_Math/crc.cpp
index 9027477166..f1dc6e91a4 100644
--- a/libraries/AP_Math/crc.cpp
+++ b/libraries/AP_Math/crc.cpp
@@ -494,3 +494,12 @@ uint64_t crc_crc64(const uint32_t *data, uint16_t num_words)
 
     return crc;
 }
+
+uint8_t sum_of_bytes_in_buffer_mod_256(uint8_t *data, uint16_t count)
+{
+    uint8_t ret = 0;
+    for (uint8_t i=0; i<count; i++) {
+        ret += data[i];
+    }
+    return ret;
+}
diff --git a/libraries/AP_Math/crc.h b/libraries/AP_Math/crc.h
index dc8daaaf6d..87d03efbf9 100644
--- a/libraries/AP_Math/crc.h
+++ b/libraries/AP_Math/crc.h
@@ -50,3 +50,7 @@ void hash_fnv_1a(uint32_t len, const uint8_t* buf, uint64_t* hash);
 
 // CRC-64-WE using the polynomial of 0x42F0E1EBA9EA3693
 uint64_t crc_crc64(const uint32_t *data, uint16_t num_words);
+
+// sums the bytes in the supplied buffer, returns that sum mod 256
+// (i.e. shoved into a uint8_t)
+uint8_t sum_of_bytes_in_buffer_mod_256(uint8_t *data, uint16_t count);
diff --git a/libraries/AP_SerialManager/AP_SerialManager.cpp b/libraries/AP_SerialManager/AP_SerialManager.cpp
index 1b04c6d0b5..80a092ef79 100644
--- a/libraries/AP_SerialManager/AP_SerialManager.cpp
+++ b/libraries/AP_SerialManager/AP_SerialManager.cpp
@@ -551,7 +551,7 @@ void AP_SerialManager::init()
 #endif
                     
                 case SerialProtocol_EFI:
-                    state[i].baud.set_default(AP_SERIALMANAGER_EFI_MS_BAUD);
+                    // state[i].baud.set_default(AP_SERIALMANAGER_EFI_MS_BAUD);
                     uart->begin(state[i].baudrate(),
                                          AP_SERIALMANAGER_EFI_MS_BUFSIZE_RX,
                                          AP_SERIALMANAGER_EFI_MS_BUFSIZE_TX);
diff --git a/libraries/AP_SerialManager/AP_SerialManager.h b/libraries/AP_SerialManager/AP_SerialManager.h
index c183016e93..794d8ccfd0 100644
--- a/libraries/AP_SerialManager/AP_SerialManager.h
+++ b/libraries/AP_SerialManager/AP_SerialManager.h
@@ -99,7 +99,7 @@
 // MegaSquirt EFI protocol
 #define AP_SERIALMANAGER_EFI_MS_BAUD           115
 #define AP_SERIALMANAGER_EFI_MS_BUFSIZE_RX     512
-#define AP_SERIALMANAGER_EFI_MS_BUFSIZE_TX     16
+#define AP_SERIALMANAGER_EFI_MS_BUFSIZE_TX     32
 
 // SBUS servo outputs
 #define AP_SERIALMANAGER_SBUS1_BAUD           100000
diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp
index baeabe9557..a6970eafe1 100644
--- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp
+++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp
@@ -123,7 +123,7 @@ const AP_Param::GroupInfo AP_UAVCAN::var_info[] = {
     // @Param: OPTION
     // @DisplayName: UAVCAN options
     // @Description: Option flags
-    // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd,3:IgnoreDNANodeUnhealthy
+    // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd,3:IgnoreDNANodeUnhealthy,5:LogNodeStatus
     // @User: Advanced
     AP_GROUPINFO("OPTION", 5, AP_UAVCAN, _options, 0),
     
@@ -456,10 +456,10 @@ void AP_UAVCAN::loop(void)
             continue;
         }
 
-        if (_SRV_armed) {
+        if (_SRV_armed_mask != 0 || _ESC_armed_mask != 0) {
             bool sent_servos = false;
 
-            if (_servo_bm > 0) {
+            if (_SRV_armed_mask != 0) {
                 // if we have any Servos in bitmask
                 uint32_t now = AP_HAL::native_micros();
                 const uint32_t servo_period_us = 1000000UL / unsigned(_servo_rate_hz.get());
@@ -474,7 +474,7 @@ void AP_UAVCAN::loop(void)
             }
 
             // if we have any ESC's in bitmask
-            if (_esc_bm > 0 && !sent_servos) {
+            if (_ESC_armed_mask != 0 && !sent_servos) {
                 SRV_send_esc();
             }
 
@@ -494,6 +494,7 @@ void AP_UAVCAN::loop(void)
 #if AP_OPENDRONEID_ENABLED
         AP::opendroneid().dronecan_send(this);
 #endif
+        logging();
     }
 }
 
@@ -525,7 +526,7 @@ void AP_UAVCAN::SRV_send_actuator(void)
              * physically possible throws at [-1:1] limits.
              */
 
-            if (_SRV_conf[starting_servo].servo_pending && ((((uint32_t) 1) << starting_servo) & _servo_bm)) {
+            if (_SRV_conf[starting_servo].servo_pending && ((((uint32_t) 1) << starting_servo) & _SRV_armed_mask)) {
                 cmd.actuator_id = starting_servo + 1;
 
                 if (option_is_set(Options::USE_ACTUATOR_PWM)) {
@@ -543,7 +544,11 @@ void AP_UAVCAN::SRV_send_actuator(void)
         }
 
         if (i > 0) {
-            act_out_array[_driver_index]->broadcast(msg);
+            if (act_out_array[_driver_index]->broadcast(msg) > 0) {
+                _srv_send_count++;
+            } else {
+                _fail_send_count++;
+            }
 
             if (i == 15) {
                 repeat_send = true;
@@ -567,7 +572,7 @@ void AP_UAVCAN::SRV_send_esc(void)
 
     // find out how many esc we have enabled and if they are active at all
     for (uint8_t i = esc_offset; i < UAVCAN_SRV_NUMBER; i++) {
-        if ((((uint32_t) 1) << i) & _esc_bm) {
+        if ((((uint32_t) 1) << i) & _ESC_armed_mask) {
             max_esc_num = i + 1;
             if (_SRV_conf[i].esc_pending) {
                 active_esc_num++;
@@ -580,7 +585,7 @@ void AP_UAVCAN::SRV_send_esc(void)
         k = 0;
 
         for (uint8_t i = esc_offset; i < max_esc_num && k < 20; i++) {
-            if ((((uint32_t) 1) << i) & _esc_bm) {
+            if ((((uint32_t) 1) << i) & _ESC_armed_mask) {
                 // TODO: ESC negative scaling for reverse thrust and reverse rotation
                 float scaled = cmd_max * (hal.rcout->scale_esc_to_unity(_SRV_conf[i].pulse) + 1.0) / 2.0;
 
@@ -594,7 +599,11 @@ void AP_UAVCAN::SRV_send_esc(void)
             k++;
         }
 
-        esc_raw[_driver_index]->broadcast(esc_msg);
+        if (esc_raw[_driver_index]->broadcast(esc_msg) > 0) {
+            _esc_send_count++;
+        } else {
+            _fail_send_count++;
+        }
     }
 }
 
@@ -611,7 +620,22 @@ void AP_UAVCAN::SRV_push_servos()
         }
     }
 
-    _SRV_armed = hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED;
+    uint16_t servo_armed_mask = _servo_bm;
+    uint16_t esc_armed_mask = _esc_bm;
+    const bool safety_off = hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED;
+    if (!safety_off) {
+        AP_BoardConfig *boardconfig = AP_BoardConfig::get_singleton();
+        if (boardconfig != nullptr) {
+            const uint16_t safety_mask = boardconfig->get_safety_mask();
+            servo_armed_mask &= safety_mask;
+            esc_armed_mask &= safety_mask;
+        } else {
+            servo_armed_mask = 0;
+            esc_armed_mask = 0;
+        }
+    }
+    _SRV_armed_mask = servo_armed_mask;
+    _ESC_armed_mask = esc_armed_mask;
 }
 
 
@@ -836,7 +860,15 @@ void AP_UAVCAN::safety_state_send()
 
     { // handle SafetyState
         ardupilot::indication::SafetyState safety_msg;
-        switch (hal.util->safety_switch_state()) {
+        auto state = hal.util->safety_switch_state();
+        if (_SRV_armed_mask != 0 || _ESC_armed_mask != 0) {
+            // if we are outputting any servos or ESCs due to
+            // BRD_SAFETY_MASK then we need to advertise safety as
+            // off, this changes LEDs to indicate unsafe and allows
+            // AP_Periph ESCs and servos to run
+            state = AP_HAL::Util::SAFETY_ARMED;
+        }
+        switch (state) {
         case AP_HAL::Util::SAFETY_ARMED:
             safety_msg.status = ardupilot::indication::SafetyState::STATUS_SAFETY_OFF;
             break;
@@ -995,13 +1027,15 @@ void AP_UAVCAN::handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const E
         .temperature_cdeg = int16_t((KELVIN_TO_C(cb.msg->temperature)) * 100),
         .voltage = cb.msg->voltage,
         .current = cb.msg->current,
+        .error_status = cb.msg->error_count,
     };
 
     ap_uavcan->update_rpm(esc_index, cb.msg->rpm);
     ap_uavcan->update_telem_data(esc_index, t,
         AP_ESC_Telem_Backend::TelemetryType::CURRENT
             | AP_ESC_Telem_Backend::TelemetryType::VOLTAGE
-            | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE);
+            | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE
+            | AP_ESC_Telem_Backend::TelemetryType::ERROR_STATUS);
 #endif
 }
 
@@ -1217,4 +1251,52 @@ bool AP_UAVCAN::prearm_check(char* fail_msg, uint8_t fail_msg_len) const
     return _dna_server->prearm_check(fail_msg, fail_msg_len);
 }
 
+/*
+  periodic logging
+ */
+void AP_UAVCAN::logging(void)
+{
+#if HAL_LOGGING_ENABLED
+    const uint32_t now_ms = AP_HAL::millis();
+    if (now_ms - last_log_ms < 1000) {
+        return;
+    }
+    last_log_ms = now_ms;
+    if (HAL_NUM_CAN_IFACES <= _driver_index) {
+        // no interface?
+        return;
+    }
+    const auto *iface = hal.can[_driver_index];
+    if (iface == nullptr) {
+        return;
+    }
+    const auto *stats = iface->get_statistics();
+    if (stats == nullptr) {
+        // statistics not implemented on this interface
+        return;
+    }
+    const auto &s = *stats;
+    AP::logger().WriteStreaming("CANS",
+                                "TimeUS,I,T,Trq,Trej,Tov,Tto,Tab,R,Rov,Rer,Bo,Etx,Stx,Ftx",
+                                "s#-------------",
+                                "F--------------",
+                                "QBIIIIIIIIIIIII",
+                                AP_HAL::micros64(),
+                                _driver_index,
+                                s.tx_success,
+                                s.tx_requests,
+                                s.tx_rejected,
+                                s.tx_overflow,
+                                s.tx_timedout,
+                                s.tx_abort,
+                                s.rx_received,
+                                s.rx_overflow,
+                                s.rx_errors,
+                                s.num_busoff_err,
+                                _esc_send_count,
+                                _srv_send_count,
+                                _fail_send_count);
+#endif // HAL_LOGGING_ENABLED
+}
+
 #endif // HAL_NUM_CAN_IFACES
diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h
index 9085a76c5a..fd8aceb6e3 100644
--- a/libraries/AP_UAVCAN/AP_UAVCAN.h
+++ b/libraries/AP_UAVCAN/AP_UAVCAN.h
@@ -204,6 +204,7 @@ class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
         CANFD_ENABLED             = (1U<<2),
         DNA_IGNORE_UNHEALTHY_NODE = (1U<<3),
         USE_ACTUATOR_PWM          = (1U<<4),
+        ENABLE_STATS              = (1U<<8),
     };
 
     // check if a option is set
@@ -247,6 +248,9 @@ class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
     // send parameter save request
     void send_parameter_save_request();
 
+    // periodic logging
+    void logging();
+    
     // set parameter on a node
     ParamGetSetIntCb *param_int_cb;
     ParamGetSetFloatCb *param_float_cb;
@@ -287,10 +291,19 @@ class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
         bool servo_pending;
     } _SRV_conf[UAVCAN_SRV_NUMBER];
 
+    uint32_t _SRV_armed_mask;
+    uint32_t _ESC_armed_mask;
+    uint32_t _esc_send_count;
+    uint32_t _srv_send_count;
+    uint32_t _fail_send_count;
+
     uint8_t _SRV_armed;
     uint32_t _SRV_last_send_us;
     HAL_Semaphore SRV_sem;
 
+    // last log time
+    uint32_t last_log_ms;
+
     ///// LED /////
     struct led_device {
         uint8_t led_index;
diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp b/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp
index 0e1b50cd03..3c97876011 100644
--- a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp
+++ b/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp
@@ -488,6 +488,16 @@ void AP_UAVCAN_DNA_Server::handleNodeStatus(uint8_t node_id, const NodeStatusCb
             server_state = HEALTHY;
         }
     }
+
+    //to calculate the delta b/w handlenode status pkt
+    uint32_t now = AP_HAL::native_millis();
+
+    if (_ap_uavcan->option_is_set(AP_UAVCAN::Options::ENABLE_STATS)) {
+        log_NodeStatus(node_id, cb.msg->uptime_sec, cb.msg->health, cb.msg->mode , now - last_can_init_delta_ms[node_id]);
+    }
+
+    last_can_init_delta_ms[node_id] = now;
+
     if (!isNodeIDVerified(node_id)) {
         //immediately begin verification of the node_id
         if (getNodeInfo_client[driver_index] != nullptr) {
@@ -580,6 +590,29 @@ void AP_UAVCAN_DNA_Server::handleNodeInfo(uint8_t node_id, uint8_t unique_id[],
     }
 }
 
+/*
+  optionally log NodeStatus packets
+ */
+void AP_UAVCAN_DNA_Server::log_NodeStatus(uint8_t node_id, uint32_t uptime_sec, uint8_t healthy, uint8_t mode, uint32_t delta)
+{
+    if (node_id > MAX_NODE_ID) {
+        return;
+    }
+
+    // @LoggerMessage: CANH
+    // @Description: CAN Health Status
+    // @Field: TimeUS: Time since system startup
+    // @Field: NodeId: Node ID
+    // @Field: Healthy: zero when node healthy
+    // @Field: UpTime: time since boot in seconds
+    AP::logger().WriteStreaming("CANH",
+                                "TimeUS," "NodeID," "Healthy," "UpTime," "delta", // labels
+                                "s"            "#"        "-"       "-"     "-", // units
+                                "F"            "-"        "-"       "-"     "-", // multipliers
+                                "Q"            "B"        "B"       "I"     "I", // types
+                                AP_HAL::micros64(), node_id, healthy, uptime_sec, delta);
+}
+
 //Trampoline call for handleNodeInfo member call
 void AP_UAVCAN_DNA_Server::trampoline_handleNodeInfo(AP_UAVCAN* ap_uavcan, uint8_t node_id, const GetNodeInfoCb& resp)
 {
diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h b/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h
index 3a6d6ae14f..33de748f88 100644
--- a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h
+++ b/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h
@@ -132,6 +132,9 @@ class AP_UAVCAN_DNA_Server
 
     //Run through the list of seen node ids for verification
     void verify_nodes();
+    // Log NodeInfo
+    void log_NodeStatus(uint8_t node_id, uint32_t uptime_sec, uint8_t healthy, uint8_t mode, uint32_t delta);
+    uint32_t last_can_init_delta_ms[126];
 };
 
 #endif
diff --git a/libraries/SITL/SIM_EFI_HIrth.cpp b/libraries/SITL/SIM_EFI_HIrth.cpp
new file mode 100644
index 0000000000..7977143b75
--- /dev/null
+++ b/libraries/SITL/SIM_EFI_HIrth.cpp
@@ -0,0 +1,166 @@
+/*
+   This program 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.
+
+   This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  simulate Hirth EFI system
+*/
+
+#include "SIM_Aircraft.h"
+#include <SITL/SITL.h>
+#include <AP_HAL/utility/sparse-endian.h>
+#include <stdio.h>
+#include "SIM_EFI_Hirth.h"
+
+using namespace SITL;
+
+void EFI_Hirth::update_receive()
+{
+    const ssize_t num_bytes_read = read_from_autopilot((char*)&receive_buf[receive_buf_ofs], ARRAY_SIZE(receive_buf) - receive_buf_ofs);
+    if (num_bytes_read < 1) {
+        return;
+    }
+    receive_buf_ofs += num_bytes_read;
+
+    if (receive_buf_ofs < 1) {
+        return;
+    }
+
+    const uint8_t expected_bytes_in_message = receive_buf[0];
+
+    if (expected_bytes_in_message == 0) {
+        AP_HAL::panic("zero bytes expected is unexpected");
+    }
+
+    if (expected_bytes_in_message > ARRAY_SIZE(receive_buf)) {
+        AP_HAL::panic("Unexpectedly large byte count");
+    }
+
+    if (receive_buf_ofs < expected_bytes_in_message) {
+        return;
+    }
+
+    // checksum is sum of all bytes except the received checksum:
+    const uint8_t expected_checksum = 256 - sum_of_bytes_in_buffer_mod_256(receive_buf, expected_bytes_in_message-1);
+    const uint8_t received_checksum = receive_buf[expected_bytes_in_message-1];
+    if (expected_checksum == received_checksum) {
+        PacketCode received_packet_code = PacketCode(receive_buf[1]);
+        if (received_packet_code == PacketCode::SetValues) {
+            // do this synchronously for now
+            handle_set_values();
+        } else {
+            assert_receive_size(3);
+            if (requested_data_record.time_ms != 0) {
+                AP_HAL::panic("Requesting too fast?");
+            }
+            requested_data_record.code = received_packet_code;
+            requested_data_record.time_ms = AP_HAL::millis();
+        }
+    } else {
+        AP_HAL::panic("checksum failed");
+        // simply throw these bytes away.  What the actual device does in the
+        // face of weird data is unknown.
+    }
+    memmove(&receive_buf[0], &receive_buf[expected_bytes_in_message], receive_buf_ofs - expected_bytes_in_message);
+    receive_buf_ofs -= expected_bytes_in_message;
+}
+
+void EFI_Hirth::assert_receive_size(uint8_t receive_size)
+{
+    if (receive_buf[0] != receive_size) {
+        AP_HAL::panic("Expected %u message size, got %u message size", receive_size, receive_buf[0]);
+    }
+}
+
+void EFI_Hirth::handle_set_values()
+{
+    assert_receive_size(23);
+    static_assert(sizeof(settings) == 20, "correct number of bytes in settings");
+    memcpy((void*)&settings, &receive_buf[2], sizeof(settings));
+}
+
+void EFI_Hirth::update_send()
+{
+    if (requested_data_record.time_ms == 0) {
+        // no outstanding request
+        return;
+    }
+    if (AP_HAL::millis() - requested_data_record.time_ms < 20) {
+        // 20ms to service a request
+        return;
+    }
+    requested_data_record.time_ms = 0;
+
+    switch (requested_data_record.code) {
+    case PacketCode::DataRecord1:
+        send_record1();
+        break;
+    case PacketCode::DataRecord2:
+        send_record2();
+        break;
+    case PacketCode::DataRecord3:
+        send_record3();
+        break;
+    default:
+        AP_HAL::panic("Unknown data record (%u) requested", (unsigned)requested_data_record.code);
+    }
+}
+
+void EFI_Hirth::update()
+{
+    auto sitl = AP::sitl();
+    if (!sitl || sitl->efi_type != SIM::EFI_TYPE_HIRTH) {
+        return;
+    }
+
+    // update throttle; interim thing to make life a little more interesting
+    // gcs().send_text(MAV_SEVERITY_INFO, "using throttle %f", settings.throttle/10.0);
+    throttle = 0.9 * throttle + 0.1 * (settings.throttle)/10.0;
+
+    update_receive();
+    update_send();
+}
+
+void SITL::EFI_Hirth::send_record1()
+{
+    const auto *sitl = AP::sitl();
+
+    auto &r = packed_record1.record;
+
+    r.rpm = sitl->state.rpm[0];
+    gcs().send_text(MAV_SEVERITY_INFO, "rpm in: %u", r.rpm);
+    r.air_temperature = AP::baro().get_temperature();
+
+    packed_record1.update_checksum();
+
+    static_assert(sizeof(record1) == 84, "correct number of bytes in record1");
+    write_to_autopilot((char*)&packed_record1, sizeof(packed_record1));
+}
+
+void SITL::EFI_Hirth::send_record2()
+{
+    packed_record2.record.throttle = uint16_t(throttle * 10.0);
+    packed_record2.update_checksum();
+
+    static_assert(sizeof(record2) == 98, "correct number of bytes in record2");
+    write_to_autopilot((char*)&packed_record2, sizeof(packed_record2));
+}
+
+
+void SITL::EFI_Hirth::send_record3()
+{
+    packed_record3.update_checksum();
+
+    static_assert(sizeof(record3) == 100, "correct number of bytes in record3");
+    write_to_autopilot((char*)&packed_record3, sizeof(packed_record3));
+}
\ No newline at end of file
diff --git a/libraries/SITL/SIM_EFI_Hirth.h b/libraries/SITL/SIM_EFI_Hirth.h
new file mode 100644
index 0000000000..02dc658543
--- /dev/null
+++ b/libraries/SITL/SIM_EFI_Hirth.h
@@ -0,0 +1,140 @@
+/*
+   This program 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.
+
+   This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  simulate Hirth EFI system
+
+./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduPlane -A --uartF=sim:hirth --speedup=1
+param set SERIAL5_PROTOCOL 24
+param set SIM_EFI_TYPE 6
+param set EFI_TYPE 6
+reboot
+status EFI_STATUS
+
+./Tools/autotest/autotest.py --gdb --debug build.Plane test.Plane.Hirth
+
+*/
+
+#pragma once
+
+#include <SITL/SITL.h>
+#include <AP_HAL/utility/Socket.h>
+#include "SIM_SerialDevice.h"
+
+namespace SITL {
+
+class EFI_Hirth : public SerialDevice {
+public:
+
+    using SerialDevice::SerialDevice;
+
+    void update();
+
+private:
+
+    void update_receive();
+    void update_send();
+
+    void assert_receive_size(uint8_t receive_size);
+
+    void handle_set_values();
+
+    // maps from an on-wire number to a record number:
+    enum class PacketCode : uint8_t {
+        DataRecord1 = 4,
+        DataRecord2 = 11,
+        DataRecord3 = 13,
+        SetValues = 201,
+    };
+
+    template <typename T>
+    class PACKED PackedRecord {
+    public:
+        PackedRecord(PacketCode _code, T _record) :
+            code(uint8_t(_code)),
+            record(_record)
+        { }
+            const uint8_t length { sizeof(T) + 3 };  // 1 each of length, code and checksum
+        const uint8_t code;
+        T record;
+        uint8_t checksum;
+
+        void update() {
+            record.update();
+            update_checksum();
+        }
+
+        void update_checksum() {
+            checksum = 256 - sum_of_bytes_in_buffer_mod_256((uint8_t*)this, length-1);
+        }
+    };
+
+    void send_record1();
+    void send_record2();
+    void send_record3();
+
+    class PACKED Record1 {
+    public:
+        uint8_t unknown1[10];
+        uint16_t rpm;
+        uint8_t unknown2[66];
+        uint16_t air_temperature;
+        uint8_t unknown3[4];
+
+        void update();
+    };
+
+    class PACKED Record2 {
+    public:
+        uint8_t unknown1[62];
+        uint16_t throttle;  // percent * 10
+        uint8_t unknown2[34];
+    };
+
+    class PACKED Record3 {
+    public:
+        uint8_t unknown1[100];
+    };
+
+    class PACKED SetValues {
+    public:
+        uint16_t throttle;  // percent * 10
+        uint8_t unknown1[18];
+    };
+
+    // these records are just used for initial values of the fields;
+    // they aren't used past that.
+    Record1 record1;
+    Record2 record2;
+    Record3 record3;
+
+
+    SetValues settings;
+
+    PackedRecord<Record1> packed_record1{PacketCode::DataRecord1, record1};
+    PackedRecord<Record2> packed_record2{PacketCode::DataRecord2, record2};
+    PackedRecord<Record3> packed_record3{PacketCode::DataRecord3, record3};
+
+    struct {
+        PacketCode code;  // code which was requested by driver
+        uint32_t time_ms;  // time that code was requested by driver
+    } requested_data_record;
+
+    uint8_t receive_buf[32];
+    uint8_t receive_buf_ofs;
+
+    float throttle;
+};
+
+}
\ No newline at end of file
diff --git a/libraries/SITL/SIM_EFI_MegaSquirt.cpp b/libraries/SITL/SIM_EFI_MegaSquirt.cpp
index 7aa29ff229..e6b2ae94a8 100644
--- a/libraries/SITL/SIM_EFI_MegaSquirt.cpp
+++ b/libraries/SITL/SIM_EFI_MegaSquirt.cpp
@@ -20,6 +20,7 @@
 #include <SITL/SITL.h>
 #include <AP_HAL/utility/sparse-endian.h>
 #include <stdio.h>
+#include "SIM_EFI_MegaSquirt.h"
 
 using namespace SITL;
 
@@ -37,7 +38,7 @@ static uint32_t CRC32_MS(const uint8_t *buf, uint32_t len)
 void EFI_MegaSquirt::update()
 {
     auto sitl = AP::sitl();
-    if (!sitl || sitl->efi_type == SIM::EFI_TYPE_NONE) {
+    if (!sitl || sitl->efi_type == SIM::EFI_TYPE_MS) {
         return;
     }
     float rpm = sitl->state.rpm[0];
diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h
index b2a39a3f87..0250252908 100644
--- a/libraries/SITL/SITL.h
+++ b/libraries/SITL/SITL.h
@@ -275,6 +275,7 @@ class SIM {
     enum EFIType {
         EFI_TYPE_NONE = 0,
         EFI_TYPE_MS = 1,
+        EFI_TYPE_HIRTH = 8,
     };
     
     AP_Int8  efi_type;