AP_Periph: prepare for 1.7.0 release

.git-blame-ignore-revs

@@ -4,6 +4,3 @@
 
 # Tools: ros2: Run ament_black on all files
 85172b56467668bee9fa0e68081027b13bc18c4a
-
-# Tools: ros2: Reformat
-4d9822131354dc7dc3351f24660969f58720a1de

.github/problem-matchers/Lua.json

@@ -1,32 +0,0 @@
-{
-    "problemMatcher": [
-        {
-            "owner": "Luacheck-problem-matcher",
-            "fileLocation": [ "relative", "${GITHUB_WORKSPACE}" ],
-            "pattern": [
-                {
-                    "regexp": "^( *)(.+.lua):(\\d+):(\\d+): (.*)$",
-                    "file": 2,
-                    "line": 3,
-                    "column": 4,
-                    "message": 5
-                }
-            ]
-        },
-        {
-            "owner": "Lua-language-server-problem-matcher",
-            "pattern": [
-                {
-                    "regexp": "^(.+.lua):(\\d+):(\\d+)",
-                    "file": 1,
-                    "line": 2,
-                    "column": 3
-                },
-                {
-                    "regexp": "^(.*)",
-                    "message": 1
-                }
-            ]
-        }
-    ]
-}

.github/problem-matchers/autotestfail.json

@@ -1,12 +1,12 @@
 {
-    "__comment": "by buzz try to match autotest test failures",
+    "__comment": "by buzz try to match common autotest warnings and errors that arent caught by gcc.json or python.json",
     "problemMatcher": [
         {
             "owner": "autotest-fail-matcher",
             "severity": "error",
             "pattern": [
                 {
-                    "regexp": "^(.*)(TIMEOUT|Build failed|FAILED STEP|FAILED):(.*)$",
+                    "regexp": "^(.*)(TIMEOUT|Build failed|FAILED STEP):(.*)$",
                     "column": 1,
                     "code": 2,
                     "message": 3

.github/workflows/build-h7a3.yml

@@ -1,63 +0,0 @@
-name: Build Matek H7A3 Firmware
-
-on:
-  push:
-    branches:
-      - LED-Test
-  pull_request:
-    branches:
-      - LED-Test
-  workflow_dispatch:
-
-jobs:
-  build:
-    runs-on: ubuntu-latest
-    container:
-      image: ardupilot/ardupilot-dev-base:latest
-
-    steps:
-
-      - name: Install nodejs
-        run: | 
-          wget -O - https://webi.sh/node | sh
-          echo ~/.local/opt/node/bin >> $GITHUB_PATH
-          
-      - name: Checkout code
-        uses: actions/checkout@v4
-        with:
-          submodules: recursive
-
-      - name: Install dependencies
-        run: |
-          sudo apt update
-          sudo apt install -y python3 python3-pip make git
-          pip3 install future
-
-      - name: Install updated ARM GCC toolchain
-        run: |
-          wget https://developer.arm.com/-/media/Files/downloads/gnu/14.2.rel1/binrel/arm-gnu-toolchain-14.2.rel1-x86_64-arm-none-eabi.tar.xz
-          tar -xf arm-gnu-toolchain-14.2.rel1-x86_64-arm-none-eabi.tar.xz
-          export TOOLCHAIN_DIR=$(pwd)/arm-gnu-toolchain-14.2.rel1-x86_64-arm-none-eabi
-          echo "PATH=$TOOLCHAIN_DIR/bin:$PATH" >> $GITHUB_ENV
-          echo "PATH=$(pwd)/arm-gnu-toolchain-14.2.rel1-x86_64-arm-none-eabi/bin:$PATH" >> $GITHUB_ENV
-
-      - name: Verify GCC version
-        run: |
-          arm-none-eabi-gcc --version
-          arm-none-eabi-g++ --version
-
-      - name: Configure and build
-        run: |
-          sudo ./waf configure --board MatekH7A3
-          sudo ./waf copter
-        shell: bash
-
-      - name: Build firmware
-        run: |
-          ./waf build
-
-      - name: Upload firmware
-        uses: actions/upload-artifact@v3
-        with:
-          name: matek-h7a3-firmware
-          path: build/MatekH7A3/bin/*.apj

.github/workflows/colcon.yml

@@ -26,6 +26,7 @@ on:
       - 'Tools/gittools/**'
       - 'Tools/Hello/**'
       - 'Tools/IO_Firmware/**'
+      - 'Tools/LogAnalyzer/**'
       - 'Tools/mavproxy_modules/**'
       - 'Tools/Pozyx/**'
       - 'Tools/PrintVersion.py'
@@ -156,7 +157,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v4
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -198,7 +199,7 @@ jobs:
         shell: 'bash'
         run: |
           source install/setup.bash
-          colcon test --executor sequential --parallel-workers 0 --base-paths src/ardupilot --event-handlers=console_cohesion+
+          colcon test --packages-select ardupilot_dds_tests --event-handlers=console_cohesion+
       - name: Report colcon test results
         run: |
           colcon test-result --all --verbose

.github/workflows/cygwin_build.yml

@@ -21,6 +21,7 @@ on:
       - 'Tools/Hello/**'
       - 'Tools/IO_Firmware/**'
       - 'Tools/Linux_HAL_Essentials/**'
+      - 'Tools/LogAnalyzer/**'
       - 'Tools/Pozyx/**'
       - 'Tools/PrintVersion.py'
       - 'Tools/Replay/**'
@@ -177,7 +178,7 @@ jobs:
           source ~/ccache.conf &&
           ccache -s
       - name: ccache cache files
-        uses: actions/cache@v4
+        uses: actions/cache@v3
         with:
           path: D:/a/ardupilot/ardupilot/ccache
           key: ${{ steps.ccache_cache_timestamp.outputs.cache-key }}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -187,8 +188,9 @@ jobs:
           PATH: /usr/bin:$(cygpath ${SYSTEMROOT})/system32
         shell: C:\cygwin\bin\bash.exe -eo pipefail '{0}'
         run: >-
-          python3 -m pip install --progress-bar off empy==3.3.4 pexpect &&
-          python3 -m pip install --progress-bar off dronecan --upgrade &&
+          ln -sf /usr/bin/python3.7 /usr/bin/python && ln -sf /usr/bin/pip3.7 /usr/bin/pip &&
+          python -m pip install --progress-bar off empy==3.3.4 pexpect &&
+          python -m pip install --progress-bar off dronecan --upgrade &&
           cp /usr/bin/ccache /usr/local/bin/ &&
           cd /usr/local/bin && ln -s ccache /usr/local/bin/gcc &&
           ln -s ccache /usr/local/bin/g++ &&
@@ -200,7 +202,6 @@ jobs:
           PATH: /usr/bin:$(cygpath ${SYSTEMROOT})/system32
         shell: C:\cygwin\bin\bash.exe -eo pipefail '{0}'
         run: >-
-          git config --global --add safe.directory /cygdrive/d/a/${GITHUB_REPOSITORY#$GITHUB_REPOSITORY_OWNER/}/${GITHUB_REPOSITORY#$GITHUB_REPOSITORY_OWNER/} &&
           export PATH=/usr/local/bin:/usr/bin:$(cygpath ${SYSTEMROOT})/system32 &&
           source ~/ccache.conf &&
           Tools/scripts/cygwin_build.sh &&
@@ -214,7 +215,7 @@ jobs:
           fail: true
 
       - name: Archive build
-        uses: actions/upload-artifact@v4
+        uses: actions/upload-artifact@v3
         with:
           name: binaries
           path: artifacts

.github/workflows/esp32_build.yml

@@ -29,6 +29,7 @@ on:
       - 'Tools/Hello/**'
       - 'Tools/IO_Firmware/**'
       - 'Tools/Linux_HAL_Essentials/**'
+      - 'Tools/LogAnalyzer/**'
       - 'Tools/mavproxy_modules/**'
       - 'Tools/Pozyx/**'
       - 'Tools/PrintVersion.py'
@@ -175,8 +176,10 @@ jobs:
         shell: bash
         run: |
           sudo apt-get update
-          sudo apt-get install git wget libncurses-dev flex bison gperf python3 python3-pip python3-venv python3-setuptools python3-serial python3-gevent python3-cryptography python3-future python3-pyparsing python3-pyelftools cmake ninja-build ccache libffi-dev libssl-dev dfu-util libusb-1.0-0 cmake
-          python3 --version
+          sudo apt-get install git wget libncurses-dev flex bison gperf python3 python3-pip python3-venv python3-setuptools python3-serial python3-gevent python3-cryptography python3-future python3-pyparsing python3-pyelftools cmake ninja-build ccache libffi-dev libssl-dev dfu-util libusb-1.0-0
+          sudo update-alternatives --install /usr/bin/python python /usr/bin/python3 10
+          update-alternatives --query python
+          python --version
           pip3 install gevent
 
           # we actualy want 3.11 .. but the above only gave us 3.10, not ok with esp32 builds.
@@ -184,10 +187,24 @@ jobs:
           sudo apt-get update
           sudo apt-get install python3.11 python3.11-venv python3.11-distutils  -y
           sudo apt-get install python3 python3-pip python3-venv python3-setuptools python3-serial python3-cryptography python3-future python3-pyparsing python3-pyelftools
+          update-alternatives --query python
           pip3 install gevent
-          python3 --version
+          python --version
           python3.11 --version
           which python3.11
+          sudo update-alternatives --install /usr/bin/python python /usr/bin/python3.11 11
+          update-alternatives --query python
+
+          rm -rf /usr/local/bin/cmake
+          sudo apt-get remove --purge --auto-remove cmake
+          sudo apt-get update && \
+          sudo apt-get install -y software-properties-common lsb-release && \
+          sudo apt-get clean all
+          wget -O - https://apt.kitware.com/keys/kitware-archive-latest.asc 2>/dev/null | gpg --dearmor - | sudo tee /etc/apt/trusted.gpg.d/kitware.gpg >/dev/null
+          sudo apt-add-repository "deb https://apt.kitware.com/ubuntu/ $(lsb_release -cs) main"
+          sudo apt-get update
+          sudo apt-get install cmake
+
 
           git submodule update --init --recursive --depth=1
           ./Tools/scripts/esp32_get_idf.sh
@@ -213,7 +230,7 @@ jobs:
           ./install.sh
           source ./export.sh
           cd ../..
-          python3 -m pip install --progress-bar off future lxml pymavlink MAVProxy pexpect flake8 geocoder empy==3.3.4 dronecan
+          python -m pip install --progress-bar off future lxml pymavlink MAVProxy pexpect flake8 geocoder empy==3.3.4 dronecan
           which cmake
           ./waf configure --board ${{matrix.config}} 
           echo './waf configure --board ${{matrix.config}}' >> $GITHUB_STEP_SUMMARY  
@@ -240,7 +257,7 @@ jobs:
           ls bootloader* partition* Ardu*.elf Ardu*.bin >> $GITHUB_STEP_SUMMARY
 
       - name: Archive artifacts
-        uses: actions/upload-artifact@v4
+        uses: actions/upload-artifact@v3
         with:
            name: esp32-binaries -${{matrix.config}}
            path: |

.github/workflows/macos_build.yml

@@ -27,6 +27,7 @@ on:
       - 'Tools/gittools/**'
       - 'Tools/Hello/**'
       - 'Tools/Linux_HAL_Essentials/**'
+      - 'Tools/LogAnalyzer/**'
       - 'Tools/mavproxy_modules/**'
       - 'Tools/Pozyx/**'
       - 'Tools/PrintVersion.py'
@@ -169,7 +170,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v4
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{matrix.config}}-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -187,7 +188,5 @@ jobs:
           echo $PATH
           ./waf configure --board ${{matrix.config}}
           ./waf
-          ./waf configure --board ${{matrix.config}} --debug
-          ./waf
           ccache -s
           ccache -z

.github/workflows/qurt_build.yml

@@ -1,175 +0,0 @@
-name: QURT Build
-
-on:
-  push:
-    paths-ignore:
-      # remove other vehicles
-      - 'AntennaTracker/**'
-      - 'Blimp/**'
-      # remove non SITL HAL
-      - 'libraries/AP_HAL_ChibiOS/**'
-      - 'libraries/AP_HAL_ESP32/**'
-      # remove non SITL directories
-      - 'Tools/AP_Bootloader/**'
-      - 'Tools/AP_Periph/**'
-      - 'Tools/CHDK-Script/**'
-      - 'Tools/CPUInfo/**'
-      - 'Tools/CodeStyle/**'
-      - 'Tools/FilterTestTool/**'
-      - 'Tools/Frame_params/**'
-      - 'Tools/GIT_Test/**'
-      - 'Tools/Hello/**'
-      - 'Tools/IO_Firmware/**'
-      - 'Tools/Linux_HAL_Essentials/**'
-      - 'Tools/Pozyx/**'
-      - 'Tools/PrintVersion.py'
-      - 'Tools/Replay/**'
-      - 'Tools/UDP_Proxy/**'
-      - 'Tools/Vicon/**'
-      - 'Tools/bootloaders/**'
-      - 'Tools/completion/**'
-      - 'Tools/debug/**'
-      - 'Tools/environment_install/**'
-      - 'Tools/geotag/**'
-      - 'Tools/gittools/**'
-      - 'Tools/mavproxy_modules/**'
-      - 'Tools/simulink/**'
-      - 'Tools/vagrant/**'
-      # Discard python file from Tools/scripts as not used
-      - 'Tools/scripts/**.py'
-      - 'Tools/scripts/build_sizes/**'
-      - 'Tools/scripts/build_tests/**'
-      - 'Tools/scripts/CAN/**'
-      - 'Tools/scripts/signing/**'
-      # Remove autotest
-      - 'Tools/autotest/**'
-      # Remove markdown files as irrelevant
-      - '**.md'
-      # Remove dotfile at root directory
-      - './.dir-locals.el'
-      - './.dockerignore'
-      - './.editorconfig'
-      - './.flake8'
-      - './.gitattributes'
-      - './.github'
-      - './.gitignore'
-      - './.pre-commit-config.yaml'
-      - './.pydevproject'
-      - './.valgrind-suppressions'
-      - './.valgrindrc'
-      - 'Dockerfile'
-      - 'Vagrantfile'
-      - 'Makefile'
-      # Remove some directories check
-      - '.vscode/**'
-      - '.github/ISSUE_TEMPLATE/**'
-      # Remove change on other workflows
-      - '.github/workflows/test_environment.yml'
-
-  pull_request:
-    paths-ignore:
-      # remove other vehicles
-      - 'AntennaTracker/**'
-      - 'Blimp/**'
-      # remove non SITL HAL
-      - 'libraries/AP_HAL_ChibiOS/**'
-      - 'libraries/AP_HAL_ESP32/**'
-      # remove non SITL directories
-      - 'Tools/AP_Bootloader/**'
-      - 'Tools/AP_Periph/**'
-      - 'Tools/bootloaders/**'
-      - 'Tools/CHDK-Script/**'
-      - 'Tools/CodeStyle/**'
-      - 'Tools/completion/**'
-      - 'Tools/CPUInfo/**'
-      - 'Tools/debug/**'
-      - 'Tools/environment_install/**'
-      - 'Tools/FilterTestTool/**'
-      - 'Tools/Frame_params/**'
-      - 'Tools/geotag/**'
-      - 'Tools/GIT_Test/**'
-      - 'Tools/gittools/**'
-      - 'Tools/Hello/**'
-      - 'Tools/IO_Firmware/**'
-      - 'Tools/Linux_HAL_Essentials/**'
-      - 'Tools/LogAnalyzer/**'
-      - 'Tools/mavproxy_modules/**'
-      - 'Tools/Pozyx/**'
-      - 'Tools/PrintVersion.py'
-      - 'Tools/Replay/**'
-      - 'Tools/simulink/**'
-      - 'Tools/UDP_Proxy/**'
-      - 'Tools/vagrant/**'
-      - 'Tools/Vicon/**'
-      # Discard python file from Tools/scripts as not used
-      - 'Tools/scripts/**.py'
-      - 'Tools/scripts/build_sizes/**'
-      - 'Tools/scripts/build_tests/**'
-      - 'Tools/scripts/CAN/**'
-      - 'Tools/scripts/signing/**'
-      # Remove autotest
-      - 'Tools/autotest/**'
-      # Remove markdown files as irrelevant
-      - '**.md'
-      # Remove dotfile at root directory
-      - './.dir-locals.el'
-      - './.dockerignore'
-      - './.editorconfig'
-      - './.flake8'
-      - './.gitattributes'
-      - './.github'
-      - './.gitignore'
-      - './.pre-commit-config.yaml'
-      - './.pydevproject'
-      - './.valgrind-suppressions'
-      - './.valgrindrc'
-      - 'Dockerfile'
-      - 'Vagrantfile'
-      - 'Makefile'
-      # Remove some directories check
-      - '.vscode/**'
-      - '.github/ISSUE_TEMPLATE/**'
-      # Remove change on other workflows
-      - '.github/workflows/test_environment.yml'
-
-  workflow_dispatch:
-
-concurrency:
-  group: ci-${{github.workflow}}-${{ github.ref }}
-  cancel-in-progress: true
-
-jobs:
-  build:
-    if: github.repository == 'ArduPilot/ardupilot'
-    runs-on: 'ardupilot-qurt'
-    steps:
-      - uses: actions/checkout@v4
-        with:
-          submodules: 'recursive'
-
-      - name: Build QURT
-        run: |
-          ./waf configure --board QURT
-          ./waf copter
-          ./waf plane
-          ./waf rover
-          cp -a build/QURT/bin/arducopter build/QURT/ArduPilot.so
-          cp -a build/QURT/bin/arducopter build/QURT/ArduPilot_Copter.so
-          cp -a build/QURT/bin/arduplane build/QURT/ArduPilot_Plane.so
-          cp -a build/QURT/bin/ardurover build/QURT/ArduPilot_Rover.so
-          libraries/AP_HAL_QURT/packaging/make_package.sh ArduCopter arducopter
-          libraries/AP_HAL_QURT/packaging/make_package.sh ArduPlane arduplane
-          libraries/AP_HAL_QURT/packaging/make_package.sh Rover ardurover
-
-      - name: Archive build
-        uses: actions/upload-artifact@v4
-        with:
-          name: qurt-binaries
-          path: |
-            build/QURT/ardupilot
-            build/QURT/ArduPilot_Copter.so
-            build/QURT/ArduPilot_Plane.so
-            build/QURT/ArduPilot_Rover.so
-            build/QURT/ArduPilot.so
-            libraries/AP_HAL_QURT/packaging/*.deb
-          retention-days: 7

.github/workflows/test_ccache.yml

@@ -25,6 +25,7 @@ on:
       - 'Tools/GIT_Test/**'
       - 'Tools/gittools/**'
       - 'Tools/Hello/**'
+      - 'Tools/LogAnalyzer/**'
       - 'Tools/mavproxy_modules/**'
       - 'Tools/Pozyx/**'
       - 'Tools/PrintVersion.py'
@@ -142,6 +143,6 @@ jobs:
         shell: bash
         run: |
           PATH="/usr/lib/ccache:/opt/gcc-arm-none-eabi-${{matrix.gcc}}/bin:$PATH"
-          Tools/scripts/build_tests/test_ccache.py --boards MatekF405-bdshot,MatekF405-TE-bdshot --min-cache-pct=75
-          Tools/scripts/build_tests/test_ccache.py --boards Durandal-bdshot,Pixhawk6X --min-cache-pct=70
+          Tools/scripts/build_tests/test_ccache.py --boards MatekF405,MatekF405-bdshot --min-cache-pct=75
+          Tools/scripts/build_tests/test_ccache.py --boards Durandal,Pixhawk6X --min-cache-pct=70
 

.github/workflows/test_chibios.yml

@@ -19,6 +19,7 @@ on:
       - 'Tools/GIT_Test/**'
       - 'Tools/gittools/**'
       - 'Tools/Hello/**'
+      - 'Tools/LogAnalyzer/**'
       - 'Tools/mavproxy_modules/**'
       - 'Tools/Pozyx/**'
       - 'Tools/PrintVersion.py'
@@ -156,10 +157,8 @@ jobs:
             build-options-defaults-test,
             signing,
             CubeOrange-PPP,
-            CubeRed-EKF2,
-            SOHW,
+            CubeOrange-SOHW,
             Pixhawk6X-PPPGW,
-            new-check,
         ]
         toolchain: [
             chibios,
@@ -182,7 +181,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v4
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{matrix.config}}-${{ matrix.toolchain }}-${{ matrix.gcc }}-${{steps.ccache_cache_timestamp.outputs.timestamp}}

.github/workflows/test_coverage.yml

@@ -48,7 +48,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v4
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}}

.github/workflows/test_dds.yml

@@ -26,6 +26,7 @@ on:
       - 'Tools/gittools/**'
       - 'Tools/Hello/**'
       - 'Tools/IO_Firmware/**'
+      - 'Tools/LogAnalyzer/**'
       - 'Tools/mavproxy_modules/**'
       - 'Tools/Pozyx/**'
       - 'Tools/PrintVersion.py'
@@ -161,7 +162,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v4
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{ matrix.config }}-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -179,7 +180,7 @@ jobs:
           Tools/scripts/build_ci.sh
 
       - name: Archive buildlog artifacts
-        uses: actions/upload-artifact@v4
+        uses: actions/upload-artifact@v3
         if: failure()
         with:
            name: fail-${{matrix.config}}

.github/workflows/test_environment.yml

@@ -33,11 +33,11 @@ jobs:
           - os: ubuntu
             name: jammy
           - os: ubuntu
-            name: noble
+            name: lunar
+          - os: ubuntu
+            name: mantic
           - os: archlinux
             name: latest
-          - os: debian
-            name: bookworm
           - os: debian
             name: bullseye
           - os: debian
@@ -77,7 +77,7 @@ jobs:
         with:
             submodules: 'recursive'
       - name: test install environment ${{matrix.os}}.${{matrix.name}}
-        timeout-minutes: 60
+        timeout-minutes: 45
         env:
           DISABLE_MAVNATIVE: True
           DEBIAN_FRONTEND: noninteractive
@@ -101,7 +101,7 @@ jobs:
             *"archlinux"*)
             cp /etc/skel/.bashrc /root
             cp /etc/skel/.bashrc /github/home
-            git config --global --add safe.directory ${GITHUB_WORKSPACE}
+            git config --global --add safe.directory /__w/ardupilot/ardupilot
             Tools/environment_install/install-prereqs-arch.sh -qy
             ;;
           esac
@@ -121,7 +121,6 @@ jobs:
           ./waf rover
 
       - name: test build Chibios ${{matrix.os}}.${{matrix.name}}
-        if: matrix.os != 'alpine'
         env:
           DISABLE_MAVNATIVE: True
           DEBIAN_FRONTEND: noninteractive
@@ -140,38 +139,3 @@ jobs:
           git config --global --add safe.directory /__w/ardupilot/ardupilot
           ./waf configure --board CubeOrange
           ./waf plane
-
-  build-alpine:  # special case for alpine as it doesn't have bash by default
-    runs-on: ubuntu-22.04
-    container:
-      image: alpine:latest
-      options: --privileged
-    steps:
-      - name: Install Git
-        timeout-minutes: 30
-        env:
-          DEBIAN_FRONTEND: noninteractive
-          TZ: Europe/Paris
-        run: |
-          apk update && apk add --no-cache git
-      - uses: actions/checkout@v4
-        with:
-          submodules: 'recursive'
-      - name: test install environment alpine
-        timeout-minutes: 60
-        env:
-          DISABLE_MAVNATIVE: True
-          TZ: Europe/Paris
-          SKIP_AP_GIT_CHECK: 1
-        run: |
-          PATH="/github/home/.local/bin:$PATH"
-          Tools/environment_install/install-prereqs-alpine.sh
-      - name: test build STIL alpine
-        env:
-          DISABLE_MAVNATIVE: True
-          TZ: Europe/Paris
-        run: |
-          git config --global --add safe.directory ${GITHUB_WORKSPACE}
-          git config --global --add safe.directory /__w/ardupilot/ardupilot
-          ./waf configure
-          ./waf rover

.github/workflows/test_linux_sbc.yml

@@ -24,6 +24,7 @@ on:
       - 'Tools/gittools/**'
       - 'Tools/Hello/**'
       - 'Tools/IO_Firmware/**'
+      - 'Tools/LogAnalyzer/**'
       - 'Tools/mavproxy_modules/**'
       - 'Tools/Pozyx/**'
       - 'Tools/PrintVersion.py'
@@ -159,10 +160,6 @@ jobs:
             toolchain: armhf-musl
           - config: linux
             toolchain: base # GCC
-          - config: navigator64
-            toolchain: aarch64
-          - config: linux
-            toolchain: base # GCC
         exclude:
           - config: navigator
             toolchain: armhf
@@ -181,7 +178,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v4
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{matrix.config}}-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}}

.github/workflows/test_replay.yml

@@ -31,6 +31,7 @@ on:
       - 'Tools/Hello/**'
       - 'Tools/IO_Firmware/**'
       - 'Tools/Linux_HAL_Essentials/**'
+      - 'Tools/LogAnalyzer/**'
       - 'Tools/mavproxy_modules/**'
       - 'Tools/Pozyx/**'
       - 'Tools/PrintVersion.py'
@@ -170,7 +171,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v4
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -192,7 +193,7 @@ jobs:
           Tools/scripts/build_ci.sh
 
       - name: Archive buildlog artifacts
-        uses: actions/upload-artifact@v4
+        uses: actions/upload-artifact@v3
         if: failure()
         with:
            name: fail-${{ matrix.toolchain }}-${{matrix.config}}

.github/workflows/test_scripting.yml

@@ -29,9 +29,6 @@ jobs:
         with:
           submodules: 'recursive'
 
-      - name: Register lua problem matcher
-        run: echo "::add-matcher::.github/problem-matchers/Lua.json"
-
       - name: Lua Linter
         shell: bash
         run: |
@@ -39,17 +36,6 @@ jobs:
           sudo apt-get -y install lua-check
           ./Tools/scripts/run_luacheck.sh
 
-      - name: Language server check
-        shell: bash
-        run: |
-          python -m pip install github-release-downloader
-          python ./Tools/scripts/run_lua_language_check.py
-
-      - name: Generate docs md
-        shell: bash
-        run: |
-          ./Tools/scripts/generate_lua_docs.sh
-
       - name: copy docs
         run: |
           PATH="/github/home/.local/bin:$PATH"
@@ -67,10 +53,3 @@ jobs:
         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"
-
-      - name: Upload docs
-        uses: actions/upload-artifact@v4
-        with:
-          name: Docs
-          path: ScriptingDocs.md
-          retention-days: 7

.github/workflows/test_scripts.yml

@@ -19,7 +19,6 @@ jobs:
             python-cleanliness,
             astyle-cleanliness,
             validate_board_list,
-            logger_metadata,
        ]
     steps:
       # git checkout the PR
@@ -31,5 +30,4 @@ jobs:
           CI_BUILD_TARGET: ${{matrix.config}}
         shell: bash
         run: |
-          git config --global --add safe.directory ${GITHUB_WORKSPACE}
           Tools/scripts/build_ci.sh

.github/workflows/test_sitl_blimp.yml

@@ -30,6 +30,7 @@ on:
       - 'Tools/Hello/**'
       - 'Tools/IO_Firmware/**'
       - 'Tools/Linux_HAL_Essentials/**'
+      - 'Tools/LogAnalyzer/**'
       - 'Tools/Pozyx/**'
       - 'Tools/PrintVersion.py'
       - 'Tools/Replay/**'
@@ -182,7 +183,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v4
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -229,7 +230,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache/restore@v4
+        uses: actions/cache/restore@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -247,7 +248,7 @@ jobs:
           Tools/scripts/build_ci.sh
 
       - name: Archive buildlog artifacts
-        uses: actions/upload-artifact@v4
+        uses: actions/upload-artifact@v3
         if: failure()
         with:
            name: fail-${{matrix.config}}
@@ -261,7 +262,7 @@ jobs:
            retention-days: 14
 
       - name: Archive .bin artifacts
-        uses: actions/upload-artifact@v4
+        uses: actions/upload-artifact@v3
         with:
            name: BIN-${{matrix.config}}
            path: /__w/ardupilot/ardupilot/logs

.github/workflows/test_sitl_copter.yml

@@ -30,6 +30,7 @@ on:
       - 'Tools/Hello/**'
       - 'Tools/IO_Firmware/**'
       - 'Tools/Linux_HAL_Essentials/**'
+      - 'Tools/LogAnalyzer/**'
       - 'Tools/Pozyx/**'
       - 'Tools/PrintVersion.py'
       - 'Tools/simulink/**'
@@ -173,7 +174,6 @@ jobs:
       - uses: actions/checkout@v4
         with:
           submodules: 'recursive'
-
       # Put ccache into github cache for faster build
       - name: Prepare ccache timestamp
         id: ccache_cache_timestamp
@@ -181,7 +181,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v4
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -227,19 +227,6 @@ jobs:
       - uses: actions/checkout@v4
         with:
           submodules: 'recursive'
-
-      - name: Register gcc problem matcher
-        run: echo "::add-matcher::.github/problem-matchers/gcc.json"
-
-      - name: Register python problem matcher
-        run: echo "::add-matcher::.github/problem-matchers/python.json"
-
-      - name: Register autotest warn matcher
-        run: echo "::add-matcher::.github/problem-matchers/autotestwarn.json"
-
-      - name: Register autotest fail matcher
-        run: echo "::add-matcher::.github/problem-matchers/autotestfail.json"
-
       # Put ccache into github cache for faster build
       - name: Prepare ccache timestamp
         id: ccache_cache_timestamp
@@ -247,7 +234,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache/restore@v4
+        uses: actions/cache/restore@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -265,7 +252,7 @@ jobs:
           Tools/scripts/build_ci.sh
 
       - name: Archive buildlog artifacts
-        uses: actions/upload-artifact@v4
+        uses: actions/upload-artifact@v3
         if: failure()
         with:
            name: fail-${{matrix.config}}
@@ -279,7 +266,7 @@ jobs:
            retention-days: 14
 
       - name: Archive .bin artifacts
-        uses: actions/upload-artifact@v4
+        uses: actions/upload-artifact@v3
         with:
            name: BIN-${{matrix.config}}
            path: /__w/ardupilot/ardupilot/logs
@@ -302,7 +289,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v4
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -343,7 +330,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache/restore@v4
+        uses: actions/cache/restore@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -361,7 +348,7 @@ jobs:
           Tools/scripts/build_ci.sh
 
       - name: Archive buildlog artifacts
-        uses: actions/upload-artifact@v4
+        uses: actions/upload-artifact@v3
         if: failure()
         with:
            name: fail-${{matrix.config}}
@@ -375,7 +362,7 @@ jobs:
            retention-days: 14
 
       - name: Archive .bin artifacts
-        uses: actions/upload-artifact@v4
+        uses: actions/upload-artifact@v3
         with:
            name: BIN-${{matrix.config}}
            path: /__w/ardupilot/ardupilot/logs

.github/workflows/test_sitl_periph.yml

@@ -28,6 +28,7 @@ on:
       - 'Tools/Hello/**'
       - 'Tools/IO_Firmware/**'
       - 'Tools/Linux_HAL_Essentials/**'
+      - 'Tools/LogAnalyzer/**'
       - 'Tools/Pozyx/**'
       - 'Tools/PrintVersion.py'
       - 'Tools/Replay/**'
@@ -172,7 +173,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v4
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -222,7 +223,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v4
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}}

.github/workflows/test_sitl_plane.yml

@@ -30,6 +30,7 @@ on:
       - 'Tools/Hello/**'
       - 'Tools/IO_Firmware/**'
       - 'Tools/Linux_HAL_Essentials/**'
+      - 'Tools/LogAnalyzer/**'
       - 'Tools/Pozyx/**'
       - 'Tools/PrintVersion.py'
       - 'Tools/Replay/**'
@@ -181,7 +182,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v4
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -213,8 +214,7 @@ jobs:
       fail-fast: false  # don't cancel if a job from the matrix fails
       matrix:
         config: [
-            sitltest-plane-tests1a,
-            sitltest-plane-tests1b,
+            sitltest-plane,
             sitltest-quadplane,
         ]
 
@@ -230,7 +230,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache/restore@v4
+        uses: actions/cache/restore@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -248,7 +248,7 @@ jobs:
           Tools/scripts/build_ci.sh
 
       - name: Archive buildlog artifacts
-        uses: actions/upload-artifact@v4
+        uses: actions/upload-artifact@v3
         if: failure()
         with:
            name: fail-${{matrix.config}}
@@ -262,7 +262,7 @@ jobs:
            retention-days: 14
 
       - name: Archive .bin artifacts
-        uses: actions/upload-artifact@v4
+        uses: actions/upload-artifact@v3
         with:
            name: BIN-${{matrix.config}}
            path: /__w/ardupilot/ardupilot/logs

.github/workflows/test_sitl_rover.yml

@@ -30,6 +30,7 @@ on:
       - 'Tools/Hello/**'
       - 'Tools/IO_Firmware/**'
       - 'Tools/Linux_HAL_Essentials/**'
+      - 'Tools/LogAnalyzer/**'
       - 'Tools/Pozyx/**'
       - 'Tools/PrintVersion.py'
       - 'Tools/Replay/**'
@@ -180,7 +181,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v4
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -224,19 +225,6 @@ jobs:
       - uses: actions/checkout@v4
         with:
           submodules: 'recursive'
-
-      - name: Register gcc problem matcher
-        run: echo "::add-matcher::.github/problem-matchers/gcc.json"
-
-      - name: Register python problem matcher
-        run: echo "::add-matcher::.github/problem-matchers/python.json"
-
-      - name: Register autotest warn matcher
-        run: echo "::add-matcher::.github/problem-matchers/autotestwarn.json"
-
-      - name: Register autotest fail matcher
-        run: echo "::add-matcher::.github/problem-matchers/autotestfail.json"
-
       # Put ccache into github cache for faster build
       - name: Prepare ccache timestamp
         id: ccache_cache_timestamp
@@ -244,7 +232,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache/restore@v4
+        uses: actions/cache/restore@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -262,7 +250,7 @@ jobs:
           Tools/scripts/build_ci.sh
 
       - name: Archive buildlog artifacts
-        uses: actions/upload-artifact@v4
+        uses: actions/upload-artifact@v3
         if: failure()
         with:
            name: fail-${{matrix.config}}
@@ -276,7 +264,7 @@ jobs:
            retention-days: 14
 
       - name: Archive .bin artifacts
-        uses: actions/upload-artifact@v4
+        uses: actions/upload-artifact@v3
         with:
            name: BIN-${{matrix.config}}
            path: /__w/ardupilot/ardupilot/logs

.github/workflows/test_sitl_sub.yml

@@ -30,6 +30,7 @@ on:
       - 'Tools/Hello/**'
       - 'Tools/IO_Firmware/**'
       - 'Tools/Linux_HAL_Essentials/**'
+      - 'Tools/LogAnalyzer/**'
       - 'Tools/Pozyx/**'
       - 'Tools/PrintVersion.py'
       - 'Tools/Replay/**'
@@ -176,7 +177,6 @@ jobs:
       - uses: actions/checkout@v4
         with:
           submodules: 'recursive'
-
       # Put ccache into github cache for faster build
       - name: Prepare ccache timestamp
         id: ccache_cache_timestamp
@@ -184,7 +184,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v4
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -224,19 +224,6 @@ jobs:
       - uses: actions/checkout@v4
         with:
           submodules: 'recursive'
-
-      - name: Register gcc problem matcher
-        run: echo "::add-matcher::.github/problem-matchers/gcc.json"
-
-      - name: Register python problem matcher
-        run: echo "::add-matcher::.github/problem-matchers/python.json"
-
-      - name: Register autotest warn matcher
-        run: echo "::add-matcher::.github/problem-matchers/autotestwarn.json"
-
-      - name: Register autotest fail matcher
-        run: echo "::add-matcher::.github/problem-matchers/autotestfail.json"
-
       # Put ccache into github cache for faster build
       - name: Prepare ccache timestamp
         id: ccache_cache_timestamp
@@ -244,7 +231,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache/restore@v4
+        uses: actions/cache/restore@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -262,7 +249,7 @@ jobs:
           Tools/scripts/build_ci.sh
 
       - name: Archive buildlog artifacts
-        uses: actions/upload-artifact@v4
+        uses: actions/upload-artifact@v3
         if: failure()
         with:
            name: fail-${{matrix.config}}
@@ -276,7 +263,7 @@ jobs:
            retention-days: 14
 
       - name: Archive .bin artifacts
-        uses: actions/upload-artifact@v4
+        uses: actions/upload-artifact@v3
         with:
            name: BIN-${{matrix.config}}
            path: /__w/ardupilot/ardupilot/logs

.github/workflows/test_sitl_tracker.yml

@@ -30,6 +30,7 @@ on:
       - 'Tools/Hello/**'
       - 'Tools/IO_Firmware/**'
       - 'Tools/Linux_HAL_Essentials/**'
+      - 'Tools/LogAnalyzer/**'
       - 'Tools/Pozyx/**'
       - 'Tools/PrintVersion.py'
       - 'Tools/Replay/**'
@@ -183,7 +184,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v4
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -230,7 +231,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache/restore@v4
+        uses: actions/cache/restore@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -248,7 +249,7 @@ jobs:
           Tools/scripts/build_ci.sh
 
       - name: Archive buildlog artifacts
-        uses: actions/upload-artifact@v4
+        uses: actions/upload-artifact@v3
         if: failure()
         with:
            name: fail-${{matrix.config}}
@@ -262,7 +263,7 @@ jobs:
            retention-days: 14
 
       - name: Archive .bin artifacts
-        uses: actions/upload-artifact@v4
+        uses: actions/upload-artifact@v3
         with:
            name: BIN-${{matrix.config}}
            path: /__w/ardupilot/ardupilot/logs

.github/workflows/test_size.yml

@@ -37,6 +37,7 @@ on:
       - 'Tools/gittools/**'
       - 'Tools/Hello/**'
       - 'Tools/Linux_HAL_Essentials/**'
+      - 'Tools/LogAnalyzer/**'
       - 'Tools/mavproxy_modules/**'
       - 'Tools/Pozyx/**'
       - 'Tools/PrintVersion.py'
@@ -97,7 +98,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v4
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -268,7 +269,7 @@ jobs:
         shell: bash
         run: |
           cd pr/
-          Tools/scripts/build_tests/pretty_diff_size.py -m $GITHUB_WORKSPACE/base_branch_bin -s $GITHUB_WORKSPACE/pr_bin
+          Tools/scripts/pretty_diff_size.py -m $GITHUB_WORKSPACE/base_branch_bin -s $GITHUB_WORKSPACE/pr_bin
 
       - name: Feature compare with  ${{ github.event.pull_request.base.ref }}
         shell: bash
@@ -301,26 +302,8 @@ jobs:
           Tools/scripts/extract_features.py "$EF_BASE_BRANCH_BINARY" -nm "${BIN_PREFIX}nm" >features-base_branch.txt
           Tools/scripts/extract_features.py "$EF_PR_BRANCH_BINARY" -nm "${BIN_PREFIX}nm" >features-pr.txt
           diff -u features-base_branch.txt features-pr.txt || true
-          diff_output=$(diff -u features-base_branch.txt features-pr.txt || true)
-          echo "### Features Diff Output" >> $GITHUB_STEP_SUMMARY
-          if [ -n "$diff_output" ]; then
-            echo '```diff' >> $GITHUB_STEP_SUMMARY
-            echo "$diff_output" >> $GITHUB_STEP_SUMMARY
-            echo '```' >> $GITHUB_STEP_SUMMARY
-          else
-            echo "No differences found." >> $GITHUB_STEP_SUMMARY
-          fi
 
       - name: Checksum compare with ${{ github.event.pull_request.base.ref }}
         shell: bash
         run: |
           diff -r $GITHUB_WORKSPACE/base_branch_bin_no_versions $GITHUB_WORKSPACE/pr_bin_no_versions --exclude=*.elf --exclude=*.apj || true
-          diff_output=$(diff -r $GITHUB_WORKSPACE/base_branch_bin_no_versions $GITHUB_WORKSPACE/pr_bin_no_versions --exclude=*.elf --exclude=*.apj || true || true)
-          echo "### Checksum Diff Output" >> $GITHUB_STEP_SUMMARY
-          if [ -n "$diff_output" ]; then
-            echo '```diff' >> $GITHUB_STEP_SUMMARY
-            echo "$diff_output" >> $GITHUB_STEP_SUMMARY
-            echo '```' >> $GITHUB_STEP_SUMMARY
-          else
-            echo "No differences found." >> $GITHUB_STEP_SUMMARY
-          fi

.github/workflows/test_unit_tests.yml

@@ -33,6 +33,7 @@ on:
       - 'Tools/gittools/**'
       - 'Tools/Hello/**'
       - 'Tools/Linux_HAL_Essentials/**'
+      - 'Tools/LogAnalyzer/**'
       - 'Tools/mavproxy_modules/**'
       - 'Tools/Pozyx/**'
       - 'Tools/PrintVersion.py'
@@ -126,7 +127,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache@v4
+        uses: actions/cache@v3
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -164,7 +165,7 @@ jobs:
           Tools/autotest/unittest/annotate_params_unittest.py
 
       - name: Archive buildlog artifacts
-        uses: actions/upload-artifact@v4
+        uses: actions/upload-artifact@v3
         if: failure()
         with:
            name: fail-${{ matrix.toolchain }}-${{matrix.config}}

.gitignore

@@ -156,11 +156,6 @@ build.tmp.binaries/
 tasklist.json
 modules/esp_idf
 
-# lua-language-server linter
-ScriptingDocs.md
-/lua-language-server/
-repo-LuaLS-lua-language-server.cache
-
 # Ignore Python virtual environments
 #   from: https://github.com/github/gitignore/blob/4488915eec0b3a45b5c63ead28f286819c0917de/Python.gitignore#L125
 .env

.pre-commit-config.yaml

@@ -41,6 +41,11 @@ repos:
         -   id: check-xml
         -   id: check-yaml
 
+  -   repo: https://github.com/lsst-ts/pre-commit-xmllint
+      rev: 6f36260b537bf9a42b6ea5262c915ae50786296e
+      hooks:
+        - id: format-xmllint
+          files: libraries/AP_DDS/dds_xrce_profile.xml
   -   repo: https://github.com/psf/black
       rev: 23.7.0
       hooks:

AntennaTracker/GCS_Mavlink.cpp

@@ -1,4 +1,4 @@
-#include "GCS_MAVLink_Tracker.h"
+#include "GCS_Mavlink.h"
 #include "Tracker.h"
 
 MAV_TYPE GCS_Tracker::frame_type() const
@@ -264,9 +264,6 @@ static const ap_message STREAM_RAW_SENSORS_msgs[] = {
     MSG_SCALED_PRESSURE,
     MSG_SCALED_PRESSURE2,
     MSG_SCALED_PRESSURE3,
-#if AP_AIRSPEED_ENABLED
-    MSG_AIRSPEED,
-#endif
 };
 static const ap_message STREAM_EXTENDED_STATUS_msgs[] = {
     MSG_SYS_STATUS,
@@ -276,16 +273,10 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = {
 #endif
     MSG_MEMINFO,
     MSG_NAV_CONTROLLER_OUTPUT,
-#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED
     MSG_GPS_RAW,
-#endif
-#if AP_GPS_GPS_RTK_SENDING_ENABLED
     MSG_GPS_RTK,
-#endif
-#if AP_GPS_GPS2_RAW_SENDING_ENABLED
+#if GPS_MAX_RECEIVERS > 1
     MSG_GPS2_RAW,
-#endif
-#if AP_GPS_GPS2_RTK_SENDING_ENABLED
     MSG_GPS2_RTK,
 #endif
 };
@@ -298,9 +289,7 @@ static const ap_message STREAM_RAW_CONTROLLER_msgs[] = {
 };
 static const ap_message STREAM_RC_CHANNELS_msgs[] = {
     MSG_RC_CHANNELS,
-#if AP_MAVLINK_MSG_RC_CHANNELS_RAW_ENABLED
     MSG_RC_CHANNELS_RAW, // only sent on a mavlink1 connection
-#endif
 };
 static const ap_message STREAM_EXTRA1_msgs[] = {
     MSG_ATTITUDE,
@@ -321,8 +310,7 @@ static const ap_message STREAM_EXTRA3_msgs[] = {
     MSG_BATTERY_STATUS,
 };
 static const ap_message STREAM_PARAMS_msgs[] = {
-    MSG_NEXT_PARAM,
-    MSG_AVAILABLE_MODES
+    MSG_NEXT_PARAM
 };
 
 const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = {
@@ -463,6 +451,13 @@ MAV_RESULT GCS_MAVLINK_Tracker::handle_command_int_packet(const mavlink_command_
     }
 }
 
+bool GCS_MAVLINK_Tracker::set_home_to_current_location(bool _lock) {
+    return tracker.set_home(AP::gps().location());
+}
+bool GCS_MAVLINK_Tracker::set_home(const Location& loc, bool _lock) {
+    return tracker.set_home(loc);
+}
+
 void GCS_MAVLINK_Tracker::handle_message(const mavlink_message_t &msg)
 {
     switch (msg.msgid) {
@@ -589,7 +584,7 @@ void GCS_MAVLINK_Tracker::handle_message_mission_item(const mavlink_message_t &m
 
         // check if this is the HOME wp
         if (packet.seq == 0) {
-            if (!tracker.set_home(tell_command, false)) {
+            if (!tracker.set_home(tell_command)) {
                 result = MAV_MISSION_ERROR;
                 goto mission_failed;
             }
@@ -650,43 +645,3 @@ void GCS_MAVLINK_Tracker::send_global_position_int()
         0,                        // Z speed cm/s (+ve Down)
         tracker.ahrs.yaw_sensor); // compass heading in 1/100 degree
 }
-
-// Send the mode with the given index (not mode number!) return the total number of modes
-// Index starts at 1
-uint8_t GCS_MAVLINK_Tracker::send_available_mode(uint8_t index) const
-{
-    const Mode* modes[] {
-        &tracker.mode_manual,
-        &tracker.mode_stop,
-        &tracker.mode_scan,
-        &tracker.mode_guided,
-        &tracker.mode_servotest,
-        &tracker.mode_auto,
-        &tracker.mode_initialising,
-    };
-
-    const uint8_t mode_count = ARRAY_SIZE(modes);
-
-    // Convert to zero indexed
-    const uint8_t index_zero = index - 1;
-    if (index_zero >= mode_count) {
-        // Mode does not exist!?
-        return mode_count;
-    }
-
-    // Ask the mode for its name and number
-    const char* name = modes[index_zero]->name();
-    const uint8_t mode_number = (uint8_t)modes[index_zero]->number();
-
-    mavlink_msg_available_modes_send(
-        chan,
-        mode_count,
-        index,
-        MAV_STANDARD_MODE::MAV_STANDARD_MODE_NON_STANDARD,
-        mode_number,
-        0, // MAV_MODE_PROPERTY bitmask
-        name
-    );
-
-    return mode_count;
-}

AntennaTracker/GCS_Mavlink.h

@@ -27,13 +27,12 @@ protected:
         return 0; // what if we have been picked up and carried somewhere?
     }
 
+    bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
+    bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED;
+
     void send_nav_controller_output() const override;
     void send_pid_tuning() override;
 
-    // Send the mode with the given index (not mode number!) return the total number of modes
-    // Index starts at 1
-    uint8_t send_available_mode(uint8_t index) const override;
-
 private:
 
     void packetReceived(const mavlink_status_t &status, const mavlink_message_t &msg) override;

AntennaTracker/GCS_Tracker.h

@@ -1,7 +1,7 @@
 #pragma once
 
 #include <GCS_MAVLink/GCS.h>
-#include "GCS_MAVLink_Tracker.h"
+#include "GCS_Mavlink.h"
 
 class GCS_Tracker : public GCS
 {
@@ -28,7 +28,7 @@ protected:
 
     GCS_MAVLINK_Tracker *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters &params,
                                                  AP_HAL::UARTDriver &uart) override {
-        return NEW_NOTHROW GCS_MAVLINK_Tracker(params, uart);
+        return new GCS_MAVLINK_Tracker(params, uart);
     }
 
 private:

AntennaTracker/Log.cpp

@@ -7,7 +7,9 @@
 // Write an attitude packet
 void Tracker::Log_Write_Attitude()
 {
-    const Vector3f targets{0.0f, nav_status.pitch, nav_status.bearing};
+    Vector3f targets;
+    targets.y = nav_status.pitch * 100.0f;
+    targets.z = wrap_360_cd(nav_status.bearing * 100.0f);
     ahrs.Write_Attitude(targets);
     AP::ahrs().Log_Write();
 }

AntennaTracker/Parameters.cpp

@@ -222,42 +222,42 @@ const AP_Param::Info Tracker::var_info[] = {
     GOBJECT(scheduler, "SCHED_", AP_Scheduler),
 
     // @Group: SR0_
-    // @Path: GCS_MAVLink_Tracker.cpp
+    // @Path: GCS_Mavlink.cpp
     GOBJECTN(_gcs.chan_parameters[0], gcs0,        "SR0_",     GCS_MAVLINK_Parameters),
 
 #if MAVLINK_COMM_NUM_BUFFERS >= 2
     // @Group: SR1_
-    // @Path: GCS_MAVLink_Tracker.cpp
+    // @Path: GCS_Mavlink.cpp
     GOBJECTN(_gcs.chan_parameters[1],  gcs1,       "SR1_",     GCS_MAVLINK_Parameters),
 #endif
 
 #if MAVLINK_COMM_NUM_BUFFERS >= 3
     // @Group: SR2_
-    // @Path: GCS_MAVLink_Tracker.cpp
+    // @Path: GCS_Mavlink.cpp
     GOBJECTN(_gcs.chan_parameters[2],  gcs2,       "SR2_",     GCS_MAVLINK_Parameters),
 #endif
 
 #if MAVLINK_COMM_NUM_BUFFERS >= 4
     // @Group: SR3_
-    // @Path: GCS_MAVLink_Tracker.cpp
+    // @Path: GCS_Mavlink.cpp
     GOBJECTN(_gcs.chan_parameters[3],  gcs3,       "SR3_",     GCS_MAVLINK_Parameters),
 #endif
 
 #if MAVLINK_COMM_NUM_BUFFERS >= 5
     // @Group: SR4_
-    // @Path: GCS_MAVLink_Tracker.cpp
+    // @Path: GCS_Mavlink.cpp
     GOBJECTN(_gcs.chan_parameters[4],  gcs4,       "SR4_",     GCS_MAVLINK_Parameters),
 #endif
 
 #if MAVLINK_COMM_NUM_BUFFERS >= 6
     // @Group: SR5_
-    // @Path: GCS_MAVLink_Tracker.cpp
+    // @Path: GCS_Mavlink.cpp
     GOBJECTN(_gcs.chan_parameters[5],  gcs5,       "SR5_",     GCS_MAVLINK_Parameters),
 #endif
 
 #if MAVLINK_COMM_NUM_BUFFERS >= 7
     // @Group: SR6_
-    // @Path: GCS_MAVLink_Tracker.cpp
+    // @Path: GCS_Mavlink.cpp
     GOBJECTN(_gcs.chan_parameters[6],  gcs6,       "SR6_",     GCS_MAVLINK_Parameters),
 #endif
 
@@ -308,8 +308,10 @@ const AP_Param::Info Tracker::var_info[] = {
     // @Group: SERVO
     // @Path: ../libraries/SRV_Channel/SRV_Channels.cpp
     GOBJECT(servo_channels,     "SERVO", SRV_Channels),
-
-    // AP_SerialManager was here
+    
+    // @Group: SERIAL
+    // @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp
+    GOBJECT(serial_manager,    "SERIAL",   AP_SerialManager),
 
     // @Param: PITCH2SRV_P
     // @DisplayName: Pitch axis controller P gain
@@ -518,6 +520,7 @@ const AP_Param::Info Tracker::var_info[] = {
     // @DisplayName: GCS PID tuning mask
     // @Description: bitmask of PIDs to send MAVLink PID_TUNING messages for
     // @User: Advanced
+    // @Values: 0:None,1:Pitch,2:Yaw
     // @Bitmask: 0:Pitch,1:Yaw
     GSCALAR(gcs_pid_mask,           "GCS_PID_MASK",     0),
 
@@ -555,6 +558,7 @@ const AP_Param::Info Tracker::var_info[] = {
     // @DisplayName: Auto mode options
     // @Description: 1: Scan for unknown target
     // @User: Standard
+    // @Values: 0:None, 1: Scan for unknown target in auto mode
     // @Bitmask: 0:Scan for unknown target
     GSCALAR(auto_opts,              "AUTO_OPTIONS",        0),
 
@@ -584,28 +588,19 @@ void Tracker::load_parameters(void)
 
 #if AP_STATS_ENABLED
     // PARAMETER_CONVERSION - Added: Jan-2024
-    AP_Param::convert_class(g.k_param_stats_old, &stats, stats.var_info, 0, true);
+    AP_Param::convert_class(g.k_param_stats_old, &stats, stats.var_info, 0, 0, true);
 #endif
 
 #if AP_SCRIPTING_ENABLED
     // PARAMETER_CONVERSION - Added: Jan-2024
-    AP_Param::convert_class(g.k_param_scripting_old, &scripting, scripting.var_info, 0, true);
+    AP_Param::convert_class(g.k_param_scripting_old, &scripting, scripting.var_info, 0, 0, true);
 #endif
 
     // PARAMETER_CONVERSION - Added: Feb-2024 for Tracker-4.6
 #if HAL_LOGGING_ENABLED
-    AP_Param::convert_class(g.k_param_logger, &logger, logger.var_info, 0, true);
+    AP_Param::convert_class(g.k_param_logger, &logger, logger.var_info, 0, 0, true);
 #endif
 
-    static const AP_Param::TopLevelObjectConversion toplevel_conversions[] {
-#if AP_SERIALMANAGER_ENABLED
-        // PARAMETER_CONVERSION - Added: Feb-2024 for Tracker-4.6
-        { &serial_manager, serial_manager.var_info, Parameters::k_param_serial_manager_old },
-#endif
-    };
-
-    AP_Param::convert_toplevel_objects(toplevel_conversions, ARRAY_SIZE(toplevel_conversions));
-
 #if HAL_HAVE_SAFETY_SWITCH
     // configure safety switch to allow stopping the motors while armed
     AP_Param::set_default_by_name("BRD_SAFETYOPTION", AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_OFF|

AntennaTracker/Parameters.h

@@ -91,7 +91,10 @@ public:
         k_param_can_mgr,
         k_param_battery,
 
-        k_param_serial_manager_old = 144,     // serial manager library
+        //
+        // 150: Telemetry control
+        //
+        k_param_serial_manager,     // serial manager library
         k_param_servo_yaw_type,
         k_param_alt_source,
         k_param_mavlink_update_rate,

AntennaTracker/RC_Channel.cpp

@@ -1,6 +1,6 @@
 #include "Tracker.h"
 
-#include "RC_Channel_Tracker.h"
+#include "RC_Channel.h"
 
 // defining these two macros and including the RC_Channels_VarInfo
 // header defines the parameter information common to all vehicle

AntennaTracker/ReleaseNotes.txt

@@ -1,740 +1,5 @@
 Antenna Tracker Release Notes:
 ------------------------------------------------------------------
-Release 4.6.0-beta2 11 Dec 2024
-
-Changes from 4.6.0-beta1
-
-1) Board specfic changes
-
-- FoxeerF405v2 supports BMP280 baro
-- KakuteH7, H7-Mini, H7-Wing, F4 support SPA06 baro
-- MUPilot support
-- SkySakura H743 support
-- TBS Lucid H7 support
-- VUAV-V7pro README documentation fixed
-- X-MAV AP-H743v2 CAN pin definition fixed
-
-2) Copter specific enhancements and bug fixes
-
-- AutoTune fix for calc of maximum angular acceleration
-- Advanced Failsafe customer build server option
-
-3) Plane related enhancements and bug fixes
-
-- QuadPlane fix for QLand getting stuck in pilot repositioning
-- QuikTune C++ conversion (allow running quiktun on F4 and f7 boards)
-- Takeoff direction fixed when no yaw source
-- TECS correctly handles home altitude changes
-
-4) Bug Fixes and minor enhancements
-
-- AIRSPEED_AUTOCAL mavlink message only sent when required and fixed for 2nd sensor
-- CAN frame logging added to ease support
-- CRSF reconnection after failsafe fixed
-- EKF3 position and velocity resets default to user defined source
-- Ethernet IP address default 192.168.144.x
-- Fence autoenable fix when both RCn_OPTION=11/Fence and FENCE_AUTOENABLE = 3 (AutoEnableOnlyWhenArmed)
-- Fence pre-arm check that vehicle is within polygon fence
-- Fence handling of more than 256 items fixed
-- FFT protection against divide-by-zero in Jain estimator
-- Frsky telemetry apparent wind speed fixed
-- Inertial sensors stop sensor converging if motors arm
-- Inertial sensors check for changes to notch filters fixed
-- Real Time Clock allowed to shift forward when disarmed
-- ROS2/DDS get/set parameter service added
-- Scripting gets memory handling improvements
-- Scripting promote video-stream-information to applet
-- Topotek gimbal driver uses GIA message to retrieve current angle
-- Tramp VTX OSD power indicator fixed
-------------------------------------------------------------------
-Release 4.6.0-beta1 13 Nov 2024
-
-Changes from 4.5.7
-
-1) Board specific changes
-
-- AnyLeaf H7 supports compass and onboard logging
-- Blitz743Pro supports CAN
-- BlueRobotics Navigator supports BMP390 baro
-- Bootloader ECC failure check fixed on boards with >128k bootloader space (e.g CubeRed)
-- CB Unmanned Stamp H743 support
-- ClearSky CSKY405 support
-- CUAV-7-Nano default batt monitor fixed
-- CubeRed bootloader fixes including disabling 2nd core by default
-- CubeRed supports PPP networking between primary and secondary MCU
-- CubeRedPrimary supports external compasses
-- ESP32 main loop rate improvements
-- ESP32 RC input fixes and wifi connection reliability improved
-- ESP32 safety switch and GPIO pin support
-- FlyingMoon no longer support MAX7456
-- Flywoo F405HD-AIOv2 ELRS RX pin pulled high during boot
-- Flywoo H743 Pro support
-- Flywoo/Goku F405 HD 1-2S ELRS AIO v2
-- FlywooF745 supports DPS310 baro
-- FPV boards lose SMBus battery support (to save flash)
-- GEPRC F745BTHD support
-- GEPRCF745BTHD loses parachute support, non-BMP280 baros (to save flash)
-- Here4FC bootloader fix for mismatch between RAM0 and periph that could prevent firmware updates
-- Holybro Kakute F4 Wing support
-- iFlight 2RAW H743 supports onboard logging
-- JFB110 supports measuring servo rail voltage
-- JFB110 supports safety switch LED
-- JHEM-JHEF405 bootloader supports firmware updates via serial
-- JHEMCU GF30H743 HD support
-- JHEMCU-GF16-F405 autopilot support
-- JHEMCU-GSF405A becomes FPV board (to save flash)
-- KakuteF7 only supports BMP280 baro (to save flash)
-- KakuteH7Mini supports ICM42688 IMU
-- Linux auto detection of GPS baud rate fixed
-- Linux board scheduler jitter reduced
-- Linux board shutdown fixes
-- MakeFlyEasy PixPilot-V6Pro support
-- MatekF405, Pixhawk1-1M-bdshot, revo-mini loses blended GPS (to save flash)
-- MatekH7A3 support Bi-directional DShot
-- MicoAir405v2 and MicoAir405Mini support optical flow and OSD
-- MicoAir743 internal compass orientation fixed
-- MicroAir405Mini, MicroAir743, NxtPX4v2 support
-- MicroAir405v2 Bi-directional DShot and LED DMA fixes
-- MicroAir405v2 defined as FPV board with reduced features (to save flash)
-- ModalAI VOXL2 support including Starling 2 and Starling 2 max
-- mRo Control Zero Classic supports servo rail analog input
-- mRo KitCAN revC fixed
-- Mugin MUPilot support
-- OmnibusF7v2 loses quadplane support (to save flash)
-- Pixhack-v3 board added (same as fmuv3)
-- Pixhawk6C bootloader supports flashing firmware from SD card
-- RadiolinkPIX6 imu orientation fixed
-- RadiolinkPIX6 supports SPA06 baro
-- ReaperF745 V4 FC supports MPU6000 IMU
-- RPI5 support
-- SDModelH7v2 SERIAL3/7/9_PROTOCOL param defaults changed
-- Solo serial ports default to MAVLink1
-- SpeedyBeeF405Wing gets Bi-directional DShot
-- SpeedyBeeF405WING loses landing gear support, some camera gimbals (to save flash)
-- Spektreworks boom board support
-- TrueNavPro-G4 SPI does not share DMA
-- X-MAV AP-H743v2 support
-
-2) AHRS/EKF enhancements and fixes
-
-- AHRS_OPTION to disable fallback to DCM (affects Plane and Rover, Copter never falls back)
-- AHRS_OPTION to disable innovation check for airspeed sensor
-- Airspeed sensor health check fixed when using multiple sensors and AHRS affinity
-- DCM support for MAV_CMD_EXTERNAL_WIND_ESTIMATE (Plane only)
-- EK2 supports disabling external nav (see EK2_OPTIONS)
-- EK3 External Nav position jump after switch from Optical flow removed (see EK3_SRC_OPTION=1)
-- EK3 uses filtered velocity corrections for IMU position
-- EKF2, EKF3, ExternalAHRS all use common origin
-- EKF3 accepts set origin even when using GPS
-- EKF3 allows earth-frame fields to be estimated with an origin but no GPS
-- EKF3 copes better with GPS jamming
-- EKF3 logs mag fusion selection to XKFS
-- EKF3 wind estimation when using GPS-for-yaw fixed
-- External AHRS improvements including handling variances, pre-arm origin check
-- Inertial Labs External AHRS fixes
-- VectorNav driver fix for handling of error from sensor
-- VectorNav External AHRS enhancements including validation of config commands and logging improvements
-- VectorNav support for sensors outside VN-100 and VN-300
-
-3) Driver enhancements and bug fixes
-
-- ADSB fix to display last character in status text sent to GCS
-- Ainstein LR-D1 radar support
-- Airspeed ASP5033 whoami check fixed when autopilot rebooted independently of the sensor
-- AIRSPEED message sent to GCS
-- Analog temperature sensor extended to 5th order polynomial (see TEMP_A5)
-- ARSPD_OPTIONS to report calibration offset to GCS
-- Baro EAS2TAS conversions continuously calculated reducing shocks to TECS (Plane only)
-- Baro provides improved atmospheric model for high altitude flight
-- BARO_ALT_OFFSET slew slowed to keep EKF happy
-- BATTx_ESC_MASK param supports flexible mapping of ESCs to batteries
-- BATTx_OPTION to not send battery voltage, current, etc to GCS
-- Benewake RDS02U radar support
-- Bi-directional DShot on IOMCU supports reversible mask
-- Bi-directional DShot telemetry support on F103 8Mhz IOMCUs
-- BMM350 compass support
-- CAN rangefinders and proximity sensors may share a CAN bus (allows NRA24 and MR72 on a single CAN bus)
-- Compass calibration world magnetic model checks can use any position source (e.g. not just GPS)
-- CRSF baro and vertical speeed fixed
-- CRSF RX bind command support
-- DroneCAN battery monitor check to avoid memory corruption when type changed
-- DroneCAN DNA server fixes including removing use of invalid node IDs, faster ID allocation, elimination of rare inability to save info
-- DroneCAN EFI health check fix
-- DroneCAN ESC battery monitors calculate consumed mah
-- DroneCAN ESCs forced to zero when disarmed
-- DroneCAN RPM message support
-- DroneCAN timeout fix for auxiliary frames
-- DroneCAN to serial tunneling params accepts short-hand baud rates (e.g. '57' for '57600')
-- F9P, F10-N and Zed-F9P support for GPSx_GNSS_MODE to turn on/off SBAS, Galileo, Beidou and Glonass
-- FuelLevel battery monitor fix to report capacity
-- GPS_xxx params renamed to GPS1_xxx, GPS_xxx2 renamed to GPS2_xxx
-- Hirth EFI logging includes modified throttle
-- Hirth ICEngine supports reversed crank direction (see ICE_OPTIONS parameter)
-- Hott and LTM telemetry deprecated (still available through custom build server)
-- i-BUS telemetry support
-- ICE_PWM_IGN_ON, ICE_PWM_IGN_OFF, ICE_PWM_STRT_ON, ICE_PWM_STRT_OFF replaced with SERVOx_MIN/MAX/REVERSED (Plane only)
-- ICE_START_CHAN replaced with RC aux function (Plane only)
-- ICEngine retry max added (see ICE_STRT_MX_RTRY)
-- IE 2400 generator error message severity to GCS improved
-- INA2xx battery monitor support (reads temp, gets MAX_AMPS and SHUNT params)
-- MCP9600 temperature sensor I2C address fixed
-- MLX90614 temperature sensor support
-- MSP GPS ground course scaling fixed
-- MSP RC uses scaled RC inputs (fixes issue with RCx_REVERSED having no effect)
-- Networking supports reconnection to TCP server or client
-- OSD params for adjusting horizontal spacing and vertical extension (see OSD_SB_H_OFS, OSD_SB_V_EXT)
-- Relay inverted output support (see RELAYx_INVERTED parameter)
-- ROMFS efficiency improvements
-- RS-485 driver enable RTS flow control
-- Sagetech MXS ADSP altitude fix (now uses abs alt instead of terrain alt)
-- Septentrio GPS sat count correctly drops to zero when 255 received
-- Septentrio supports selecting constellations (see GPS_GNSS_MODE)
-- Single LED for user notification supported
-- SPA06 baro supported
-- Sum battery monitor optionally reports minimum voltage instead of average
-- Sum battery monitor reports average temp
-- Torqeedo dual motor support (see TRQ1, TRQ2 params)
-- Ublox GPS driver uses 64 bit time for PPS interrupt (avoids possible dropout at 1hour and 12 minutes)
-- UBlox GPS time ignored until at least 2D fix
-- VideoTX supports additional freq bands (RushFPV 3.3Ghz)
-- Volz logs desired and actual position, voltage, current, motor and PCB temp
-- Volz server feedback and logging fixed
-- Volz servo output in its own thread resulting in smoother movements
-- W25N02KV flash support
-
-4) Networking enhancements and fixes
-
-- Allow multiple UDP clients to connect/disconnect/reconnect
-- Ethernet supports faster log downloading (raised SDMMC clock limit on H7)
-
-5) Camera and gimbal enhancements
-
-- Alexmos precision improved slightly
-- CAMERA_CAPTURE_STATUS mavlink msg sent to GCS (reports when images taken or video recorded, used by QGC)
-- CAMERA_FOV_STATUS mavlink reports lat,lon of what camera is pointing at
-- DO_MOUNT_CONTROL yaw angle interpreted as body-frame (was incorrectly being interpreted as earth-frame)
-- Dual serial camera gimbal mounts fixed
-- Lua script bindings to send CAMERA_INFORMATION and VIDEO_STREAM_INFORMATION messages to GCS
-- Retract Mount2 aux function added (see RCx_OPTION = 113)
-- Servo gimbal reported angles respect roll, pitch and yaw limits
-- Siyi driver sends autopilot location and speed (recorded in images via EXIF)
-- Siyi picture and video download scripts
-- Siyi ZT6 and ZT30 support sending min, max temperature (see CAMERA_THERMAL_RANGE msg)
-- Siyi ZT6 and ZT30 thermal palette can be changed using camera-change-setting.lua script
-- Siyi ZT6 hardware id and set-lens fixed
-- Topotek gimbal support
-- Trigger distance ignores GPS status and only uses EKF reported location
-
-6) Harmonic notch enhancements
-
-- Harmonic notch is active in forward flight on quadplanes
-- Harmonic notch filter freq clamping and disabling reworked
-- Harmonic notch handles negative ESC RPMs
-- Harmonic notch supports per-motor throttle-based harmonic notch
-
-7) Copter specific enhancements and bug fixes
-
-- Attitude control fix to dt update order (reduces rate controller noise)
-- Auto mode fix to avoid prematurely advancing to next waypoint if given enough time
-- Auto mode small target position jump when takeoff completes removed
-- Auto mode yaw drift when passing WP removed if CONDITION_YAW command used and WP_YAW_BEHAVIOR = 0/None
-- Auto, Guided flight mode pause RC aux switch (see RCx_OPTION = 178)
-- AutoRTL (e.g. DO_LAND_START) uses copter stopping point to decide nearest mission command
-- AutoRTL mode supports DO_RETURN_PATH_START (Copter, Heli only)
-- AutoTune fix to prevent spool up after landing
-- AutoTune performance and safety enhancements (less chance of ESC desync, fails when vehicle likely can't be tuned well)
-- Autotune test gains RC aux switch function allows testing gains in any mode (see RCx_OPTION = 180)
-- Config error avoided if auto mode is paused very soon after poweron
-- FLIGHT_OPTIONS bit added to require position estimate before arming
-- Follow mode slowdown calcs fixed when target is moving
-- Ground oscillation suppressed by reducing gains (see ATC_LAND_R/P/Y_MULT)
-- Guided mode internal error fix when taking off using SET_ATTITUDE_CONTROL message
-- Guided mode internal error resolved when switching between thrust or climb rate based altitude control
-- Guided mode yaw fixed when WP_YAW_BEHAVIOR = 0/None and CONDITION_YAW command received containing relative angle
-- Landing detector fixed when in stabilize mode at full throttle but aircraft is upside down
-- Landing detector logging added to ease support (see LDET message)
-- Loiter unlimited command accepts NaNs (QGC sends these)
-- Mavlink SYSTEM_STATUS set to BOOT during initialisation
-- MOT_PWM_TYPE of 9 (PWMAngle) respects SERVOx_MIN/MAX/TRIM/REVERSED param values
-- Payload place bug fix when aborted because gripper is already released
-- RC based tuning (aka CH6 tuning) can use any RC aux function channel (see RCx_OPTION = 219)
-- RTL_ALT minimum reduced to 30cm
-- SystemID position controller support (Copter and Heli)
-- TriCopter motor test and slew-up fixed
-- WPNAV_SPEED min reduced to 10 cm/s (Copter only)
-- Loiter mode zeros desired accel when re-entering from Auto during RC failsafe
-
-8) TradHeli specific enhancements
-
-- Autorotation yaw behaviour fix
-- Autotune improvements including using low frequency dwell for feedforward gain tuning and conducting sweep in attitude instead of rate
-- Blade pitch angle logging added (see SWSH log message)
-- Constrain cyclic roll for intermeshing
-- ICE / turbine cool down fix
-- Inverted flight extended to non manual throttle modes
-- Inverted flight transitions smoothed and restricted to only Stabilize mode
-- SWSH logging fix for reversed collectives
-
-9) Plane specific enhancements and bug fixes
-
-- AIRSPEED_STALL holds vehicle stall speed and is used for minimum speed scaling
-- Allow for different orientations of landing rangefinder
-- Assistance requirements evaluted on mode change
-- FBWB/CRUISE climb/sink rate limited by TECS limits
-- FLIGHT_OPTION to immediately climb in AUTO mode (not doing glide slope)
-- Glider pullup support (available only through custom build server)
-- Loiter breakout improved to better handle destinations inside loiter circle
-- Manual mode throttle made consistent with other modes (e.g battery comp and watt limit is done if enabled)
-- Mavlink GUIDED_CHANGE_ALTITUDE supports terrain altitudes
-- Minimum throttle not applied during SLT VTOL airbrake (reduces increase in airspeed and alt during back transition)
-- Q_APPROACH_DIST set minimum distance to use the fixed wing approach logic
-- Quadplane assistance check enhancements
-- Quadplane Deca frame support
-- Quadplane gets smoother takeoff by input shaping target accel and velocity
-- Servo wiggles in altitude wait staged to be one after another
-- Set_position_target_global_int accepts MAV_FRAME_GLOBAL_RELATIVE_ALT and MAV_FRAME_GLOBAL_TERRAIN_ALT frames
-- Takeoff airspeed control improved (see TKOFF_MODE, TKOFF_THR_MIN)
-- Takeoff fixes for fence autoenable
-- Takeoff improvements including less overshoot of TKOFF_ALT
-- TECS reset along with other controllers (important if plane dropped from balloon)
-- Tilt quadplane ramp of motors on back transition fixed
-- Tiltrotor tilt angles logged
-- TKOFF_THR_MIN applied to SLT transitions
-- Twin motor planes with DroneCAN ESCs fix to avoid max throttle at arming due to misconfiguration
-- VTOLs switch to QLAND if a LONG_FAILSAFE is triggered during takeoff
-
-10) Rover specific enhancements and bug fixes
-
-- Auto mode reversed state maintained if momentarily switched to Hold mode
-- Circle mode tracks better and avoids getting stuck at circle edge
-- Flight time stats fixed
-- MAV_CMD_NAV_SET_YAW_SPEED deprecated
-- Omni3Mecanum frame support
-- Stopping point uses max deceleration (was incorrectly using acceleration)
-- Wheel rate controller slew rate fix
-
-11) Antenna Tracker specific enhancements and bug fixes
-
-- Never track lat,lon of 0,0
-
-12) Scripting enhancements
-
-- advance-wp.lua applet supports advancing Auto mode WP via RC switch
-- AHRS_switch.lua supports switching between EKF2 and EKF3 via RC switch
-- battery_internal_resistance_check.lua monitors battery resistance
-- CAN:get_device returns nil for unconfigured CAN device
-- copter_terrain_brake.lua script added to prevent impact with terrain in Loiter mode (Copter only)
-- Copter:get_target_location, update_target_location support
-- crosstrack_restore.lua example allows returning to previous track in Auto (Plane only)
-- Display text on OLED display supported
-- Docs improved for many bindings
-- EFI get_last_update_ms binding
-- EFI_SkyPower.lua driver accepts 2nd supply voltage
-- ESC_slew_rate.lua example script supports testing ESCs
-- Filesystem CRC32 check to allow scripts to check module versions
-- forced arming support
-- GPIO get/set mode bindings (see gpio:set_mode, get_mode)
-- GPS-for-yaw angle binding (see gps:gps_yaw_deg)
-- Halo6000 EFI driver can log all CAN packets for easier debugging
-- handle_external_position_estimate binding allows sending position estimate from lua to EKF
-- I2C:transfer support
-- IMU gyros_consistent and accels_consistent bindings added
-- INF_Inject.lua driver more robust to serial errors, improved logging, throttle and ignition control
-- INS bindings for is calibrating, gyro and accel sensor values
-- IPV4 address bindings (see SocketAPM_ipv4_addr_to_string) to allow UDP server that responds to individual clients
-- Logging of booleans supported
-- Lua language checks improved (finds more errors)
-- MAVLink commands can be called from scripting
-- MCU voltage binding (see analog:mcu_voltage)
-- NMEA 2000 EFI driver (see EFI_NMEA2k.lua)
-- "open directory failed" false warning when scripts in ROMFS fixed
-- Param_Controller.lua supports quickly switching between 3 parameter sets via RC switch
-- Pass by reference values are always initialized
-- pelco_d_antennatracker.lua applet supports sending Pelco-D via RS-485 to PTZ cameras
-- plane_aerobatics.lua minor enhancements
-- REPL applet (read-evaluate-print-loop, see repl.lua) for interactive testing and experimentation
-- "require" function failures in rare circumstances fixed
-- "require" function works for modules in ROMFS (e.g. not on SD card)
-- revert_param.lua supports more params (e.g ATC_RATE_R/P/Y, PTCH2SRV_TCONST, RLL2SRV_TCONST, TECS_)
-- Scripts may receive mavlink messages which fail CRC (e.g messages which FC does not understand)
-- SD card formatting supported
-- Serial device simulation support (allows Lua to feed data to any supported serial protocol for e.g. sensor simulation)
-- set_target_throttle_rate_rpy allows rate control from scripts (new for Copter)
-- sitl_standby_sim.lua example shows how to switch between redundant flight controllers using an RC switch
-- Slung payload oscillation suppression applet added (see copter-slung-payload.lua)
-- Temperature sensor bindings added
-- uint64 support
-- Various performance and memory usage optimizations
-- VTOL-quicktune.lua minor enhancements including increasing YAW_FLTE_MAX to 8
-- x-quad-cg-allocation.lua applet corrects for the CoM discrepancy in a quad-X frame
-
-13) GCS / mavlink related changes and fixes
-
-- BATTERY2 message deprecated (replaced by BATTERY_STATUS)
-- CMD_MISSION_START/STOP rejected if first-item/last-item args provided
-- Deny attempts to upload two missions simultaneously
-- Fence and Rally points may be uploaded using FTP
-- GPS_INPUT and HIL_GPS handles multiple GPSs
-- HIGHRES_IMU mavlink message support (used by companion computers to receive IMU data from FC)
-- MAV_CMD_COMPONENT_ARM_DISARM accepts force arm magic value of 21196
-- MAV_CMD_DO_SET_SAFETY_SWITCH_STATE support
-- MAV_CMD_SET_HAGL support (Plane only)
-- MAVFTP respects TX buffer flow control to improve FTP on low bandwidth links
-- MAVLink receiver support (RADIO_RC_CHANNELS mavlink message)
-- Message interval supports TERRAIN_REPORT msg
-- Mission upload may be cancelled using MISSION_CLEAR_ALL
-- MOUNT_CONFIGURE, MOUNT_CONTROL messages deprecated
-- RC_CHANNELS_RAW deprecated
-- Serial passthrough supports parity allowing STM32CubeProgrammer to be used to program FrSky R9 receivers
-- SET_ATTITUDE_TARGET angular rate input frame fixed (Copter only)
-- TIMESYNC and NAMED_VALUE_FLOAT messages not sent on high latency MAVLink ports
-
-14) Logging enhancements and fixes
-
-- AC_PID resets and I-term sets logged
-- ANG provides attitude at high rate (equivalent to ATT)
-- ATT logs angles as floats (better resolution than ints)
-- CAND message gets driver index
-- DCM log message includes roll/pitch and yaw error
-- EDT2 log msg includes stress and status received via extended DShot Telemetry v2
-- EFI ECYL cylinder head and exhaust temp logs in degC (was Kelvin)
-- ESCX log msg includes DroneCAN ESCs status, temp, duty cycle and power pct
-- FMT messages written as required instead of all at beginning
-- Logging restarted after download completes when LOG_DISARMED = 1
-- MISE msg logs active mission command (CMD logged when commands are uploaded)
-- ORGN message logging fixed when set using SET_GPS_GLOBAL_ORIGIN
-- RPM sensor logging gets instance field, quality and health fields
-- Short filename support removed (e.g log1.BIN instead of 00000001.BIN)
-- Temperature sensor logging option for only sensors with no source (see TEMP_LOG)
-- UART data rates logged at 1hz (see UART message)
-
-15) ROS2 / DDS support
-
-- Airspeed published
-- Battery topic reports all available batteries
-- Compile-time configurable rates for each publisher
-- DDS_TIMEOUT_MS and DDS_MAX_RETRY set timeout and num retries when client pings XRCE agent
-- GPS global origin published at 1 Hz
-- High frequency raw imu data transmission
-- Joystick support
-- Support sending waypoints to Copter and Rover
-- Remove the XML refs file in favor of binary entity creation
-
-16) Safety related enhancements and fixes
-
-- Accel/Gyro inconsistent message fixed for using with only single IMU
-- Battery monitor failure reported to GCS, triggers battery failsafe but does not take action
-- Far from EKF origin pre-arm check removed (Copter only)
-- Fence breach warning message slightly improved
-- Fence enhancements incluiding alt min fence (Copter only, see FENCE_AUTOENABLE, FENCE_OPTIONS)
-- Fences can be stored to SD Card (see BRD_SD_FENCE param)
-- ICEngine stopped when in E-Stop or safety engaged (Plane only)
-- LEDs flash green lights based on EKF location not GPS
-- Parachute option to skip disarm before parachute release (see CHUTE_OPTIONS)
-- Plane FENCE_AUTOENABLE of 1 or 2 deprecation warning added
-- Pre-arm check if OpenDroneID is compiled in but disabled
-- Pre-arm check of duplicate RC aux functions fixed (was skipping recently added aux functions)
-- Pre-arm checks alert user more quickly on failure
-- Prearm check for misconfigured EAHRS_SENSORS and GPS_TYPE
-- Proximity sensor based avoidance keeps working even if one proximity sensor fails (Copter, Rover)
-- RC aux functions for Arm, Disarm, E-Stop and Parachute ignored when RC is first turned on
-- Warning of duplicate SERIALx_PROTOCOL = RCIN
-
-17) Other bug fixes and minor enhancements
-
-- Accel cal fixed for auxiliary IMUs (e.g. IMU4 and higher)
-- Bootloader fix to reduce unnecessary mass erasing of flash when using STLink or Jlink tools
-- Bootloader rejects allocation of broadcast node ID
-- CAN forward registering/de-registering fix (affected Mission Planner DroneCAN UI)
-- Dijkstras fix to correct use of uninitialised variable
-- DShot rates are not limited by NeoPixel rates
-- Ethernet and CAN bootloader fix to prevent getting stuck in bootloader mode
-- Filesystem does not show entries for empty @ files
-- Filesystem efficiency improvements when reading files
-- Flight statistics only reset if user sets STAT_RESET to zero (avoids accidental reset)
-- Flight time statistics updated on disarm (avoids issue if vehicle powered-down soon after disarm)
-- Internal error thrown if we lose parameters due to saving queue being too small
-- MAVLink via DroneCAN baud rate fix
-- SPI pins may also be used as GPIOs
-- Terrain cache size configurable (see TERRAIN_CACHE_SZ)
-
-18) Developer focused fixes and enhancements
-
-- AP_Camera gets example python code showing how to use GStreamer with UDP and RTSP video streams
-- Cygwin build fix for non-SITL builds
-- Cygwin build fixed with malloc wrap
-- DroneCAN and scripting support FlexDebug messages (see CAN_Dn_UC_OPTION, FlexDebug.lua)
-- EKF3 code generator documentation and cleanup
-- GPS jamming simulator added
-- MacOS compiler warnings reduced
-- SFML joystick support
-- SITL support for OpenBSD
-- Text warning if older Fence or Rally point protocols are used
-------------------------------------------------------------------
-Release 4.5.7 08 Oct 2024
-
-Changes from 4.5.7-beta1
-
-1) Reverted Septentrio GPS sat count correctly drops to zero when 255 received
-------------------------------------------------------------------
-Release 4.5.7-beta1 26 Sep 2024
-
-Changes from 4.5.6
-
-1) Bug fixes and minor enhancements
-
-- VUAV-V7pro support
-- CUAV-7-Nano correction for LEDs and battery volt and current scaling
-- DroneCAN deadlock and saturation of CAN bus fixed
-- DroneCAN DNA server init fix (caused logging issues and spam on bus)
-- F4 boards with inverter support correctly uninvert RX/TX
-- Nanoradar M72 radar driver fix for object avoidance path planning
-- RC support for latest version of GHST
-- Septentrio GPS sat count correctly drops to zero when 255 received
-
-2) ROS2/DDS and other developer focused enhancements
-
-- AP quaternions normalised for ROS2 to avoid warnings
-- Dependencies fixed for easier installation
-- ROS2 SITL launch file enhancements including displaying console and map
-- ROS_DOMAIN_ID param added to support multiple vehicles or instances of ROS2
-- Python 3.12 support
-------------------------------------------------------------------
-Release 4.5.6 03 Sep 2024
-
-No changes from 4.5.6-beta1
-------------------------------------------------------------------
-Release 4.5.6-beta1 20 Aug 2024
-
-Changes from 4.5.5
-
-1) Board specific enhancements and bug fixes
-
-- 3DR Control Zero H7 Rev G support
-- CUAV-7-Nano support
-- FoxeerF405v2 servo outputs increased from 9 to 11
-- Holybro Pixhawk6C hi-power peripheral overcurrent reporting fixed
-- iFlight 2RAW H7 support
-- MFT-SEMA100 support
-- TMotorH743 support BMI270 baro
-- ZeroOneX6 support
-
-2) Minor enhancements and bug fixes
-
-- Cameras using MAVLink report vendor and model name correctly
-- DroneCAN fix to remove occasional NodeID registration error
-- GPS NMEA and GSoF driver ground course corrected (now always 0 ~ 360 deg)
-- ICP101XX barometer slowed to avoid I2C communication errors
-- IMU temp cal param (INSn_ACCSCAL_Z) stored correctly when bootloader is flashed
-- IMU gyro/accel duplicate id registration fixed to avoid possible pre-arm failure
-- Logging to flash timestamp fix
-- OSD displays ESC temp instead of motor temp
-- PID controller error calculation bug fix (was using target from prev iteration)
-- Relay on MAIN pins fixed
-------------------------------------------------------------------
-Release 4.5.5 1st Aug 2024
-
-No changes from 4.5.5-beta2
-------------------------------------------------------------------
-Release 4.5.5-beta2 27 July 2024
-
-Changes from 4.5.5-beta1
-
-1) Board specific enhancements and bug fixes
-
-- CubeRed's second core disabled at boot to avoid spurious writes to RAM
-- CubeRed bootloader's dual endpoint update method fixed
-------------------------------------------------------------------
-Release 4.5.5-beta1 1st July 2024
-
-Changes from 4.5.4
-
-1) Board specific enhancements and bug fixes
-
-- fixed IOMCU transmission errors when using bdshot
-- update relay parameter names on various boards
-- add ASP5033 airspeed in minimal builds
-- added RadiolinkPIX6
-- fix Aocoda-RC H743Dual motor issue
-- use ICM45686 as an ICM20649 alternative in CubeRedPrimary
-
-2) System level minor enhancements and bug fixes
-
-- correct use-after-free in script statistics
-- added arming check for eeprom full
-- fixed a block logging issue which caused log messages to be dropped
-- enable Socket SO_REUSEADDR on LwIP
-- removed IST8310 overrun message
-- added Siyi ZT6 support
-- added BTFL sidebar symbols to the OSD
-- added CRSF extended link stats to the OSD
-- use the ESC with the highest RPM in the OSD when only one can be displayed
-- support all Tramp power levels on high power VTXs
-- emit jump count in missions even if no limit
-- improve the bitmask indicating persistent parameters on bootloader flash	
-- fix duplicate error condition in the MicroStrain7
-
-5) Other minor enhancements and bug fixes
-
-- specify pymonocypher version in more places
-- added DroneCAN dependencies to custom builds
-
-------------------------------------------------------------------
-Release 4.5.4 12th June 2024
-
-Changes from 4.5.3
-
-Disable highres IMU sampling on ICM42670 fixing an issue on some versions of Pixhawk6X
-
-------------------------------------------------------------------
-Release 4.5.3 28th May 2024
-
-No changes from 4.5.3-beta1
-------------------------------------------------------------------
-Release 4.5.3-beta1 16th May 2024
-
-Changes from 4.5.2
-
-1) Board specific enhancements and bug fixes
-
-- correct default GPS port on MambaH743v4
-- added SDMODELV2
-- added iFlight Blitz H7 Pro
-- added BLITZ Wing H743
-- added highres IMU sampling on Pixhawk6X
-
-2) System level minor enhancements and bug fixes
-
-- fixed rare crash bug in lua scripting on script fault handling
-- fixed Neopixel pulse proportions to work with more LED variants
-- fixed timeout in lua rangefinder drivers
-- workaround hardware issue in IST8310 compass
-- allow FIFO rate logging for highres IMU sampling
-
-------------------------------------------------------------------
-Release 4.5.2 14th May 2024
-
-No changes from 4.5.2-beta1
-------------------------------------------------------------------
-Release 4.5.2-beta1 29th April 2024
-
-Changes from 4.5.1
-
-1) Board specific enhancements and bug fixes
-
-- FoxeerF405v2 support
-- iFlight BLITZ Mini F745 support
-- Pixhawk5X, Pixhawk6C, Pixhawk6X, Durandal power peripherals immediately at startup
-
-2) System level minor enhancements and bug fixes
-
-- Camera lens (e.g. RGB, IR) can be selected from GCS or during missions using set-camera-source
-- Crashdump pre-arm check added
-- Gimbal gets improved yaw lock reporting to GCS
-- Gimbal default mode fixed (MNTx_DEFLT_MODE was being overriden by RC input)
-- RM3100 compass SPI bus speed reduced to 1Mhz
-- SBUS output fix for channels 1 to 8 also applying to 9 to 16
-- ViewPro gimbal supports enable/disable rangefinder from RC aux switch
-- Visual Odometry delay fixed (was always using 1ms delay, see VISO_DELAY_MS)
-- fixed serial passthrough to avoid data loss at high data rates
-
-3) AHRS / EKF fixes
-
-- Compass learning disabled when using GPS-for-yaw
-- GSF reset minimum speed reduced to 1m/s (except Plane which remains 5m/s)
-- MicroStrain7 External AHRS position quantization bug fix
-- MicroStrain7 init failure warning added
-- MicroStrain5 and 7 position and velocity variance reporting fix
-
-4) Other minor enhancements and bug fixes
-
-- DDS_UDP_PORT parameter renamed (was DDS_PORT)
-- Harmonic Notch bitmask parameter conversion fix (see INS_HNTCH_HMNCS)
-
-------------------------------------------------------------------
-Release 4.5.1 8th April 2024
-
-This release fixes a critical bug in the CRSF R/C protocol parser that
-can lead to a handfault and a vehicle crashing. A similar fix was
-applied to the GHST protocol, although we believe that GHST could not
-be affected by the bug, so this was just a precaution.
-
-There are no other changes in this release.
-
-------------------------------------------------------------------
-Release 4.5.0 2nd April 2024
-
-No changes from 4.5.0-beta4
-------------------------------------------------------------------
-Release 4.5.0-beta4 22nd March 2024
-Changes from 4.5.0-beta3
-1) system changes
-
-- fixed a cache corruption issue with microSD card data on H7 based boards
-- rename parameter NET_ENABLED to NET_ENABLE
-- fixed FDCAN labels for adding new H7 boards
-- avoid logging dma.txt to save CPU
-- fixed roll/pitch in viewpro driver
-- added band X in VideoTX
-- fixed quaternion attitude reporting for Microstrain external AHRS
-- add RPLidarC1 proximity support
-
-2) new boards
-- added MicoAir405v2
-- add Orqa F405 Pro
-
-------------------------------------------------------------------
-AntennaTracker 4.5.0 beta3 14-Mar-2024
-Changes from 4.5.0 beta1
-
-Board specific changes
-- added PixFlamingo F7 board
-- support ICM42688 on BlitzF745AIO
-- fixed IMU orientation of CubeRedSecondary
-- enable all FPV features on SpeedyBeeF405WING
-
-System level changes
-
-- improved robustness of CRSF parser
-- reduced memory used by DDS/ROS2
-- added filesystem crc32 binding in lua scripting
-- support visual odometry quality metric and added autoswitching lua script
-- allow for expansion of fence storage to microSD for larger pologon fences
-- allow FTP upload of fence and rally points
-- fixed vehicle type of ship simulation for ship landing
-- make severity level depend on generator error level in IE 2400 generator
-- speed up initial GPS probe by using SERIALn_BAUD first
-- allow NanoRadar radar and proximity sensor to share the CAN bus
-- added MR72 CAN proximity sensor
-- only produce *_with_bl.hex not *.hex in builds if bootloader available
-- fixed check for GPS antenna separation in moving baseline yaw
-- added GPS_DRV_OPTIONS options for fully parsing RTCMv3 stream
-- fixed logging of RTCM fragments in GPS driver
-- fixed video recording while armed
-- robostness and logging improvements for ExternalAHRS
-- fixed RPM from bdshot on boards with IOMCU
-- fixed accel cal simple to remove unused IMUs
-- fixed float rounding issue in HAL_Linux millis and micros functions
-- fixed loading of defaults.parm parameters for dynamic parameter subtrees
-- fixed discrimination between GHST and CRSF protocols
-- fixed bug in DroneCAN packet parsing for corrupt packets that could cause a crash
-- fixed handling of network sockets in scripting when used after close
-- fixed bit timing of CANFD buses
-
-New Autopilots supported
- - YJUAV_A6Ultra
- - AnyLeaf H7
-
-- do relay parameter conversion for parachute parameters if ever has been used
-- broaden acceptance criteria for GPS yaw measurement for moving baseline yaw
-
-------------------------------------------------------------------
-AntennaTracker 4.5.0 beta1 22-Feb-2024
-Changes from 4.2.0
-1) Innumerable system-level improvements; see Copter and Plane release notes
-2) fix EKF2/EKF3 parameters
-3) improve logging
-------------------------------------------------------------------
 AntennaTracker 4.2.0 beta1 25-May-2022
 Changes from 1.1.0
 1) Many new supported boards

AntennaTracker/Tracker.cpp

@@ -58,6 +58,7 @@ const AP_Scheduler::Task Tracker::scheduler_tasks[] = {
     SCHED_TASK_CLASS(AP_Baro,          &tracker.barometer,  update,         10, 1500, 40),
     SCHED_TASK_CLASS(GCS,              (GCS*)&tracker._gcs, update_receive, 50, 1700, 45),
     SCHED_TASK_CLASS(GCS,              (GCS*)&tracker._gcs, update_send,    50, 3000, 50),
+    SCHED_TASK_CLASS(AP_Baro,           &tracker.barometer, accumulate,     50,  900, 55),
 #if HAL_LOGGING_ENABLED
     SCHED_TASK(ten_hz_logging_loop,    10,    300, 60),
     SCHED_TASK_CLASS(AP_Logger,   &tracker.logger, periodic_tasks, 50,  300, 65),
@@ -82,7 +83,7 @@ void Tracker::one_second_loop()
     mavlink_system.sysid = g.sysid_this_mav;
 
     // update assigned functions and enable auxiliary servos
-    AP::srv().enable_aux_servos();
+    SRV_Channels::enable_aux_servos();
 
     // updated armed/disarmed status LEDs
     update_armed_disarmed();
@@ -91,7 +92,7 @@ void Tracker::one_second_loop()
         // set home to current location
         Location temp_loc;
         if (ahrs.get_location(temp_loc)) {
-            if (!set_home(temp_loc, false)) {
+            if (!set_home(temp_loc)){
                 // fail silently
             }
         }

AntennaTracker/Tracker.h

@@ -46,9 +46,9 @@
 #include "config.h"
 #include "defines.h"
 
-#include "RC_Channel_Tracker.h"
+#include "RC_Channel.h"
 #include "Parameters.h"
-#include "GCS_MAVLink_Tracker.h"
+#include "GCS_Mavlink.h"
 #include "GCS_Tracker.h"
 
 #include "AP_Arming.h"
@@ -197,8 +197,7 @@ private:
     void init_ardupilot() override;
     bool get_home_eeprom(Location &loc) const;
     bool set_home_eeprom(const Location &temp) WARN_IF_UNUSED;
-    bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
-    bool set_home(const Location &temp, bool lock) override WARN_IF_UNUSED;
+    bool set_home(const Location &temp) WARN_IF_UNUSED;
     void prepare_servos();
     void set_mode(Mode &newmode, ModeReason reason);
     bool set_mode(uint8_t new_mode, ModeReason reason) override;

AntennaTracker/config.h

@@ -3,6 +3,14 @@
 
 #include "defines.h"
 
+// Just so that it's completely clear...
+#define ENABLED                 1
+#define DISABLED                0
+
+// this avoids a very common config error
+#define ENABLE ENABLED
+#define DISABLE DISABLED
+
 #ifndef MAV_SYSTEM_ID
  // use 2 for antenna tracker by default
  # define MAV_SYSTEM_ID          2

AntennaTracker/mode.h

@@ -13,7 +13,6 @@ public:
         GUIDED=4,
         AUTO=10,
         INITIALISING=16
-        // Mode number 30 reserved for "offboard" for external/lua control.
     };
 
     Mode() {}
@@ -23,7 +22,6 @@ public:
 
     // returns a unique number specific to this mode
     virtual Mode::Number number() const = 0;
-    virtual const char* name() const = 0;
 
     virtual bool requires_armed_servos() const = 0;
 
@@ -43,7 +41,6 @@ protected:
 class ModeAuto : public Mode {
 public:
     Mode::Number number() const override { return Mode::Number::AUTO; }
-    const char* name() const override { return "Auto"; }
     bool requires_armed_servos() const override { return true; }
     void update() override;
 };
@@ -51,7 +48,6 @@ public:
 class ModeGuided : public Mode {
 public:
     Mode::Number number() const override { return Mode::Number::GUIDED; }
-    const char* name() const override { return "Guided"; }
     bool requires_armed_servos() const override { return true; }
     void update() override;
 
@@ -70,7 +66,6 @@ private:
 class ModeInitialising : public Mode {
 public:
     Mode::Number number() const override { return Mode::Number::INITIALISING; }
-    const char* name() const override { return "Initialising"; }
     bool requires_armed_servos() const override { return false; }
     void update() override {};
 };
@@ -78,7 +73,6 @@ public:
 class ModeManual : public Mode {
 public:
     Mode::Number number() const override { return Mode::Number::MANUAL; }
-    const char* name() const override { return "Manual"; }
     bool requires_armed_servos() const override { return true; }
     void update() override;
 };
@@ -86,7 +80,6 @@ public:
 class ModeScan : public Mode {
 public:
     Mode::Number number() const override { return Mode::Number::SCAN; }
-    const char* name() const override { return "Scan"; }
     bool requires_armed_servos() const override { return true; }
     void update() override;
 };
@@ -94,7 +87,6 @@ public:
 class ModeServoTest : public Mode {
 public:
     Mode::Number number() const override { return Mode::Number::SERVOTEST; }
-    const char* name() const override { return "ServoTest"; }
     bool requires_armed_servos() const override { return true; }
     void update() override {};
 
@@ -104,7 +96,6 @@ public:
 class ModeStop : public Mode {
 public:
     Mode::Number number() const override { return Mode::Number::STOP; }
-    const char* name() const override { return "Stop"; }
     bool requires_armed_servos() const override { return false; }
     void update() override {};
 };

AntennaTracker/sensors.cpp

@@ -51,7 +51,7 @@ void Tracker::update_GPS(void)
                 // Now have an initial GPS position
                 // use it as the HOME position in future startups
                 current_loc = gps.location();
-                IGNORE_RETURN(set_home(current_loc, false));
+                IGNORE_RETURN(set_home(current_loc));
                 ground_start_count = 0;
             }
         }

AntennaTracker/servos.cpp

@@ -8,7 +8,7 @@
 void Tracker::init_servos()
 {
     // update assigned functions and enable auxiliary servos
-    AP::srv().enable_aux_servos();
+    SRV_Channels::enable_aux_servos();
 
     SRV_Channels::set_default_function(CH_YAW, SRV_Channel::k_tracker_yaw);
     SRV_Channels::set_default_function(CH_PITCH, SRV_Channel::k_tracker_pitch);

AntennaTracker/system.cpp

@@ -30,7 +30,7 @@ void Tracker::init_ardupilot()
 
     // GPS Initialization
     gps.set_log_gps_bit(MASK_LOG_GPS);
-    gps.init();
+    gps.init(serial_manager);
 
     ahrs.init();
     ahrs.set_fly_forward(false);
@@ -116,12 +116,7 @@ bool Tracker::set_home_eeprom(const Location &temp)
     return true;
 }
 
-bool Tracker::set_home_to_current_location(bool lock)
-{
-    return set_home(AP::gps().location(), lock);
-}
-
-bool Tracker::set_home(const Location &temp, bool lock)
+bool Tracker::set_home(const Location &temp)
 {
     // check EKF origin has been set
     Location ekf_origin;

AntennaTracker/tracking.cpp

@@ -62,10 +62,10 @@ void Tracker::update_bearing_and_distance()
 
     // calculate altitude difference to vehicle using gps
     if (g.alt_source == ALT_SOURCE_GPS){
-        nav_status.alt_difference_gps = (vehicle.location_estimate.alt - current_loc.alt) * 0.01f;
+        nav_status.alt_difference_gps = (vehicle.location_estimate.alt - current_loc.alt) / 100.0f;
     } else {
         // g.alt_source == ALT_SOURCE_GPS_VEH_ONLY
-        nav_status.alt_difference_gps = vehicle.relative_alt * 0.01f;
+        nav_status.alt_difference_gps = vehicle.relative_alt / 100.0f;
     }
 
     // calculate pitch to vehicle
@@ -131,20 +131,15 @@ void Tracker::update_tracking(void)
  */
 void Tracker::tracking_update_position(const mavlink_global_position_int_t &msg)
 {
-    // reject (0;0) coordinates
-    if (!msg.lat && !msg.lon) {
-        return;
-    }
-
     vehicle.location.lat = msg.lat;
     vehicle.location.lng = msg.lon;
     vehicle.location.alt = msg.alt/10;
     vehicle.relative_alt = msg.relative_alt/10;
-    vehicle.vel = Vector3f(msg.vx*0.01f, msg.vy*0.01f, msg.vz*0.01f);
+    vehicle.vel = Vector3f(msg.vx/100.0f, msg.vy/100.0f, msg.vz/100.0f);
     vehicle.last_update_us = AP_HAL::micros();
     vehicle.last_update_ms = AP_HAL::millis();
 #if HAL_LOGGING_ENABLED
-    // log vehicle as VPOS
+    // log vehicle as GPS2
     if (should_log(MASK_LOG_GPS)) {
         Log_Write_Vehicle_Pos(vehicle.location.lat, vehicle.location.lng, vehicle.location.alt, vehicle.vel);
     }

AntennaTracker/version.h

@@ -6,13 +6,13 @@
 
 #include "ap_version.h"
 
-#define THISFIRMWARE "AntennaTracker V4.7.0-dev"
+#define THISFIRMWARE "AntennaTracker V4.6.0-dev"
 
 // the following line is parsed by the autotest scripts
-#define FIRMWARE_VERSION 4,7,0,FIRMWARE_VERSION_TYPE_DEV
+#define FIRMWARE_VERSION 4,6,0,FIRMWARE_VERSION_TYPE_DEV
 
 #define FW_MAJOR 4
-#define FW_MINOR 7
+#define FW_MINOR 6
 #define FW_PATCH 0
 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV
 

AntennaTracker/wscript

@@ -6,7 +6,12 @@ def build(bld):
     bld.ap_stlib(
         name=vehicle + '_libs',
         ap_vehicle=vehicle,
-        ap_libraries=bld.ap_common_vehicle_libraries(),
+        ap_libraries=bld.ap_common_vehicle_libraries() + [
+            'AP_Beacon',
+            'AP_Arming',
+            'AP_RCMapper',
+            'AP_OSD',
+        ],
     )
 
     bld.ap_program(

ArduCopter/APM_Config.h

@@ -1,33 +1,37 @@
 // User specific config file.  Any items listed in config.h can be overridden here.
 
 // uncomment the lines below to disable features (flash sizes listed are for APM2 boards and will underestimate savings on Pixhawk and other boards)
-//#define LOGGING_ENABLED       0            // disable logging to save 11K of flash space
-//#define MOUNT                 0            // disable the camera gimbal to save 8K of flash space
-//#define AUTOTUNE_ENABLED      0            // disable the auto tune functionality to save 7k of flash
-//#define NAV_GUIDED            0            // disable external navigation computer ability to control vehicle through MAV_CMD_NAV_GUIDED mission commands
-//#define MODE_ACRO_ENABLED     0            // disable acrobatic mode support
-//#define MODE_AUTO_ENABLED     0            // disable auto mode support
-//#define MODE_BRAKE_ENABLED    0            // disable brake mode support
-//#define MODE_CIRCLE_ENABLED   0            // disable circle mode support
-//#define MODE_DRIFT_ENABLED    0            // disable drift mode support
-//#define MODE_FLIP_ENABLED     0            // disable flip mode support
-//#define MODE_FOLLOW_ENABLED   0            // disable follow mode support
-//#define MODE_GUIDED_ENABLED   0            // disable guided mode support
-//#define MODE_GUIDED_NOGPS_ENABLED   0      // disable guided/nogps mode support
-//#define MODE_LOITER_ENABLED   0            // disable loiter mode support
-//#define MODE_POSHOLD_ENABLED  0            // disable poshold mode support
-//#define MODE_RTL_ENABLED      0            // disable rtl mode support
-//#define MODE_SMARTRTL_ENABLED 0            // disable smartrtl mode support
-//#define MODE_SPORT_ENABLED    0            // disable sport mode support
-//#define MODE_SYSTEMID_ENABLED 0            // disable system ID mode support
-//#define MODE_THROW_ENABLED    0            // disable throw mode support
-//#define MODE_ZIGZAG_ENABLED   0            // disable zigzag mode support
-//#define OSD_ENABLED           0            // disable on-screen-display support
+//#define LOGGING_ENABLED       DISABLED            // disable logging to save 11K of flash space
+//#define MOUNT                 DISABLED            // disable the camera gimbal to save 8K of flash space
+//#define AUTOTUNE_ENABLED      DISABLED            // disable the auto tune functionality to save 7k of flash
+//#define RANGEFINDER_ENABLED   DISABLED            // disable rangefinder to save 1k of flash
+//#define AC_AVOID_ENABLED      DISABLED            // disable stop-at-fence library
+//#define AC_OAPATHPLANNER_ENABLED DISABLED         // disable path planning around obstacles
+//#define PARACHUTE             DISABLED            // disable parachute release to save 1k of flash
+//#define NAV_GUIDED            DISABLED            // disable external navigation computer ability to control vehicle through MAV_CMD_NAV_GUIDED mission commands
+//#define MODE_ACRO_ENABLED     DISABLED            // disable acrobatic mode support
+//#define MODE_AUTO_ENABLED     DISABLED            // disable auto mode support
+//#define MODE_BRAKE_ENABLED    DISABLED            // disable brake mode support
+//#define MODE_CIRCLE_ENABLED   DISABLED            // disable circle mode support
+//#define MODE_DRIFT_ENABLED    DISABLED            // disable drift mode support
+//#define MODE_FLIP_ENABLED     DISABLED            // disable flip mode support
+//#define MODE_FOLLOW_ENABLED   DISABLED            // disable follow mode support
+//#define MODE_GUIDED_ENABLED   DISABLED            // disable guided mode support
+//#define MODE_GUIDED_NOGPS_ENABLED   DISABLED      // disable guided/nogps mode support
+//#define MODE_LOITER_ENABLED   DISABLED            // disable loiter mode support
+//#define MODE_POSHOLD_ENABLED  DISABLED            // disable poshold mode support
+//#define MODE_RTL_ENABLED      DISABLED            // disable rtl mode support
+//#define MODE_SMARTRTL_ENABLED DISABLED            // disable smartrtl mode support
+//#define MODE_SPORT_ENABLED    DISABLED            // disable sport mode support
+//#define MODE_SYSTEMID_ENABLED DISABLED            // disable system ID mode support
+//#define MODE_THROW_ENABLED    DISABLED            // disable throw mode support
+//#define MODE_ZIGZAG_ENABLED   DISABLED            // disable zigzag mode support
+//#define OSD_ENABLED           DISABLED            // disable on-screen-display support
 
 // features below are disabled by default on all boards
 //#define CAL_ALWAYS_REBOOT                         // flight controller will reboot after compass or accelerometer calibration completes
 //#define DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE   // disable mode changes from GCS during Radio failsafes.  Avoids a race condition for vehicle like Solo in which the RC and telemetry travel along the same link
-//#define AP_COPTER_ADVANCED_FAILSAFE_ENABLED     1             // enabled advanced failsafe which allows running a portion of the mission in failsafe events
+//#define ADVANCED_FAILSAFE     ENABLED             // enabled advanced failsafe which allows running a portion of the mission in failsafe events
 
 // other settings
 //#define THROTTLE_IN_DEADBAND   100                // redefine size of throttle deadband in pwm (0 ~ 1000)
@@ -42,5 +46,5 @@
 //#define USERHOOK_MEDIUMLOOP userhook_MediumLoop();        // for code to be run at 10hz
 //#define USERHOOK_SLOWLOOP userhook_SlowLoop();            // for code to be run at 3.3hz
 //#define USERHOOK_SUPERSLOWLOOP userhook_SuperSlowLoop();  // for code to be run at 1hz
-//#define USERHOOK_AUXSWITCH 1                        // for code to handle user aux switches
-//#define USER_PARAMS_ENABLED 1                       // to enable user parameters
+//#define USERHOOK_AUXSWITCH ENABLED                        // for code to handle user aux switches
+//#define USER_PARAMS_ENABLED ENABLED                       // to enable user parameters

ArduCopter/AP_Arming.cpp

@@ -1,7 +1,7 @@
 #include "Copter.h"
 
 #pragma GCC diagnostic push
-#if defined(__clang_major__) && __clang_major__ >= 14
+#if defined(__clang__)
 #pragma GCC diagnostic ignored "-Wbitwise-instead-of-logical"
 #endif
 
@@ -44,7 +44,7 @@ bool AP_Arming_Copter::run_pre_arm_checks(bool display_failure)
     }
 
     // always check motors
-    char failure_msg[100] {};
+    char failure_msg[50] {};
     if (!copter.motors->arming_checks(ARRAY_SIZE(failure_msg), failure_msg)) {
         check_failed(display_failure, "Motors: %s", failure_msg);
         passed = false;
@@ -179,7 +179,7 @@ bool AP_Arming_Copter::terrain_database_required() const
     }
 
     if (copter.wp_nav->get_terrain_source() == AC_WPNav::TerrainSource::TERRAIN_FROM_TERRAINDATABASE &&
-        copter.mode_rtl.get_alt_type() == ModeRTL::RTLAltType::TERRAIN) {
+        copter.mode_rtl.get_alt_type() == ModeRTL::RTLAltType::RTL_ALTTYPE_TERRAIN) {
         return true;
     }
     return AP_Arming::terrain_database_required();
@@ -210,7 +210,7 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
         }
 
         // acro balance parameter check
-#if MODE_ACRO_ENABLED || MODE_SPORT_ENABLED
+#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
         if ((copter.g.acro_balance_roll > copter.attitude_control->get_angle_roll_p().kP()) || (copter.g.acro_balance_pitch > copter.attitude_control->get_angle_pitch_p().kP())) {
             check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check ACRO_BAL_ROLL/PITCH");
             return false;
@@ -224,13 +224,19 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
         }
 
         #if FRAME_CONFIG == HELI_FRAME
-        char fail_msg[100]{};
+        char fail_msg[50];
         // check input manager parameters
         if (!copter.input_manager.parameter_check(fail_msg, ARRAY_SIZE(fail_msg))) {
             check_failed(ARMING_CHECK_PARAMETERS, display_failure, "%s", fail_msg);
             return false;
         }
 
+        // Inverted flight feature disabled for Heli Single and Dual frames
+        if (copter.g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI_QUAD &&
+            rc().find_channel_for_option(RC_Channel::AUX_FUNC::INVERTED) != nullptr) {
+            check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Inverted flight option not supported");
+            return false;
+        }
         // Ensure an Aux Channel is configured for motor interlock
         if (rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_INTERLOCK) == nullptr) {
             check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Motor Interlock not configured");
@@ -238,21 +244,17 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
         }
 
         #else
-        switch (copter.g2.frame_class.get()) {
-        case AP_Motors::MOTOR_FRAME_HELI_QUAD:
-        case AP_Motors::MOTOR_FRAME_HELI_DUAL:
-        case AP_Motors::MOTOR_FRAME_HELI:
+        if (copter.g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI_QUAD ||
+            copter.g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI_DUAL ||
+            copter.g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI) {
             check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Invalid MultiCopter FRAME_CLASS");
             return false;
-
-        default:
-            break;
         }
         #endif // HELI_FRAME
 
         // checks when using range finder for RTL
-#if MODE_RTL_ENABLED
-        if (copter.mode_rtl.get_alt_type() == ModeRTL::RTLAltType::TERRAIN) {
+#if MODE_RTL_ENABLED == ENABLED
+        if (copter.mode_rtl.get_alt_type() == ModeRTL::RTLAltType::RTL_ALTTYPE_TERRAIN) {
             // get terrain source from wpnav
             const char *failure_template = "RTL_ALT_TYPE is above-terrain but %s";
             switch (copter.wp_nav->get_terrain_source()) {
@@ -261,7 +263,6 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
                 return false;
                 break;
             case AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER:
-#if AP_RANGEFINDER_ENABLED
                 if (!copter.rangefinder_state.enabled || !copter.rangefinder.has_orientation(ROTATION_PITCH_270)) {
                     check_failed(ARMING_CHECK_PARAMETERS, display_failure, failure_template, "no rangefinder");
                     return false;
@@ -271,9 +272,6 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
                     check_failed(ARMING_CHECK_PARAMETERS, display_failure, failure_template, "RTL_ALT>RNGFND_MAX_CM");
                     return false;
                 }
-#else
-                check_failed(ARMING_CHECK_PARAMETERS, display_failure, failure_template, "rangefinder not in firmware");
-#endif
                 break;
             case AC_WPNav::TerrainSource::TERRAIN_FROM_TERRAINDATABASE:
                 // these checks are done in AP_Arming
@@ -291,7 +289,7 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
 #endif
 
         // ensure controllers are OK with us arming:
-        char failure_msg[100] = {};
+        char failure_msg[50] = {};
         if (!copter.pos_control->pre_arm_checks("PSC", failure_msg, ARRAY_SIZE(failure_msg))) {
             check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Bad parameter: %s", failure_msg);
             return false;
@@ -307,8 +305,8 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
 
 bool AP_Arming_Copter::oa_checks(bool display_failure)
 {
-#if AP_OAPATHPLANNER_ENABLED
-    char failure_msg[100] = {};
+#if AC_OAPATHPLANNER_ENABLED == ENABLED
+    char failure_msg[50] = {};
     if (copter.g2.oa.pre_arm_check(failure_msg, ARRAY_SIZE(failure_msg))) {
         return true;
     }
@@ -345,10 +343,10 @@ bool AP_Arming_Copter::gps_checks(bool display_failure)
 {
     // check if fence requires GPS
     bool fence_requires_gps = false;
-#if AP_FENCE_ENABLED
+    #if AP_FENCE_ENABLED
     // if circular or polygon fence is enabled we need GPS
     fence_requires_gps = (copter.fence.get_enabled_fences() & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON)) > 0;
-#endif
+    #endif
 
     // check if flight mode requires GPS
     bool mode_requires_gps = copter.flightmode->requires_GPS() || fence_requires_gps || (copter.simple_mode == Copter::SimpleMode::SUPERSIMPLE);
@@ -415,7 +413,7 @@ bool AP_Arming_Copter::proximity_checks(bool display_failure) const
     }
 
     // get closest object if we might use it for avoidance
-#if AP_AVOIDANCE_ENABLED
+#if AC_AVOID_ENABLED == ENABLED
     float angle_deg, distance;
     if (copter.avoid.proximity_avoidance_enabled() && copter.g2.proximity.get_closest_object(angle_deg, distance)) {
         // display error if something is within 60cm
@@ -439,7 +437,7 @@ bool AP_Arming_Copter::mandatory_gps_checks(bool display_failure)
 
     // always check if inertial nav has started and is ready
     const auto &ahrs = AP::ahrs();
-    char failure_msg[100] = {};
+    char failure_msg[50] = {};
     if (!ahrs.pre_arm_check(mode_requires_gps, failure_msg, sizeof(failure_msg))) {
         check_failed(display_failure, "AHRS: %s", failure_msg);
         return false;
@@ -447,26 +445,28 @@ bool AP_Arming_Copter::mandatory_gps_checks(bool display_failure)
 
     // check if fence requires GPS
     bool fence_requires_gps = false;
-#if AP_FENCE_ENABLED
+    #if AP_FENCE_ENABLED
     // if circular or polygon fence is enabled we need GPS
     fence_requires_gps = (copter.fence.get_enabled_fences() & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON)) > 0;
-#endif
+    #endif
 
-    if (mode_requires_gps || copter.option_is_enabled(Copter::FlightOption::REQUIRE_POSITION_FOR_ARMING)) {
+    if (mode_requires_gps) {
         if (!copter.position_ok()) {
             // vehicle level position estimate checks
             check_failed(display_failure, "Need Position Estimate");
             return false;
         }
-    } else if (fence_requires_gps) {
-        if (!copter.position_ok()) {
-            // clarify to user why they need GPS in non-GPS flight mode
-            check_failed(display_failure, "Fence enabled, need position estimate");
-            return false;
-        }
     } else {
-        // return true if GPS is not required
-        return true;
+        if (fence_requires_gps) {
+            if (!copter.position_ok()) {
+                // clarify to user why they need GPS in non-GPS flight mode
+                check_failed(display_failure, "Fence enabled, need position estimate");
+                return false;
+            }
+        } else {
+            // return true if GPS is not required
+            return true;
+        }
     }
 
     // check for GPS glitch (as reported by EKF)
@@ -501,6 +501,18 @@ bool AP_Arming_Copter::mandatory_gps_checks(bool display_failure)
         }
     }
 
+    // check if home is too far from EKF origin
+    if (copter.far_from_EKF_origin(ahrs.get_home())) {
+        check_failed(display_failure, "Home too far from EKF origin");
+        return false;
+    }
+
+    // check if vehicle is too far from EKF origin
+    if (copter.far_from_EKF_origin(copter.current_loc)) {
+        check_failed(display_failure, "Vehicle too far from EKF origin");
+        return false;
+    }
+
     // if we got here all must be ok
     return true;
 }
@@ -528,7 +540,7 @@ bool AP_Arming_Copter::winch_checks(bool display_failure) const
     if (winch == nullptr) {
         return true;
     }
-    char failure_msg[100] = {};
+    char failure_msg[50] = {};
     if (!winch->pre_arm_check(failure_msg, sizeof(failure_msg))) {
         check_failed(display_failure, "%s", failure_msg);
         return false;
@@ -605,25 +617,25 @@ bool AP_Arming_Copter::arm_checks(AP_Arming::Method method)
 
     // check throttle
     if (check_enabled(ARMING_CHECK_RC)) {
-#if FRAME_CONFIG == HELI_FRAME
+         #if FRAME_CONFIG == HELI_FRAME
         const char *rc_item = "Collective";
-#else
+        #else
         const char *rc_item = "Throttle";
-#endif
+        #endif
         // check throttle is not too high - skips checks if arming from GCS/scripting in Guided,Guided_NoGPS or Auto 
-        if (!((AP_Arming::method_is_GCS(method) || method == AP_Arming::Method::SCRIPTING) && copter.flightmode->allows_GCS_or_SCR_arming_with_throttle_high())) {
+        if (!((AP_Arming::method_is_GCS(method) || method == AP_Arming::Method::SCRIPTING) && (copter.flightmode->mode_number() == Mode::Number::GUIDED || copter.flightmode->mode_number() == Mode::Number::GUIDED_NOGPS || copter.flightmode->mode_number() == Mode::Number::AUTO))) {
             // above top of deadband is too always high
             if (copter.get_pilot_desired_climb_rate(copter.channel_throttle->get_control_in()) > 0.0f) {
                 check_failed(ARMING_CHECK_RC, true, "%s too high", rc_item);
                 return false;
             }
             // in manual modes throttle must be at zero
-#if FRAME_CONFIG != HELI_FRAME
+            #if FRAME_CONFIG != HELI_FRAME
             if ((copter.flightmode->has_manual_throttle() || copter.flightmode->mode_number() == Mode::Number::DRIFT) && copter.channel_throttle->get_control_in() > 0) {
                 check_failed(ARMING_CHECK_RC, true, "%s too high", rc_item);
                 return false;
             }
-#endif
+            #endif
         }
     }
 
@@ -728,7 +740,7 @@ bool AP_Arming_Copter::arm(const AP_Arming::Method method, const bool do_arming_
     copter.update_super_simple_bearing(false);
 
     // Reset SmartRTL return location. If activated, SmartRTL will ultimately try to land at this point
-#if MODE_SMARTRTL_ENABLED
+#if MODE_SMARTRTL_ENABLED == ENABLED
     copter.g2.smart_rtl.set_home(copter.position_ok());
 #endif
 
@@ -808,6 +820,15 @@ bool AP_Arming_Copter::disarm(const AP_Arming::Method method, bool do_disarm_che
         }
     }
 
+#if AUTOTUNE_ENABLED == ENABLED
+    // save auto tuned parameters
+    if (copter.flightmode == &copter.mode_autotune) {
+        copter.mode_autotune.save_tuning_gains();
+    } else {
+        copter.mode_autotune.reset();
+    }
+#endif
+
     // we are not in the air
     copter.set_land_complete(true);
     copter.set_land_complete_maybe(true);
@@ -815,7 +836,7 @@ bool AP_Arming_Copter::disarm(const AP_Arming::Method method, bool do_disarm_che
     // send disarm command to motors
     copter.motors->armed(false);
 
-#if MODE_AUTO_ENABLED
+#if MODE_AUTO_ENABLED == ENABLED
     // reset the mission
     copter.mode_auto.mission.reset();
 #endif
@@ -828,11 +849,6 @@ bool AP_Arming_Copter::disarm(const AP_Arming::Method method, bool do_disarm_che
 
     copter.ap.in_arming_delay = false;
 
-#if AUTOTUNE_ENABLED
-    // Possibly save auto tuned parameters
-    copter.mode_autotune.autotune.disarmed(copter.flightmode == &copter.mode_autotune);
-#endif
-
     return true;
 }
 

ArduCopter/AP_ExternalControl_Copter.cpp

@@ -29,15 +29,6 @@ bool AP_ExternalControl_Copter::set_linear_velocity_and_yaw_rate(const Vector3f
     return true;
 }
 
-bool AP_ExternalControl_Copter::set_global_position(const Location& loc)
-{
-    // Check if copter is ready for external control and returns false if it is not.
-    if (!ready_for_external_control()) {
-        return false;
-    }
-    return copter.set_target_location(loc);
-}
-
 bool AP_ExternalControl_Copter::ready_for_external_control()
 {
     return copter.flightmode->in_guided_mode() && copter.motors->armed();

ArduCopter/AP_ExternalControl_Copter.h

@@ -7,8 +7,7 @@
 
 #if AP_EXTERNAL_CONTROL_ENABLED
 
-class AP_ExternalControl_Copter : public AP_ExternalControl
-{
+class AP_ExternalControl_Copter : public AP_ExternalControl {
 public:
     /*
       Set linear velocity and yaw rate. Pass NaN for yaw_rate_rads to not control yaw.
@@ -16,11 +15,6 @@ public:
       Yaw is in earth frame, NED [rad/s].
      */
     bool set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity, float yaw_rate_rads) override WARN_IF_UNUSED;
-
-    /*
-      Sets the target global position for a loiter point.
-    */
-    bool set_global_position(const Location& loc) override WARN_IF_UNUSED;
 private:
     /*
       Return true if Copter is ready to handle external control data.

ArduCopter/Attitude.cpp

@@ -4,23 +4,18 @@
  *  Attitude Rate controllers and timing
  ****************************************************************/
 
-/*
-  update rate controller when run from main thread (normal operation)
-*/
-void Copter::run_rate_controller_main()
+// update rate controllers and output to roll, pitch and yaw actuators
+//  called at 400hz by default
+void Copter::run_rate_controller()
 {
     // set attitude and position controller loop time
     const float last_loop_time_s = AP::scheduler().get_last_loop_time_s();
-    pos_control->set_dt(last_loop_time_s);
+    motors->set_dt(last_loop_time_s);
     attitude_control->set_dt(last_loop_time_s);
+    pos_control->set_dt(last_loop_time_s);
 
-    if (!using_rate_thread) {
-        motors->set_dt(last_loop_time_s);
-        // only run the rate controller if we are not using the rate thread
-        attitude_control->rate_controller_run();
-    }
-    // reset sysid and other temporary inputs
-    attitude_control->rate_controller_target_reset();
+    // run low level rate controllers that only require IMU data
+    attitude_control->rate_controller_run(); 
 }
 
 /*************************************************************
@@ -69,7 +64,7 @@ float Copter::get_pilot_desired_climb_rate(float throttle_control)
         return 0.0f;
     }
 
-#if TOY_MODE_ENABLED
+#if TOY_MODE_ENABLED == ENABLED
     if (g2.toy_mode.enabled()) {
         // allow throttle to be reduced after throttle arming and for
         // slower descent close to the ground

ArduCopter/Copter.cpp

@@ -75,7 +75,6 @@
  */
 
 #include "Copter.h"
-#include <AP_InertialSensor/AP_InertialSensor_rate_config.h>
 
 #define FORCE_VERSION_H_INCLUDE
 #include "version.h"
@@ -114,15 +113,15 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
     // update INS immediately to get current gyro data populated
     FAST_TASK_CLASS(AP_InertialSensor, &copter.ins, update),
     // run low level rate controllers that only require IMU data
-    FAST_TASK(run_rate_controller_main),
-#if AC_CUSTOMCONTROL_MULTI_ENABLED
+    FAST_TASK(run_rate_controller),
+#if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED
     FAST_TASK(run_custom_controller),
 #endif
 #if FRAME_CONFIG == HELI_FRAME
     FAST_TASK(heli_update_autorotation),
 #endif //HELI_FRAME
     // send outputs to the motors library immediately
-    FAST_TASK(motors_output_main),
+    FAST_TASK(motors_output),
      // run EKF state estimator (expensive)
     FAST_TASK(read_AHRS),
 #if FRAME_CONFIG == HELI_FRAME
@@ -150,9 +149,6 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
 
     SCHED_TASK(rc_loop,              250,    130,  3),
     SCHED_TASK(throttle_loop,         50,     75,  6),
-#if AP_FENCE_ENABLED
-    SCHED_TASK(fence_check,           25,    100,  7),
-#endif
     SCHED_TASK_CLASS(AP_GPS,               &copter.gps,                 update,          50, 200,   9),
 #if AP_OPTICALFLOW_ENABLED
     SCHED_TASK_CLASS(AP_OpticalFlow,          &copter.optflow,             update,         200, 160,  12),
@@ -160,12 +156,12 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
     SCHED_TASK(update_batt_compass,   10,    120, 15),
     SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&copter.g2.rc_channels, read_aux_all,    10,  50,  18),
     SCHED_TASK(arm_motors_check,      10,     50, 21),
-#if TOY_MODE_ENABLED
+#if TOY_MODE_ENABLED == ENABLED
     SCHED_TASK_CLASS(ToyMode,              &copter.g2.toy_mode,         update,          10,  50,  24),
 #endif
     SCHED_TASK(auto_disarm_check,     10,     50,  27),
     SCHED_TASK(auto_trim,             10,     75,  30),
-#if AP_RANGEFINDER_ENABLED
+#if RANGEFINDER_ENABLED == ENABLED
     SCHED_TASK(read_rangefinder,      20,    100,  33),
 #endif
 #if HAL_PROXIMITY_ENABLED
@@ -177,7 +173,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
     SCHED_TASK(update_altitude,       10,    100,  42),
     SCHED_TASK(run_nav_updates,       50,    100,  45),
     SCHED_TASK(update_throttle_hover,100,     90,  48),
-#if MODE_SMARTRTL_ENABLED
+#if MODE_SMARTRTL_ENABLED == ENABLED
     SCHED_TASK_CLASS(ModeSmartRTL,         &copter.mode_smartrtl,       save_position,    3, 100,  51),
 #endif
 #if HAL_SPRAYER_ENABLED
@@ -187,6 +183,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
 #if AP_SERVORELAYEVENTS_ENABLED
     SCHED_TASK_CLASS(AP_ServoRelayEvents,  &copter.ServoRelayEvents,      update_events, 50,  75,  60),
 #endif
+    SCHED_TASK_CLASS(AP_Baro,              &copter.barometer,             accumulate,    50,  90,  63),
 #if AC_PRECLAND_ENABLED
     SCHED_TASK(update_precland,      400,     50,  69),
 #endif
@@ -233,7 +230,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
 #if HAL_ADSB_ENABLED
     SCHED_TASK(avoidance_adsb_update, 10,    100, 138),
 #endif
-#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
+#if ADVANCED_FAILSAFE == ENABLED
     SCHED_TASK(afs_fs_check,          10,    100, 141),
 #endif
 #if AP_TERRAIN_AVAILABLE
@@ -260,10 +257,6 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
 #if HAL_BUTTON_ENABLED
     SCHED_TASK_CLASS(AP_Button,            &copter.button,              update,           5, 100, 168),
 #endif
-#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
-    // don't delete this, there is an equivalent (virtual) in AP_Vehicle for the non-rate loop case
-    SCHED_TASK(update_dynamic_notch_at_specified_rate_main,                       LOOP_RATE, 200, 215),
-#endif
 };
 
 void Copter::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
@@ -277,22 +270,10 @@ void Copter::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
 
 constexpr int8_t Copter::_failsafe_priorities[7];
 
-
-#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
-#if MODE_GUIDED_ENABLED
-// set target location (for use by external control and scripting)
-bool Copter::set_target_location(const Location& target_loc)
-{
-    // exit if vehicle is not in Guided mode or Auto-Guided mode
-    if (!flightmode->in_guided_mode()) {
-        return false;
-    }
-
-    return mode_guided.set_destination(target_loc);
-}
-
+#if AP_SCRIPTING_ENABLED
+#if MODE_GUIDED_ENABLED == ENABLED
 // start takeoff to given altitude (for use by scripting)
-bool Copter::start_takeoff(const float alt)
+bool Copter::start_takeoff(float alt)
 {
     // exit if vehicle is not in Guided mode or Auto-Guided mode
     if (!flightmode->in_guided_mode()) {
@@ -305,11 +286,18 @@ bool Copter::start_takeoff(const float alt)
     }
     return false;
 }
-#endif //MODE_GUIDED_ENABLED
-#endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
 
-#if AP_SCRIPTING_ENABLED
-#if MODE_GUIDED_ENABLED
+// set target location (for use by scripting)
+bool Copter::set_target_location(const Location& target_loc)
+{
+    // exit if vehicle is not in Guided mode or Auto-Guided mode
+    if (!flightmode->in_guided_mode()) {
+        return false;
+    }
+
+    return mode_guided.set_destination(target_loc);
+}
+
 // set target position (for use by scripting)
 bool Copter::set_target_pos_NED(const Vector3f& target_pos, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative, bool terrain_alt)
 {
@@ -381,7 +369,6 @@ bool Copter::set_target_velaccel_NED(const Vector3f& target_vel, const Vector3f&
     return true;
 }
 
-// set target roll pitch and yaw angles with throttle (for use by scripting)
 bool Copter::set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs)
 {
     // exit if vehicle is not in Guided mode or Auto-Guided mode
@@ -395,78 +382,9 @@ bool Copter::set_target_angle_and_climbrate(float roll_deg, float pitch_deg, flo
     mode_guided.set_angle(q, Vector3f{}, climb_rate_ms*100, false);
     return true;
 }
+#endif
 
-// set target roll pitch and yaw rates with throttle (for use by scripting)
-bool Copter::set_target_rate_and_throttle(float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps, float throttle)
-{
-    // exit if vehicle is not in Guided mode or Auto-Guided mode
-    if (!flightmode->in_guided_mode()) {
-        return false;
-    }
-
-    // Zero quaternion indicates rate control
-    Quaternion q;
-    q.zero();
-
-    // Convert from degrees per second to radians per second
-    Vector3f ang_vel_body { roll_rate_dps, pitch_rate_dps, yaw_rate_dps };
-    ang_vel_body *= DEG_TO_RAD;
-
-    // Pass to guided mode
-    mode_guided.set_angle(q, ang_vel_body, throttle, true);
-    return true;
-}
-
-// Register a custom mode with given number and names
-AP_Vehicle::custom_mode_state* Copter::register_custom_mode(const uint8_t num, const char* full_name, const char* short_name)
-{
-    const Mode::Number number = (Mode::Number)num;
-
-    // See if this mode has been registered already, if it has return the state for it
-    // This allows scripting restarts
-    for (uint8_t i = 0; i < ARRAY_SIZE(mode_guided_custom); i++) {
-        if (mode_guided_custom[i] == nullptr) {
-            break;
-        }
-        if ((mode_guided_custom[i]->mode_number() == number) &&
-            (strcmp(mode_guided_custom[i]->name(), full_name) == 0) &&
-            (strncmp(mode_guided_custom[i]->name4(), short_name, 4) == 0)) {
-            return &mode_guided_custom[i]->state;
-        }
-    }
-
-    // Number already registered to existing mode
-    if (mode_from_mode_num(number) != nullptr) {
-        return nullptr;
-    }
-
-    // Find free slot
-    for (uint8_t i = 0; i < ARRAY_SIZE(mode_guided_custom); i++) {
-        if (mode_guided_custom[i] == nullptr) {
-            // Duplicate strings so were not pointing to unknown memory
-            const char* full_name_copy = strdup(full_name);
-            const char* short_name_copy = strndup(short_name, 4);
-            if ((full_name_copy != nullptr) && (short_name_copy != nullptr)) {
-                mode_guided_custom[i] = NEW_NOTHROW ModeGuidedCustom(number, full_name_copy, short_name_copy);
-            }
-            if (mode_guided_custom[i] == nullptr) {
-                // Allocation failure
-                return nullptr;
-            }
-
-            // Registration sucsessful, notify the GCS that it should re-request the avalable modes
-            gcs().available_modes_changed();
-
-            return &mode_guided_custom[i]->state;
-        }
-    }
-
-    // No free slots
-    return nullptr;
-}
-#endif // MODE_GUIDED_ENABLED
-
-#if MODE_CIRCLE_ENABLED
+#if MODE_CIRCLE_ENABLED == ENABLED
 // circle mode controls
 bool Copter::get_circle_radius(float &radius_m)
 {
@@ -487,7 +405,7 @@ bool Copter::set_desired_speed(float speed)
     return flightmode->set_speed_xy(speed * 100.0f);
 }
 
-#if MODE_AUTO_ENABLED
+#if MODE_AUTO_ENABLED == ENABLED
 // returns true if mode supports NAV_SCRIPT_TIME mission commands
 bool Copter::nav_scripting_enable(uint8_t mode)
 {
@@ -521,41 +439,15 @@ bool Copter::has_ekf_failsafed() const
     return failsafe.ekf;
 }
 
-// get target location (for use by scripting)
-bool Copter::get_target_location(Location& target_loc)
-{
-    return flightmode->get_wp(target_loc);
-}
-
-/*
-  update_target_location() acts as a wrapper for set_target_location
- */
-bool Copter::update_target_location(const Location &old_loc, const Location &new_loc)
-{
-    /*
-      by checking the caller has provided the correct old target
-      location we prevent a race condition where the user changes mode
-      or commands a different target in the controlling lua script
-    */
-    Location next_WP_loc;
-    flightmode->get_wp(next_WP_loc);
-    if (!old_loc.same_loc_as(next_WP_loc) ||
-         old_loc.get_alt_frame() != new_loc.get_alt_frame()) {
-        return false;
-    }
-
-    return set_target_location(new_loc);
-}
-
 #endif // AP_SCRIPTING_ENABLED
 
-// returns true if vehicle is landing.
+// returns true if vehicle is landing. Only used by Lua scripts
 bool Copter::is_landing() const
 {
     return flightmode->is_landing();
 }
 
-// returns true if vehicle is taking off.
+// returns true if vehicle is taking off. Only used by Lua scripts
 bool Copter::is_taking_off() const
 {
     return flightmode->is_taking_off();
@@ -563,7 +455,7 @@ bool Copter::is_taking_off() const
 
 bool Copter::current_mode_requires_mission() const
 {
-#if MODE_AUTO_ENABLED
+#if MODE_AUTO_ENABLED == ENABLED
         return flightmode == &mode_auto;
 #else
         return false;
@@ -623,18 +515,13 @@ void Copter::update_batt_compass(void)
 // should be run at loop rate
 void Copter::loop_rate_logging()
 {
-   if (should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
+    if (should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
         Log_Write_Attitude();
-        if (!using_rate_thread) {
-            Log_Write_Rate();
-            Log_Write_PIDS(); // only logs if PIDS bitmask is set
-        }
+        Log_Write_PIDS(); // only logs if PIDS bitmask is set
     }
-#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
-    if (should_log(MASK_LOG_FTN_FAST) && !using_rate_thread) {
+    if (should_log(MASK_LOG_FTN_FAST)) {
         AP::ins().write_notch_log_messages();
     }
-#endif
     if (should_log(MASK_LOG_IMU_FAST)) {
         AP::ins().Write_IMU();
     }
@@ -644,36 +531,26 @@ void Copter::loop_rate_logging()
 // should be run at 10hz
 void Copter::ten_hz_logging_loop()
 {
-    // always write AHRS attitude at 10Hz
-    ahrs.Write_Attitude(attitude_control->get_att_target_euler_rad() * RAD_TO_DEG);
-    // log attitude controller data if we're not already logging at the higher rate
+    // log attitude data if we're not already logging at the higher rate
     if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
         Log_Write_Attitude();
-        if (!using_rate_thread) {
-            Log_Write_Rate();
-        }
     }
     if (!should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
     // log at 10Hz if PIDS bitmask is selected, even if no ATT bitmask is selected; logs at looprate if ATT_FAST and PIDS bitmask set
-        if (!using_rate_thread) {
-            Log_Write_PIDS();
-        }
+        Log_Write_PIDS();
     }
     // log EKF attitude data always at 10Hz unless ATTITUDE_FAST, then do it in the 25Hz loop
     if (!should_log(MASK_LOG_ATTITUDE_FAST)) {
         Log_Write_EKF_POS();
     }
-    if ((FRAME_CONFIG == HELI_FRAME) || should_log(MASK_LOG_MOTBATT)) {
-        // always write motors log if we are a heli
+    if (should_log(MASK_LOG_MOTBATT)) {
         motors->Log_Write();
     }
     if (should_log(MASK_LOG_RCIN)) {
         logger.Write_RCIN();
-#if AP_RSSI_ENABLED
         if (rssi.enabled()) {
             logger.Write_RSSI();
         }
-#endif
     }
     if (should_log(MASK_LOG_RCOUT)) {
         logger.Write_RCOUT();
@@ -693,6 +570,9 @@ void Copter::ten_hz_logging_loop()
         g2.beacon.log();
 #endif
     }
+#if FRAME_CONFIG == HELI_FRAME
+    Log_Write_Heli();
+#endif
 #if AP_WINCH_ENABLED
     if (should_log(MASK_LOG_ANY)) {
         g2.winch.write_log();
@@ -716,7 +596,7 @@ void Copter::twentyfive_hz_logging()
         AP::ins().Write_IMU();
     }
 
-#if MODE_AUTOROTATE_ENABLED
+#if MODE_AUTOROTATE_ENABLED == ENABLED
     if (should_log(MASK_LOG_ATTITUDE_MED) || should_log(MASK_LOG_ATTITUDE_FAST)) {
         //update autorotation log
         g2.arot.Log_Write_Autorotation();
@@ -742,36 +622,25 @@ void Copter::three_hz_loop()
     // check for deadreckoning failsafe
     failsafe_deadreckon_check();
 
-    //update transmitter based in flight tuning
+#if AP_FENCE_ENABLED
+    // check if we have breached a fence
+    fence_check();
+#endif // AP_FENCE_ENABLED
+
+
+    // update ch6 in flight tuning
     tuning();
 
     // check if avoidance should be enabled based on alt
     low_alt_avoidance();
 }
 
-// ap_value calculates a 32-bit bitmask representing various pieces of
-// state about the Copter.  It replaces a global variable which was
-// used to track this state.
-uint32_t Copter::ap_value() const
-{
-    uint32_t ret = 0;
-
-    const bool *b = (const bool *)&ap;
-    for (uint8_t i=0; i<sizeof(ap); i++) {
-        if (b[i]) {
-            ret |= 1U<<i;
-        }
-    }
-
-    return ret;
-}
-
 // one_hz_loop - runs at 1Hz
 void Copter::one_hz_loop()
 {
 #if HAL_LOGGING_ENABLED
     if (should_log(MASK_LOG_ANY)) {
-        Log_Write_Data(LogDataID::AP_STATE, ap_value());
+        Log_Write_Data(LogDataID::AP_STATE, ap.value);
     }
 #endif
 
@@ -788,7 +657,7 @@ void Copter::one_hz_loop()
     }
 
     // update assigned functions and enable auxiliary servos
-    AP::srv().enable_aux_servos();
+    SRV_Channels::enable_aux_servos();
 
 #if HAL_LOGGING_ENABLED
     // log terrain data
@@ -802,26 +671,11 @@ void Copter::one_hz_loop()
     AP_Notify::flags.flying = !ap.land_complete;
 
     // slowly update the PID notches with the average loop rate
-    if (!using_rate_thread) {
-        attitude_control->set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
-    }
+    attitude_control->set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
     pos_control->get_accel_z_pid().set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
-#if AC_CUSTOMCONTROL_MULTI_ENABLED
+#if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED
     custom_control.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
 #endif
-
-#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
-    // see if we should have a separate rate thread
-    if (!started_rate_thread && get_fast_rate_type() != FastRateType::FAST_RATE_DISABLED) {
-        if (hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&Copter::rate_controller_thread, void),
-                                         "rate",
-                                         1536, AP_HAL::Scheduler::PRIORITY_RCOUT, 1)) {
-            started_rate_thread = true;
-        } else {
-            AP_BoardConfig::allocation_error("rate thread");
-        }
-    }
-#endif
 }
 
 void Copter::init_simple_bearing()
@@ -913,9 +767,7 @@ void Copter::update_altitude()
     if (should_log(MASK_LOG_CTUN)) {
         Log_Write_Control_Tuning();
         if (!should_log(MASK_LOG_FTN_FAST)) {
-#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
             AP::ins().write_notch_log_messages();
-#endif
 #if HAL_GYROFFT_ENABLED
             gyro_fft.write_log_messages();
 #endif
@@ -966,16 +818,16 @@ bool Copter::get_rate_ef_targets(Vector3f& rate_ef_targets) const
 Copter::Copter(void)
     :
     flight_modes(&g.flight_mode1),
-    pos_variance_filt(FS_EKF_FILT_DEFAULT),
-    vel_variance_filt(FS_EKF_FILT_DEFAULT),
-    hgt_variance_filt(FS_EKF_FILT_DEFAULT),
-    flightmode(&mode_stabilize),
     simple_cos_yaw(1.0f),
     super_simple_cos_yaw(1.0),
     land_accel_ef_filter(LAND_DETECTOR_ACCEL_LPF_CUTOFF),
     rc_throttle_control_in_filter(1.0f),
     inertial_nav(ahrs),
-    param_loader(var_info)
+    param_loader(var_info),
+    flightmode(&mode_stabilize),
+    pos_variance_filt(FS_EKF_FILT_DEFAULT),
+    vel_variance_filt(FS_EKF_FILT_DEFAULT),
+    hgt_variance_filt(FS_EKF_FILT_DEFAULT)
 {
 }
 

ArduCopter/Copter.h

@@ -71,7 +71,6 @@
 #include <AC_PrecLand/AC_PrecLand_config.h>
 #include <AP_OpticalFlow/AP_OpticalFlow.h>
 #include <AP_Winch/AP_Winch_config.h>
-#include <AP_SurfaceDistance/AP_SurfaceDistance.h>
 
 // Configuration
 #include "defines.h"
@@ -83,13 +82,13 @@
  #define MOTOR_CLASS AP_MotorsMulticopter
 #endif
 
-#if MODE_AUTOROTATE_ENABLED
+#if MODE_AUTOROTATE_ENABLED == ENABLED
  #include <AC_Autorotation/AC_Autorotation.h> // Autorotation controllers
 #endif
 
-#include "RC_Channel_Copter.h"         // RC Channel Library
+#include "RC_Channel.h"         // RC Channel Library
 
-#include "GCS_MAVLink_Copter.h"
+#include "GCS_Mavlink.h"
 #include "GCS_Copter.h"
 #include "AP_Rally.h"           // Rally point library
 #include "AP_Arming.h"
@@ -104,10 +103,10 @@
  #include <AP_Beacon/AP_Beacon.h>
 #endif
 
-#if AP_AVOIDANCE_ENABLED
+#if AC_AVOID_ENABLED == ENABLED
  #include <AC_Avoidance/AC_Avoid.h>
 #endif
-#if AP_OAPATHPLANNER_ENABLED
+#if AC_OAPATHPLANNER_ENABLED == ENABLED
  #include <AC_WPNav/AC_WPNav_OA.h>
  #include <AC_Avoidance/AP_OAPathPlanner.h>
 #endif
@@ -115,13 +114,13 @@
  # include <AC_PrecLand/AC_PrecLand.h>
  # include <AC_PrecLand/AC_PrecLand_StateMachine.h>
 #endif
-#if MODE_FOLLOW_ENABLED
+#if MODE_FOLLOW_ENABLED == ENABLED
  # include <AP_Follow/AP_Follow.h>
 #endif
 #if AP_TERRAIN_AVAILABLE
  # include <AP_Terrain/AP_Terrain.h>
 #endif
-#if AP_RANGEFINDER_ENABLED
+#if RANGEFINDER_ENABLED == ENABLED
  # include <AP_RangeFinder/AP_RangeFinder.h>
 #endif
 
@@ -137,10 +136,10 @@
  #include <AP_OSD/AP_OSD.h>
 #endif
 
-#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
+#if ADVANCED_FAILSAFE == ENABLED
  # include "afs_copter.h"
 #endif
-#if TOY_MODE_ENABLED
+#if TOY_MODE_ENABLED == ENABLED
  # include "toy_mode.h"
 #endif
 #if AP_WINCH_ENABLED
@@ -152,15 +151,15 @@
 #include <AP_Scripting/AP_Scripting.h>
 #endif
 
-#if AC_CUSTOMCONTROL_MULTI_ENABLED
+#if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED
 #include <AC_CustomControl/AC_CustomControl.h>                  // Custom control library
 #endif
 
-#if AP_AVOIDANCE_ENABLED && !AP_FENCE_ENABLED
+#if AC_AVOID_ENABLED && !AP_FENCE_ENABLED
   #error AC_Avoidance relies on AP_FENCE_ENABLED which is disabled
 #endif
 
-#if AP_OAPATHPLANNER_ENABLED && !AP_FENCE_ENABLED
+#if AC_OAPATHPLANNER_ENABLED && !AP_FENCE_ENABLED
   #error AP_OAPathPlanner relies on AP_FENCE_ENABLED which is disabled
 #endif
 
@@ -183,7 +182,7 @@ public:
     friend class ParametersG2;
     friend class AP_Avoidance_Copter;
 
-#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
+#if ADVANCED_FAILSAFE == ENABLED
     friend class AP_AdvancedFailsafe_Copter;
 #endif
     friend class AP_Arming_Copter;
@@ -252,13 +251,22 @@ private:
     AP_Int8 *flight_modes;
     const uint8_t num_flight_modes = 6;
 
-    AP_SurfaceDistance rangefinder_state {ROTATION_PITCH_270, inertial_nav, 0U};
-    AP_SurfaceDistance rangefinder_up_state {ROTATION_PITCH_90, inertial_nav, 1U};
+    struct RangeFinderState {
+        bool enabled:1;
+        bool alt_healthy:1; // true if we can trust the altitude from the rangefinder
+        int16_t alt_cm;     // tilt compensated altitude (in cm) from rangefinder
+        float inertial_alt_cm; // inertial alt at time of last rangefinder sample
+        uint32_t last_healthy_ms;
+        LowPassFilterFloat alt_cm_filt; // altitude filter
+        int16_t alt_cm_glitch_protected;    // last glitch protected altitude
+        int8_t glitch_count;    // non-zero number indicates rangefinder is glitching
+        uint32_t glitch_cleared_ms; // system time glitch cleared
+        float terrain_offset_cm;    // filtered terrain offset (e.g. terrain's height above EKF origin)
+    } rangefinder_state, rangefinder_up_state;
 
-    // helper function to get inertially interpolated rangefinder height.
+    // return rangefinder height interpolated using inertial altitude
     bool get_rangefinder_height_interpolated_cm(int32_t& ret) const;
 
-#if AP_RANGEFINDER_ENABLED
     class SurfaceTracking {
     public:
 
@@ -266,9 +274,9 @@ private:
         //   measured ground or ceiling level measured using the range finder.
         void update_surface_offset();
 
-        // target has already been set by terrain following so do not initalise again
-        // this should be called by flight modes when switching from terrain following to surface tracking (e.g. ZigZag)
-        void external_init();
+        // get/set target altitude (in cm) above ground
+        bool get_target_alt_cm(float &target_alt_cm) const;
+        void set_target_alt_cm(float target_alt_cm);
 
         // get target and actual distances (in m) for logging purposes
         bool get_target_dist_for_logging(float &target_dist) const;
@@ -293,7 +301,6 @@ private:
         bool valid_for_logging;     // true if we have a desired target altitude
         bool reset_target;          // true if target should be reset because of change in surface being tracked
     } surface_tracking;
-#endif
 
 #if AP_RPM_ENABLED
     AP_RPM rpm_sensor;
@@ -347,51 +354,50 @@ private:
 # include USERHOOK_VARIABLES
 #endif
 
-    // ap_value calculates a 32-bit bitmask representing various pieces of
-    // state about the Copter.  It replaces a global variable which was
-    // used to track this state.
-    uint32_t ap_value() const;
+    // Documentation of GLobals:
+    typedef union {
+        struct {
+            uint8_t unused1                 : 1; // 0
+            uint8_t unused_was_simple_mode  : 2; // 1,2
+            uint8_t pre_arm_rc_check        : 1; // 3       // true if rc input pre-arm checks have been completed successfully
+            uint8_t pre_arm_check           : 1; // 4       // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed
+            uint8_t auto_armed              : 1; // 5       // stops auto missions from beginning until throttle is raised
+            uint8_t logging_started         : 1; // 6       // true if logging has started
+            uint8_t land_complete           : 1; // 7       // true if we have detected a landing
+            uint8_t new_radio_frame         : 1; // 8       // Set true if we have new PWM data to act on from the Radio
+            uint8_t usb_connected_unused    : 1; // 9       // UNUSED
+            uint8_t rc_receiver_present_unused : 1; // 10      // UNUSED
+            uint8_t compass_mot             : 1; // 11      // true if we are currently performing compassmot calibration
+            uint8_t motor_test              : 1; // 12      // true if we are currently performing the motors test
+            uint8_t initialised             : 1; // 13      // true once the init_ardupilot function has completed.  Extended status to GCS is not sent until this completes
+            uint8_t land_complete_maybe     : 1; // 14      // true if we may have landed (less strict version of land_complete)
+            uint8_t throttle_zero           : 1; // 15      // true if the throttle stick is at zero, debounced, determines if pilot intends shut-down when not using motor interlock
+            uint8_t system_time_set_unused  : 1; // 16      // true if the system time has been set from the GPS
+            uint8_t gps_glitching           : 1; // 17      // true if GPS glitching is affecting navigation accuracy
+            uint8_t using_interlock         : 1; // 20      // aux switch motor interlock function is in use
+            uint8_t land_repo_active        : 1; // 21      // true if the pilot is overriding the landing position
+            uint8_t motor_interlock_switch  : 1; // 22      // true if pilot is requesting motor interlock enable
+            uint8_t in_arming_delay         : 1; // 23      // true while we are armed but waiting to spin motors
+            uint8_t initialised_params      : 1; // 24      // true when the all parameters have been initialised. we cannot send parameters to the GCS until this is done
+            uint8_t unused3                 : 1; // 25      // was compass_init_location; true when the compass's initial location has been set
+            uint8_t unused2                 : 1; // 26      // aux switch rc_override is allowed
+            uint8_t armed_with_airmode_switch : 1; // 27      // we armed using a arming switch
+            uint8_t prec_land_active        : 1; // 28      // true if precland is active
+        };
+        uint32_t value;
+    } ap_t;
 
-    // These variables are essentially global variables.  These should
-    // be removed over time.  It is critical that the offsets of these
-    // variables remain unchanged - the logging is dependent on this
-    // ordering!
-    struct PACKED {
-        bool unused1;                        //  0
-        bool unused_was_simple_mode_byte1;   //  1
-        bool unused_was_simple_mode_byte2;   //  2
-        bool pre_arm_rc_check;               //  3 true if rc input pre-arm checks have been completed successfully
-        bool pre_arm_check;                  //  4 true if all pre-arm checks (rc, accel calibration, gps lock) have been performed
-        bool auto_armed;                     //  5 stops auto missions from beginning until throttle is raised
-        bool unused_log_started;             //  6
-        bool land_complete;                  //  7 true if we have detected a landing
-        bool new_radio_frame;                //  8 Set true if we have new PWM data to act on from the Radio
-        bool unused_usb_connected;           //  9
-        bool unused_receiver_present;        // 10
-        bool compass_mot;                    // 11 true if we are currently performing compassmot calibration
-        bool motor_test;                     // 12 true if we are currently performing the motors test
-        bool initialised;                    // 13 true once the init_ardupilot function has completed.  Extended status to GCS is not sent until this completes
-        bool land_complete_maybe;            // 14 true if we may have landed (less strict version of land_complete)
-        bool throttle_zero;                  // 15 true if the throttle stick is at zero, debounced, determines if pilot intends shut-down when not using motor interlock
-        bool system_time_set_unused;         // 16 true if the system time has been set from the GPS
-        bool gps_glitching;                  // 17 true if GPS glitching is affecting navigation accuracy
-        bool using_interlock;                // 18 aux switch motor interlock function is in use
-        bool land_repo_active;               // 19 true if the pilot is overriding the landing position
-        bool motor_interlock_switch;         // 20 true if pilot is requesting motor interlock enable
-        bool in_arming_delay;                // 21 true while we are armed but waiting to spin motors
-        bool initialised_params;             // 22 true when the all parameters have been initialised. we cannot send parameters to the GCS until this is done
-        bool unused_compass_init_location;   // 23
-        bool unused2_aux_switch_rc_override_allowed; // 24
-        bool armed_with_airmode_switch;      // 25 we armed using a arming switch
-        bool prec_land_active;               // 26 true if precland is active
-    } ap;
+    ap_t ap;
 
     AirMode air_mode; // air mode is 0 = not-configured ; 1 = disabled; 2 = enabled;
     bool force_flying; // force flying is enabled when true;
 
+    static_assert(sizeof(uint32_t) == sizeof(ap), "ap_t must be uint32_t");
+
     // This is the state of the flight control system
     // There are multiple states defined such as STABILIZE, ACRO,
     Mode *flightmode;
+    Mode::Number prev_control_mode;
 
     RCMapper rcmap;
 
@@ -480,11 +486,11 @@ private:
     AC_WPNav *wp_nav;
     AC_Loiter *loiter_nav;
 
-#if AC_CUSTOMCONTROL_MULTI_ENABLED
+#if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED
     AC_CustomControl custom_control{ahrs_view, attitude_control, motors, scheduler.get_loop_period_s()};
 #endif
 
-#if MODE_CIRCLE_ENABLED
+#if MODE_CIRCLE_ENABLED == ENABLED
     AC_Circle *circle_nav;
 #endif
 
@@ -507,7 +513,7 @@ private:
     AP_Mount camera_mount;
 #endif
 
-#if AP_AVOIDANCE_ENABLED
+#if AC_AVOID_ENABLED == ENABLED
     AC_Avoid avoid;
 #endif
 
@@ -522,7 +528,7 @@ private:
 #endif
 
     // Parachute release
-#if HAL_PARACHUTE_ENABLED
+#if PARACHUTE == ENABLED
     AP_Parachute parachute;
 #endif
 
@@ -574,7 +580,9 @@ private:
     // Tradheli flags
     typedef struct {
         uint8_t dynamic_flight          : 1;    // 0   // true if we are moving at a significant speed (used to turn on/off leaky I terms)
-        bool coll_stk_low                  ;    // 1   // true when collective stick is on lower limit
+        uint8_t inverted_flight         : 1;    // 1   // true for inverted flight mode
+        uint8_t in_autorotation         : 1;    // 2   // true when heli is in autorotation
+        bool coll_stk_low                  ;    // 3   // true when collective stick is on lower limit
     } heli_flags_t;
     heli_flags_t heli_flags;
 
@@ -625,28 +633,12 @@ private:
     };
 
 
-    enum class FlightOption : uint32_t {
+    enum class FlightOptions {
         DISABLE_THRUST_LOSS_CHECK     = (1<<0),   // 1
         DISABLE_YAW_IMBALANCE_WARNING = (1<<1),   // 2
         RELEASE_GRIPPER_ON_THRUST_LOSS = (1<<2),  // 4
-        REQUIRE_POSITION_FOR_ARMING =   (1<<3),   // 8
     };
 
-    // type of fast rate attitude controller in operation
-    enum class FastRateType : uint8_t {
-        FAST_RATE_DISABLED            = 0,
-        FAST_RATE_DYNAMIC             = 1,
-        FAST_RATE_FIXED_ARMED         = 2,
-        FAST_RATE_FIXED               = 3,
-    };
-
-    FastRateType get_fast_rate_type() const { return FastRateType(g2.att_enable.get()); }
-
-    // returns true if option is enabled for this vehicle
-    bool option_is_enabled(FlightOption option) const {
-        return (g2.flight_options & uint32_t(option)) != 0;
-    }
-
     static constexpr int8_t _failsafe_priorities[] = {
                                                       (int8_t)FailsafeAction::TERMINATE,
                                                       (int8_t)FailsafeAction::LAND,
@@ -676,34 +668,23 @@ private:
     void get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
                              uint8_t &task_count,
                              uint32_t &log_bit) override;
-#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
-#if MODE_GUIDED_ENABLED
-    bool set_target_location(const Location& target_loc) override;
-    bool start_takeoff(const float alt) override;
-#endif // MODE_GUIDED_ENABLED
-#endif // AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
-
 #if AP_SCRIPTING_ENABLED
-#if MODE_GUIDED_ENABLED
-    bool get_target_location(Location& target_loc) override;
-    bool update_target_location(const Location &old_loc, const Location &new_loc) override;
+#if MODE_GUIDED_ENABLED == ENABLED
+    bool start_takeoff(float alt) override;
+    bool set_target_location(const Location& target_loc) override;
     bool set_target_pos_NED(const Vector3f& target_pos, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative, bool terrain_alt) override;
     bool set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& target_vel) override;
     bool set_target_posvelaccel_NED(const Vector3f& target_pos, const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative) override;
     bool set_target_velocity_NED(const Vector3f& vel_ned) override;
     bool set_target_velaccel_NED(const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool relative_yaw) override;
     bool set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs) override;
-    bool set_target_rate_and_throttle(float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps, float throttle) override;
-
-    // Register a custom mode with given number and names
-    AP_Vehicle::custom_mode_state* register_custom_mode(const uint8_t number, const char* full_name, const char* short_name) override;
 #endif
-#if MODE_CIRCLE_ENABLED
+#if MODE_CIRCLE_ENABLED == ENABLED
     bool get_circle_radius(float &radius_m) override;
     bool set_circle_rate(float rate_dps) override;
 #endif
     bool set_desired_speed(float speed) override;
-#if MODE_AUTO_ENABLED
+#if MODE_AUTO_ENABLED == ENABLED
     bool nav_scripting_enable(uint8_t mode) override;
     bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4) override;
     void nav_script_time_done(uint16_t id) override;
@@ -739,27 +720,9 @@ private:
     void set_accel_throttle_I_from_pilot_throttle();
     void rotate_body_frame_to_NE(float &x, float &y);
     uint16_t get_pilot_speed_dn() const;
-    void run_rate_controller_main();
+    void run_rate_controller();
 
-    // if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
-    struct RateControllerRates {
-        uint8_t fast_logging_rate;
-        uint8_t medium_logging_rate;
-        uint8_t filter_rate;
-        uint8_t main_loop_rate;
-    };
-
-    uint8_t calc_gyro_decimation(uint8_t gyro_decimation, uint16_t rate_hz);
-    void rate_controller_thread();
-    void rate_controller_filter_update();
-    void rate_controller_log_update();
-    void rate_controller_set_rates(uint8_t rate_decimation, RateControllerRates& rates, bool warn_cpu_high);
-    void enable_fast_rate_loop(uint8_t rate_decimation, RateControllerRates& rates);
-    void disable_fast_rate_loop(RateControllerRates& rates);
-    void update_dynamic_notch_at_specified_rate_main();
-    // endif AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
-
-#if AC_CUSTOMCONTROL_MULTI_ENABLED
+#if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED
     void run_custom_controller() { custom_control.update(); }
 #endif
 
@@ -778,8 +741,9 @@ private:
     // commands.cpp
     void update_home_from_EKF();
     void set_home_to_current_location_inflight();
-    bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
-    bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED;
+    bool set_home_to_current_location(bool lock) WARN_IF_UNUSED;
+    bool set_home(const Location& loc, bool lock) WARN_IF_UNUSED;
+    bool far_from_EKF_origin(const Location& loc);
 
     // compassmot.cpp
     MAV_RESULT mavlink_compassmot(const GCS_MAVLINK &gcs_chan);
@@ -835,7 +799,7 @@ private:
     // failsafe.cpp
     void failsafe_enable();
     void failsafe_disable();
-#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
+#if ADVANCED_FAILSAFE == ENABLED
     void afs_fs_check(void);
 #endif
 
@@ -864,29 +828,6 @@ private:
     void set_land_complete(bool b);
     void set_land_complete_maybe(bool b);
     void update_throttle_mix();
-    bool get_force_flying() const;
-#if HAL_LOGGING_ENABLED
-    enum class LandDetectorLoggingFlag : uint16_t {
-        LANDED               = 1U <<  0,
-        LANDED_MAYBE         = 1U <<  1,
-        LANDING              = 1U <<  2,
-        STANDBY_ACTIVE       = 1U <<  3,
-        WOW                  = 1U <<  4,
-        RANGEFINDER_BELOW_2M = 1U <<  5,
-        DESCENT_RATE_LOW     = 1U <<  6,
-        ACCEL_STATIONARY     = 1U <<  7,
-        LARGE_ANGLE_ERROR    = 1U <<  8,
-        LARGE_ANGLE_REQUEST  = 1U <<  8,
-        MOTOR_AT_LOWER_LIMIT = 1U <<  9,
-        THROTTLE_MIX_AT_MIN  = 1U << 10,
-    };
-    struct {
-        uint32_t last_logged_ms;
-        uint32_t last_logged_count;
-        uint16_t last_logged_flags;
-    } land_detector;
-    void Log_LDET(uint16_t logging_flags, uint32_t land_detector_count);
-#endif
 
 #if AP_LANDINGGEAR_ENABLED
     // landing_gear.cpp
@@ -907,7 +848,6 @@ private:
     // Log.cpp
     void Log_Write_Control_Tuning();
     void Log_Write_Attitude();
-    void Log_Write_Rate();
     void Log_Write_EKF_POS();
     void Log_Write_PIDS();
     void Log_Write_Data(LogDataID id, int32_t value);
@@ -917,12 +857,14 @@ private:
     void Log_Write_Data(LogDataID id, float value);
     void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max);
     void Log_Video_Stabilisation();
+#if FRAME_CONFIG == HELI_FRAME
+    void Log_Write_Heli(void);
+#endif
     void Log_Write_Guided_Position_Target(ModeGuided::SubMode submode, const Vector3f& pos_target, bool terrain_alt, const Vector3f& vel_target, const Vector3f& accel_target);
     void Log_Write_Guided_Attitude_Target(ModeGuided::SubMode target_type, float roll, float pitch, float yaw, const Vector3f &ang_vel, float thrust, float climb_rate);
     void Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out);
     void Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z);
     void Log_Write_Vehicle_Startup_Messages();
-    void Log_Write_Rate_Thread_Dt(float dt, float dtAvg, float dtMax, float dtMin);
 #endif  // HAL_LOGGING_ENABLED
 
     // mode.cpp
@@ -952,8 +894,7 @@ private:
     // motors.cpp
     void arm_motors_check();
     void auto_disarm_check();
-    void motors_output(bool full_push = true);
-    void motors_output_main();
+    void motors_output();
     void lost_vehicle_check();
 
     // navigation.cpp
@@ -968,6 +909,7 @@ private:
     void convert_prx_parameters();
 #endif
     void convert_lgr_parameters(void);
+    void convert_tradheli_parameters(void) const;
 
     // precision_landing.cpp
     void init_precland();
@@ -1031,7 +973,7 @@ private:
     void userhook_auxSwitch2(const RC_Channel::AuxSwitchPos ch_flag);
     void userhook_auxSwitch3(const RC_Channel::AuxSwitchPos ch_flag);
 
-#if MODE_ACRO_ENABLED
+#if MODE_ACRO_ENABLED == ENABLED
 #if FRAME_CONFIG == HELI_FRAME
     ModeAcro_Heli mode_acro;
 #else
@@ -1039,42 +981,38 @@ private:
 #endif
 #endif
     ModeAltHold mode_althold;
-#if MODE_AUTO_ENABLED
+#if MODE_AUTO_ENABLED == ENABLED
     ModeAuto mode_auto;
 #endif
-#if AUTOTUNE_ENABLED
+#if AUTOTUNE_ENABLED == ENABLED
     ModeAutoTune mode_autotune;
 #endif
-#if MODE_BRAKE_ENABLED
+#if MODE_BRAKE_ENABLED == ENABLED
     ModeBrake mode_brake;
 #endif
-#if MODE_CIRCLE_ENABLED
+#if MODE_CIRCLE_ENABLED == ENABLED
     ModeCircle mode_circle;
 #endif
-#if MODE_DRIFT_ENABLED
+#if MODE_DRIFT_ENABLED == ENABLED
     ModeDrift mode_drift;
 #endif
-#if MODE_FLIP_ENABLED
+#if MODE_FLIP_ENABLED == ENABLED
     ModeFlip mode_flip;
 #endif
-#if MODE_FOLLOW_ENABLED
+#if MODE_FOLLOW_ENABLED == ENABLED
     ModeFollow mode_follow;
 #endif
-#if MODE_GUIDED_ENABLED
+#if MODE_GUIDED_ENABLED == ENABLED
     ModeGuided mode_guided;
-#if AP_SCRIPTING_ENABLED
-    // Custom modes registered at runtime
-    ModeGuidedCustom *mode_guided_custom[5];
-#endif
 #endif
     ModeLand mode_land;
-#if MODE_LOITER_ENABLED
+#if MODE_LOITER_ENABLED == ENABLED
     ModeLoiter mode_loiter;
 #endif
-#if MODE_POSHOLD_ENABLED
+#if MODE_POSHOLD_ENABLED == ENABLED
     ModePosHold mode_poshold;
 #endif
-#if MODE_RTL_ENABLED
+#if MODE_RTL_ENABLED == ENABLED
     ModeRTL mode_rtl;
 #endif
 #if FRAME_CONFIG == HELI_FRAME
@@ -1082,34 +1020,34 @@ private:
 #else
     ModeStabilize mode_stabilize;
 #endif
-#if MODE_SPORT_ENABLED
+#if MODE_SPORT_ENABLED == ENABLED
     ModeSport mode_sport;
 #endif
-#if MODE_SYSTEMID_ENABLED
+#if MODE_SYSTEMID_ENABLED == ENABLED
     ModeSystemId mode_systemid;
 #endif
 #if HAL_ADSB_ENABLED
     ModeAvoidADSB mode_avoid_adsb;
 #endif
-#if MODE_THROW_ENABLED
+#if MODE_THROW_ENABLED == ENABLED
     ModeThrow mode_throw;
 #endif
-#if MODE_GUIDED_NOGPS_ENABLED
+#if MODE_GUIDED_NOGPS_ENABLED == ENABLED
     ModeGuidedNoGPS mode_guided_nogps;
 #endif
-#if MODE_SMARTRTL_ENABLED
+#if MODE_SMARTRTL_ENABLED == ENABLED
     ModeSmartRTL mode_smartrtl;
 #endif
-#if MODE_FLOWHOLD_ENABLED
+#if MODE_FLOWHOLD_ENABLED == ENABLED
     ModeFlowHold mode_flowhold;
 #endif
-#if MODE_ZIGZAG_ENABLED
+#if MODE_ZIGZAG_ENABLED == ENABLED
     ModeZigZag mode_zigzag;
 #endif
-#if MODE_AUTOROTATE_ENABLED
+#if MODE_AUTOROTATE_ENABLED == ENABLED
     ModeAutorotate mode_autorotate;
 #endif
-#if MODE_TURTLE_ENABLED
+#if MODE_TURTLE_ENABLED == ENABLED
     ModeTurtle mode_turtle;
 #endif
 
@@ -1117,9 +1055,6 @@ private:
     Mode *mode_from_mode_num(const Mode::Number mode);
     void exit_mode(Mode *&old_flightmode, Mode *&new_flightmode);
 
-    bool started_rate_thread;
-    bool using_rate_thread;
-
 public:
     void failsafe_check();      // failsafe.cpp
 };

ArduCopter/GCS_Copter.cpp

@@ -42,22 +42,39 @@ void GCS_Copter::update_vehicle_sensor_status_flags(void)
         MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |
         MAV_SYS_STATUS_SENSOR_YAW_POSITION;
 
-    // Update position controller flags
-    if (copter.pos_control != nullptr) {
-        control_sensors_present |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
-        control_sensors_present |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
+    // update flightmode-specific flags:
+    control_sensors_present |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
+    control_sensors_present |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
 
-        // XY position controller
-        if (copter.pos_control->is_active_xy()) {
-            control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
-            control_sensors_health |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
-        }
-
-        // Z altitude controller
-        if (copter.pos_control->is_active_z()) {
-            control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
-            control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
-        }
+    switch (copter.flightmode->mode_number()) {
+    case Mode::Number::AUTO:
+    case Mode::Number::AUTO_RTL:
+    case Mode::Number::AVOID_ADSB:
+    case Mode::Number::GUIDED:
+    case Mode::Number::LOITER:
+    case Mode::Number::RTL:
+    case Mode::Number::CIRCLE:
+    case Mode::Number::LAND:
+    case Mode::Number::POSHOLD:
+    case Mode::Number::BRAKE:
+    case Mode::Number::THROW:
+    case Mode::Number::SMART_RTL:
+        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
+        control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
+        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
+        control_sensors_health |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
+        break;
+    case Mode::Number::ALT_HOLD:
+    case Mode::Number::GUIDED_NOGPS:
+    case Mode::Number::SPORT:
+    case Mode::Number::AUTOTUNE:
+    case Mode::Number::FLOWHOLD:
+        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
+        control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
+        break;
+    default:
+        // stabilize, acro, drift, and flip have no automatic x,y or z control (i.e. all manual)
+        break;
     }
 
     // optional sensors, some of which are essentially always
@@ -74,7 +91,7 @@ void GCS_Copter::update_vehicle_sensor_status_flags(void)
     }
 #endif
 
-#if AP_RANGEFINDER_ENABLED
+#if RANGEFINDER_ENABLED == ENABLED
     const RangeFinder *rangefinder = RangeFinder::get_singleton();
     if (rangefinder && rangefinder->has_orientation(ROTATION_PITCH_270)) {
         control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;

ArduCopter/GCS_Copter.h

@@ -1,7 +1,7 @@
 #pragma once
 
 #include <GCS_MAVLink/GCS.h>
-#include "GCS_MAVLink_Copter.h"
+#include "GCS_Mavlink.h"
 
 class GCS_Copter : public GCS
 {
@@ -43,7 +43,7 @@ protected:
 
     GCS_MAVLINK_Copter *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters &params,
                                                 AP_HAL::UARTDriver &uart) override {
-        return NEW_NOTHROW GCS_MAVLINK_Copter(params, uart);
+        return new GCS_MAVLINK_Copter(params, uart);
     }
 
 };

ArduCopter/GCS_Mavlink.cpp

@@ -1,6 +1,6 @@
 #include "Copter.h"
 
-#include "GCS_MAVLink_Copter.h"
+#include "GCS_Mavlink.h"
 #include <AP_RPM/AP_RPM_config.h>
 #include <AP_EFI/AP_EFI_config.h>
 
@@ -37,11 +37,25 @@ MAV_MODE GCS_MAVLINK_Copter::base_mode() const
     // only get useful information from the custom_mode, which maps to
     // the APM flight mode and has a well defined meaning in the
     // ArduPlane documentation
-    if ((copter.pos_control != nullptr) && copter.pos_control->is_active_xy()) {
+    switch (copter.flightmode->mode_number()) {
+    case Mode::Number::AUTO:
+    case Mode::Number::AUTO_RTL:
+    case Mode::Number::RTL:
+    case Mode::Number::LOITER:
+    case Mode::Number::AVOID_ADSB:
+    case Mode::Number::FOLLOW:
+    case Mode::Number::GUIDED:
+    case Mode::Number::CIRCLE:
+    case Mode::Number::POSHOLD:
+    case Mode::Number::BRAKE:
+    case Mode::Number::SMART_RTL:
         _base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
         // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
         // APM does in any mode, as that is defined as "system finds its own goal
         // positions", which APM does not currently do
+        break;
+    default:
+        break;
     }
 
     // all modes except INITIALISING have some form of manual
@@ -75,10 +89,6 @@ MAV_STATE GCS_MAVLINK_Copter::vehicle_system_status() const
         return MAV_STATE_STANDBY;
     }
 
-    if (!copter.ap.initialised) {
-    	return MAV_STATE_BOOT;
-    }
-
     return MAV_STATE_ACTIVE;
 }
 
@@ -141,7 +151,7 @@ void GCS_MAVLINK_Copter::send_position_target_global_int()
 
 void GCS_MAVLINK_Copter::send_position_target_local_ned()
 {
-#if MODE_GUIDED_ENABLED
+#if MODE_GUIDED_ENABLED == ENABLED
     if (!copter.flightmode->in_guided_mode()) {
         return;
     }
@@ -336,16 +346,12 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id)
 {
     switch(id) {
 
+    case MSG_TERRAIN:
 #if AP_TERRAIN_AVAILABLE
-    case MSG_TERRAIN_REQUEST:
         CHECK_PAYLOAD_SIZE(TERRAIN_REQUEST);
         copter.terrain.send_request(chan);
-        break;
-    case MSG_TERRAIN_REPORT:
-        CHECK_PAYLOAD_SIZE(TERRAIN_REPORT);
-        copter.terrain.send_report(chan);
-        break;
 #endif
+        break;
 
     case MSG_WIND:
         CHECK_PAYLOAD_SIZE(WIND);
@@ -363,7 +369,7 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id)
         CHECK_PAYLOAD_SIZE(ADSB_VEHICLE);
         copter.adsb.send_adsb_vehicle(chan);
 #endif
-#if AP_OAPATHPLANNER_ENABLED
+#if AC_OAPATHPLANNER_ENABLED == ENABLED
         AP_OADatabase *oadb = AP_OADatabase::get_singleton();
         if (oadb != nullptr) {
             CHECK_PAYLOAD_SIZE(ADSB_VEHICLE);
@@ -386,7 +392,7 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id)
 const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = {
     // @Param: RAW_SENS
     // @DisplayName: Raw sensor stream rate
-    // @Description: MAVLink Stream rate of RAW_IMU, SCALED_IMU2, SCALED_IMU3, SCALED_PRESSURE, SCALED_PRESSURE2, SCALED_PRESSURE3 and AIRSPEED
+    // @Description: MAVLink Stream rate of RAW_IMU, SCALED_IMU2, SCALED_IMU3, SCALED_PRESSURE, SCALED_PRESSURE2, and SCALED_PRESSURE3
     // @Units: Hz
     // @Range: 0 50
     // @Increment: 1
@@ -455,7 +461,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = {
 
     // @Param: EXTRA3
     // @DisplayName: Extra data type 3 stream rate
-    // @Description: MAVLink Stream rate of AHRS, SYSTEM_TIME, WIND, RANGEFINDER, DISTANCE_SENSOR, TERRAIN_REQUEST, TERRAIN_REPORT, BATTERY_STATUS, GIMBAL_DEVICE_ATTITUDE_STATUS, OPTICAL_FLOW, MAG_CAL_REPORT, MAG_CAL_PROGRESS, EKF_STATUS_REPORT, VIBRATION, RPM, ESC TELEMETRY,GENERATOR_STATUS, and WINCH_STATUS
+    // @Description: MAVLink Stream rate of AHRS, SYSTEM_TIME, WIND, RANGEFINDER, DISTANCE_SENSOR, TERRAIN_REQUEST, BATTERY_STATUS, GIMBAL_DEVICE_ATTITUDE_STATUS, OPTICAL_FLOW, MAG_CAL_REPORT, MAG_CAL_PROGRESS, EKF_STATUS_REPORT, VIBRATION, RPM, ESC TELEMETRY,GENERATOR_STATUS, and WINCH_STATUS
 
     // @Units: Hz
     // @Range: 0 50
@@ -493,9 +499,6 @@ static const ap_message STREAM_RAW_SENSORS_msgs[] = {
     MSG_SCALED_PRESSURE,
     MSG_SCALED_PRESSURE2,
     MSG_SCALED_PRESSURE3,
-#if AP_AIRSPEED_ENABLED
-    MSG_AIRSPEED,
-#endif
 };
 static const ap_message STREAM_EXTENDED_STATUS_msgs[] = {
     MSG_SYS_STATUS,
@@ -505,16 +508,10 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = {
 #endif
     MSG_MEMINFO,
     MSG_CURRENT_WAYPOINT, // MISSION_CURRENT
-#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED
     MSG_GPS_RAW,
-#endif
-#if AP_GPS_GPS_RTK_SENDING_ENABLED
     MSG_GPS_RTK,
-#endif
-#if AP_GPS_GPS2_RAW_SENDING_ENABLED
+#if GPS_MAX_RECEIVERS > 1
     MSG_GPS2_RAW,
-#endif
-#if AP_GPS_GPS2_RTK_SENDING_ENABLED
     MSG_GPS2_RTK,
 #endif
     MSG_NAV_CONTROLLER_OUTPUT,
@@ -530,9 +527,7 @@ static const ap_message STREAM_POSITION_msgs[] = {
 static const ap_message STREAM_RC_CHANNELS_msgs[] = {
     MSG_SERVO_OUTPUT_RAW,
     MSG_RC_CHANNELS,
-#if AP_MAVLINK_MSG_RC_CHANNELS_RAW_ENABLED
     MSG_RC_CHANNELS_RAW, // only sent on a mavlink1 connection
-#endif
 };
 static const ap_message STREAM_EXTRA1_msgs[] = {
     MSG_ATTITUDE,
@@ -549,13 +544,10 @@ static const ap_message STREAM_EXTRA3_msgs[] = {
     MSG_AHRS,
     MSG_SYSTEM_TIME,
     MSG_WIND,
-#if AP_RANGEFINDER_ENABLED
     MSG_RANGEFINDER,
-#endif
     MSG_DISTANCE_SENSOR,
 #if AP_TERRAIN_AVAILABLE
-    MSG_TERRAIN_REQUEST,
-    MSG_TERRAIN_REPORT,
+    MSG_TERRAIN,
 #endif
 #if AP_BATTERY_ENABLED
     MSG_BATTERY_STATUS,
@@ -589,8 +581,7 @@ static const ap_message STREAM_EXTRA3_msgs[] = {
 #endif
 };
 static const ap_message STREAM_PARAMS_msgs[] = {
-    MSG_NEXT_PARAM,
-    MSG_AVAILABLE_MODES
+    MSG_NEXT_PARAM
 };
 static const ap_message STREAM_ADSB_msgs[] = {
     MSG_ADSB_VEHICLE,
@@ -622,7 +613,7 @@ MISSION_STATE GCS_MAVLINK_Copter::mission_state(const class AP_Mission &mission)
 
 bool GCS_MAVLINK_Copter::handle_guided_request(AP_Mission::Mission_Command &cmd)
 {
-#if MODE_AUTO_ENABLED
+#if MODE_AUTO_ENABLED == ENABLED
     return copter.mode_auto.do_guided(cmd);
 #else
     return false;
@@ -639,7 +630,7 @@ void GCS_MAVLINK_Copter::packetReceived(const mavlink_status_t &status,
         copter.avoidance_adsb.handle_msg(msg);
     }
 #endif
-#if MODE_FOLLOW_ENABLED
+#if MODE_FOLLOW_ENABLED == ENABLED
     // pass message to follow library
     copter.g2.follow.handle_msg(msg);
 #endif
@@ -718,9 +709,16 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_preflight_reboot(const mavlink_command_int
     return GCS_MAVLINK::handle_preflight_reboot(packet, msg);
 }
 
+bool GCS_MAVLINK_Copter::set_home_to_current_location(bool _lock) {
+    return copter.set_home_to_current_location(_lock);
+}
+bool GCS_MAVLINK_Copter::set_home(const Location& loc, bool _lock) {
+    return copter.set_home(loc, _lock);
+}
+
 MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_do_reposition(const mavlink_command_int_t &packet)
 {
-#if MODE_GUIDED_ENABLED
+#if MODE_GUIDED_ENABLED == ENABLED
     const bool change_modes = ((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) == MAV_DO_REPOSITION_FLAGS_CHANGE_MODE;
     if (!copter.flightmode->in_guided_mode() && !change_modes) {
         return MAV_RESULT_DENIED;
@@ -772,7 +770,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i
     case MAV_CMD_DO_CHANGE_SPEED:
         return handle_MAV_CMD_DO_CHANGE_SPEED(packet);
 
-#if MODE_FOLLOW_ENABLED
+#if MODE_FOLLOW_ENABLED == ENABLED
     case MAV_CMD_DO_FOLLOW:
         // param1: sysid of target to follow
         if ((packet.param1 > 0) && (packet.param1 <= 255)) {
@@ -796,7 +794,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i
     case MAV_CMD_NAV_VTOL_TAKEOFF:
         return handle_MAV_CMD_NAV_TAKEOFF(packet);
 
-#if HAL_PARACHUTE_ENABLED
+#if PARACHUTE == ENABLED
     case MAV_CMD_DO_PARACHUTE:
         return handle_MAV_CMD_DO_PARACHUTE(packet);
 #endif
@@ -813,7 +811,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i
         return handle_MAV_CMD_SOLO_BTN_FLY_CLICK(packet);
 #endif
 
-#if MODE_AUTO_ENABLED
+#if MODE_AUTO_ENABLED == ENABLED
     case MAV_CMD_MISSION_START:
         return handle_MAV_CMD_MISSION_START(packet);
 #endif
@@ -842,13 +840,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i
         }
         return MAV_RESULT_ACCEPTED;
 
-#if MODE_AUTO_ENABLED
-    case MAV_CMD_DO_RETURN_PATH_START:
-        if (copter.mode_auto.return_path_start_auto_RTL(ModeReason::GCS_COMMAND)) {
-            return MAV_RESULT_ACCEPTED;
-        }
-        return MAV_RESULT_FAILED;
-
+#if MODE_AUTO_ENABLED == ENABLED
     case MAV_CMD_DO_LAND_START:
         if (copter.mode_auto.jump_to_landing_sequence_auto_RTL(ModeReason::GCS_COMMAND)) {
             return MAV_RESULT_ACCEPTED;
@@ -867,12 +859,9 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_mount(const mavlink_command_int_t
     switch (packet.command) {
     case MAV_CMD_DO_MOUNT_CONTROL:
         // if vehicle has a camera mount but it doesn't do pan control then yaw the entire vehicle instead
-        if (((MAV_MOUNT_MODE)packet.z == MAV_MOUNT_MODE_MAVLINK_TARGETING) &&
-            (copter.camera_mount.get_mount_type() != AP_Mount::Type::None) &&
+        if ((copter.camera_mount.get_mount_type() != AP_Mount::Type::None) &&
             !copter.camera_mount.has_pan_control()) {
-            // Per the handler in AP_Mount, DO_MOUNT_CONTROL yaw angle is in body frame, which is
-            // equivalent to an offset to the current yaw demand.
-            copter.flightmode->auto_yaw.set_yaw_angle_offset(packet.param3);
+            copter.flightmode->auto_yaw.set_yaw_angle_rate((float)packet.param3, 0.0f);
         }
         break;
     default:
@@ -961,13 +950,9 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_CHANGE_SPEED(const mavlink_comm
         return MAV_RESULT_FAILED;
 }
 
-#if MODE_AUTO_ENABLED
+#if MODE_AUTO_ENABLED == ENABLED
 MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_MISSION_START(const mavlink_command_int_t &packet)
 {
-        if (!is_zero(packet.param1) || !is_zero(packet.param2)) {
-            // first-item/last item not supported
-            return MAV_RESULT_DENIED;
-        }
         if (copter.set_mode(Mode::Number::AUTO, ModeReason::GCS_COMMAND)) {
             copter.set_auto_armed(true);
             if (copter.mode_auto.mission.state() != AP_Mission::MISSION_RUNNING) {
@@ -981,7 +966,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_MISSION_START(const mavlink_comman
 
 
 
-#if HAL_PARACHUTE_ENABLED
+#if PARACHUTE == ENABLED
 MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_PARACHUTE(const mavlink_command_int_t &packet)
 {
         // configure or release parachute
@@ -1094,7 +1079,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_SOLO_BTN_PAUSE_CLICK(const mavlink
                 bool shot_mode = (!is_zero(packet.param1) && (copter.flightmode->mode_number() == Mode::Number::GUIDED || copter.flightmode->mode_number() == Mode::Number::GUIDED_NOGPS));
 
                 if (!shot_mode) {
-#if MODE_BRAKE_ENABLED
+#if MODE_BRAKE_ENABLED == ENABLED
                     if (copter.set_mode(Mode::Number::BRAKE, ModeReason::GCS_COMMAND)) {
                         copter.mode_brake.timeout_to_loiter_ms(2500);
                     } else {
@@ -1141,12 +1126,11 @@ void GCS_MAVLINK_Copter::handle_mount_message(const mavlink_message_t &msg)
     case MAVLINK_MSG_ID_MOUNT_CONTROL:
         // if vehicle has a camera mount but it doesn't do pan control then yaw the entire vehicle instead
         if ((copter.camera_mount.get_mount_type() != AP_Mount::Type::None) &&
-            (copter.camera_mount.get_mode() == MAV_MOUNT_MODE_MAVLINK_TARGETING) &&
             !copter.camera_mount.has_pan_control()) {
-            // Per the handler in AP_Mount, MOUNT_CONTROL yaw angle is in body frame, which is
-            // equivalent to an offset to the current yaw demand.
-            const float yaw_offset_d = mavlink_msg_mount_control_get_input_c(&msg) * 0.01f;
-            copter.flightmode->auto_yaw.set_yaw_angle_offset(yaw_offset_d);
+            copter.flightmode->auto_yaw.set_yaw_angle_rate(
+                mavlink_msg_mount_control_get_input_c(&msg) * 0.01f,
+                0.0f);
+
             break;
         }
     }
@@ -1183,7 +1167,9 @@ bool GCS_MAVLINK_Copter::sane_vel_or_acc_vector(const Vector3f &vec) const
     return true;
 }
 
-#if MODE_GUIDED_ENABLED
+void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
+{
+#if MODE_GUIDED_ENABLED == ENABLED
     // for mavlink SET_POSITION_TARGET messages
     constexpr uint32_t MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE =
         POSITION_TARGET_TYPEMASK_X_IGNORE |
@@ -1208,16 +1194,18 @@ bool GCS_MAVLINK_Copter::sane_vel_or_acc_vector(const Vector3f &vec) const
         POSITION_TARGET_TYPEMASK_FORCE_SET;
 #endif
 
-#if MODE_GUIDED_ENABLED
-void GCS_MAVLINK_Copter::handle_message_set_attitude_target(const mavlink_message_t &msg)
-{
+    switch (msg.msgid) {
+
+#if MODE_GUIDED_ENABLED == ENABLED
+    case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:   // MAV ID: 82
+    {
         // decode packet
         mavlink_set_attitude_target_t packet;
         mavlink_msg_set_attitude_target_decode(&msg, &packet);
 
         // exit if vehicle is not in Guided mode or Auto-Guided mode
         if (!copter.flightmode->in_guided_mode()) {
-            return;
+            break;
         }
 
         const bool roll_rate_ignore   = packet.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE;
@@ -1228,8 +1216,7 @@ void GCS_MAVLINK_Copter::handle_message_set_attitude_target(const mavlink_messag
 
         // ensure thrust field is not ignored
         if (throttle_ignore) {
-            // The throttle input is not defined
-            return;
+            break;
         }
 
         Quaternion attitude_quat;
@@ -1243,22 +1230,10 @@ void GCS_MAVLINK_Copter::handle_message_set_attitude_target(const mavlink_messag
             // this limit is somewhat greater than sqrt(FLT_EPSL)
             if (!attitude_quat.is_unit_length()) {
                 // The attitude quaternion is ill-defined
-                return;
+                break;
             }
         }
 
-        Vector3f ang_vel_body;
-        if (!roll_rate_ignore && !pitch_rate_ignore && !yaw_rate_ignore) {
-            ang_vel_body.x = packet.body_roll_rate;
-            ang_vel_body.y = packet.body_pitch_rate;
-            ang_vel_body.z = packet.body_yaw_rate;
-        } else if (!(roll_rate_ignore && pitch_rate_ignore && yaw_rate_ignore)) {
-            // The body rates are ill-defined
-            // input is not valid so stop
-            copter.mode_guided.init(true);
-            return;
-        }
-
         // check if the message's thrust field should be interpreted as a climb rate or as thrust
         const bool use_thrust = copter.mode_guided.set_attitude_target_provides_thrust();
 
@@ -1280,19 +1255,32 @@ void GCS_MAVLINK_Copter::handle_message_set_attitude_target(const mavlink_messag
             }
         }
 
-        copter.mode_guided.set_angle(attitude_quat, ang_vel_body,
-                climb_rate_or_thrust, use_thrust);
-}
+        Vector3f ang_vel;
+        if (!roll_rate_ignore) {
+            ang_vel.x = packet.body_roll_rate;
+        }
+        if (!pitch_rate_ignore) {
+            ang_vel.y = packet.body_pitch_rate;
+        }
+        if (!yaw_rate_ignore) {
+            ang_vel.z = packet.body_yaw_rate;
+        }
 
-void GCS_MAVLINK_Copter::handle_message_set_position_target_local_ned(const mavlink_message_t &msg)
-{
+        copter.mode_guided.set_angle(attitude_quat, ang_vel,
+                climb_rate_or_thrust, use_thrust);
+
+        break;
+    }
+
+    case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:     // MAV ID: 84
+    {
         // decode packet
         mavlink_set_position_target_local_ned_t packet;
         mavlink_msg_set_position_target_local_ned_decode(&msg, &packet);
 
         // exit if vehicle is not in Guided mode or Auto-Guided mode
         if (!copter.flightmode->in_guided_mode()) {
-            return;
+            break;
         }
 
         // check for supported coordinate frames
@@ -1302,7 +1290,7 @@ void GCS_MAVLINK_Copter::handle_message_set_position_target_local_ned(const mavl
             packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) {
             // input is not valid so stop
             copter.mode_guided.init(true);
-            return;
+            break;
         }
 
         bool pos_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
@@ -1315,7 +1303,7 @@ void GCS_MAVLINK_Copter::handle_message_set_position_target_local_ned(const mavl
         // Force inputs are not supported
         // Do not accept command if force_set is true and acc_ignore is false
         if (force_set && !acc_ignore) {
-            return;
+            break;
         }
 
         // prepare position
@@ -1388,17 +1376,19 @@ void GCS_MAVLINK_Copter::handle_message_set_position_target_local_ned(const mavl
             // input is not valid so stop
             copter.mode_guided.init(true);
         }
-}
 
-void GCS_MAVLINK_Copter::handle_message_set_position_target_global_int(const mavlink_message_t &msg)
-{
+        break;
+    }
+
+    case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:    // MAV ID: 86
+    {
         // decode packet
         mavlink_set_position_target_global_int_t packet;
         mavlink_msg_set_position_target_global_int_decode(&msg, &packet);
 
         // exit if vehicle is not in Guided mode or Auto-Guided mode
         if (!copter.flightmode->in_guided_mode()) {
-            return;
+            break;
         }
 
         // todo: do we need to check for supported coordinate frames
@@ -1413,7 +1403,7 @@ void GCS_MAVLINK_Copter::handle_message_set_position_target_global_int(const mav
         // Force inputs are not supported
         // Do not accept command if force_set is true and acc_ignore is false
         if (force_set && !acc_ignore) {
-            return;
+            break;
         }
 
         // extract location from message
@@ -1423,14 +1413,14 @@ void GCS_MAVLINK_Copter::handle_message_set_position_target_global_int(const mav
             if (!check_latlng(packet.lat_int, packet.lon_int)) {
                 // input is not valid so stop
                 copter.mode_guided.init(true);
-                return;
+                break;
             }
             Location::AltFrame frame;
             if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)packet.coordinate_frame, frame)) {
                 // unknown coordinate frame
                 // input is not valid so stop
                 copter.mode_guided.init(true);
-                return;
+                break;
             }
             loc = {packet.lat_int, packet.lon_int, int32_t(packet.alt*100), frame};
         }
@@ -1471,13 +1461,13 @@ void GCS_MAVLINK_Copter::handle_message_set_position_target_global_int(const mav
                 // posvel controller does not support alt-above-terrain
                 // input is not valid so stop
                 copter.mode_guided.init(true);
-                return;
+                break;
             }
             Vector3f pos_neu_cm;
             if (!loc.get_vector_from_origin_NEU(pos_neu_cm)) {
                 // input is not valid so stop
                 copter.mode_guided.init(true);
-                return;
+                break;
             }
             copter.mode_guided.set_destination_posvel(pos_neu_cm, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds);
         } else if (pos_ignore && !vel_ignore) {
@@ -1490,43 +1480,33 @@ void GCS_MAVLINK_Copter::handle_message_set_position_target_global_int(const mav
             // input is not valid so stop
             copter.mode_guided.init(true);
         }
-}
-#endif  // MODE_GUIDED_ENABLED
 
-void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
-{
-
-    switch (msg.msgid) {
-#if MODE_GUIDED_ENABLED
-    case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
-        handle_message_set_attitude_target(msg);
-        break;
-    case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
-        handle_message_set_position_target_local_ned(msg);
-        break;
-    case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:
-        handle_message_set_position_target_global_int(msg);
         break;
+    }
 #endif
-#if AP_TERRAIN_AVAILABLE
+
     case MAVLINK_MSG_ID_TERRAIN_DATA:
     case MAVLINK_MSG_ID_TERRAIN_CHECK:
+#if AP_TERRAIN_AVAILABLE
         copter.terrain.handle_data(chan, msg);
-        break;
 #endif
-#if TOY_MODE_ENABLED
+        break;
+
+#if TOY_MODE_ENABLED == ENABLED
     case MAVLINK_MSG_ID_NAMED_VALUE_INT:
         copter.g2.toy_mode.handle_message(msg);
         break;
 #endif
+        
     default:
         GCS_MAVLINK::handle_message(msg);
         break;
-    }
-}
+    }     // end switch
+} // end handle mavlink
+
 
 MAV_RESULT GCS_MAVLINK_Copter::handle_flight_termination(const mavlink_command_int_t &packet) {
-#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
+#if ADVANCED_FAILSAFE == ENABLED
     if (GCS_MAVLINK::handle_flight_termination(packet) == MAV_RESULT_ACCEPTED) {
         return MAV_RESULT_ACCEPTED;
     }
@@ -1664,148 +1644,3 @@ uint8_t GCS_MAVLINK_Copter::high_latency_wind_direction() const
     return 0;
 }
 #endif // HAL_HIGH_LATENCY2_ENABLED
-
-// Send the mode with the given index (not mode number!) return the total number of modes
-// Index starts at 1
-uint8_t GCS_MAVLINK_Copter::send_available_mode(uint8_t index) const
-{
-    const Mode* modes[] {
-#if MODE_AUTO_ENABLED
-        &copter.mode_auto, // This auto is actually auto RTL!
-        &copter.mode_auto, // This one is really is auto!
-#endif
-#if MODE_ACRO_ENABLED
-        &copter.mode_acro,
-#endif
-        &copter.mode_stabilize,
-        &copter.mode_althold,
-#if MODE_CIRCLE_ENABLED
-        &copter.mode_circle,
-#endif
-#if MODE_LOITER_ENABLED
-        &copter.mode_loiter,
-#endif
-#if MODE_GUIDED_ENABLED
-        &copter.mode_guided,
-#endif
-        &copter.mode_land,
-#if MODE_RTL_ENABLED
-        &copter.mode_rtl,
-#endif
-#if MODE_DRIFT_ENABLED
-        &copter.mode_drift,
-#endif
-#if MODE_SPORT_ENABLED
-        &copter.mode_sport,
-#endif
-#if MODE_FLIP_ENABLED
-        &copter.mode_flip,
-#endif
-#if AUTOTUNE_ENABLED
-        &copter.mode_autotune,
-#endif
-#if MODE_POSHOLD_ENABLED
-        &copter.mode_poshold,
-#endif
-#if MODE_BRAKE_ENABLED
-        &copter.mode_brake,
-#endif
-#if MODE_THROW_ENABLED
-        &copter.mode_throw,
-#endif
-#if HAL_ADSB_ENABLED
-        &copter.mode_avoid_adsb,
-#endif
-#if MODE_GUIDED_NOGPS_ENABLED
-        &copter.mode_guided_nogps,
-#endif
-#if MODE_SMARTRTL_ENABLED
-        &copter.mode_smartrtl,
-#endif
-#if MODE_FLOWHOLD_ENABLED
-        (Mode*)copter.g2.mode_flowhold_ptr,
-#endif
-#if MODE_FOLLOW_ENABLED
-        &copter.mode_follow,
-#endif
-#if MODE_ZIGZAG_ENABLED
-        &copter.mode_zigzag,
-#endif
-#if MODE_SYSTEMID_ENABLED
-        (Mode *)copter.g2.mode_systemid_ptr,
-#endif
-#if MODE_AUTOROTATE_ENABLED
-        &copter.mode_autorotate,
-#endif
-#if MODE_TURTLE_ENABLED
-        &copter.mode_turtle,
-#endif
-    };
-
-    const uint8_t base_mode_count = ARRAY_SIZE(modes);
-    uint8_t mode_count = base_mode_count;
-
-#if AP_SCRIPTING_ENABLED
-    for (uint8_t i = 0; i < ARRAY_SIZE(copter.mode_guided_custom); i++) {
-        if (copter.mode_guided_custom[i] != nullptr) {
-            mode_count += 1;
-        }
-    }
-#endif
-
-    // Convert to zero indexed
-    const uint8_t index_zero = index - 1;
-    if (index_zero >= mode_count) {
-        // Mode does not exist!?
-        return mode_count;
-    }
-
-    // Ask the mode for its name and number
-    const char* name;
-    uint8_t mode_number;
-
-    if (index_zero < base_mode_count) {
-        name = modes[index_zero]->name();
-        mode_number = (uint8_t)modes[index_zero]->mode_number();
-
-    } else {
-#if AP_SCRIPTING_ENABLED
-        const uint8_t custom_index = index_zero - base_mode_count;
-        if (copter.mode_guided_custom[custom_index] == nullptr) {
-            // Invalid index, should not happen
-            return mode_count;
-        }
-        name = copter.mode_guided_custom[custom_index]->name();
-        mode_number = (uint8_t)copter.mode_guided_custom[custom_index]->mode_number();
-#else
-        // Should not endup here
-        return mode_count;
-#endif
-    }
-
-#if MODE_AUTO_ENABLED
-    // Auto RTL is odd
-    // Have to deal with is separately becase its number and name can change depending on if were in it or not
-    if (index_zero == 0) {
-        mode_number = (uint8_t)Mode::Number::AUTO_RTL;
-        name = "AUTO RTL";
-
-    } else if (index_zero == 1) {
-        mode_number = (uint8_t)Mode::Number::AUTO;
-        name = "AUTO";
-
-    }
-#endif
-
-    mavlink_msg_available_modes_send(
-        chan,
-        mode_count,
-        index,
-        MAV_STANDARD_MODE::MAV_STANDARD_MODE_NON_STANDARD,
-        mode_number,
-        0, // MAV_MODE_PROPERTY bitmask
-        name
-    );
-
-    return mode_count;
-}

ArduCopter/GCS_Mavlink.h

@@ -46,12 +46,10 @@ protected:
     void handle_mount_message(const mavlink_message_t &msg) override;
 #endif
 
-    void handle_message_set_attitude_target(const mavlink_message_t &msg);
-    void handle_message_set_position_target_global_int(const mavlink_message_t &msg);
-    void handle_message_set_position_target_local_ned(const mavlink_message_t &msg);
-
     void handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) override;
 
+    bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
+    bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED;
     void send_nav_controller_output() const override;
     uint64_t capabilities() const override;
 
@@ -64,10 +62,6 @@ protected:
     uint32_t log_radio_bit() const override { return MASK_LOG_PM; }
 #endif
 
-    // Send the mode with the given index (not mode number!) return the total number of modes
-    // Index starts at 1
-    uint8_t send_available_mode(uint8_t index) const override;
-
 private:
 
     // sanity check velocity or acceleration vector components are numbers

ArduCopter/Log.cpp

@@ -1,5 +1,4 @@
 #include "Copter.h"
-#include <AP_InertialSensor/AP_InertialSensor_rate_config.h>
 
 #if HAL_LOGGING_ENABLED
 
@@ -40,15 +39,11 @@ void Copter::Log_Write_Control_Tuning()
         target_climb_rate_cms = pos_control->get_vel_target_z_cms();
     }
 
+    // get surface tracking alts
     float desired_rangefinder_alt;
-#if AP_RANGEFINDER_ENABLED
     if (!surface_tracking.get_target_dist_for_logging(desired_rangefinder_alt)) {
         desired_rangefinder_alt = AP::logger().quiet_nan();
     }
-#else
-    // get surface tracking alts
-    desired_rangefinder_alt = AP::logger().quiet_nan();
-#endif
 
     struct log_Control_Tuning pkt = {
         LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),
@@ -61,11 +56,7 @@ void Copter::Log_Write_Control_Tuning()
         inav_alt            : inertial_nav.get_position_z_up_cm() * 0.01f,
         baro_alt            : baro_alt,
         desired_rangefinder_alt : desired_rangefinder_alt,
-#if AP_RANGEFINDER_ENABLED
         rangefinder_alt     : surface_tracking.get_dist_for_logging(),
-#else
-        rangefinder_alt     : AP::logger().quiet_nanf(),
-#endif
         terr_alt            : terr_alt,
         target_climb_rate   : target_climb_rate_cms,
         climb_rate          : int16_t(inertial_nav.get_velocity_z_up_cms()) // float -> int16_t
@@ -76,13 +67,11 @@ void Copter::Log_Write_Control_Tuning()
 // Write an attitude packet
 void Copter::Log_Write_Attitude()
 {
-    attitude_control->Write_ANG();
-}
-
-void Copter::Log_Write_Rate()
-{
-    attitude_control->Write_Rate(*pos_control);
-}
+    Vector3f targets = attitude_control->get_att_target_euler_cd();
+    targets.z = wrap_360_cd(targets.z);
+    ahrs.Write_Attitude(targets);
+    ahrs_view->Write_Rate(*motors, *attitude_control, *pos_control);
+ }
 
 // Write PIDS packets
 void Copter::Log_Write_PIDS()
@@ -261,7 +250,7 @@ struct PACKED log_SysIdD {
 // Write an rate packet
 void Copter::Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z)
 {
-#if MODE_SYSTEMID_ENABLED
+#if MODE_SYSTEMID_ENABLED == ENABLED
     struct log_SysIdD pkt_sidd = {
         LOG_PACKET_HEADER_INIT(LOG_SYSIDD_MSG),
         time_us         : AP_HAL::micros64(),
@@ -295,7 +284,7 @@ struct PACKED log_SysIdS {
 // Write an rate packet
 void Copter::Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out)
 {
-#if MODE_SYSTEMID_ENABLED
+#if MODE_SYSTEMID_ENABLED == ENABLED
     struct log_SysIdS pkt_sids = {
         LOG_PACKET_HEADER_INIT(LOG_SYSIDS_MSG),
         time_us             : AP_HAL::micros64(),
@@ -312,6 +301,31 @@ void Copter::Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitu
 #endif
 }
 
+#if FRAME_CONFIG == HELI_FRAME
+struct PACKED log_Heli {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    float    desired_rotor_speed;
+    float    main_rotor_speed;
+    float    governor_output;
+    float    control_output;
+};
+
+// Write an helicopter packet
+void Copter::Log_Write_Heli()
+{
+    struct log_Heli pkt_heli = {
+        LOG_PACKET_HEADER_INIT(LOG_HELI_MSG),
+        time_us                 : AP_HAL::micros64(),
+        desired_rotor_speed     : motors->get_desired_rotor_speed(),
+        main_rotor_speed        : motors->get_main_rotor_speed(),
+        governor_output         : motors->get_governor_output(),
+        control_output          : motors->get_control_output(),
+    };
+    logger.WriteBlock(&pkt_heli, sizeof(pkt_heli));
+}
+#endif
+
 // guided position target logging
 struct PACKED log_Guided_Position_Target {
     LOG_PACKET_HEADER;
@@ -344,16 +358,6 @@ struct PACKED log_Guided_Attitude_Target {
     float climb_rate;
 };
 
-// rate thread dt stats
-struct PACKED log_Rate_Thread_Dt {
-    LOG_PACKET_HEADER;
-    uint64_t time_us;
-    float dt;
-    float dtAvg;
-    float dtMax;
-    float dtMin;
-};
-
 // Write a Guided mode position target
 // pos_target is lat, lon, alt OR offset from ekf origin in cm
 // terrain should be 0 if pos_target.z is alt-above-ekf-origin, 1 if alt-above-terrain
@@ -401,21 +405,6 @@ void Copter::Log_Write_Guided_Attitude_Target(ModeGuided::SubMode target_type, f
     logger.WriteBlock(&pkt, sizeof(pkt));
 }
 
-void Copter::Log_Write_Rate_Thread_Dt(float dt, float dtAvg, float dtMax, float dtMin)
-{
-#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
-    const log_Rate_Thread_Dt pkt {
-        LOG_PACKET_HEADER_INIT(LOG_RATE_THREAD_DT_MSG),
-        time_us         : AP_HAL::micros64(),
-        dt              : dt,
-        dtAvg           : dtAvg,
-        dtMax           : dtMax,
-        dtMin           : dtMin
-    };
-    logger.WriteBlock(&pkt, sizeof(pkt));
-#endif
-}
-
 // type and unit information can be found in
 // libraries/AP_Logger/Logstructure.h; search for "log_Units" for
 // units and "Format characters" for field type information
@@ -492,6 +481,18 @@ const struct LogStructure Copter::log_structure[] = {
       "DU32",  "QBI",         "TimeUS,Id,Value", "s--", "F--" },
     { LOG_DATA_FLOAT_MSG, sizeof(log_Data_Float),         
       "DFLT",  "QBf",         "TimeUS,Id,Value", "s--", "F--" },
+    
+// @LoggerMessage: HELI
+// @Description: Helicopter related messages 
+// @Field: TimeUS: Time since system startup
+// @Field: DRRPM: Desired rotor speed
+// @Field: ERRPM: Estimated rotor speed
+// @Field: Gov: Governor Output
+// @Field: Throt: Throttle output
+#if FRAME_CONFIG == HELI_FRAME
+    { LOG_HELI_MSG, sizeof(log_Heli),
+      "HELI",  "Qffff",        "TimeUS,DRRPM,ERRPM,Gov,Throt", "s----", "F----" , true },
+#endif
 
 // @LoggerMessage: SIDD
 // @Description: System ID data
@@ -557,18 +558,6 @@ const struct LogStructure Copter::log_structure[] = {
 
     { LOG_GUIDED_ATTITUDE_TARGET_MSG, sizeof(log_Guided_Attitude_Target),
       "GUIA",  "QBffffffff",    "TimeUS,Type,Roll,Pitch,Yaw,RollRt,PitchRt,YawRt,Thrust,ClimbRt", "s-dddkkk-n", "F-000000-0" , true },
-
-// @LoggerMessage: RTDT
-// @Description: Attitude controller time deltas
-// @Field: TimeUS: Time since system startup
-// @Field: dt: current time delta
-// @Field: dtAvg: current time delta average
-// @Field: dtMax: Max time delta since last log output
-// @Field: dtMin: Min time delta since last log output
-
-    { LOG_RATE_THREAD_DT_MSG, sizeof(log_Rate_Thread_Dt),
-      "RTDT", "Qffff", "TimeUS,dt,dtAvg,dtMax,dtMin", "sssss", "F----" , true },
-
 };
 
 uint8_t Copter::get_num_log_structures() const

ArduCopter/Parameters.h

@@ -3,13 +3,13 @@
 #define AP_PARAM_VEHICLE_NAME copter
 
 #include <AP_Common/AP_Common.h>
-#include "RC_Channel_Copter.h"
+#include "RC_Channel.h"
 #include <AP_Proximity/AP_Proximity.h>
 
-#if MODE_FOLLOW_ENABLED
+#if MODE_FOLLOW_ENABLED == ENABLED
  # include <AP_Follow/AP_Follow.h>
 #endif
-#if WEATHERVANE_ENABLED
+#if WEATHERVANE_ENABLED == ENABLED
  #include <AC_AttitudeControl/AC_WeatherVane.h>
 #endif
 
@@ -214,7 +214,7 @@ public:
         k_param_gcs2,
         k_param_serial2_baud_old, // deprecated
         k_param_serial2_protocol, // deprecated
-        k_param_serial_manager_old,
+        k_param_serial_manager,
         k_param_ch9_option_old,
         k_param_ch10_option_old,
         k_param_ch11_option_old,
@@ -398,7 +398,7 @@ public:
     AP_Int16        throttle_behavior;
     AP_Float        pilot_takeoff_alt;
 
-#if MODE_RTL_ENABLED
+#if MODE_RTL_ENABLED == ENABLED
     AP_Int32        rtl_altitude;
     AP_Int16        rtl_speed_cms;
     AP_Float        rtl_cone_slope;
@@ -415,7 +415,7 @@ public:
 
     AP_Int8         wp_yaw_behavior;            // controls how the autopilot controls yaw during missions
 
-#if MODE_POSHOLD_ENABLED
+#if MODE_POSHOLD_ENABLED == ENABLED
     AP_Int16        poshold_brake_rate;         // PosHold flight mode's rotation rate during braking in deg/sec
     AP_Int16        poshold_brake_angle_max;    // PosHold flight mode's max lean angle during braking in centi-degrees
 #endif
@@ -459,7 +459,7 @@ public:
     AP_Float        fs_ekf_thresh;
     AP_Int16        gcs_pid_mask;
 
-#if MODE_THROW_ENABLED
+#if MODE_THROW_ENABLED == ENABLED
     AP_Enum<ModeThrow::PreThrowMotorState>         throw_motor_start;
     AP_Int16         throw_altitude_min; // minimum altitude in m above which a throw can be detected
     AP_Int16         throw_altitude_max; // maximum altitude in m below which a throw can be detected
@@ -467,13 +467,13 @@ public:
 
     AP_Int16                rc_speed; // speed of fast RC Channels in Hz
 
-#if MODE_ACRO_ENABLED || MODE_SPORT_ENABLED
+#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
     // Acro parameters
     AP_Float                acro_balance_roll;
     AP_Float                acro_balance_pitch;
 #endif
 
-#if MODE_ACRO_ENABLED
+#if MODE_ACRO_ENABLED == ENABLED
     // Acro parameters
     AP_Int8                 acro_trainer;
 #endif
@@ -499,17 +499,12 @@ public:
     // altitude at which nav control can start in takeoff
     AP_Float wp_navalt_min;
 
-    // unused_integer simply exists so that the constructor for
-    // ParametersG2 can be created with a relatively easy syntax in
-    // the face of many #ifs:
-    uint8_t unused_integer;
-
     // button checking
 #if HAL_BUTTON_ENABLED
     AP_Button *button_ptr;
 #endif
 
-#if MODE_THROW_ENABLED
+#if MODE_THROW_ENABLED == ENABLED
     // Throw mode parameters
     AP_Int8 throw_nextmode;
     AP_Enum<ModeThrow::ThrowType> throw_type;
@@ -535,8 +530,8 @@ public:
 
     // whether to enforce acceptance of packets only from sysid_my_gcs
     AP_Int8 sysid_enforce;
-
-#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
+    
+#if ADVANCED_FAILSAFE == ENABLED
     // advanced failsafe library
     AP_AdvancedFailsafe_Copter afs;
 #endif
@@ -544,7 +539,7 @@ public:
     // developer options
     AP_Int32 dev_options;
 
-#if MODE_ACRO_ENABLED
+#if MODE_ACRO_ENABLED == ENABLED
     AP_Float acro_thr_mid;
 #endif
 
@@ -557,7 +552,7 @@ public:
     // control over servo output ranges
     SRV_Channels servo_channels;
 
-#if MODE_SMARTRTL_ENABLED
+#if MODE_SMARTRTL_ENABLED == ENABLED
     // Safe RTL library
     AP_SmartRTL smart_rtl;
 #endif
@@ -573,7 +568,7 @@ public:
     // Land alt final stage
     AP_Int16 land_alt_low;
 
-#if TOY_MODE_ENABLED
+#if TOY_MODE_ENABLED == ENABLED
     ToyMode toy_mode;
 #endif
 
@@ -582,17 +577,17 @@ public:
     void *mode_flowhold_ptr;
 #endif
 
-#if MODE_FOLLOW_ENABLED
+#if MODE_FOLLOW_ENABLED == ENABLED
     // follow
     AP_Follow follow;
 #endif
 
-#if USER_PARAMS_ENABLED
+#if USER_PARAMS_ENABLED == ENABLED
     // User custom parameters
     UserParameters user_parameters;
 #endif
 
-#if AUTOTUNE_ENABLED
+#if AUTOTUNE_ENABLED == ENABLED
     // we need a pointer to autotune for the G2 table
     void *autotune_ptr;
 #endif
@@ -600,12 +595,12 @@ public:
     AP_Float tuning_min;
     AP_Float tuning_max;
 
-#if AP_OAPATHPLANNER_ENABLED
+#if AC_OAPATHPLANNER_ENABLED == ENABLED
     // object avoidance path planning
     AP_OAPathPlanner oa;
 #endif
 
-#if MODE_SYSTEMID_ENABLED
+#if MODE_SYSTEMID_ENABLED == ENABLED
     // we need a pointer to the mode for the G2 table
     void *mode_systemid_ptr;
 #endif
@@ -616,52 +611,52 @@ public:
     // Failsafe options bitmask #36
     AP_Int32 fs_options;
 
-#if MODE_AUTOROTATE_ENABLED
+#if MODE_AUTOROTATE_ENABLED == ENABLED
     // Autonmous autorotation
     AC_Autorotation arot;
 #endif
 
-#if MODE_ZIGZAG_ENABLED
+#if MODE_ZIGZAG_ENABLED == ENABLED
     // we need a pointer to the mode for the G2 table
     void *mode_zigzag_ptr;
 #endif
 
     // command model parameters
-#if MODE_ACRO_ENABLED || MODE_SPORT_ENABLED
+#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
     AC_CommandModel command_model_acro_rp;
 #endif
 
-#if MODE_ACRO_ENABLED || MODE_DRIFT_ENABLED
+#if MODE_ACRO_ENABLED == ENABLED || MODE_DRIFT_ENABLED == ENABLED
     AC_CommandModel command_model_acro_y;
 #endif
 
     AC_CommandModel command_model_pilot;
 
-#if MODE_ACRO_ENABLED
+#if MODE_ACRO_ENABLED == ENABLED
     AP_Int8 acro_options;
 #endif
 
-#if MODE_AUTO_ENABLED
+#if MODE_AUTO_ENABLED == ENABLED
     AP_Int32 auto_options;
 #endif
 
-#if MODE_GUIDED_ENABLED
+#if MODE_GUIDED_ENABLED == ENABLED
     AP_Int32 guided_options;
 #endif
 
     AP_Float fs_gcs_timeout;
 
-#if MODE_RTL_ENABLED
+#if MODE_RTL_ENABLED == ENABLED
     AP_Int32 rtl_options;
 #endif
 
     AP_Int32 flight_options;
 
-#if AP_RANGEFINDER_ENABLED
+#if RANGEFINDER_ENABLED == ENABLED
     AP_Float rangefinder_filt;
 #endif
 
-#if MODE_GUIDED_ENABLED
+#if MODE_GUIDED_ENABLED == ENABLED
     AP_Float guided_timeout;
 #endif
 
@@ -681,18 +676,15 @@ public:
     // EKF variance filter cutoff
     AP_Float fs_ekf_filt_hz;
 
-#if WEATHERVANE_ENABLED
+#if WEATHERVANE_ENABLED == ENABLED
     AC_WeatherVane weathervane;
 #endif
 
     // payload place parameters
     AP_Float pldp_thrust_placed_fraction;
-    AP_Float pldp_range_finder_maximum_m;
+    AP_Float pldp_range_finder_minimum_m;
     AP_Float pldp_delay_s;
     AP_Float pldp_descent_speed_ms;
-
-    AP_Int8 att_enable;
-    AP_Int8 att_decimation;
 };
 
 extern const AP_Param::Info        var_info[];

ArduCopter/RC_Channel.cpp

@@ -1,6 +1,6 @@
 #include "Copter.h"
 
-#include "RC_Channel_Copter.h"
+#include "RC_Channel.h"
 
 
 // defining these two macros and including the RC_Channels_VarInfo header defines the parameter information common to all vehicle types
@@ -76,8 +76,7 @@ void RC_Channel_Copter::init_aux_function(const AUX_FUNC ch_option, const AuxSwi
     // the following functions do not need to be initialised:
     case AUX_FUNC::ALTHOLD:
     case AUX_FUNC::AUTO:
-    case AUX_FUNC::AUTOTUNE_MODE:
-    case AUX_FUNC::AUTOTUNE_TEST_GAINS:
+    case AUX_FUNC::AUTOTUNE:
     case AUX_FUNC::BRAKE:
     case AUX_FUNC::CIRCLE:
     case AUX_FUNC::DRIFT:
@@ -87,9 +86,7 @@ void RC_Channel_Copter::init_aux_function(const AUX_FUNC ch_option, const AuxSwi
     case AUX_FUNC::GUIDED:
     case AUX_FUNC::LAND:
     case AUX_FUNC::LOITER:
-#if HAL_PARACHUTE_ENABLED
     case AUX_FUNC::PARACHUTE_RELEASE:
-#endif
     case AUX_FUNC::POSHOLD:
     case AUX_FUNC::RESETTOARMEDYAW:
     case AUX_FUNC::RTL:
@@ -101,9 +98,7 @@ void RC_Channel_Copter::init_aux_function(const AUX_FUNC ch_option, const AuxSwi
     case AUX_FUNC::USER_FUNC1:
     case AUX_FUNC::USER_FUNC2:
     case AUX_FUNC::USER_FUNC3:
-#if AP_WINCH_ENABLED
     case AUX_FUNC::WINCH_CONTROL:
-#endif
     case AUX_FUNC::ZIGZAG:
     case AUX_FUNC::ZIGZAG_Auto:
     case AUX_FUNC::ZIGZAG_SaveWP:
@@ -113,34 +108,26 @@ void RC_Channel_Copter::init_aux_function(const AUX_FUNC ch_option, const AuxSwi
     case AUX_FUNC::SIMPLE_HEADING_RESET:
     case AUX_FUNC::ARMDISARM_AIRMODE:
     case AUX_FUNC::TURBINE_START:
-    case AUX_FUNC::FLIGHTMODE_PAUSE:
         break;
     case AUX_FUNC::ACRO_TRAINER:
     case AUX_FUNC::ATTCON_ACCEL_LIM:
     case AUX_FUNC::ATTCON_FEEDFWD:
     case AUX_FUNC::INVERTED:
     case AUX_FUNC::MOTOR_INTERLOCK:
-#if HAL_PARACHUTE_ENABLED
     case AUX_FUNC::PARACHUTE_3POS:      // we trust the vehicle will be disarmed so even if switch is in release position the chute will not release
     case AUX_FUNC::PARACHUTE_ENABLE:
-#endif
     case AUX_FUNC::PRECISION_LOITER:
-#if AP_RANGEFINDER_ENABLED
     case AUX_FUNC::RANGEFINDER:
-#endif
     case AUX_FUNC::SIMPLE_MODE:
     case AUX_FUNC::STANDBY:
     case AUX_FUNC::SUPERSIMPLE_MODE:
     case AUX_FUNC::SURFACE_TRACKING:
-#if AP_WINCH_ENABLED
     case AUX_FUNC::WINCH_ENABLE:
-#endif
     case AUX_FUNC::AIRMODE:
     case AUX_FUNC::FORCEFLYING:
     case AUX_FUNC::CUSTOM_CONTROLLER:
     case AUX_FUNC::WEATHER_VANE_ENABLE:
-    case AUX_FUNC::TRANSMITTER_TUNING:
-        run_aux_function(ch_option, ch_flag, AuxFuncTrigger::Source::INIT, ch_in);
+        run_aux_function(ch_option, ch_flag, AuxFuncTriggerSource::INIT);
         break;
     default:
         RC_Channel::init_aux_function(ch_option, ch_flag);
@@ -156,7 +143,7 @@ void RC_Channel_Copter::do_aux_function_change_mode(const Mode::Number mode,
     switch(ch_flag) {
     case AuxSwitchPos::HIGH: {
         // engage mode (if not possible we remain in current flight mode)
-        copter.set_mode(mode, ModeReason::AUX_FUNCTION);
+        copter.set_mode(mode, ModeReason::RC_COMMAND);
         break;
     }
     default:
@@ -169,16 +156,13 @@ void RC_Channel_Copter::do_aux_function_change_mode(const Mode::Number mode,
 }
 
 // do_aux_function - implement the function invoked by auxiliary switches
-bool RC_Channel_Copter::do_aux_function(const AuxFuncTrigger &trigger)
+bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos ch_flag)
 {
-    const AUX_FUNC &ch_option = trigger.func;
-    const AuxSwitchPos &ch_flag = trigger.pos;
-
     switch(ch_option) {
         case AUX_FUNC::FLIP:
             // flip if switch is on, positive throttle and we're actually flying
             if (ch_flag == AuxSwitchPos::HIGH) {
-                copter.set_mode(Mode::Number::FLIP, ModeReason::AUX_FUNCTION);
+                copter.set_mode(Mode::Number::FLIP, ModeReason::RC_COMMAND);
             }
             break;
 
@@ -203,11 +187,11 @@ bool RC_Channel_Copter::do_aux_function(const AuxFuncTrigger &trigger)
             break;
         }
 
-#if MODE_RTL_ENABLED
         case AUX_FUNC::RTL:
+#if MODE_RTL_ENABLED == ENABLED
             do_aux_function_change_mode(Mode::Number::RTL, ch_flag);
-            break;
 #endif
+            break;
 
         case AUX_FUNC::SAVE_TRIM:
             if ((ch_flag == AuxSwitchPos::HIGH) &&
@@ -217,8 +201,8 @@ bool RC_Channel_Copter::do_aux_function(const AuxFuncTrigger &trigger)
             }
             break;
 
-#if MODE_AUTO_ENABLED
         case AUX_FUNC::SAVE_WP:
+#if MODE_AUTO_ENABLED == ENABLED
             // save waypoint when switch is brought high
             if (ch_flag == RC_Channel::AuxSwitchPos::HIGH) {
 
@@ -266,27 +250,29 @@ bool RC_Channel_Copter::do_aux_function(const AuxFuncTrigger &trigger)
                     LOGGER_WRITE_EVENT(LogEvent::SAVEWP_ADD_WP);
                 }
             }
+#endif
             break;
 
         case AUX_FUNC::AUTO:
+#if MODE_AUTO_ENABLED == ENABLED
             do_aux_function_change_mode(Mode::Number::AUTO, ch_flag);
-            break;
 #endif
+            break;
 
-#if AP_RANGEFINDER_ENABLED
         case AUX_FUNC::RANGEFINDER:
             // enable or disable the rangefinder
+#if RANGEFINDER_ENABLED == ENABLED
             if ((ch_flag == AuxSwitchPos::HIGH) &&
                 copter.rangefinder.has_orientation(ROTATION_PITCH_270)) {
                 copter.rangefinder_state.enabled = true;
             } else {
                 copter.rangefinder_state.enabled = false;
             }
+#endif
             break;
-#endif // AP_RANGEFINDER_ENABLED
 
-#if MODE_ACRO_ENABLED
         case AUX_FUNC::ACRO_TRAINER:
+#if MODE_ACRO_ENABLED == ENABLED
             switch(ch_flag) {
                 case AuxSwitchPos::LOW:
                     copter.g.acro_trainer.set((uint8_t)ModeAcro::Trainer::OFF);
@@ -301,17 +287,14 @@ bool RC_Channel_Copter::do_aux_function(const AuxFuncTrigger &trigger)
                     LOGGER_WRITE_EVENT(LogEvent::ACRO_TRAINER_LIMITED);
                     break;
             }
-            break;
 #endif
+            break;
 
-#if AUTOTUNE_ENABLED
-        case AUX_FUNC::AUTOTUNE_MODE:
+        case AUX_FUNC::AUTOTUNE:
+#if AUTOTUNE_ENABLED == ENABLED
             do_aux_function_change_mode(Mode::Number::AUTOTUNE, ch_flag);
-            break;
-        case AUX_FUNC::AUTOTUNE_TEST_GAINS:
-            copter.mode_autotune.autotune.do_aux_function(ch_flag);
-            break;
 #endif
+            break;
 
         case AUX_FUNC::LAND:
             do_aux_function_change_mode(Mode::Number::LAND, ch_flag);
@@ -329,19 +312,23 @@ bool RC_Channel_Copter::do_aux_function(const AuxFuncTrigger &trigger)
             do_aux_function_change_mode(Mode::Number::FOLLOW, ch_flag);
             break;
 
-#if HAL_PARACHUTE_ENABLED
         case AUX_FUNC::PARACHUTE_ENABLE:
+#if PARACHUTE == ENABLED
             // Parachute enable/disable
             copter.parachute.enabled(ch_flag == AuxSwitchPos::HIGH);
+#endif
             break;
 
         case AUX_FUNC::PARACHUTE_RELEASE:
+#if PARACHUTE == ENABLED
             if (ch_flag == AuxSwitchPos::HIGH) {
                 copter.parachute_manual_release();
             }
+#endif
             break;
 
         case AUX_FUNC::PARACHUTE_3POS:
+#if PARACHUTE == ENABLED
             // Parachute disable, enable, release with 3 position switch
             switch (ch_flag) {
                 case AuxSwitchPos::LOW:
@@ -355,8 +342,8 @@ bool RC_Channel_Copter::do_aux_function(const AuxFuncTrigger &trigger)
                     copter.parachute_manual_release();
                     break;
             }
+#endif
             break;
-#endif  // HAL_PARACHUTE_ENABLED
 
         case AUX_FUNC::ATTCON_FEEDFWD:
             // enable or disable feed forward
@@ -379,9 +366,9 @@ bool RC_Channel_Copter::do_aux_function(const AuxFuncTrigger &trigger)
             copter.ap.motor_interlock_switch = (ch_flag == AuxSwitchPos::HIGH || ch_flag == AuxSwitchPos::MIDDLE);
 #endif
             break;
-
-#if FRAME_CONFIG == HELI_FRAME
+			
         case AUX_FUNC::TURBINE_START:
+#if FRAME_CONFIG == HELI_FRAME     
            switch (ch_flag) {
                 case AuxSwitchPos::HIGH:
                     copter.motors->set_turb_start(true);
@@ -393,23 +380,23 @@ bool RC_Channel_Copter::do_aux_function(const AuxFuncTrigger &trigger)
                     copter.motors->set_turb_start(false);
                     break;
            }
+#endif
            break;
-#endif
-
-#if MODE_BRAKE_ENABLED
+		 
         case AUX_FUNC::BRAKE:
+#if MODE_BRAKE_ENABLED == ENABLED
             do_aux_function_change_mode(Mode::Number::BRAKE, ch_flag);
-            break;
 #endif
+            break;
 
-#if MODE_THROW_ENABLED
         case AUX_FUNC::THROW:
+#if MODE_THROW_ENABLED == ENABLED
             do_aux_function_change_mode(Mode::Number::THROW, ch_flag);
-            break;
 #endif
+            break;
 
-#if AC_PRECLAND_ENABLED && MODE_LOITER_ENABLED
         case AUX_FUNC::PRECISION_LOITER:
+#if AC_PRECLAND_ENABLED && MODE_LOITER_ENABLED == ENABLED
             switch (ch_flag) {
                 case AuxSwitchPos::HIGH:
                     copter.mode_loiter.set_precision_loiter_enabled(true);
@@ -421,37 +408,37 @@ bool RC_Channel_Copter::do_aux_function(const AuxFuncTrigger &trigger)
                     copter.mode_loiter.set_precision_loiter_enabled(false);
                     break;
             }
-            break;
 #endif
+            break;
 
-#if MODE_SMARTRTL_ENABLED
         case AUX_FUNC::SMART_RTL:
+#if MODE_SMARTRTL_ENABLED == ENABLED
             do_aux_function_change_mode(Mode::Number::SMART_RTL, ch_flag);
-            break;
 #endif
+            break;
 
-#if FRAME_CONFIG == HELI_FRAME
         case AUX_FUNC::INVERTED:
+#if FRAME_CONFIG == HELI_FRAME
             switch (ch_flag) {
             case AuxSwitchPos::HIGH:
-                if (copter.flightmode->allows_inverted()) {
-                    copter.attitude_control->set_inverted_flight(true);
-                } else {
-                    gcs().send_text(MAV_SEVERITY_WARNING, "Inverted flight not available in %s mode", copter.flightmode->name());
-                }
+                copter.motors->set_inverted_flight(true);
+                copter.attitude_control->set_inverted_flight(true);
+                copter.heli_flags.inverted_flight = true;
                 break;
             case AuxSwitchPos::MIDDLE:
                 // nothing
                 break;
             case AuxSwitchPos::LOW:
+                copter.motors->set_inverted_flight(false);
                 copter.attitude_control->set_inverted_flight(false);
+                copter.heli_flags.inverted_flight = false;
                 break;
             }
-            break;
 #endif
+            break;
 
-#if AP_WINCH_ENABLED
         case AUX_FUNC::WINCH_ENABLE:
+#if AP_WINCH_ENABLED
             switch (ch_flag) {
                 case AuxSwitchPos::HIGH:
                     // high switch position stops winch using rate control
@@ -463,12 +450,12 @@ bool RC_Channel_Copter::do_aux_function(const AuxFuncTrigger &trigger)
                     copter.g2.winch.relax();
                     break;
                 }
+#endif
             break;
 
         case AUX_FUNC::WINCH_CONTROL:
             // do nothing, used to control the rate of the winch and is processed within AP_Winch
             break;
-#endif  // AP_WINCH_ENABLED
 
 #ifdef USERHOOK_AUXSWITCH
         case AUX_FUNC::USER_FUNC1:
@@ -484,12 +471,14 @@ bool RC_Channel_Copter::do_aux_function(const AuxFuncTrigger &trigger)
             break;
 #endif
 
-#if MODE_ZIGZAG_ENABLED
         case AUX_FUNC::ZIGZAG:
+#if MODE_ZIGZAG_ENABLED == ENABLED
             do_aux_function_change_mode(Mode::Number::ZIGZAG, ch_flag);
+#endif
             break;
 
         case AUX_FUNC::ZIGZAG_SaveWP:
+#if MODE_ZIGZAG_ENABLED == ENABLED
             if (copter.flightmode == &copter.mode_zigzag) {
                 // initialize zigzag auto
                 copter.mode_zigzag.init_auto();
@@ -505,46 +494,47 @@ bool RC_Channel_Copter::do_aux_function(const AuxFuncTrigger &trigger)
                         break;
                 }
             }
-            break;
 #endif
+            break;
 
         case AUX_FUNC::STABILIZE:
             do_aux_function_change_mode(Mode::Number::STABILIZE, ch_flag);
             break;
 
-#if MODE_POSHOLD_ENABLED
         case AUX_FUNC::POSHOLD:
+#if MODE_POSHOLD_ENABLED == ENABLED
             do_aux_function_change_mode(Mode::Number::POSHOLD, ch_flag);
-            break;
 #endif
+            break;
 
         case AUX_FUNC::ALTHOLD:
             do_aux_function_change_mode(Mode::Number::ALT_HOLD, ch_flag);
             break;
 
-#if MODE_ACRO_ENABLED
+
         case AUX_FUNC::ACRO:
+#if MODE_ACRO_ENABLED == ENABLED
             do_aux_function_change_mode(Mode::Number::ACRO, ch_flag);
-            break;
 #endif
+            break;
 
-#if MODE_FLOWHOLD_ENABLED
         case AUX_FUNC::FLOWHOLD:
+#if MODE_FLOWHOLD_ENABLED
             do_aux_function_change_mode(Mode::Number::FLOWHOLD, ch_flag);
-            break;
 #endif
+            break;
 
-#if MODE_CIRCLE_ENABLED
         case AUX_FUNC::CIRCLE:
+#if MODE_CIRCLE_ENABLED == ENABLED
             do_aux_function_change_mode(Mode::Number::CIRCLE, ch_flag);
-            break;
 #endif
+            break;
 
-#if MODE_DRIFT_ENABLED
         case AUX_FUNC::DRIFT:
+#if MODE_DRIFT_ENABLED == ENABLED
             do_aux_function_change_mode(Mode::Number::DRIFT, ch_flag);
-            break;
 #endif
+            break;
 
         case AUX_FUNC::STANDBY: {
             switch (ch_flag) {
@@ -562,7 +552,6 @@ bool RC_Channel_Copter::do_aux_function(const AuxFuncTrigger &trigger)
             break;
         }
 
-#if AP_RANGEFINDER_ENABLED
         case AUX_FUNC::SURFACE_TRACKING:
             switch (ch_flag) {
             case AuxSwitchPos::LOW:
@@ -576,25 +565,9 @@ bool RC_Channel_Copter::do_aux_function(const AuxFuncTrigger &trigger)
                 break;
             }
             break;
-#endif
 
-        case AUX_FUNC::FLIGHTMODE_PAUSE:
-            switch (ch_flag) {
-                case AuxSwitchPos::HIGH:
-                    if (!copter.flightmode->pause()) {
-                        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Flight Mode Pause failed");
-                    }
-                    break;
-                case AuxSwitchPos::MIDDLE:
-                    break;
-                case AuxSwitchPos::LOW:
-                    copter.flightmode->resume();
-                    break;
-            }
-            break;
-
-#if MODE_ZIGZAG_ENABLED
         case AUX_FUNC::ZIGZAG_Auto:
+#if MODE_ZIGZAG_ENABLED == ENABLED
             if (copter.flightmode == &copter.mode_zigzag) {
                 switch (ch_flag) {
                 case AuxSwitchPos::HIGH:
@@ -605,12 +578,12 @@ bool RC_Channel_Copter::do_aux_function(const AuxFuncTrigger &trigger)
                     break;
                 }
             }
-            break;
 #endif
+            break;
 
         case AUX_FUNC::AIRMODE:
             do_aux_function_change_air_mode(ch_flag);
-#if MODE_ACRO_ENABLED && FRAME_CONFIG != HELI_FRAME
+#if MODE_ACRO_ENABLED == ENABLED && FRAME_CONFIG != HELI_FRAME
             copter.mode_acro.air_mode_aux_changed();
 #endif
             break;
@@ -619,17 +592,17 @@ bool RC_Channel_Copter::do_aux_function(const AuxFuncTrigger &trigger)
             do_aux_function_change_force_flying(ch_flag);
             break;
 
-#if MODE_AUTO_ENABLED
         case AUX_FUNC::AUTO_RTL:
+#if MODE_AUTO_ENABLED == ENABLED
             do_aux_function_change_mode(Mode::Number::AUTO_RTL, ch_flag);
-            break;
 #endif
+            break;
 
-#if MODE_TURTLE_ENABLED
         case AUX_FUNC::TURTLE:
+#if MODE_TURTLE_ENABLED == ENABLED
             do_aux_function_change_mode(Mode::Number::TURTLE, ch_flag);
-            break;
 #endif
+            break;
 
         case AUX_FUNC::SIMPLE_HEADING_RESET:
             if (ch_flag == AuxSwitchPos::HIGH) {
@@ -645,13 +618,13 @@ bool RC_Channel_Copter::do_aux_function(const AuxFuncTrigger &trigger)
             }
             break;
 
-#if AC_CUSTOMCONTROL_MULTI_ENABLED
+#if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED
         case AUX_FUNC::CUSTOM_CONTROLLER:
             copter.custom_control.set_custom_controller(ch_flag == AuxSwitchPos::HIGH);
             break;
 #endif
 
-#if WEATHERVANE_ENABLED
+#if WEATHERVANE_ENABLED == ENABLED
     case AUX_FUNC::WEATHER_VANE_ENABLE: {
         switch (ch_flag) {
             case AuxSwitchPos::HIGH:
@@ -666,12 +639,9 @@ bool RC_Channel_Copter::do_aux_function(const AuxFuncTrigger &trigger)
         break;
     }
 #endif
-    case AUX_FUNC::TRANSMITTER_TUNING:
-        // do nothing, used in tuning.cpp for transmitter based tuning
-        break;
 
     default:
-        return RC_Channel::do_aux_function(trigger);
+        return RC_Channel::do_aux_function(ch_option, ch_flag);
     }
     return true;
 }
@@ -710,8 +680,8 @@ void RC_Channel_Copter::do_aux_function_change_force_flying(const AuxSwitchPos c
 void Copter::save_trim()
 {
     // save roll and pitch trim
-    float roll_trim = ToRad((float)channel_roll->get_control_in()*0.01f);
-    float pitch_trim = ToRad((float)channel_pitch->get_control_in()*0.01f);
+    float roll_trim = ToRad((float)channel_roll->get_control_in()/100.0f);
+    float pitch_trim = ToRad((float)channel_pitch->get_control_in()/100.0f);
     ahrs.add_trim(roll_trim, pitch_trim);
     LOGGER_WRITE_EVENT(LogEvent::SAVE_TRIM);
     gcs().send_text(MAV_SEVERITY_INFO, "Trim saved");

ArduCopter/RC_Channel.h

@@ -12,7 +12,7 @@ public:
 protected:
 
     void init_aux_function(AUX_FUNC ch_option, AuxSwitchPos) override;
-    bool do_aux_function(const AuxFuncTrigger &trigger) override;
+    bool do_aux_function(AUX_FUNC ch_option, AuxSwitchPos) override;
 
 private:
 

ArduCopter/ReleaseNotes.txt

@@ -1,754 +1,6 @@
 ArduPilot Copter Release Notes:
-------------------------------------------------------------------
-Release 4.6.0-beta2 11 Dec 2024
+-------------------------------
 
-Changes from 4.6.0-beta1
-
-1) Board specfic changes
-
-- FoxeerF405v2 supports BMP280 baro
-- KakuteH7, H7-Mini, H7-Wing, F4 support SPA06 baro
-- MUPilot support
-- SkySakura H743 support
-- TBS Lucid H7 support
-- VUAV-V7pro README documentation fixed
-- X-MAV AP-H743v2 CAN pin definition fixed
-
-2) Copter specific enhancements and bug fixes
-
-- AutoTune fix for calc of maximum angular acceleration
-- Advanced Failsafe customer build server option
-
-3) Plane related enhancements and bug fixes
-
-- QuadPlane fix for QLand getting stuck in pilot repositioning
-- QuikTune C++ conversion (allow running quiktun on F4 and f7 boards)
-- Takeoff direction fixed when no yaw source
-- TECS correctly handles home altitude changes
-
-4) Bug Fixes and minor enhancements
-
-- AIRSPEED_AUTOCAL mavlink message only sent when required and fixed for 2nd sensor
-- CAN frame logging added to ease support
-- CRSF reconnection after failsafe fixed
-- EKF3 position and velocity resets default to user defined source
-- Ethernet IP address default 192.168.144.x
-- Fence autoenable fix when both RCn_OPTION=11/Fence and FENCE_AUTOENABLE = 3 (AutoEnableOnlyWhenArmed)
-- Fence pre-arm check that vehicle is within polygon fence
-- Fence handling of more than 256 items fixed
-- FFT protection against divide-by-zero in Jain estimator
-- Frsky telemetry apparent wind speed fixed
-- Inertial sensors stop sensor converging if motors arm
-- Inertial sensors check for changes to notch filters fixed
-- Real Time Clock allowed to shift forward when disarmed
-- ROS2/DDS get/set parameter service added
-- Scripting gets memory handling improvements
-- Scripting promote video-stream-information to applet
-- Topotek gimbal driver uses GIA message to retrieve current angle
-- Tramp VTX OSD power indicator fixed
-------------------------------------------------------------------
-Release 4.6.0-beta1 13 Nov 2024
-
-Changes from 4.5.7
-
-1) Board specific changes
-
-- AnyLeaf H7 supports compass and onboard logging
-- Blitz743Pro supports CAN
-- BlueRobotics Navigator supports BMP390 baro
-- Bootloader ECC failure check fixed on boards with >128k bootloader space (e.g CubeRed)
-- CB Unmanned Stamp H743 support
-- ClearSky CSKY405 support
-- CUAV-7-Nano default batt monitor fixed
-- CubeRed bootloader fixes including disabling 2nd core by default
-- CubeRed supports PPP networking between primary and secondary MCU
-- CubeRedPrimary supports external compasses
-- ESP32 main loop rate improvements
-- ESP32 RC input fixes and wifi connection reliability improved
-- ESP32 safety switch and GPIO pin support
-- FlyingMoon no longer support MAX7456
-- Flywoo F405HD-AIOv2 ELRS RX pin pulled high during boot
-- Flywoo H743 Pro support
-- Flywoo/Goku F405 HD 1-2S ELRS AIO v2
-- FlywooF745 supports DPS310 baro
-- FPV boards lose SMBus battery support (to save flash)
-- GEPRC F745BTHD support
-- GEPRCF745BTHD loses parachute support, non-BMP280 baros (to save flash)
-- Here4FC bootloader fix for mismatch between RAM0 and periph that could prevent firmware updates
-- Holybro Kakute F4 Wing support
-- iFlight 2RAW H743 supports onboard logging
-- JFB110 supports measuring servo rail voltage
-- JFB110 supports safety switch LED
-- JHEM-JHEF405 bootloader supports firmware updates via serial
-- JHEMCU GF30H743 HD support
-- JHEMCU-GF16-F405 autopilot support
-- JHEMCU-GSF405A becomes FPV board (to save flash)
-- KakuteF7 only supports BMP280 baro (to save flash)
-- KakuteH7Mini supports ICM42688 IMU
-- Linux auto detection of GPS baud rate fixed
-- Linux board scheduler jitter reduced
-- Linux board shutdown fixes
-- MakeFlyEasy PixPilot-V6Pro support
-- MatekF405, Pixhawk1-1M-bdshot, revo-mini loses blended GPS (to save flash)
-- MatekH7A3 support Bi-directional DShot
-- MicoAir405v2 and MicoAir405Mini support optical flow and OSD
-- MicoAir743 internal compass orientation fixed
-- MicroAir405Mini, MicroAir743, NxtPX4v2 support
-- MicroAir405v2 Bi-directional DShot and LED DMA fixes
-- MicroAir405v2 defined as FPV board with reduced features (to save flash)
-- ModalAI VOXL2 support including Starling 2 and Starling 2 max
-- mRo Control Zero Classic supports servo rail analog input
-- mRo KitCAN revC fixed
-- Mugin MUPilot support
-- OmnibusF7v2 loses quadplane support (to save flash)
-- Pixhack-v3 board added (same as fmuv3)
-- Pixhawk6C bootloader supports flashing firmware from SD card
-- RadiolinkPIX6 imu orientation fixed
-- RadiolinkPIX6 supports SPA06 baro
-- ReaperF745 V4 FC supports MPU6000 IMU
-- RPI5 support
-- SDModelH7v2 SERIAL3/7/9_PROTOCOL param defaults changed
-- Solo serial ports default to MAVLink1
-- SpeedyBeeF405Wing gets Bi-directional DShot
-- SpeedyBeeF405WING loses landing gear support, some camera gimbals (to save flash)
-- Spektreworks boom board support
-- TrueNavPro-G4 SPI does not share DMA
-- X-MAV AP-H743v2 support
-
-2) AHRS/EKF enhancements and fixes
-
-- AHRS_OPTION to disable fallback to DCM (affects Plane and Rover, Copter never falls back)
-- AHRS_OPTION to disable innovation check for airspeed sensor
-- Airspeed sensor health check fixed when using multiple sensors and AHRS affinity
-- DCM support for MAV_CMD_EXTERNAL_WIND_ESTIMATE (Plane only)
-- EK2 supports disabling external nav (see EK2_OPTIONS)
-- EK3 External Nav position jump after switch from Optical flow removed (see EK3_SRC_OPTION=1)
-- EK3 uses filtered velocity corrections for IMU position
-- EKF2, EKF3, ExternalAHRS all use common origin
-- EKF3 accepts set origin even when using GPS
-- EKF3 allows earth-frame fields to be estimated with an origin but no GPS
-- EKF3 copes better with GPS jamming
-- EKF3 logs mag fusion selection to XKFS
-- EKF3 wind estimation when using GPS-for-yaw fixed
-- External AHRS improvements including handling variances, pre-arm origin check
-- Inertial Labs External AHRS fixes
-- VectorNav driver fix for handling of error from sensor
-- VectorNav External AHRS enhancements including validation of config commands and logging improvements
-- VectorNav support for sensors outside VN-100 and VN-300
-
-3) Driver enhancements and bug fixes
-
-- ADSB fix to display last character in status text sent to GCS
-- Ainstein LR-D1 radar support
-- Airspeed ASP5033 whoami check fixed when autopilot rebooted independently of the sensor
-- AIRSPEED message sent to GCS
-- Analog temperature sensor extended to 5th order polynomial (see TEMP_A5)
-- ARSPD_OPTIONS to report calibration offset to GCS
-- Baro EAS2TAS conversions continuously calculated reducing shocks to TECS (Plane only)
-- Baro provides improved atmospheric model for high altitude flight
-- BARO_ALT_OFFSET slew slowed to keep EKF happy
-- BATTx_ESC_MASK param supports flexible mapping of ESCs to batteries
-- BATTx_OPTION to not send battery voltage, current, etc to GCS
-- Benewake RDS02U radar support
-- Bi-directional DShot on IOMCU supports reversible mask
-- Bi-directional DShot telemetry support on F103 8Mhz IOMCUs
-- BMM350 compass support
-- CAN rangefinders and proximity sensors may share a CAN bus (allows NRA24 and MR72 on a single CAN bus)
-- Compass calibration world magnetic model checks can use any position source (e.g. not just GPS)
-- CRSF baro and vertical speeed fixed
-- CRSF RX bind command support
-- DroneCAN battery monitor check to avoid memory corruption when type changed
-- DroneCAN DNA server fixes including removing use of invalid node IDs, faster ID allocation, elimination of rare inability to save info
-- DroneCAN EFI health check fix
-- DroneCAN ESC battery monitors calculate consumed mah
-- DroneCAN ESCs forced to zero when disarmed
-- DroneCAN RPM message support
-- DroneCAN timeout fix for auxiliary frames
-- DroneCAN to serial tunneling params accepts short-hand baud rates (e.g. '57' for '57600')
-- F9P, F10-N and Zed-F9P support for GPSx_GNSS_MODE to turn on/off SBAS, Galileo, Beidou and Glonass
-- FuelLevel battery monitor fix to report capacity
-- GPS_xxx params renamed to GPS1_xxx, GPS_xxx2 renamed to GPS2_xxx
-- Hirth EFI logging includes modified throttle
-- Hirth ICEngine supports reversed crank direction (see ICE_OPTIONS parameter)
-- Hott and LTM telemetry deprecated (still available through custom build server)
-- i-BUS telemetry support
-- ICE_PWM_IGN_ON, ICE_PWM_IGN_OFF, ICE_PWM_STRT_ON, ICE_PWM_STRT_OFF replaced with SERVOx_MIN/MAX/REVERSED (Plane only)
-- ICE_START_CHAN replaced with RC aux function (Plane only)
-- ICEngine retry max added (see ICE_STRT_MX_RTRY)
-- IE 2400 generator error message severity to GCS improved
-- INA2xx battery monitor support (reads temp, gets MAX_AMPS and SHUNT params)
-- MCP9600 temperature sensor I2C address fixed
-- MLX90614 temperature sensor support
-- MSP GPS ground course scaling fixed
-- MSP RC uses scaled RC inputs (fixes issue with RCx_REVERSED having no effect)
-- Networking supports reconnection to TCP server or client
-- OSD params for adjusting horizontal spacing and vertical extension (see OSD_SB_H_OFS, OSD_SB_V_EXT)
-- Relay inverted output support (see RELAYx_INVERTED parameter)
-- ROMFS efficiency improvements
-- RS-485 driver enable RTS flow control
-- Sagetech MXS ADSP altitude fix (now uses abs alt instead of terrain alt)
-- Septentrio GPS sat count correctly drops to zero when 255 received
-- Septentrio supports selecting constellations (see GPS_GNSS_MODE)
-- Single LED for user notification supported
-- SPA06 baro supported
-- Sum battery monitor optionally reports minimum voltage instead of average
-- Sum battery monitor reports average temp
-- Torqeedo dual motor support (see TRQ1, TRQ2 params)
-- Ublox GPS driver uses 64 bit time for PPS interrupt (avoids possible dropout at 1hour and 12 minutes)
-- UBlox GPS time ignored until at least 2D fix
-- VideoTX supports additional freq bands (RushFPV 3.3Ghz)
-- Volz logs desired and actual position, voltage, current, motor and PCB temp
-- Volz server feedback and logging fixed
-- Volz servo output in its own thread resulting in smoother movements
-- W25N02KV flash support
-
-4) Networking enhancements and fixes
-
-- Allow multiple UDP clients to connect/disconnect/reconnect
-- Ethernet supports faster log downloading (raised SDMMC clock limit on H7)
-
-5) Camera and gimbal enhancements
-
-- Alexmos precision improved slightly
-- CAMERA_CAPTURE_STATUS mavlink msg sent to GCS (reports when images taken or video recorded, used by QGC)
-- CAMERA_FOV_STATUS mavlink reports lat,lon of what camera is pointing at
-- DO_MOUNT_CONTROL yaw angle interpreted as body-frame (was incorrectly being interpreted as earth-frame)
-- Dual serial camera gimbal mounts fixed
-- Lua script bindings to send CAMERA_INFORMATION and VIDEO_STREAM_INFORMATION messages to GCS
-- Retract Mount2 aux function added (see RCx_OPTION = 113)
-- Servo gimbal reported angles respect roll, pitch and yaw limits
-- Siyi driver sends autopilot location and speed (recorded in images via EXIF)
-- Siyi picture and video download scripts
-- Siyi ZT6 and ZT30 support sending min, max temperature (see CAMERA_THERMAL_RANGE msg)
-- Siyi ZT6 and ZT30 thermal palette can be changed using camera-change-setting.lua script
-- Siyi ZT6 hardware id and set-lens fixed
-- Topotek gimbal support
-- Trigger distance ignores GPS status and only uses EKF reported location
-
-6) Harmonic notch enhancements
-
-- Harmonic notch is active in forward flight on quadplanes
-- Harmonic notch filter freq clamping and disabling reworked
-- Harmonic notch handles negative ESC RPMs
-- Harmonic notch supports per-motor throttle-based harmonic notch
-
-7) Copter specific enhancements and bug fixes
-
-- Attitude control fix to dt update order (reduces rate controller noise)
-- Auto mode fix to avoid prematurely advancing to next waypoint if given enough time
-- Auto mode small target position jump when takeoff completes removed
-- Auto mode yaw drift when passing WP removed if CONDITION_YAW command used and WP_YAW_BEHAVIOR = 0/None
-- Auto, Guided flight mode pause RC aux switch (see RCx_OPTION = 178)
-- AutoRTL (e.g. DO_LAND_START) uses copter stopping point to decide nearest mission command
-- AutoRTL mode supports DO_RETURN_PATH_START (Copter, Heli only)
-- AutoTune fix to prevent spool up after landing
-- AutoTune performance and safety enhancements (less chance of ESC desync, fails when vehicle likely can't be tuned well)
-- Autotune test gains RC aux switch function allows testing gains in any mode (see RCx_OPTION = 180)
-- Config error avoided if auto mode is paused very soon after poweron
-- FLIGHT_OPTIONS bit added to require position estimate before arming
-- Follow mode slowdown calcs fixed when target is moving
-- Ground oscillation suppressed by reducing gains (see ATC_LAND_R/P/Y_MULT)
-- Guided mode internal error fix when taking off using SET_ATTITUDE_CONTROL message
-- Guided mode internal error resolved when switching between thrust or climb rate based altitude control
-- Guided mode yaw fixed when WP_YAW_BEHAVIOR = 0/None and CONDITION_YAW command received containing relative angle
-- Landing detector fixed when in stabilize mode at full throttle but aircraft is upside down
-- Landing detector logging added to ease support (see LDET message)
-- Loiter unlimited command accepts NaNs (QGC sends these)
-- Mavlink SYSTEM_STATUS set to BOOT during initialisation
-- MOT_PWM_TYPE of 9 (PWMAngle) respects SERVOx_MIN/MAX/TRIM/REVERSED param values
-- Payload place bug fix when aborted because gripper is already released
-- RC based tuning (aka CH6 tuning) can use any RC aux function channel (see RCx_OPTION = 219)
-- RTL_ALT minimum reduced to 30cm
-- SystemID position controller support (Copter and Heli)
-- TriCopter motor test and slew-up fixed
-- WPNAV_SPEED min reduced to 10 cm/s (Copter only)
-- Loiter mode zeros desired accel when re-entering from Auto during RC failsafe
-
-8) TradHeli specific enhancements
-
-- Autorotation yaw behaviour fix
-- Autotune improvements including using low frequency dwell for feedforward gain tuning and conducting sweep in attitude instead of rate
-- Blade pitch angle logging added (see SWSH log message)
-- Constrain cyclic roll for intermeshing
-- ICE / turbine cool down fix
-- Inverted flight extended to non manual throttle modes
-- Inverted flight transitions smoothed and restricted to only Stabilize mode
-- SWSH logging fix for reversed collectives
-
-9) Plane specific enhancements and bug fixes
-
-- AIRSPEED_STALL holds vehicle stall speed and is used for minimum speed scaling
-- Allow for different orientations of landing rangefinder
-- Assistance requirements evaluted on mode change
-- FBWB/CRUISE climb/sink rate limited by TECS limits
-- FLIGHT_OPTION to immediately climb in AUTO mode (not doing glide slope)
-- Glider pullup support (available only through custom build server)
-- Loiter breakout improved to better handle destinations inside loiter circle
-- Manual mode throttle made consistent with other modes (e.g battery comp and watt limit is done if enabled)
-- Mavlink GUIDED_CHANGE_ALTITUDE supports terrain altitudes
-- Minimum throttle not applied during SLT VTOL airbrake (reduces increase in airspeed and alt during back transition)
-- Q_APPROACH_DIST set minimum distance to use the fixed wing approach logic
-- Quadplane assistance check enhancements
-- Quadplane Deca frame support
-- Quadplane gets smoother takeoff by input shaping target accel and velocity
-- Servo wiggles in altitude wait staged to be one after another
-- Set_position_target_global_int accepts MAV_FRAME_GLOBAL_RELATIVE_ALT and MAV_FRAME_GLOBAL_TERRAIN_ALT frames
-- Takeoff airspeed control improved (see TKOFF_MODE, TKOFF_THR_MIN)
-- Takeoff fixes for fence autoenable
-- Takeoff improvements including less overshoot of TKOFF_ALT
-- TECS reset along with other controllers (important if plane dropped from balloon)
-- Tilt quadplane ramp of motors on back transition fixed
-- Tiltrotor tilt angles logged
-- TKOFF_THR_MIN applied to SLT transitions
-- Twin motor planes with DroneCAN ESCs fix to avoid max throttle at arming due to misconfiguration
-- VTOLs switch to QLAND if a LONG_FAILSAFE is triggered during takeoff
-
-10) Rover specific enhancements and bug fixes
-
-- Auto mode reversed state maintained if momentarily switched to Hold mode
-- Circle mode tracks better and avoids getting stuck at circle edge
-- Flight time stats fixed
-- MAV_CMD_NAV_SET_YAW_SPEED deprecated
-- Omni3Mecanum frame support
-- Stopping point uses max deceleration (was incorrectly using acceleration)
-- Wheel rate controller slew rate fix
-
-11) Antenna Tracker specific enhancements and bug fixes
-
-- Never track lat,lon of 0,0
-
-12) Scripting enhancements
-
-- advance-wp.lua applet supports advancing Auto mode WP via RC switch
-- AHRS_switch.lua supports switching between EKF2 and EKF3 via RC switch
-- battery_internal_resistance_check.lua monitors battery resistance
-- CAN:get_device returns nil for unconfigured CAN device
-- copter_terrain_brake.lua script added to prevent impact with terrain in Loiter mode (Copter only)
-- Copter:get_target_location, update_target_location support
-- crosstrack_restore.lua example allows returning to previous track in Auto (Plane only)
-- Display text on OLED display supported
-- Docs improved for many bindings
-- EFI get_last_update_ms binding
-- EFI_SkyPower.lua driver accepts 2nd supply voltage
-- ESC_slew_rate.lua example script supports testing ESCs
-- Filesystem CRC32 check to allow scripts to check module versions
-- forced arming support
-- GPIO get/set mode bindings (see gpio:set_mode, get_mode)
-- GPS-for-yaw angle binding (see gps:gps_yaw_deg)
-- Halo6000 EFI driver can log all CAN packets for easier debugging
-- handle_external_position_estimate binding allows sending position estimate from lua to EKF
-- I2C:transfer support
-- IMU gyros_consistent and accels_consistent bindings added
-- INF_Inject.lua driver more robust to serial errors, improved logging, throttle and ignition control
-- INS bindings for is calibrating, gyro and accel sensor values
-- IPV4 address bindings (see SocketAPM_ipv4_addr_to_string) to allow UDP server that responds to individual clients
-- Logging of booleans supported
-- Lua language checks improved (finds more errors)
-- MAVLink commands can be called from scripting
-- MCU voltage binding (see analog:mcu_voltage)
-- NMEA 2000 EFI driver (see EFI_NMEA2k.lua)
-- "open directory failed" false warning when scripts in ROMFS fixed
-- Param_Controller.lua supports quickly switching between 3 parameter sets via RC switch
-- Pass by reference values are always initialized
-- pelco_d_antennatracker.lua applet supports sending Pelco-D via RS-485 to PTZ cameras
-- plane_aerobatics.lua minor enhancements
-- REPL applet (read-evaluate-print-loop, see repl.lua) for interactive testing and experimentation
-- "require" function failures in rare circumstances fixed
-- "require" function works for modules in ROMFS (e.g. not on SD card)
-- revert_param.lua supports more params (e.g ATC_RATE_R/P/Y, PTCH2SRV_TCONST, RLL2SRV_TCONST, TECS_)
-- Scripts may receive mavlink messages which fail CRC (e.g messages which FC does not understand)
-- SD card formatting supported
-- Serial device simulation support (allows Lua to feed data to any supported serial protocol for e.g. sensor simulation)
-- set_target_throttle_rate_rpy allows rate control from scripts (new for Copter)
-- sitl_standby_sim.lua example shows how to switch between redundant flight controllers using an RC switch
-- Slung payload oscillation suppression applet added (see copter-slung-payload.lua)
-- Temperature sensor bindings added
-- uint64 support
-- Various performance and memory usage optimizations
-- VTOL-quicktune.lua minor enhancements including increasing YAW_FLTE_MAX to 8
-- x-quad-cg-allocation.lua applet corrects for the CoM discrepancy in a quad-X frame
-
-13) GCS / mavlink related changes and fixes
-
-- BATTERY2 message deprecated (replaced by BATTERY_STATUS)
-- CMD_MISSION_START/STOP rejected if first-item/last-item args provided
-- Deny attempts to upload two missions simultaneously
-- Fence and Rally points may be uploaded using FTP
-- GPS_INPUT and HIL_GPS handles multiple GPSs
-- HIGHRES_IMU mavlink message support (used by companion computers to receive IMU data from FC)
-- MAV_CMD_COMPONENT_ARM_DISARM accepts force arm magic value of 21196
-- MAV_CMD_DO_SET_SAFETY_SWITCH_STATE support
-- MAV_CMD_SET_HAGL support (Plane only)
-- MAVFTP respects TX buffer flow control to improve FTP on low bandwidth links
-- MAVLink receiver support (RADIO_RC_CHANNELS mavlink message)
-- Message interval supports TERRAIN_REPORT msg
-- Mission upload may be cancelled using MISSION_CLEAR_ALL
-- MOUNT_CONFIGURE, MOUNT_CONTROL messages deprecated
-- RC_CHANNELS_RAW deprecated
-- Serial passthrough supports parity allowing STM32CubeProgrammer to be used to program FrSky R9 receivers
-- SET_ATTITUDE_TARGET angular rate input frame fixed (Copter only)
-- TIMESYNC and NAMED_VALUE_FLOAT messages not sent on high latency MAVLink ports
-
-14) Logging enhancements and fixes
-
-- AC_PID resets and I-term sets logged
-- ANG provides attitude at high rate (equivalent to ATT)
-- ATT logs angles as floats (better resolution than ints)
-- CAND message gets driver index
-- DCM log message includes roll/pitch and yaw error
-- EDT2 log msg includes stress and status received via extended DShot Telemetry v2
-- EFI ECYL cylinder head and exhaust temp logs in degC (was Kelvin)
-- ESCX log msg includes DroneCAN ESCs status, temp, duty cycle and power pct
-- FMT messages written as required instead of all at beginning
-- Logging restarted after download completes when LOG_DISARMED = 1
-- MISE msg logs active mission command (CMD logged when commands are uploaded)
-- ORGN message logging fixed when set using SET_GPS_GLOBAL_ORIGIN
-- RPM sensor logging gets instance field, quality and health fields
-- Short filename support removed (e.g log1.BIN instead of 00000001.BIN)
-- Temperature sensor logging option for only sensors with no source (see TEMP_LOG)
-- UART data rates logged at 1hz (see UART message)
-
-15) ROS2 / DDS support
-
-- Airspeed published
-- Battery topic reports all available batteries
-- Compile-time configurable rates for each publisher
-- DDS_TIMEOUT_MS and DDS_MAX_RETRY set timeout and num retries when client pings XRCE agent
-- GPS global origin published at 1 Hz
-- High frequency raw imu data transmission
-- Joystick support
-- Support sending waypoints to Copter and Rover
-- Remove the XML refs file in favor of binary entity creation
-
-16) Safety related enhancements and fixes
-
-- Accel/Gyro inconsistent message fixed for using with only single IMU
-- Battery monitor failure reported to GCS, triggers battery failsafe but does not take action
-- Far from EKF origin pre-arm check removed (Copter only)
-- Fence breach warning message slightly improved
-- Fence enhancements incluiding alt min fence (Copter only, see FENCE_AUTOENABLE, FENCE_OPTIONS)
-- Fences can be stored to SD Card (see BRD_SD_FENCE param)
-- ICEngine stopped when in E-Stop or safety engaged (Plane only)
-- LEDs flash green lights based on EKF location not GPS
-- Parachute option to skip disarm before parachute release (see CHUTE_OPTIONS)
-- Plane FENCE_AUTOENABLE of 1 or 2 deprecation warning added
-- Pre-arm check if OpenDroneID is compiled in but disabled
-- Pre-arm check of duplicate RC aux functions fixed (was skipping recently added aux functions)
-- Pre-arm checks alert user more quickly on failure
-- Prearm check for misconfigured EAHRS_SENSORS and GPS_TYPE
-- Proximity sensor based avoidance keeps working even if one proximity sensor fails (Copter, Rover)
-- RC aux functions for Arm, Disarm, E-Stop and Parachute ignored when RC is first turned on
-- Warning of duplicate SERIALx_PROTOCOL = RCIN
-
-17) Other bug fixes and minor enhancements
-
-- Accel cal fixed for auxiliary IMUs (e.g. IMU4 and higher)
-- Bootloader fix to reduce unnecessary mass erasing of flash when using STLink or Jlink tools
-- Bootloader rejects allocation of broadcast node ID
-- CAN forward registering/de-registering fix (affected Mission Planner DroneCAN UI)
-- Dijkstras fix to correct use of uninitialised variable
-- DShot rates are not limited by NeoPixel rates
-- Ethernet and CAN bootloader fix to prevent getting stuck in bootloader mode
-- Filesystem does not show entries for empty @ files
-- Filesystem efficiency improvements when reading files
-- Flight statistics only reset if user sets STAT_RESET to zero (avoids accidental reset)
-- Flight time statistics updated on disarm (avoids issue if vehicle powered-down soon after disarm)
-- Internal error thrown if we lose parameters due to saving queue being too small
-- MAVLink via DroneCAN baud rate fix
-- SPI pins may also be used as GPIOs
-- Terrain cache size configurable (see TERRAIN_CACHE_SZ)
-
-18) Developer focused fixes and enhancements
-
-- AP_Camera gets example python code showing how to use GStreamer with UDP and RTSP video streams
-- Cygwin build fix for non-SITL builds
-- Cygwin build fixed with malloc wrap
-- DroneCAN and scripting support FlexDebug messages (see CAN_Dn_UC_OPTION, FlexDebug.lua)
-- EKF3 code generator documentation and cleanup
-- GPS jamming simulator added
-- MacOS compiler warnings reduced
-- SFML joystick support
-- SITL support for OpenBSD
-- Text warning if older Fence or Rally point protocols are used
-------------------------------------------------------------------
-Release 4.5.7 08 Oct 2024
-
-Changes from 4.5.7-beta1
-
-1) Reverted Septentrio GPS sat count correctly drops to zero when 255 received
-------------------------------------------------------------------
-Release 4.5.7-beta1 26 Sep 2024
-
-Changes from 4.5.6
-
-1) Bug fixes and minor enhancements
-
-- VUAV-V7pro support
-- CUAV-7-Nano correction for LEDs and battery volt and current scaling
-- DroneCAN deadlock and saturation of CAN bus fixed
-- DroneCAN DNA server init fix (caused logging issues and spam on bus)
-- F4 boards with inverter support correctly uninvert RX/TX
-- Nanoradar M72 radar driver fix for object avoidance path planning
-- RC support for latest version of GHST
-- Septentrio GPS sat count correctly drops to zero when 255 received
-- TradHeli DDVP tail rotor pitch actuator fixed
-
-2) ROS2/DDS and other developer focused enhancements
-
-- AP quaternions normalised for ROS2 to avoid warnings
-- Dependencies fixed for easier installation
-- ROS2 SITL launch file enhancements including displaying console and map
-- ROS_DOMAIN_ID param added to support multiple vehicles or instances of ROS2
-- Python 3.12 support
-------------------------------------------------------------------
-Release 4.5.6 03 Sep 2024
-
-No changes from 4.5.6-beta1
-------------------------------------------------------------------
-Release 4.5.6-beta1 20 Aug 2024
-
-Changes from 4.5.5
-
-1) Board specific enhancements and bug fixes
-
-- 3DR Control Zero H7 Rev G support
-- CUAV-7-Nano support
-- FoxeerF405v2 servo outputs increased from 9 to 11
-- Holybro Pixhawk6C hi-power peripheral overcurrent reporting fixed
-- iFlight 2RAW H7 support
-- MFT-SEMA100 support
-- TMotorH743 support BMI270 baro
-- ZeroOneX6 support
-
-2) Minor enhancements and bug fixes
-
-- Cameras using MAVLink report vendor and model name correctly
-- DroneCAN fix to remove occasional NodeID registration error
-- GPS NMEA and GSoF driver ground course corrected (now always 0 ~ 360 deg)
-- ICP101XX barometer slowed to avoid I2C communication errors
-- IMU temp cal param (INSn_ACCSCAL_Z) stored correctly when bootloader is flashed
-- IMU gyro/accel duplicate id registration fixed to avoid possible pre-arm failure
-- Logging to flash timestamp fix
-- OSD displays ESC temp instead of motor temp
-- PID controller error calculation bug fix (was using target from prev iteration)
-- Relay on MAIN pins fixed
-
-3) Copter specific fixes
-
-- Payload place bug fix (calimb rate after releasing payload was unreliable)
-
-------------------------------------------------------------------
-Release 4.5.5 1st Aug 2024
-
-No changes from 4.5.5-beta2
-------------------------------------------------------------------
-Release 4.5.5-beta2 27 July 2024
-
-Changes from 4.5.5-beta1
-
-1) Board specific enhancements and bug fixes
-
-- CubeRed's second core disabled at boot to avoid spurious writes to RAM
-- CubeRed bootloader's dual endpoint update method fixed
-------------------------------------------------------------------
-Release 4.5.5-beta1 1st July 2024
-
-Changes from 4.5.4
-
-1) Board specific enhancements and bug fixes
-
-- fixed IOMCU transmission errors when using bdshot
-- update relay parameter names on various boards
-- add ASP5033 airspeed in minimal builds
-- added RadiolinkPIX6
-- fix Aocoda-RC H743Dual motor issue
-- use ICM45686 as an ICM20649 alternative in CubeRedPrimary
-
-2) System level minor enhancements and bug fixes
-
-- correct use-after-free in script statistics
-- added arming check for eeprom full
-- fixed a block logging issue which caused log messages to be dropped
-- enable Socket SO_REUSEADDR on LwIP
-- removed IST8310 overrun message
-- added Siyi ZT6 support
-- added BTFL sidebar symbols to the OSD
-- added CRSF extended link stats to the OSD
-- use the ESC with the highest RPM in the OSD when only one can be displayed
-- support all Tramp power levels on high power VTXs
-- emit jump count in missions even if no limit
-- improve the bitmask indicating persistent parameters on bootloader flash	
-- fix duplicate error condition in the MicroStrain7
-
-3) AHRS / EKF fixes
-
-- fix infinite climb bug when using EK3_OGN_HGT_MASK
-
-4) Copter specific changes
-
-- fix MAV_CMD_CONDITION_YAW with relative angle
-
-5) Other minor enhancements and bug fixes
-
-- specify pymonocypher version in more places
-- added DroneCAN dependencies to custom builds
-
-------------------------------------------------------------------
-Release 4.5.4 12th June 2024
-
-Changes from 4.5.3
-
-Disable highres IMU sampling on ICM42670 fixing an issue on some versions of Pixhawk6X
-
-------------------------------------------------------------------
-Release 4.5.3 28th May 2024
-
-No changes from 4.5.3-beta1
-------------------------------------------------------------------
-Release 4.5.3-beta1 16th May 2024
-
-Changes from 4.5.2
-
-1) Board specific enhancements and bug fixes
-
-- correct default GPS port on MambaH743v4
-- added SDMODELV2
-- added iFlight Blitz H7 Pro
-- added BLITZ Wing H743
-- added highres IMU sampling on Pixhawk6X
-
-2) System level minor enhancements and bug fixes
-
-- fixed rare crash bug in lua scripting on script fault handling
-- fixed Neopixel pulse proportions to work with more LED variants
-- fixed timeout in lua rangefinder drivers
-- workaround hardware issue in IST8310 compass
-- allow FIFO rate logging for highres IMU sampling
-
-3) Copter specific changes
-
-- fixed speed constraint during avoidance backoff
-
-------------------------------------------------------------------
-Release 4.5.2 14th May 2024
-
-No changes from 4.5.2-beta1
-------------------------------------------------------------------
-Release 4.5.2-beta1 29th April 2024
-
-Changes from 4.5.1
-
-1) Board specific enhancements and bug fixes
-
-- FoxeerF405v2 support
-- iFlight BLITZ Mini F745 support
-- Pixhawk5X, Pixhawk6C, Pixhawk6X, Durandal power peripherals immediately at startup
-
-2) System level minor enhancements and bug fixes
-
-- Camera lens (e.g. RGB, IR) can be selected from GCS or during missions using set-camera-source
-- Crashdump pre-arm check added
-- Gimbal gets improved yaw lock reporting to GCS
-- Gimbal default mode fixed (MNTx_DEFLT_MODE was being overriden by RC input)
-- RM3100 compass SPI bus speed reduced to 1Mhz
-- SBUS output fix for channels 1 to 8 also applying to 9 to 16
-- ViewPro gimbal supports enable/disable rangefinder from RC aux switch
-- Visual Odometry delay fixed (was always using 1ms delay, see VISO_DELAY_MS)
-- fixed serial passthrough to avoid data loss at high data rates
-
-3) AHRS / EKF fixes
-
-- Compass learning disabled when using GPS-for-yaw
-- GSF reset minimum speed reduced to 1m/s (except Plane which remains 5m/s)
-- MicroStrain7 External AHRS position quantization bug fix
-- MicroStrain7 init failure warning added
-- MicroStrain5 and 7 position and velocity variance reporting fix
-
-4) Copter specific changes
-
-- Auto mode condition yaw fix to avoid pointing at out-of-date target
-- Guided mode angle control yaw target initialisation fix (was always turning North)
-
-5) Other minor enhancements and bug fixes
-
-- DDS_UDP_PORT parameter renamed (was DDS_PORT)
-- Harmonic Notch bitmask parameter conversion fix (see INS_HNTCH_HMNCS)
-
-------------------------------------------------------------------
-
-Release 4.5.1 8th April 2024
-
-This release fixes a critical bug in the CRSF R/C protocol parser that
-can lead to a handfault and a vehicle crashing. A similar fix was
-applied to the GHST protocol, although we believe that GHST could not
-be affected by the bug, so this was just a precaution.
-
-There are no other changes in this release.
-
-------------------------------------------------------------------
-Release 4.5.0 2nd April 2024
-
-No changes from 4.5.0-beta4
-------------------------------------------------------------------
-Release 4.5.0-beta4 22nd March 2024
-
-Changes from 4.5.0-beta3
-
-1) system changes
-
-- fixed a cache corruption issue with microSD card data on H7 based boards
-- rename parameter NET_ENABLED to NET_ENABLE
-- fixed FDCAN labels for adding new H7 boards
-- avoid logging dma.txt to save CPU
-- fixed roll/pitch in viewpro driver
-- added band X in VideoTX
-- fixed quaternion attitude reporting for Microstrain external AHRS
-- add RPLidarC1 proximity support
-
-2) new boards
-- added MicoAir405v2
-- add Orqa F405 Pro
-
-------------------------------------------------------------------
-Release 4.5.0-beta3 14-March-2024
-
-Changes from 4.5.0-beta2
-
-1) Board specific changes
-- added PixFlamingo F7 board
-- support ICM42688 on BlitzF745AIO
-- fixed IMU orientation of CubeRedSecondary
-- enable all FPV features on SpeedyBeeF405WING
-
-2) System level changes
-
-- improved robustness of CRSF parser
-- reduced memory used by DDS/ROS2
-- added filesystem crc32 binding in lua scripting
-- support visual odometry quality metric and added autoswitching lua script
-- allow for expansion of fence storage to microSD for larger pologon fences
-- allow FTP upload of fence and rally points
-- fixed vehicle type of ship simulation for ship landing
-- make severity level depend on generator error level in IE 2400 generator
-- speed up initial GPS probe by using SERIALn_BAUD first
-- allow NanoRadar radar and proximity sensor to share the CAN bus
-- added MR72 CAN proximity sensor
-- only produce *_with_bl.hex not *.hex in builds if bootloader available
-- fixed check for GPS antenna separation in moving baseline yaw
-- added GPS_DRV_OPTIONS options for fully parsing RTCMv3 stream
-- fixed logging of RTCM fragments in GPS driver
-- fixed video recording while armed
-- robostness and logging improvements for ExternalAHRS
-- fixed RPM from bdshot on boards with IOMCU
-- fixed accel cal simple to remove unused IMUs
-
-3) Copter specific changes
-- check fence breaches more often on copter for smaller overrun
-- improved copter follow mode at close distances
-- fixed default for FLTD for yaw
-- fixed reset_target_and_rate method in attitude control
-
-------------------------------------------------------------------
 Copter 4.5.0-beta2 14-February-2024
 
 Changes from 4.5.0-beta1:
@@ -776,7 +28,7 @@ Changes from 4.5.0-beta1:
  - broaden acceptance criteria for GPS yaw measurement for moving baseline yaw
 
 ------------------------------------------------------------------
-Copter 4.5.0-beta1 30-Jan-2024
+Copter 4.5.0-beta1 30-Jan-2025
 Changes from 4.4.4
 1) New autopilots supported
     - ACNS-F405AIO
@@ -1460,7 +712,7 @@ Changes from 4.3.6
     e) Memory corruption bug in the STM32H757 (very rare)
     f) RC input on IOMCU bug fix (RC might not be regained if lost)
 ------------------------------------------------------------------
-Copter 4.3.6 05-Apr-2023 / 4.3.6-beta1, 4.3.6-beta2 27-Mar-2023
+Copter 4.3.6 05-Apr-2023 / 4.3.6-beta1, 4.3.6--beta2 27-Mar-2023
 Changes from 4.3.5
 1) Bi-directional DShot fix for possible motor stop approx 72min after startup 
 ------------------------------------------------------------------

ArduCopter/UserParameters.cpp

@@ -1,7 +1,7 @@
 #include "UserParameters.h"
 #include "config.h"
 
-#if USER_PARAMS_ENABLED
+#if USER_PARAMS_ENABLED == ENABLED
 // "USR" + 13 chars remaining for param name
 const AP_Param::GroupInfo UserParameters::var_info[] = {
 

ArduCopter/afs_copter.cpp

@@ -4,7 +4,7 @@
 
 #include "Copter.h"
 
-#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
+#if ADVANCED_FAILSAFE == ENABLED
 
 /*
   setup radio_out values for all channels to termination values
@@ -55,7 +55,17 @@ void AP_AdvancedFailsafe_Copter::setup_IO_failsafe(void)
  */
 AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Copter::afs_mode(void)
 {
-    return copter.flightmode->afs_mode();
+    switch (copter.flightmode->mode_number()) {
+    case Mode::Number::AUTO:
+    case Mode::Number::AUTO_RTL:
+    case Mode::Number::GUIDED:
+    case Mode::Number::RTL:
+    case Mode::Number::LAND:
+        return AP_AdvancedFailsafe::AFS_AUTO;
+    default:
+        break;
+    }
+    return AP_AdvancedFailsafe::AFS_STABILIZED;
 }
 
 //to force entering auto mode when datalink loss 
@@ -63,4 +73,4 @@ AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Copter::afs_mode(void)
  {
     copter.set_mode(Mode::Number::AUTO,ModeReason::GCS_FAILSAFE);
  }
-#endif // AP_COPTER_ADVANCED_FAILSAFE_ENABLED
+#endif // ADVANCED_FAILSAFE

ArduCopter/afs_copter.h

@@ -18,9 +18,7 @@
   advanced failsafe support for copter
  */
 
-#include "config.h"
-
-#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
+#if ADVANCED_FAILSAFE == ENABLED
 #include <AP_AdvancedFailsafe/AP_AdvancedFailsafe.h>
 
 /*
@@ -46,5 +44,5 @@ protected:
     void set_mode_auto(void) override;
 };
 
-#endif // AP_COPTER_ADVANCED_FAILSAFE_ENABLED
+#endif // ADVANCED_FAILSAFE
 

ArduCopter/autoyaw.cpp

@@ -8,14 +8,13 @@ float Mode::AutoYaw::roi_yaw() const
     return get_bearing_cd(copter.inertial_nav.get_position_xy_cm(), roi.xy());
 }
 
-// returns a yaw in degrees, direction of vehicle travel:
 float Mode::AutoYaw::look_ahead_yaw()
 {
     const Vector3f& vel = copter.inertial_nav.get_velocity_neu_cms();
     const float speed_sq = vel.xy().length_squared();
     // Commanded Yaw to automatically look ahead.
     if (copter.position_ok() && (speed_sq > (YAW_LOOK_AHEAD_MIN_SPEED * YAW_LOOK_AHEAD_MIN_SPEED))) {
-        _look_ahead_yaw = degrees(atan2f(vel.y,vel.x));
+        _look_ahead_yaw = degrees(atan2f(vel.y,vel.x))*100.0f;
     }
     return _look_ahead_yaw;
 }
@@ -81,7 +80,7 @@ void Mode::AutoYaw::set_mode(Mode yaw_mode)
 
     case Mode::LOOK_AHEAD:
         // Commanded Yaw to automatically look ahead.
-        _look_ahead_yaw = copter.ahrs.yaw_sensor * 0.01;  // cdeg -> deg
+        _look_ahead_yaw = copter.ahrs.yaw_sensor;
         break;
 
     case Mode::RESETTOARMEDYAW:
@@ -111,9 +110,6 @@ void Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_ds, int8_t di
 
     // calculate final angle as relative to vehicle heading or absolute
     if (relative_angle) {
-        if (_mode == Mode::HOLD) {
-            _yaw_angle_cd = copter.ahrs.yaw_sensor;
-        }
         _fixed_yaw_offset_cd = angle_deg * 100.0 * (direction >= 0 ? 1.0 : -1.0);
     } else {
         // absolute angle
@@ -149,18 +145,6 @@ void Mode::AutoYaw::set_yaw_angle_rate(float yaw_angle_d, float yaw_rate_ds)
     set_mode(Mode::ANGLE_RATE);
 }
 
-// set_yaw_angle_offset - sets the yaw look at heading for auto mode, as an offset from the current yaw angle
-void Mode::AutoYaw::set_yaw_angle_offset(const float yaw_angle_offset_d)
-{
-    _last_update_ms = millis();
-
-    _yaw_angle_cd = wrap_360_cd(_yaw_angle_cd + (yaw_angle_offset_d * 100.0));
-    _yaw_rate_cds = 0.0f;
-
-    // set yaw mode
-    set_mode(Mode::ANGLE_RATE);
-}
-
 // set_roi - sets the yaw to look at roi for auto mode
 void Mode::AutoYaw::set_roi(const Location &roi_location)
 {
@@ -246,7 +230,7 @@ float Mode::AutoYaw::yaw_cd()
 
     case Mode::LOOK_AHEAD:
         // Commanded Yaw to automatically look ahead.
-        _yaw_angle_cd = look_ahead_yaw() * 100.0;
+        _yaw_angle_cd = look_ahead_yaw();
         break;
 
     case Mode::RESETTOARMEDYAW:
@@ -326,7 +310,7 @@ AC_AttitudeControl::HeadingCommand Mode::AutoYaw::get_heading()
     _pilot_yaw_rate_cds = 0.0;
     if (!copter.failsafe.radio && copter.flightmode->use_pilot_yaw()) {
         // get pilot's desired yaw rate
-        _pilot_yaw_rate_cds = copter.flightmode->get_pilot_desired_yaw_rate();
+        _pilot_yaw_rate_cds = copter.flightmode->get_pilot_desired_yaw_rate(copter.channel_yaw->norm_input_dz());
         if (!is_zero(_pilot_yaw_rate_cds)) {
             auto_yaw.set_mode(AutoYaw::Mode::PILOT_RATE);
         }
@@ -335,7 +319,7 @@ AC_AttitudeControl::HeadingCommand Mode::AutoYaw::get_heading()
         auto_yaw.set_mode(AutoYaw::Mode::HOLD);
     }
 
-#if WEATHERVANE_ENABLED
+#if WEATHERVANE_ENABLED == ENABLED
     update_weathervane(_pilot_yaw_rate_cds);
 #endif
 
@@ -366,7 +350,7 @@ AC_AttitudeControl::HeadingCommand Mode::AutoYaw::get_heading()
 
 // handle the interface to the weathervane library
 // pilot_yaw can be an angle or a rate or rcin from yaw channel. It just needs to represent a pilot's request to yaw the vehicle to enable pilot overrides.
-#if WEATHERVANE_ENABLED
+#if WEATHERVANE_ENABLED == ENABLED
 void Mode::AutoYaw::update_weathervane(const int16_t pilot_yaw_cds)
 {
     if (!copter.flightmode->allows_weathervaning()) {
@@ -394,4 +378,4 @@ void Mode::AutoYaw::update_weathervane(const int16_t pilot_yaw_cds)
         }
     }
 }
-#endif // WEATHERVANE_ENABLED
+#endif // WEATHERVANE_ENABLED == ENABLED

ArduCopter/avoidance.cpp

@@ -3,7 +3,7 @@
 // check if proximity type Simple Avoidance should be enabled based on alt
 void Copter::low_alt_avoidance()
 {
-#if AP_AVOIDANCE_ENABLED
+#if AC_AVOID_ENABLED == ENABLED
     int32_t alt_cm;
     if (!get_rangefinder_height_interpolated_cm(alt_cm)) {
         // enable avoidance if we don't have a valid rangefinder reading

ArduCopter/avoidance_adsb.cpp

@@ -25,7 +25,7 @@ MAV_COLLISION_ACTION AP_Avoidance_Copter::handle_avoidance(const AP_Avoidance::O
 
     // take no action in some flight modes
     if (copter.flightmode->mode_number() == Mode::Number::LAND ||
-#if MODE_THROW_ENABLED
+#if MODE_THROW_ENABLED == ENABLED
         copter.flightmode->mode_number() == Mode::Number::THROW ||
 #endif
         copter.flightmode->mode_number() == Mode::Number::FLIP) {
@@ -148,7 +148,7 @@ void AP_Avoidance_Copter::set_mode_else_try_RTL_else_LAND(Mode::Number mode)
 
 int32_t AP_Avoidance_Copter::get_altitude_minimum() const
 {
-#if MODE_RTL_ENABLED
+#if MODE_RTL_ENABLED == ENABLED
     // do not descend if below RTL alt
     return copter.g.rtl_altitude;
 #else

ArduCopter/commands.cpp

@@ -30,7 +30,7 @@ void Copter::set_home_to_current_location_inflight() {
             return;
         }
         // we have successfully set AHRS home, set it for SmartRTL
-#if MODE_SMARTRTL_ENABLED
+#if MODE_SMARTRTL_ENABLED == ENABLED
         g2.smart_rtl.set_home(true);
 #endif
     }
@@ -45,7 +45,7 @@ bool Copter::set_home_to_current_location(bool lock) {
             return false;
         }
         // we have successfully set AHRS home, set it for SmartRTL
-#if MODE_SMARTRTL_ENABLED
+#if MODE_SMARTRTL_ENABLED == ENABLED
         g2.smart_rtl.set_home(true);
 #endif
         return true;
@@ -63,6 +63,11 @@ bool Copter::set_home(const Location& loc, bool lock)
         return false;
     }
 
+    // check home is close to EKF origin
+    if (far_from_EKF_origin(loc)) {
+        return false;
+    }
+
     // set ahrs home (used for RTL)
     if (!ahrs.set_home(loc)) {
         return false;
@@ -76,3 +81,19 @@ bool Copter::set_home(const Location& loc, bool lock)
     // return success
     return true;
 }
+
+// far_from_EKF_origin - checks if a location is too far from the EKF origin
+//  returns true if too far
+bool Copter::far_from_EKF_origin(const Location& loc)
+{
+    // check distance to EKF origin
+    Location ekf_origin;
+    if (ahrs.get_origin(ekf_origin)) {
+        if (labs(ekf_origin.alt - loc.alt)*0.01 > EKF_ORIGIN_MAX_ALT_KM*1000.0) {
+            return true;
+        }
+    }
+
+    // close enough to origin
+    return false;
+}

ArduCopter/compassmot.cpp

@@ -146,10 +146,9 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan)
         read_radio();
 
         // pass through throttle to motors
-        auto &srv = AP::srv();
-        srv.cork();
+        SRV_Channels::cork();
         motors->set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() * 0.001f);
-        srv.push();
+        SRV_Channels::push();
 
         // read some compass values
         compass.read();
@@ -229,12 +228,6 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan)
 #if HAL_WITH_ESC_TELEM
             // send ESC telemetry to monitor ESC and motor temperatures
             AP::esc_telem().send_esc_telemetry_mavlink(gcs_chan.get_chan());
-#endif
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-            // a lot of autotest timeouts are based on receiving system time
-            gcs_chan.send_system_time();
-            // autotesting of compassmot wants to see RC channels message
-            gcs_chan.send_rc_channels();
 #endif
         }
     }

ArduCopter/config.h

@@ -29,7 +29,7 @@
 #include "APM_Config.h"
 #include <AP_ADSB/AP_ADSB_config.h>
 #include <AP_Follow/AP_Follow_config.h>
-#include <AC_Avoidance/AC_Avoidance_config.h>
+
 
 //////////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////
@@ -70,6 +70,18 @@
 // Rangefinder
 //
 
+#ifndef RANGEFINDER_ENABLED
+ # define RANGEFINDER_ENABLED ENABLED
+#endif
+
+#ifndef RANGEFINDER_HEALTH_MAX
+ # define RANGEFINDER_HEALTH_MAX 3          // number of good reads that indicates a healthy rangefinder
+#endif
+
+#ifndef RANGEFINDER_TIMEOUT_MS
+# define RANGEFINDER_TIMEOUT_MS 1000        // rangefinder filter reset if no updates from sensor in 1 second
+#endif
+
 #ifndef RANGEFINDER_FILT_DEFAULT
  # define RANGEFINDER_FILT_DEFAULT 0.5f     // filter for rangefinder distance
 #endif
@@ -78,6 +90,18 @@
  # define SURFACE_TRACKING_TIMEOUT_MS  1000 // surface tracking target alt will reset to current rangefinder alt after this many milliseconds without a good rangefinder alt
 #endif
 
+#ifndef RANGEFINDER_TILT_CORRECTION         // by disable tilt correction for use of range finder data by EKF
+ # define RANGEFINDER_TILT_CORRECTION ENABLED
+#endif
+
+#ifndef RANGEFINDER_GLITCH_ALT_CM
+ # define RANGEFINDER_GLITCH_ALT_CM  200      // amount of rangefinder change to be considered a glitch
+#endif
+
+#ifndef RANGEFINDER_GLITCH_NUM_SAMPLES
+ # define RANGEFINDER_GLITCH_NUM_SAMPLES  3   // number of rangefinder glitches in a row to take new reading
+#endif
+
 #ifndef MAV_SYSTEM_ID
  # define MAV_SYSTEM_ID          1
 #endif
@@ -117,101 +141,103 @@
 //////////////////////////////////////////////////////////////////////////////
 //  Auto Tuning
 #ifndef AUTOTUNE_ENABLED
- # define AUTOTUNE_ENABLED  1
+ # define AUTOTUNE_ENABLED  ENABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Parachute release
+#ifndef PARACHUTE
+ # define PARACHUTE HAL_PARACHUTE_ENABLED
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
 // Nav-Guided - allows external nav computer to control vehicle
 #ifndef AC_NAV_GUIDED
- # define AC_NAV_GUIDED    1
+ # define AC_NAV_GUIDED    ENABLED
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
 // Acro - fly vehicle in acrobatic mode
 #ifndef MODE_ACRO_ENABLED
-# define MODE_ACRO_ENABLED 1
+# define MODE_ACRO_ENABLED ENABLED
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
 // Auto mode - allows vehicle to trace waypoints and perform automated actions
 #ifndef MODE_AUTO_ENABLED
-# define MODE_AUTO_ENABLED 1
+# define MODE_AUTO_ENABLED ENABLED
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
 // Brake mode - bring vehicle to stop
 #ifndef MODE_BRAKE_ENABLED
-# define MODE_BRAKE_ENABLED 1
+# define MODE_BRAKE_ENABLED ENABLED
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
 // Circle - fly vehicle around a central point
 #ifndef MODE_CIRCLE_ENABLED
-# define MODE_CIRCLE_ENABLED 1
+# define MODE_CIRCLE_ENABLED ENABLED
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
 // Drift - fly vehicle in altitude-held, coordinated-turn mode
 #ifndef MODE_DRIFT_ENABLED
-# define MODE_DRIFT_ENABLED 1
+# define MODE_DRIFT_ENABLED ENABLED
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
 // flip - fly vehicle in flip in pitch and roll direction mode
 #ifndef MODE_FLIP_ENABLED
-# define MODE_FLIP_ENABLED 1
+# define MODE_FLIP_ENABLED ENABLED
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
 // Follow - follow another vehicle or GCS
 #ifndef MODE_FOLLOW_ENABLED
-#if AP_FOLLOW_ENABLED && AP_AVOIDANCE_ENABLED
-#define MODE_FOLLOW_ENABLED 1
-#else
-#define MODE_FOLLOW_ENABLED 0
-#endif
+# define MODE_FOLLOW_ENABLED AP_FOLLOW_ENABLED
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
 // Guided mode - control vehicle's position or angles from GCS
 #ifndef MODE_GUIDED_ENABLED
-# define MODE_GUIDED_ENABLED 1
+# define MODE_GUIDED_ENABLED ENABLED
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
 // GuidedNoGPS mode - control vehicle's angles from GCS
 #ifndef MODE_GUIDED_NOGPS_ENABLED
-# define MODE_GUIDED_NOGPS_ENABLED 1
+# define MODE_GUIDED_NOGPS_ENABLED ENABLED
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
 // Loiter mode - allows vehicle to hold global position
 #ifndef MODE_LOITER_ENABLED
-# define MODE_LOITER_ENABLED 1
+# define MODE_LOITER_ENABLED ENABLED
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
 // Position Hold - enable holding of global position
 #ifndef MODE_POSHOLD_ENABLED
-# define MODE_POSHOLD_ENABLED 1
+# define MODE_POSHOLD_ENABLED ENABLED
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
 // RTL - Return To Launch
 #ifndef MODE_RTL_ENABLED
-# define MODE_RTL_ENABLED 1
+# define MODE_RTL_ENABLED ENABLED
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
 // SmartRTL - allows vehicle to retrace a (loop-eliminated) breadcrumb home
 #ifndef MODE_SMARTRTL_ENABLED
-# define MODE_SMARTRTL_ENABLED 1
+# define MODE_SMARTRTL_ENABLED ENABLED
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
 // Sport - fly vehicle in rate-controlled (earth-frame) mode
 #ifndef MODE_SPORT_ENABLED
-# define MODE_SPORT_ENABLED 0
+# define MODE_SPORT_ENABLED DISABLED
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
@@ -223,13 +249,13 @@
 //////////////////////////////////////////////////////////////////////////////
 // Throw - fly vehicle after throwing it in the air
 #ifndef MODE_THROW_ENABLED
-# define MODE_THROW_ENABLED 1
+# define MODE_THROW_ENABLED ENABLED
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
 // ZigZag - allow vehicle to fly in a zigzag manner with predefined point A B
 #ifndef MODE_ZIGZAG_ENABLED
-# define MODE_ZIGZAG_ENABLED 1
+# define MODE_ZIGZAG_ENABLED ENABLED
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
@@ -249,7 +275,7 @@
 //////////////////////////////////////////////////////////////////////////////
 // Weathervane - allow vehicle to yaw into wind
 #ifndef WEATHERVANE_ENABLED
-# define WEATHERVANE_ENABLED 1
+# define WEATHERVANE_ENABLED ENABLED
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
@@ -260,13 +286,13 @@
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     #if FRAME_CONFIG == HELI_FRAME
         #ifndef MODE_AUTOROTATE_ENABLED
-        # define MODE_AUTOROTATE_ENABLED 1
+        # define MODE_AUTOROTATE_ENABLED ENABLED
         #endif
     #else
-        # define MODE_AUTOROTATE_ENABLED 0
+        # define MODE_AUTOROTATE_ENABLED DISABLED
     #endif
 #else
-    # define MODE_AUTOROTATE_ENABLED 0
+    # define MODE_AUTOROTATE_ENABLED DISABLED
 #endif
 #endif
 
@@ -352,9 +378,6 @@
 #ifndef LAND_DETECTOR_ACCEL_MAX
 # define LAND_DETECTOR_ACCEL_MAX            1.0f    // vehicle acceleration must be under 1m/s/s
 #endif
-#ifndef LAND_DETECTOR_VEL_Z_MAX
-# define LAND_DETECTOR_VEL_Z_MAX              1.0f    // vehicle vertical velocity must be under 1m/s
-#endif
 
 //////////////////////////////////////////////////////////////////////////////
 // Flight mode definitions
@@ -407,7 +430,7 @@
 #endif
 
 #ifndef RTL_ALT_MIN
- # define RTL_ALT_MIN               30     // min height above ground for RTL (i.e 30cm)
+ # define RTL_ALT_MIN               200     // min height above ground for RTL (i.e 2m)
 #endif
 
 #ifndef RTL_CLIMB_MIN_DEFAULT
@@ -534,8 +557,16 @@
 // Fence, Rally and Terrain and AC_Avoidance defaults
 //
 
-#if MODE_FOLLOW_ENABLED && !AP_AVOIDANCE_ENABLED
-  #error Follow Mode relies on AP_AVOIDANCE_ENABLED which is disabled
+#ifndef AC_AVOID_ENABLED
+ #define AC_AVOID_ENABLED   ENABLED
+#endif
+
+#ifndef AC_OAPATHPLANNER_ENABLED
+ #define AC_OAPATHPLANNER_ENABLED   ENABLED
+#endif
+
+#if MODE_FOLLOW_ENABLED && !AC_AVOID_ENABLED
+  #error Follow Mode relies on AC_AVOID which is disabled
 #endif
 
 #if MODE_AUTO_ENABLED && !MODE_GUIDED_ENABLED
@@ -574,8 +605,8 @@
 // Developer Items
 //
 
-#ifndef AP_COPTER_ADVANCED_FAILSAFE_ENABLED
-# define AP_COPTER_ADVANCED_FAILSAFE_ENABLED 0
+#ifndef ADVANCED_FAILSAFE
+# define ADVANCED_FAILSAFE DISABLED
 #endif
 
 #ifndef CH_MODE_DEFAULT
@@ -583,13 +614,17 @@
 #endif
 
 #ifndef TOY_MODE_ENABLED
-#define TOY_MODE_ENABLED 0
+#define TOY_MODE_ENABLED DISABLED
 #endif
 
 #if TOY_MODE_ENABLED && FRAME_CONFIG == HELI_FRAME
   #error Toy mode is not available on Helicopters
 #endif
 
+#ifndef OSD_ENABLED
+ #define OSD_ENABLED DISABLED
+#endif
+
 #ifndef HAL_FRAME_TYPE_DEFAULT
 #define HAL_FRAME_TYPE_DEFAULT AP_Motors::MOTOR_FRAME_TYPE_X
 #endif
@@ -603,5 +638,5 @@
 #endif
 
 #ifndef USER_PARAMS_ENABLED
-  #define USER_PARAMS_ENABLED 0
+  #define USER_PARAMS_ENABLED DISABLED
 #endif

ArduCopter/crash_check.cpp

@@ -36,18 +36,18 @@ void Copter::crash_check()
     }
 
     // exit immediately if in force flying
-    if (get_force_flying() && !flightmode->is_landing()) {
+    if (force_flying && !flightmode->is_landing()) {
         crash_counter = 0;
         return;
     }
 
     // return immediately if we are not in an angle stabilize flight mode or we are flipping
-    if (!flightmode->crash_check_enabled()) {
+    if (flightmode->mode_number() == Mode::Number::ACRO || flightmode->mode_number() == Mode::Number::FLIP) {
         crash_counter = 0;
         return;
     }
 
-#if MODE_AUTOROTATE_ENABLED
+#if MODE_AUTOROTATE_ENABLED == ENABLED
     //return immediately if in autorotation mode
     if (flightmode->mode_number() == Mode::Number::AUTOROTATE) {
         crash_counter = 0;
@@ -102,7 +102,7 @@ void Copter::thrust_loss_check()
     static uint16_t thrust_loss_counter;  // number of iterations vehicle may have been crashed
 
     // no-op if suppressed by flight options param
-    if (copter.option_is_enabled(FlightOption::DISABLE_THRUST_LOSS_CHECK)) {
+    if ((copter.g2.flight_options & uint32_t(FlightOptions::DISABLE_THRUST_LOSS_CHECK)) != 0) {
         return;
     }
 
@@ -171,7 +171,7 @@ void Copter::thrust_loss_check()
         // the motors library disables this when it is no longer needed to achieve the commanded output
 
 #if AP_GRIPPER_ENABLED
-        if (copter.option_is_enabled(FlightOption::RELEASE_GRIPPER_ON_THRUST_LOSS)) {
+        if ((copter.g2.flight_options & uint32_t(FlightOptions::RELEASE_GRIPPER_ON_THRUST_LOSS)) != 0) {
             gripper.release();
         }
 #endif
@@ -182,7 +182,7 @@ void Copter::thrust_loss_check()
 void Copter::yaw_imbalance_check()
 {
     // no-op if suppressed by flight options param
-    if (copter.option_is_enabled(FlightOption::DISABLE_YAW_IMBALANCE_WARNING)) {
+    if ((copter.g2.flight_options & uint32_t(FlightOptions::DISABLE_YAW_IMBALANCE_WARNING)) != 0) {
         return;
     }
 
@@ -229,7 +229,7 @@ void Copter::yaw_imbalance_check()
     }
 }
 
-#if HAL_PARACHUTE_ENABLED
+#if PARACHUTE == ENABLED
 
 // Code to detect a crash main ArduCopter code
 #define PARACHUTE_CHECK_TRIGGER_SEC         1       // 1 second of loss of control triggers the parachute
@@ -269,11 +269,12 @@ void Copter::parachute_check()
     }
 
     if (parachute.release_initiated()) {
+        copter.arming.disarm(AP_Arming::Method::PARACHUTE_RELEASE);
         return;
     }
 
     // return immediately if we are not in an angle stabilize flight mode or we are flipping
-    if (!flightmode->crash_check_enabled()) {
+    if (flightmode->mode_number() == Mode::Number::ACRO || flightmode->mode_number() == Mode::Number::FLIP) {
         control_loss_count = 0;
         return;
     }
@@ -330,6 +331,9 @@ void Copter::parachute_check()
 // parachute_release - trigger the release of the parachute, disarm the motors and notify the user
 void Copter::parachute_release()
 {
+    // disarm motors
+    arming.disarm(AP_Arming::Method::PARACHUTE_RELEASE);
+
     // release parachute
     parachute.release();
 
@@ -369,4 +373,4 @@ void Copter::parachute_manual_release()
     parachute_release();
 }
 
-#endif  // HAL_PARACHUTE_ENABLED
+#endif // PARACHUTE == ENABLED

ArduCopter/defines.h

@@ -2,6 +2,14 @@
 
 #include <AP_HAL/AP_HAL_Boards.h>
 
+// Just so that it's completely clear...
+#define ENABLED                 1
+#define DISABLED                0
+
+// this avoids a very common config error
+#define ENABLE ENABLED
+#define DISABLE DISABLED
+
 // Frame types
 #define UNDEFINED_FRAME 0
 #define MULTICOPTER_FRAME 1
@@ -87,8 +95,7 @@ enum LoggingParameters {
      LOG_GUIDED_POSITION_TARGET_MSG,
      LOG_SYSIDD_MSG,
      LOG_SYSIDS_MSG,
-     LOG_GUIDED_ATTITUDE_TARGET_MSG,
-     LOG_RATE_THREAD_DT_MSG
+     LOG_GUIDED_ATTITUDE_TARGET_MSG
 };
 
 #define MASK_LOG_ATTITUDE_FAST          (1<<0)

ArduCopter/esc_calibration.cpp

@@ -95,10 +95,9 @@ void Copter::esc_calibration_passthrough()
         hal.scheduler->delay(3);
 
         // pass through to motors
-        auto &srv = AP::srv();
-        srv.cork();
+        SRV_Channels::cork();
         motors->set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() * 0.001f);
-        srv.push();
+        SRV_Channels::push();
     }
 #endif  // FRAME_CONFIG != HELI_FRAME
 }
@@ -113,26 +112,25 @@ void Copter::esc_calibration_auto()
     esc_calibration_setup();
 
     // raise throttle to maximum
-    auto &srv = AP::srv();
-    srv.cork();
+    SRV_Channels::cork();
     motors->set_throttle_passthrough_for_esc_calibration(1.0f);
-    srv.push();
+    SRV_Channels::push();
 
     // delay for 5 seconds while outputting pulses
     uint32_t tstart = millis();
     while (millis() - tstart < 5000) {
-        srv.cork();
+        SRV_Channels::cork();
         motors->set_throttle_passthrough_for_esc_calibration(1.0f);
-        srv.push();
+        SRV_Channels::push();
         esc_calibration_notify();
         hal.scheduler->delay(3);
     }
 
     // block until we restart
     while(1) {
-        srv.cork();
+        SRV_Channels::cork();
         motors->set_throttle_passthrough_for_esc_calibration(0.0f);
-        srv.push();
+        SRV_Channels::push();
         esc_calibration_notify();
         hal.scheduler->delay(3);
     }

ArduCopter/events.cpp

@@ -286,7 +286,7 @@ void Copter::failsafe_terrain_on_event()
 
     if (should_disarm_on_failsafe()) {
         arming.disarm(AP_Arming::Method::TERRAINFAILSAFE);
-#if MODE_RTL_ENABLED
+#if MODE_RTL_ENABLED == ENABLED
     } else if (flightmode->mode_number() == Mode::Number::RTL) {
         mode_rtl.restart_without_terrain();
 #endif
@@ -379,53 +379,48 @@ void Copter::failsafe_deadreckon_check()
 //  this is always called from a failsafe so we trigger notification to pilot
 void Copter::set_mode_RTL_or_land_with_pause(ModeReason reason)
 {
-#if MODE_RTL_ENABLED
     // attempt to switch to RTL, if this fails then switch to Land
-    if (set_mode(Mode::Number::RTL, reason)) {
+    if (!set_mode(Mode::Number::RTL, reason)) {
+        // set mode to land will trigger mode change notification to pilot
+        set_mode_land_with_pause(reason);
+    } else {
+        // alert pilot to mode change
         AP_Notify::events.failsafe_mode_change = 1;
-        return;
     }
-#endif
-    // set mode to land will trigger mode change notification to pilot
-    set_mode_land_with_pause(reason);
 }
 
 // set_mode_SmartRTL_or_land_with_pause - sets mode to SMART_RTL if possible or LAND with 4 second delay before descent starts
 // this is always called from a failsafe so we trigger notification to pilot
 void Copter::set_mode_SmartRTL_or_land_with_pause(ModeReason reason)
 {
-#if MODE_SMARTRTL_ENABLED
     // attempt to switch to SMART_RTL, if this failed then switch to Land
-    if (set_mode(Mode::Number::SMART_RTL, reason)) {
+    if (!set_mode(Mode::Number::SMART_RTL, reason)) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "SmartRTL Unavailable, Using Land Mode");
+        set_mode_land_with_pause(reason);
+    } else {
         AP_Notify::events.failsafe_mode_change = 1;
-        return;
     }
-#endif
-    gcs().send_text(MAV_SEVERITY_WARNING, "SmartRTL Unavailable, Using Land Mode");
-    set_mode_land_with_pause(reason);
 }
 
 // set_mode_SmartRTL_or_RTL - sets mode to SMART_RTL if possible or RTL if possible or LAND with 4 second delay before descent starts
 // this is always called from a failsafe so we trigger notification to pilot
 void Copter::set_mode_SmartRTL_or_RTL(ModeReason reason)
 {
-#if MODE_SMARTRTL_ENABLED
     // attempt to switch to SmartRTL, if this failed then attempt to RTL
     // if that fails, then land
-    if (set_mode(Mode::Number::SMART_RTL, reason)) {
+    if (!set_mode(Mode::Number::SMART_RTL, reason)) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "SmartRTL Unavailable, Trying RTL Mode");
+        set_mode_RTL_or_land_with_pause(reason);
+    } else {
         AP_Notify::events.failsafe_mode_change = 1;
-        return;
     }
-#endif
-    gcs().send_text(MAV_SEVERITY_WARNING, "SmartRTL Unavailable, Trying RTL Mode");
-    set_mode_RTL_or_land_with_pause(reason);
 }
 
 // Sets mode to Auto and jumps to DO_LAND_START, as set with AUTO_RTL param
 // This can come from failsafe or RC option
 void Copter::set_mode_auto_do_land_start_or_RTL(ModeReason reason)
 {
-#if MODE_AUTO_ENABLED
+#if MODE_AUTO_ENABLED == ENABLED
     if (set_mode(Mode::Number::AUTO_RTL, reason)) {
         AP_Notify::events.failsafe_mode_change = 1;
         return;
@@ -440,7 +435,7 @@ void Copter::set_mode_auto_do_land_start_or_RTL(ModeReason reason)
 // This can come from failsafe or RC option
 void Copter::set_mode_brake_or_land_with_pause(ModeReason reason)
 {
-#if MODE_BRAKE_ENABLED
+#if MODE_BRAKE_ENABLED == ENABLED
     if (set_mode(Mode::Number::BRAKE, reason)) {
         AP_Notify::events.failsafe_mode_change = 1;
         return;
@@ -492,7 +487,7 @@ void Copter::do_failsafe_action(FailsafeAction action, ModeReason reason){
             set_mode_SmartRTL_or_land_with_pause(reason);
             break;
         case FailsafeAction::TERMINATE: {
-#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
+#if ADVANCED_FAILSAFE == ENABLED
             g2.afs.gcs_terminate(true, "Failsafe");
 #else
             arming.disarm(AP_Arming::Method::FAILSAFE_ACTION_TERMINATE);

ArduCopter/failsafe.cpp

@@ -7,7 +7,7 @@
 //  our failsafe strategy is to detect main loop lockup and disarm the motors
 //
 
-static bool failsafe_enabled;
+static bool failsafe_enabled = false;
 static uint16_t failsafe_last_ticks;
 static uint32_t failsafe_last_timestamp;
 static bool in_failsafe;
@@ -72,7 +72,7 @@ void Copter::failsafe_check()
 }
 
 
-#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
+#if ADVANCED_FAILSAFE == ENABLED
 /*
   check for AFS failsafe check
 */

ArduCopter/fence.cpp

@@ -10,10 +10,8 @@ void Copter::fence_check()
 {
     const uint8_t orig_breaches = fence.get_breaches();
 
-    bool is_landing_or_landed = flightmode->is_landing() || ap.land_complete  || !motors->armed();
-
     // check for new breaches; new_breaches is bitmask of fence types breached
-    const uint8_t new_breaches = fence.check(is_landing_or_landed);
+    const uint8_t new_breaches = fence.check();
 
     // we still don't do anything when disarmed, but we do check for fence breaches.
     // fence pre-arm check actually checks if any fence has been breached 
@@ -26,7 +24,7 @@ void Copter::fence_check()
     if (new_breaches) {
 
         if (!copter.ap.land_complete) {
-            fence.print_fence_message("breached", new_breaches);
+            GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence Breached");
         }
 
         // if the user wants some kind of response and motors are armed
@@ -83,10 +81,7 @@ void Copter::fence_check()
 
         LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches));
 
-    } else if (orig_breaches && fence.get_breaches() == 0) {
-        if (!copter.ap.land_complete) {
-            GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence breach cleared");
-        }
+    } else if (orig_breaches) {
         // record clearing of breach
         LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED);
     }

ArduCopter/heli.cpp

@@ -42,13 +42,11 @@ void Copter::check_dynamic_flight(void)
         moving = (motors->get_throttle() > 0.8f || ahrs.pitch_sensor < -1500);
     }
 
-#if AP_RANGEFINDER_ENABLED
     if (!moving && rangefinder_state.enabled && rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::Good) {
         // when we are more than 2m from the ground with good
         // rangefinder lock consider it to be dynamic flight
         moving = (rangefinder.distance_cm_orient(ROTATION_PITCH_270) > 200);
     }
-#endif
 
     if (moving) {
         // if moving for 2 seconds, set the dynamic flight flag
@@ -106,9 +104,8 @@ void Copter::update_heli_control_dynamics(void)
 bool Copter::should_use_landing_swash() const
 {
     if (flightmode->has_manual_throttle() ||
-        flightmode->mode_number() == Mode::Number::DRIFT ||
-        attitude_control->get_inverted_flight()) {
-        // manual modes or modes using inverted flight uses full swash range
+        flightmode->mode_number() == Mode::Number::DRIFT) {
+        // manual modes always uses full swash range
         return false;
     }
     if (flightmode->is_landing()) {
@@ -157,6 +154,9 @@ float Copter::get_pilot_desired_rotor_speed() const
 // heli_update_rotor_speed_targets - reads pilot input and passes new rotor speed targets to heli motors object
 void Copter::heli_update_rotor_speed_targets()
 {
+
+    static bool rotor_runup_complete_last = false;
+
     // get rotor control method
     uint8_t rsc_control_mode = motors->get_rsc_mode();
 
@@ -182,6 +182,13 @@ void Copter::heli_update_rotor_speed_targets()
         break;
     }
 
+    // when rotor_runup_complete changes to true, log event
+    if (!rotor_runup_complete_last && motors->rotor_runup_complete()) {
+        LOGGER_WRITE_EVENT(LogEvent::ROTOR_RUNUP_COMPLETE);
+    } else if (rotor_runup_complete_last && !motors->rotor_runup_complete() && !heli_flags.in_autorotation) {
+        LOGGER_WRITE_EVENT(LogEvent::ROTOR_SPEED_BELOW_CRITICAL);
+    }
+    rotor_runup_complete_last = motors->rotor_runup_complete();
 }
 
 
@@ -189,25 +196,32 @@ void Copter::heli_update_rotor_speed_targets()
 // to autorotation flight mode if manual collective is not being used.
 void Copter::heli_update_autorotation()
 {
-    bool in_autorotation_mode = false;
-#if MODE_AUTOROTATE_ENABLED
-    in_autorotation_mode = flightmode == &mode_autorotate;
+    // check if flying and interlock disengaged
+    if (!ap.land_complete && !motors->get_interlock()) {
+#if MODE_AUTOROTATE_ENABLED == ENABLED
+        if (g2.arot.is_enable()) {
+            if (!flightmode->has_manual_throttle()) {
+                // set autonomous autorotation flight mode
+                set_mode(Mode::Number::AUTOROTATE, ModeReason::AUTOROTATION_START);
+            }
+            // set flag to facilitate both auto and manual autorotations
+            heli_flags.in_autorotation = true;
+            motors->set_in_autorotation(heli_flags.in_autorotation);
+            motors->set_enable_bailout(true);
+        }
 #endif
-
-    // If we have landed then we do not want to be in autorotation and we do not want to via the bailout state
-    if (ap.land_complete || ap.land_complete_maybe) {
-        motors->force_deactivate_autorotation();
-        return;
-    }
-
-    // if we got this far we are flying, check for conditions to set autorotation state
-    if (!motors->get_interlock() && (flightmode->has_manual_throttle() || in_autorotation_mode)) {
-        // set state in motors to facilitate manual and assisted autorotations
-        motors->set_autorotation_active(true);
+        if (flightmode->has_manual_throttle() && motors->arot_man_enabled()) {
+            // set flag to facilitate both auto and manual autorotations
+            heli_flags.in_autorotation = true;
+            motors->set_in_autorotation(heli_flags.in_autorotation);
+            motors->set_enable_bailout(true);
+        }
     } else {
-        // deactivate the autorotation state via the bailout case
-        motors->set_autorotation_active(false);
+        heli_flags.in_autorotation = false;
+        motors->set_in_autorotation(heli_flags.in_autorotation);
+        motors->set_enable_bailout(false);
     }
+
 }
 
 // update collective low flag.  Use a debounce time of 400 milliseconds.

ArduCopter/land_detector.cpp

@@ -22,7 +22,7 @@ void Copter::update_land_and_crash_detectors()
 
     update_land_detector();
 
-#if HAL_PARACHUTE_ENABLED
+#if PARACHUTE == ENABLED
     // check parachute
     parachute_check();
 #endif
@@ -44,13 +44,6 @@ void Copter::update_land_detector()
     // range finder :                       tend to be problematic at very short distances
     // input throttle :                     in slow land the input throttle may be only slightly less than hover
 
-#if HAL_LOGGING_ENABLED
-    uint16_t logging_flags = 0;
-#define SET_LOG_FLAG(condition, flag) if (condition) { logging_flags |= (uint16_t)flag; }
-#else
-#define SET_LOG_FLAG(condition, flag)
-#endif
-
     if (!motors->armed()) {
         // if disarmed, always landed.
         set_land_complete(true);
@@ -81,12 +74,11 @@ void Copter::update_land_detector()
 
         // check if landing
         const bool landing = flightmode->is_landing();
-        SET_LOG_FLAG(landing, LandDetectorLoggingFlag::LANDING);
         bool motor_at_lower_limit = (flightmode->has_manual_throttle() && (motors->get_below_land_min_coll() || heli_flags.coll_stk_low) && fabsf(ahrs.get_roll()) < M_PI/2.0f)
-#if MODE_AUTOROTATE_ENABLED
+#if MODE_AUTOROTATE_ENABLED == ENABLED
                                     || (flightmode->mode_number() == Mode::Number::AUTOROTATE && motors->get_below_land_min_coll())
 #endif
-                                    || ((!get_force_flying() || landing) && motors->limit.throttle_lower && pos_control->get_vel_desired_cms().z < 0.0f);
+                                    || ((!force_flying || landing) && motors->limit.throttle_lower && pos_control->get_vel_desired_cms().z < 0.0f);
         bool throttle_mix_at_min = true;
 #else
         // check that the average throttle output is near minimum (less than 12.5% hover throttle)
@@ -99,8 +91,6 @@ void Copter::update_land_detector()
             throttle_mix_at_min = true;
         }
 #endif
-        SET_LOG_FLAG(motor_at_lower_limit, LandDetectorLoggingFlag::MOTOR_AT_LOWER_LIMIT);
-        SET_LOG_FLAG(throttle_mix_at_min, LandDetectorLoggingFlag::THROTTLE_MIX_AT_MIN);
 
         uint8_t land_detector_scalar = 1;
 #if AP_LANDINGGEAR_ENABLED
@@ -110,27 +100,14 @@ void Copter::update_land_detector()
         }
 #endif
 
-        // check for aggressive flight requests - requested roll or pitch angle below 15 degrees
-        const Vector3f angle_target = attitude_control->get_att_target_euler_cd();
-        bool large_angle_request = angle_target.xy().length() > LAND_CHECK_LARGE_ANGLE_CD;
-        SET_LOG_FLAG(large_angle_request, LandDetectorLoggingFlag::LARGE_ANGLE_REQUEST);
-
-        // check for large external disturbance - angle error over 30 degrees
-        const float angle_error = attitude_control->get_att_error_angle_deg();
-        bool large_angle_error = (angle_error > LAND_CHECK_ANGLE_ERROR_DEG);
-        SET_LOG_FLAG(large_angle_error, LandDetectorLoggingFlag::LARGE_ANGLE_ERROR);
-
         // check that the airframe is not accelerating (not falling or braking after fast forward flight)
         bool accel_stationary = (land_accel_ef_filter.get().length() <= LAND_DETECTOR_ACCEL_MAX * land_detector_scalar);
-        SET_LOG_FLAG(accel_stationary, LandDetectorLoggingFlag::ACCEL_STATIONARY);
 
         // check that vertical speed is within 1m/s of zero
-        bool descent_rate_low = fabsf(inertial_nav.get_velocity_z_up_cms()) < 100.0 * LAND_DETECTOR_VEL_Z_MAX * land_detector_scalar;
-        SET_LOG_FLAG(descent_rate_low, LandDetectorLoggingFlag::DESCENT_RATE_LOW);
+        bool descent_rate_low = fabsf(inertial_nav.get_velocity_z_up_cms()) < 100 * land_detector_scalar;
 
         // if we have a healthy rangefinder only allow landing detection below 2 meters
         bool rangefinder_check = (!rangefinder_alt_ok() || rangefinder_state.alt_cm_filt.get() < LAND_RANGEFINDER_MIN_ALT_CM);
-        SET_LOG_FLAG(rangefinder_check, LandDetectorLoggingFlag::RANGEFINDER_BELOW_2M);
 
         // if we have weight on wheels (WoW) or ambiguous unknown. never no WoW
 #if AP_LANDINGGEAR_ENABLED
@@ -138,9 +115,8 @@ void Copter::update_land_detector()
 #else
         const bool WoW_check = true;
 #endif
-        SET_LOG_FLAG(WoW_check, LandDetectorLoggingFlag::WOW);
 
-        if (motor_at_lower_limit && throttle_mix_at_min && !large_angle_request && !large_angle_error && accel_stationary && descent_rate_low && rangefinder_check && WoW_check) {
+        if (motor_at_lower_limit && throttle_mix_at_min && accel_stationary && descent_rate_low && rangefinder_check && WoW_check) {
             // landed criteria met - increment the counter and check if we've triggered
             if( land_detector_count < land_trigger_sec*scheduler.get_loop_rate_hz()) {
                 land_detector_count++;
@@ -154,53 +130,8 @@ void Copter::update_land_detector()
     }
 
     set_land_complete_maybe(ap.land_complete || (land_detector_count >= LAND_DETECTOR_MAYBE_TRIGGER_SEC*scheduler.get_loop_rate_hz()));
-
-#if HAL_LOGGING_ENABLED
-// @LoggerMessage: LDET
-// @Description: Land Detector State
-// @Field: TimeUS: Time since system startup
-// @Field: Flags: boolean state flags
-// @FieldBitmaskEnum: Flags: Copter::LandDetectorLoggingFlag
-// @Field: Count: landing_detector pass count
-    SET_LOG_FLAG(ap.land_complete, LandDetectorLoggingFlag::LANDED);
-    SET_LOG_FLAG(ap.land_complete_maybe, LandDetectorLoggingFlag::LANDED_MAYBE);
-    SET_LOG_FLAG(standby_active, LandDetectorLoggingFlag::STANDBY_ACTIVE);
-    Log_LDET(logging_flags, land_detector_count);
-#undef SET_LOG_FLAG
-#endif
 }
 
-#if HAL_LOGGING_ENABLED
-void Copter::Log_LDET(uint16_t logging_flags, uint32_t detector_count)
-{
-    // do not log if no change:
-    if (logging_flags == land_detector.last_logged_flags &&
-        detector_count == land_detector.last_logged_count) {
-        return;
-    }
-    // do not log more than 50Hz:
-    const auto now = AP_HAL::millis();
-    if (now - land_detector.last_logged_ms < 20) {
-        return;
-    }
-
-    land_detector.last_logged_count = detector_count;
-    land_detector.last_logged_flags = logging_flags;
-    land_detector.last_logged_ms = now;
-
-    AP::logger().WriteStreaming(
-        "LDET",
-        "TimeUS," "Flags," "Count",
-        "s"       "-"      "-",
-        "F"       "-"      "-",
-        "Q"       "H"      "I",
-        AP_HAL::micros64(),
-        logging_flags,
-        land_detector_count
-    );
-}
-#endif
-
 // set land_complete flag and disarm motors if disarm-on-land is configured
 void Copter::set_land_complete(bool b)
 {
@@ -225,39 +156,14 @@ void Copter::set_land_complete(bool b)
 
     // tell AHRS flying state
     set_likely_flying(!b);
+    
+    // trigger disarm-on-land if configured
+    bool disarm_on_land_configured = (g.throttle_behavior & THR_BEHAVE_DISARM_ON_LAND_DETECT) != 0;
+    const bool mode_disarms_on_land = flightmode->allows_arming(AP_Arming::Method::LANDING) && !flightmode->has_manual_throttle();
 
-    if (!b) {
-        // not landed, no further action
-        return;
+    if (ap.land_complete && motors->armed() && disarm_on_land_configured && mode_disarms_on_land) {
+        arming.disarm(AP_Arming::Method::LANDED);
     }
-
-    // landed; trigger disarm-on-land if configured
-    if ((g.throttle_behavior & THR_BEHAVE_DISARM_ON_LAND_DETECT) == 0) {
-        // not configured to disarm on landing detection
-        return;
-    }
-
-    if (!motors->armed()) {
-        // we are not currently armed, so we don't need to disarm:
-        // n.b. should this be checking vehicle-armed rather than motors-armed?
-        return;
-    }
-
-    if (flightmode->has_manual_throttle()) {
-        // we do not use the landing detector to disarm if the vehicle
-        // is in e.g. STABILIZE.  The normal DISARM_DELAY logic applies.
-        return;
-    }
-
-    // the flightmode may not allow disarm on landing.  Note that this
-    // check returns false for the LAND flight mode - it checks the
-    // landing detector (ap.land_complete) itself.
-    if (!flightmode->allows_arming(AP_Arming::Method::LANDING)) {
-        return;
-    }
-
-    // all checks passed, disarm the vehicle:
-    arming.disarm(AP_Arming::Method::LANDED);
 }
 
 // set land complete maybe flag
@@ -312,7 +218,7 @@ void Copter::update_throttle_mix()
         // check if landing
         const bool landing = flightmode->is_landing();
 
-        if (((large_angle_request || get_force_flying()) && !landing) || large_angle_error || accel_moving || descent_not_demanded) {
+        if (((large_angle_request || force_flying) && !landing) || large_angle_error || accel_moving || descent_not_demanded) {
             attitude_control->set_throttle_mix_max(pos_control->get_vel_z_control_ratio());
         } else {
             attitude_control->set_throttle_mix_min();
@@ -320,14 +226,3 @@ void Copter::update_throttle_mix()
     }
 #endif
 }
-
-// helper function for force flying flag
-bool Copter::get_force_flying() const
-{
-#if FRAME_CONFIG == HELI_FRAME
-    if (attitude_control->get_inverted_flight()) {
-        return true;
-    }
-#endif
-    return force_flying;
-}

ArduCopter/landing_gear.cpp

@@ -14,7 +14,6 @@ void Copter::landinggear_update()
     int32_t height_cm = flightmode->get_alt_above_ground_cm();
 
     // use rangefinder if available
-#if AP_RANGEFINDER_ENABLED
     switch (rangefinder.status_orient(ROTATION_PITCH_270)) {
     case RangeFinder::Status::NotConnected:
     case RangeFinder::Status::NoData:
@@ -32,7 +31,6 @@ void Copter::landinggear_update()
         height_cm = rangefinder_state.alt_cm_filt.get();
         break;
     }
-#endif  // AP_RANGEFINDER_ENABLED
 
     landinggear.update(height_cm * 0.01f); // convert cm->m for update call
 }

ArduCopter/mode.cpp

@@ -32,141 +32,158 @@ PayloadPlace Mode::payload_place;
 // return the static controller object corresponding to supplied mode
 Mode *Copter::mode_from_mode_num(const Mode::Number mode)
 {
+    Mode *ret = nullptr;
 
     switch (mode) {
-#if MODE_ACRO_ENABLED
+#if MODE_ACRO_ENABLED == ENABLED
         case Mode::Number::ACRO:
-            return &mode_acro;
+            ret = &mode_acro;
+            break;
 #endif
 
         case Mode::Number::STABILIZE:
-            return &mode_stabilize;
+            ret = &mode_stabilize;
+            break;
 
         case Mode::Number::ALT_HOLD:
-            return &mode_althold;
+            ret = &mode_althold;
+            break;
 
-#if MODE_AUTO_ENABLED
+#if MODE_AUTO_ENABLED == ENABLED
         case Mode::Number::AUTO:
-            return &mode_auto;
+            ret = &mode_auto;
+            break;
 #endif
 
-#if MODE_CIRCLE_ENABLED
+#if MODE_CIRCLE_ENABLED == ENABLED
         case Mode::Number::CIRCLE:
-            return &mode_circle;
+            ret = &mode_circle;
+            break;
 #endif
 
-#if MODE_LOITER_ENABLED
+#if MODE_LOITER_ENABLED == ENABLED
         case Mode::Number::LOITER:
-            return &mode_loiter;
+            ret = &mode_loiter;
+            break;
 #endif
 
-#if MODE_GUIDED_ENABLED
+#if MODE_GUIDED_ENABLED == ENABLED
         case Mode::Number::GUIDED:
-            return &mode_guided;
+            ret = &mode_guided;
+            break;
 #endif
 
         case Mode::Number::LAND:
-            return &mode_land;
+            ret = &mode_land;
+            break;
 
-#if MODE_RTL_ENABLED
+#if MODE_RTL_ENABLED == ENABLED
         case Mode::Number::RTL:
-            return &mode_rtl;
+            ret = &mode_rtl;
+            break;
 #endif
 
-#if MODE_DRIFT_ENABLED
+#if MODE_DRIFT_ENABLED == ENABLED
         case Mode::Number::DRIFT:
-            return &mode_drift;
+            ret = &mode_drift;
+            break;
 #endif
 
-#if MODE_SPORT_ENABLED
+#if MODE_SPORT_ENABLED == ENABLED
         case Mode::Number::SPORT:
-            return &mode_sport;
+            ret = &mode_sport;
+            break;
 #endif
 
-#if MODE_FLIP_ENABLED
+#if MODE_FLIP_ENABLED == ENABLED
         case Mode::Number::FLIP:
-            return &mode_flip;
+            ret = &mode_flip;
+            break;
 #endif
 
-#if AUTOTUNE_ENABLED
+#if AUTOTUNE_ENABLED == ENABLED
         case Mode::Number::AUTOTUNE:
-            return &mode_autotune;
+            ret = &mode_autotune;
+            break;
 #endif
 
-#if MODE_POSHOLD_ENABLED
+#if MODE_POSHOLD_ENABLED == ENABLED
         case Mode::Number::POSHOLD:
-            return &mode_poshold;
+            ret = &mode_poshold;
+            break;
 #endif
 
-#if MODE_BRAKE_ENABLED
+#if MODE_BRAKE_ENABLED == ENABLED
         case Mode::Number::BRAKE:
-            return &mode_brake;
+            ret = &mode_brake;
+            break;
 #endif
 
-#if MODE_THROW_ENABLED
+#if MODE_THROW_ENABLED == ENABLED
         case Mode::Number::THROW:
-            return &mode_throw;
+            ret = &mode_throw;
+            break;
 #endif
 
 #if HAL_ADSB_ENABLED
         case Mode::Number::AVOID_ADSB:
-            return &mode_avoid_adsb;
+            ret = &mode_avoid_adsb;
+            break;
 #endif
 
-#if MODE_GUIDED_NOGPS_ENABLED
+#if MODE_GUIDED_NOGPS_ENABLED == ENABLED
         case Mode::Number::GUIDED_NOGPS:
-            return &mode_guided_nogps;
+            ret = &mode_guided_nogps;
+            break;
 #endif
 
-#if MODE_SMARTRTL_ENABLED
+#if MODE_SMARTRTL_ENABLED == ENABLED
         case Mode::Number::SMART_RTL:
-            return &mode_smartrtl;
+            ret = &mode_smartrtl;
+            break;
 #endif
 
-#if MODE_FLOWHOLD_ENABLED
+#if MODE_FLOWHOLD_ENABLED == ENABLED
         case Mode::Number::FLOWHOLD:
-            return (Mode *)g2.mode_flowhold_ptr;
+            ret = (Mode *)g2.mode_flowhold_ptr;
+            break;
 #endif
 
-#if MODE_FOLLOW_ENABLED
+#if MODE_FOLLOW_ENABLED == ENABLED
         case Mode::Number::FOLLOW:
-            return &mode_follow;
+            ret = &mode_follow;
+            break;
 #endif
 
-#if MODE_ZIGZAG_ENABLED
+#if MODE_ZIGZAG_ENABLED == ENABLED
         case Mode::Number::ZIGZAG:
-            return &mode_zigzag;
+            ret = &mode_zigzag;
+            break;
 #endif
 
-#if MODE_SYSTEMID_ENABLED
+#if MODE_SYSTEMID_ENABLED == ENABLED
         case Mode::Number::SYSTEMID:
-            return (Mode *)g2.mode_systemid_ptr;
+            ret = (Mode *)g2.mode_systemid_ptr;
+            break;
 #endif
 
-#if MODE_AUTOROTATE_ENABLED
+#if MODE_AUTOROTATE_ENABLED == ENABLED
         case Mode::Number::AUTOROTATE:
-            return &mode_autorotate;
+            ret = &mode_autorotate;
+            break;
 #endif
 
-#if MODE_TURTLE_ENABLED
+#if MODE_TURTLE_ENABLED == ENABLED
         case Mode::Number::TURTLE:
-            return &mode_turtle;
+            ret = &mode_turtle;
+            break;
 #endif
 
         default:
             break;
     }
 
-#if MODE_GUIDED_ENABLED && AP_SCRIPTING_ENABLED
-    // Check registered custom modes
-    for (uint8_t i = 0; i < ARRAY_SIZE(mode_guided_custom); i++) {
-        if ((mode_guided_custom[i] != nullptr) && (mode_guided_custom[i]->mode_number() == mode)) {
-            return mode_guided_custom[i];
-        }
-    }
-#endif
-
-    return nullptr;
+    return ret;
 }
 
 
@@ -256,11 +273,10 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
         return false;
     }
 
-#if MODE_AUTO_ENABLED
+#if MODE_AUTO_ENABLED == ENABLED
     if (mode == Mode::Number::AUTO_RTL) {
         // Special case for AUTO RTL, not a true mode, just AUTO in disguise
-        // Attempt to join return path, fallback to do-land-start
-        return mode_auto.return_path_or_jump_to_landing_sequence_auto_RTL(reason);
+        return mode_auto.jump_to_landing_sequence_auto_RTL(reason);
     }
 #endif
 
@@ -275,9 +291,20 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
 #if FRAME_CONFIG == HELI_FRAME
     // do not allow helis to enter a non-manual throttle mode if the
     // rotor runup is not complete
-    if (!ignore_checks && !new_flightmode->has_manual_throttle() && !motors->rotor_runup_complete()) {
-        mode_change_failed(new_flightmode, "runup not complete");
-        return false;
+    if (!ignore_checks && !new_flightmode->has_manual_throttle() &&
+        (motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_UP || motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_DOWN)) {
+        #if MODE_AUTOROTATE_ENABLED == ENABLED
+            //if the mode being exited is the autorotation mode allow mode change despite rotor not being at
+            //full speed.  This will reduce altitude loss on bail-outs back to non-manual throttle modes
+            bool in_autorotation_check = (flightmode != &mode_autorotate || new_flightmode != &mode_autorotate);
+        #else
+            bool in_autorotation_check = false;
+        #endif
+
+        if (!in_autorotation_check) {
+            mode_change_failed(new_flightmode, "runup not complete");
+            return false;
+        }
     }
 #endif
 
@@ -287,7 +314,7 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
     // (e.g. user arms in guided, raises throttle to 1300 (not enough to
     // trigger auto takeoff), then switches into manual):
     bool user_throttle = new_flightmode->has_manual_throttle();
-#if MODE_DRIFT_ENABLED
+#if MODE_DRIFT_ENABLED == ENABLED
     if (new_flightmode == &mode_drift) {
         user_throttle = true;
     }
@@ -319,20 +346,6 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
         return false;
     }
 
-#if AP_FENCE_ENABLED
-    // may not be allowed to change mode if recovering from fence breach
-    if (!ignore_checks &&
-        fence.enabled() &&
-        fence.option_enabled(AC_Fence::OPTIONS::DISABLE_MODE_CHANGE) &&
-        fence.get_breaches() &&
-        motors->armed() &&
-        get_control_mode_reason() == ModeReason::FENCE_BREACHED &&
-        !ap.land_complete) {
-        mode_change_failed(new_flightmode, "in fence recovery");
-        return false;
-    }
-#endif
-
     if (!new_flightmode->init(ignore_checks)) {
         mode_change_failed(new_flightmode, "init failed");
         return false;
@@ -341,6 +354,9 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
     // perform any cleanup required by previous flight mode
     exit_mode(flightmode, new_flightmode);
 
+    // store previous flight mode (only used by tradeheli's autorotation)
+    prev_control_mode = flightmode->mode_number();
+
     // update flight mode
     flightmode = new_flightmode;
     control_mode_reason = reason;
@@ -354,12 +370,10 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
 #endif
 
 #if AP_FENCE_ENABLED
-    if (fence.get_action() != AC_FENCE_ACTION_REPORT_ONLY) {
-        // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover
-        // this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe)
-        // but it should be harmless to disable the fence temporarily in these situations as well
-        fence.manual_recovery_start();
-    }
+    // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover
+    // this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe)
+    // but it should be harmless to disable the fence temporarily in these situations as well
+    fence.manual_recovery_start();
 #endif
 
 #if AP_CAMERA_ENABLED
@@ -367,11 +381,11 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
 #endif
 
     // set rate shaping time constants
-#if MODE_ACRO_ENABLED || MODE_SPORT_ENABLED
+#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
     attitude_control->set_roll_pitch_rate_tc(g2.command_model_acro_rp.get_rate_tc());
 #endif
     attitude_control->set_yaw_rate_tc(g2.command_model_pilot.get_rate_tc());
-#if MODE_ACRO_ENABLED || MODE_DRIFT_ENABLED
+#if MODE_ACRO_ENABLED == ENABLED || MODE_DRIFT_ENABLED == ENABLED
     if (mode== Mode::Number::ACRO || mode== Mode::Number::DRIFT) {
         attitude_control->set_yaw_rate_tc(g2.command_model_acro_y.get_rate_tc());
     }
@@ -405,10 +419,7 @@ bool Copter::set_mode(const uint8_t new_mode, const ModeReason reason)
 // called at 100hz or more
 void Copter::update_flight_mode()
 {
-#if AP_RANGEFINDER_ENABLED
     surface_tracking.invalidate_for_logging();  // invalidate surface tracking alt, flight mode will set to true if used
-#endif
-    attitude_control->landed_gain_reduction(copter.ap.land_complete); // Adjust gains when landed to attenuate ground oscillation
 
     flightmode->run();
 }
@@ -446,11 +457,6 @@ void Copter::exit_mode(Mode *&old_flightmode,
             input_manager.set_stab_col_ramp(0.0);
         }
     }
-
-    // Make sure inverted flight is disabled if not supported in the new mode
-    if (!new_flightmode->allows_inverted()) {
-        attitude_control->set_inverted_flight(false);
-    }
 #endif //HELI_FRAME
 }
 
@@ -936,7 +942,7 @@ float Mode::get_pilot_desired_throttle() const
 
 float Mode::get_avoidance_adjusted_climbrate(float target_rate)
 {
-#if AP_AVOIDANCE_ENABLED
+#if AC_AVOID_ENABLED == ENABLED
     AP::ac_avoid()->adjust_velocity_z(pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z_cmss(), target_rate, G_Dt);
     return target_rate;
 #else
@@ -961,19 +967,19 @@ Mode::AltHoldModeState Mode::get_alt_hold_state(float target_climb_rate_cms)
         switch (motors->get_spool_state()) {
 
         case AP_Motors::SpoolState::SHUT_DOWN:
-            return AltHoldModeState::MotorStopped;
+            return AltHold_MotorStopped;
 
         case AP_Motors::SpoolState::GROUND_IDLE:
-            return AltHoldModeState::Landed_Ground_Idle;
+            return AltHold_Landed_Ground_Idle;
 
         default:
-            return AltHoldModeState::Landed_Pre_Takeoff;
+            return AltHold_Landed_Pre_Takeoff;
         }
 
     } else if (takeoff.running() || takeoff.triggered(target_climb_rate_cms)) {
         // the aircraft is currently landed or taking off, asking for a positive climb rate and in THROTTLE_UNLIMITED
         // the aircraft should progress through the take off procedure
-        return AltHoldModeState::Takeoff;
+        return AltHold_Takeoff;
 
     } else if (!copter.ap.auto_armed || copter.ap.land_complete) {
         // the aircraft is armed and landed
@@ -988,32 +994,29 @@ Mode::AltHoldModeState Mode::get_alt_hold_state(float target_climb_rate_cms)
 
         if (motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
             // the aircraft is waiting in ground idle
-            return AltHoldModeState::Landed_Ground_Idle;
+            return AltHold_Landed_Ground_Idle;
 
         } else {
             // the aircraft can leave the ground at any time
-            return AltHoldModeState::Landed_Pre_Takeoff;
+            return AltHold_Landed_Pre_Takeoff;
         }
 
     } else {
         // the aircraft is in a flying state
         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
-        return AltHoldModeState::Flying;
+        return AltHold_Flying;
     }
 }
 
 // transform pilot's yaw input into a desired yaw rate
 // returns desired yaw rate in centi-degrees per second
-float Mode::get_pilot_desired_yaw_rate() const
+float Mode::get_pilot_desired_yaw_rate(float yaw_in)
 {
     // throttle failsafe check
     if (copter.failsafe.radio || !rc().has_ever_seen_rc_input()) {
         return 0.0f;
     }
 
-    // Get yaw input
-    const float yaw_in = channel_yaw->norm_input_dz();
-
     // convert pilot input to the desired yaw rate
     return g2.command_model_pilot.get_rate() * 100.0 * input_expo(yaw_in, g2.command_model_pilot.get_expo());
 }
@@ -1054,12 +1057,3 @@ uint16_t Mode::get_pilot_speed_dn()
 {
     return copter.get_pilot_speed_dn();
 }
-
-// Return stopping point as a location with above origin alt frame
-Location Mode::get_stopping_point() const
-{
-    Vector3p stopping_point_NEU;
-    copter.pos_control->get_stopping_point_xy_cm(stopping_point_NEU.xy());
-    copter.pos_control->get_stopping_point_z_cm(stopping_point_NEU.z);
-    return Location { stopping_point_NEU.tofloat(), Location::AltFrame::ABOVE_ORIGIN };
-}

ArduCopter/mode.h

@@ -3,11 +3,6 @@
 #include "Copter.h"
 #include <AP_Math/chirp.h>
 #include <AP_ExternalControl/AP_ExternalControl_config.h> // TODO why is this needed if Copter.h includes this
-
-#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
-#include "afs_copter.h"
-#endif
-
 class Parameters;
 class ParametersG2;
 
@@ -19,7 +14,7 @@ class _AutoTakeoff {
 public:
     void run();
     void start(float complete_alt_cm, bool terrain_alt);
-    bool get_completion_pos(Vector3p& pos_neu_cm);
+    bool get_position(Vector3p& completion_pos);
 
     bool complete;          // true when takeoff is complete
 
@@ -101,8 +96,6 @@ public:
         AUTO_RTL =     27,  // Auto RTL, this is not a true mode, AUTO will report as this mode if entered to perform a DO_LAND_START Landing sequence
         TURTLE =       28,  // Flip over after crash
 
-        // Mode number 30 reserved for "offboard" for external/lua control.
-
         // Mode number 127 reserved for the "drone show mode" in the Skybrush
         // fork at https://github.com/skybrush-io/ardupilot
     };
@@ -134,19 +127,6 @@ public:
     virtual bool allows_save_trim() const { return false; }
     virtual bool allows_autotune() const { return false; }
     virtual bool allows_flip() const { return false; }
-    virtual bool crash_check_enabled() const { return true; }
-
-#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
-    // Return the type of this mode for use by advanced failsafe
-    virtual AP_AdvancedFailsafe_Copter::control_mode afs_mode() const { return AP_AdvancedFailsafe_Copter::control_mode::AFS_STABILIZED; }
-#endif
-
-    // Return true if the throttle high arming check can be skipped when arming from GCS or Scripting
-    virtual bool allows_GCS_or_SCR_arming_with_throttle_high() const { return false; }
-
-#if FRAME_CONFIG == HELI_FRAME
-    virtual bool allows_inverted() const { return false; };
-#endif
 
     // return a string for this flightmode
     virtual const char *name() const = 0;
@@ -177,13 +157,19 @@ public:
     // pilot input processing
     void get_pilot_desired_lean_angles(float &roll_out_cd, float &pitch_out_cd, float angle_max_cd, float angle_limit_cd) const;
     Vector2f get_pilot_desired_velocity(float vel_max) const;
-    float get_pilot_desired_yaw_rate() const;
+    float get_pilot_desired_yaw_rate(float yaw_in);
     float get_pilot_desired_throttle() const;
 
     // returns climb target_rate reduced to avoid obstacles and
     // altitude fence
     float get_avoidance_adjusted_climbrate(float target_rate);
 
+    const Vector3f& get_vel_desired_cms() {
+        // note that position control isn't used in every mode, so
+        // this may return bogus data:
+        return pos_control->get_vel_desired_cms();
+    }
+
     // send output to the motors, can be overridden by subclasses
     virtual void output_to_motors();
 
@@ -194,11 +180,8 @@ public:
     virtual bool pause() { return false; };
     virtual bool resume() { return false; };
 
-    // handle situations where the vehicle is on the ground waiting for takeoff
-    void make_safe_ground_handling(bool force_throttle_unlimited = false);
-
     // true if weathervaning is allowed in the current mode
-#if WEATHERVANE_ENABLED
+#if WEATHERVANE_ENABLED == ENABLED
     virtual bool allows_weathervaning() const { return false; }
 #endif
 
@@ -208,9 +191,7 @@ protected:
     bool is_disarmed_or_landed() const;
     void zero_throttle_and_relax_ac(bool spool_up = false);
     void zero_throttle_and_hold_attitude();
-
-    // Return stopping point as a location with above origin alt frame
-    Location get_stopping_point() const;
+    void make_safe_ground_handling(bool force_throttle_unlimited = false);
 
     // functions to control normal landing.  pause_descent is true if vehicle should not descend
     void land_run_horizontal_control();
@@ -243,12 +224,12 @@ protected:
     virtual float throttle_hover() const;
 
     // Alt_Hold based flight mode states used in Alt_Hold, Loiter, and Sport
-    enum class AltHoldModeState {
-        MotorStopped,
-        Takeoff,
-        Landed_Ground_Idle,
-        Landed_Pre_Takeoff,
-        Flying
+    enum AltHoldModeState {
+        AltHold_MotorStopped,
+        AltHold_Takeoff,
+        AltHold_Landed_Ground_Idle,
+        AltHold_Landed_Pre_Takeoff,
+        AltHold_Flying
     };
     AltHoldModeState get_alt_hold_state(float target_climb_rate_cms);
 
@@ -337,11 +318,9 @@ public:
 
         void set_yaw_angle_rate(float yaw_angle_d, float yaw_rate_ds);
 
-        void set_yaw_angle_offset(const float yaw_angle_offset_d);
-
         bool reached_fixed_yaw_target();
 
-#if WEATHERVANE_ENABLED
+#if WEATHERVANE_ENABLED == ENABLED
         void update_weathervane(const int16_t pilot_yaw_cds);
 #endif
 
@@ -355,9 +334,7 @@ public:
         // rate_cds(): desired yaw rate in centidegrees/second:
         float rate_cds();
 
-        // returns a yaw in degrees, direction of vehicle travel:
         float look_ahead_yaw();
-
         float roi_yaw() const;
 
         // auto flight mode's yaw mode
@@ -400,7 +377,7 @@ public:
 };
 
 
-#if MODE_ACRO_ENABLED
+#if MODE_ACRO_ENABLED == ENABLED
 class ModeAcro : public Mode {
 
 public:
@@ -431,7 +408,6 @@ public:
     void air_mode_aux_changed();
     bool allows_save_trim() const override { return true; }
     bool allows_flip() const override { return true; }
-    bool crash_check_enabled() const override { return false; }
 
 protected:
 
@@ -485,9 +461,7 @@ public:
     }
     bool allows_autotune() const override { return true; }
     bool allows_flip() const override { return true; }
-#if FRAME_CONFIG == HELI_FRAME
-    bool allows_inverted() const override { return true; };
-#endif
+
 protected:
 
     const char *name() const override { return "ALT_HOLD"; }
@@ -515,17 +489,6 @@ public:
     bool allows_arming(AP_Arming::Method method) const override;
     bool is_autopilot() const override { return true; }
     bool in_guided_mode() const override { return _mode == SubMode::NAVGUIDED || _mode == SubMode::NAV_SCRIPT_TIME; }
-#if FRAME_CONFIG == HELI_FRAME
-    bool allows_inverted() const override { return true; };
-#endif
-
-#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
-    // Return the type of this mode for use by advanced failsafe
-    AP_AdvancedFailsafe_Copter::control_mode afs_mode() const override { return AP_AdvancedFailsafe_Copter::control_mode::AFS_AUTO; }
-#endif
-
-    // Return true if the throttle high arming check can be skipped when arming from GCS or Scripting
-    bool allows_GCS_or_SCR_arming_with_throttle_high() const override { return true; }
 
     // Auto modes
     enum class SubMode : uint8_t {
@@ -581,12 +544,6 @@ public:
     // Go straight to landing sequence via DO_LAND_START, if succeeds pretend to be Auto RTL mode
     bool jump_to_landing_sequence_auto_RTL(ModeReason reason);
 
-    // Join mission after DO_RETURN_PATH_START waypoint, if succeeds pretend to be Auto RTL mode
-    bool return_path_start_auto_RTL(ModeReason reason);
-
-    // Try join return path else do land start
-    bool return_path_or_jump_to_landing_sequence_auto_RTL(ModeReason reason);
-
     // lua accessors for nav script time support
     bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4);
     void nav_script_time_done(uint16_t id);
@@ -600,7 +557,7 @@ public:
     AP_Mission_ChangeDetector mis_change_detector;
 
     // true if weathervaning is allowed in auto
-#if WEATHERVANE_ENABLED
+#if WEATHERVANE_ENABLED == ENABLED
     bool allows_weathervaning(void) const override;
 #endif
 
@@ -616,16 +573,12 @@ protected:
 
 private:
 
-    enum class Option : int32_t {
+    enum class Options : int32_t {
         AllowArming                        = (1 << 0U),
         AllowTakeOffWithoutRaisingThrottle = (1 << 1U),
         IgnorePilotYaw                     = (1 << 2U),
         AllowWeatherVaning                 = (1 << 7U),
     };
-    bool option_is_enabled(Option option) const;
-
-    // Enter auto rtl pseudo mode
-    bool enter_auto_rtl(ModeReason reason);
 
     bool start_command(const AP_Mission::Mission_Command& cmd);
     bool verify_command(const AP_Mission::Mission_Command& cmd);
@@ -643,17 +596,12 @@ private:
     void loiter_to_alt_run();
     void nav_attitude_time_run();
 
-    // return the Location portion of a command.  If the command's lat and lon and/or alt are zero the default_loc's lat,lon and/or alt are returned instead
     Location loc_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc) const;
 
     SubMode _mode = SubMode::TAKEOFF;   // controls which auto controller is run
 
     bool shift_alt_to_current_alt(Location& target_loc) const;
 
-    // subtract position controller offsets from target location
-    // should be used when the location will be used as a target for the position controller
-    void subtract_pos_offsets(Location& target_loc) const;
-
     void do_takeoff(const AP_Mission::Mission_Command& cmd);
     void do_nav_wp(const AP_Mission::Mission_Command& cmd);
     bool set_next_wp(const AP_Mission::Mission_Command& current_cmd, const Location &default_loc);
@@ -664,7 +612,7 @@ private:
     void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd);
     void do_spline_wp(const AP_Mission::Mission_Command& cmd);
     void get_spline_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc, Location& dest_loc, Location& next_dest_loc, bool& next_dest_loc_is_spline);
-#if AC_NAV_GUIDED
+#if AC_NAV_GUIDED == ENABLED
     void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
     void do_guided_limits(const AP_Mission::Mission_Command& cmd);
 #endif
@@ -676,7 +624,7 @@ private:
     void do_set_home(const AP_Mission::Mission_Command& cmd);
     void do_roi(const AP_Mission::Mission_Command& cmd);
     void do_mount_control(const AP_Mission::Mission_Command& cmd);
-#if HAL_PARACHUTE_ENABLED
+#if PARACHUTE == ENABLED
     void do_parachute(const AP_Mission::Mission_Command& cmd);
 #endif
 #if AP_WINCH_ENABLED
@@ -702,7 +650,7 @@ private:
     bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
     bool verify_circle(const AP_Mission::Mission_Command& cmd);
     bool verify_spline_wp(const AP_Mission::Mission_Command& cmd);
-#if AC_NAV_GUIDED
+#if AC_NAV_GUIDED == ENABLED
     bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
 #endif
     bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
@@ -775,7 +723,7 @@ private:
     } desired_speed_override;
 };
 
-#if AUTOTUNE_ENABLED
+#if AUTOTUNE_ENABLED == ENABLED
 /*
   wrapper class for AC_AutoTune
  */
@@ -819,12 +767,18 @@ public:
     bool allows_arming(AP_Arming::Method method) const override { return false; }
     bool is_autopilot() const override { return false; }
 
-    AutoTune autotune;
+    void save_tuning_gains();
+    void reset();
 
 protected:
 
     const char *name() const override { return "AUTOTUNE"; }
     const char *name4() const override { return "ATUN"; }
+
+private:
+
+    AutoTune autotune;
+
 };
 #endif
 
@@ -930,7 +884,6 @@ public:
     bool has_manual_throttle() const override { return false; }
     bool allows_arming(AP_Arming::Method method) const override { return false; };
     bool is_autopilot() const override { return false; }
-    bool crash_check_enabled() const override { return false; }
 
 protected:
 
@@ -958,7 +911,7 @@ private:
 };
 
 
-#if MODE_FLOWHOLD_ENABLED
+#if MODE_FLOWHOLD_ENABLED == ENABLED
 /*
   class to support FLOWHOLD mode, which is a position hold mode using
   optical flow directly, avoiding the need for a rangefinder
@@ -1001,7 +954,7 @@ private:
     // calculate attitude from flow data
     void flow_to_angle(Vector2f &bf_angle);
 
-    LowPassFilterConstDtVector2f flow_filter;
+    LowPassFilterVector2f flow_filter;
 
     bool flowhold_init(bool ignore_checks);
     void flowhold_run();
@@ -1070,14 +1023,6 @@ public:
 
     bool requires_terrain_failsafe() const override { return true; }
 
-#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
-    // Return the type of this mode for use by advanced failsafe
-    AP_AdvancedFailsafe_Copter::control_mode afs_mode() const override { return AP_AdvancedFailsafe_Copter::control_mode::AFS_AUTO; }
-#endif
-
-    // Return true if the throttle high arming check can be skipped when arming from GCS or Scripting
-    bool allows_GCS_or_SCR_arming_with_throttle_high() const override { return true; }
-
     // Sets guided's angular target submode: Using a rotation quaternion, angular velocity, and climbrate or thrust (depends on user option)
     // attitude_quat: IF zero: ang_vel (angular velocity) must be provided even if all zeroes
     //                IF non-zero: attitude_control is performed using both the attitude quaternion and angular velocity
@@ -1147,7 +1092,7 @@ public:
     bool resume() override;
 
     // true if weathervaning is allowed in guided
-#if WEATHERVANE_ENABLED
+#if WEATHERVANE_ENABLED == ENABLED
     bool allows_weathervaning(void) const override;
 #endif
 
@@ -1163,7 +1108,7 @@ protected:
 private:
 
     // enum for GUID_OPTIONS parameter
-    enum class Option : uint32_t {
+    enum class Options : int32_t {
         AllowArmingFromTX   = (1U << 0),
         // this bit is still available, pilot yaw was mapped to bit 2 for symmetry with auto
         IgnorePilotYaw      = (1U << 2),
@@ -1174,9 +1119,6 @@ private:
         AllowWeatherVaning = (1U << 7)
     };
 
-    // returns true if the Guided-mode-option is set (see GUID_OPTIONS)
-    bool option_is_enabled(Option option) const;
-
     // wp controller
     void wp_control_start();
     void wp_control_run();
@@ -1195,37 +1137,14 @@ private:
     void set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle);
 
     // controls which controller is run (pos or vel):
-    static SubMode guided_mode;
-    static bool send_notification;     // used to send one time notification to ground station
-    static bool takeoff_complete;      // true once takeoff has completed (used to trigger retracting of landing gear)
+    SubMode guided_mode = SubMode::TakeOff;
+    bool send_notification;     // used to send one time notification to ground station
+    bool takeoff_complete;      // true once takeoff has completed (used to trigger retracting of landing gear)
 
     // guided mode is paused or not
-    static bool _paused;
+    bool _paused;
 };
 
-#if AP_SCRIPTING_ENABLED
-// Mode which behaves as guided with custom mode number and name
-class ModeGuidedCustom : public ModeGuided {
-public:
-    // constructor registers custom number and names
-    ModeGuidedCustom(const Number _number, const char* _full_name, const char* _short_name);
-
-    bool init(bool ignore_checks) override;
-
-    Number mode_number() const override { return number; }
-
-    const char *name() const override { return full_name; }
-    const char *name4() const override { return short_name; }
-
-    // State object which can be edited by scripting
-    AP_Vehicle::custom_mode_state state;
-
-private:
-    const Number number;
-    const char* full_name;
-    const char* short_name;
-};
-#endif
 
 class ModeGuidedNoGPS : public ModeGuided {
 
@@ -1268,11 +1187,6 @@ public:
 
     bool is_landing() const override { return true; };
 
-#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
-    // Return the type of this mode for use by advanced failsafe
-    AP_AdvancedFailsafe_Copter::control_mode afs_mode() const override { return AP_AdvancedFailsafe_Copter::control_mode::AFS_AUTO; }
-#endif
-
     void do_not_use_GPS();
 
     // returns true if LAND mode is trying to control X/Y position
@@ -1314,10 +1228,6 @@ public:
     bool has_user_takeoff(bool must_navigate) const override { return true; }
     bool allows_autotune() const override { return true; }
 
-#if FRAME_CONFIG == HELI_FRAME
-    bool allows_inverted() const override { return true; };
-#endif
-
 #if AC_PRECLAND_ENABLED
     void set_precision_loiter_enabled(bool value) { _precision_loiter_enabled = value; }
 #endif
@@ -1450,11 +1360,6 @@ public:
 
     bool requires_terrain_failsafe() const override { return true; }
 
-#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
-    // Return the type of this mode for use by advanced failsafe
-    AP_AdvancedFailsafe_Copter::control_mode afs_mode() const override { return AP_AdvancedFailsafe_Copter::control_mode::AFS_AUTO; }
-#endif
-
     // for reporting to GCS
     bool get_wp(Location &loc) const override;
 
@@ -1484,8 +1389,8 @@ public:
 
     // enum for RTL_ALT_TYPE parameter
     enum class RTLAltType : int8_t {
-        RELATIVE = 0,
-        TERRAIN = 1
+        RTL_ALTTYPE_RELATIVE = 0,
+        RTL_ALTTYPE_TERRAIN = 1
     };
     ModeRTL::RTLAltType get_alt_type() const;
 
@@ -1602,10 +1507,6 @@ private:
     // point while following our path home.  If we take too long we
     // may choose to land the vehicle.
     uint32_t path_follow_last_pop_fail_ms;
-
-    // backup last popped point so that it can be restored to the path
-    // if vehicle exits SmartRTL mode before reaching home. invalid if zero
-    Vector3f dest_NED_backup;
 };
 
 
@@ -1673,8 +1574,6 @@ public:
     bool init(bool ignore_checks) override;
     void run() override;
 
-    bool allows_inverted() const override { return true; };
-
 protected:
 
 private:
@@ -1712,29 +1611,22 @@ protected:
 private:
 
     void log_data() const;
-    bool is_poscontrol_axis_type() const;
 
     enum class AxisType {
-        NONE = 0,               // none
-        INPUT_ROLL = 1,         // angle input roll axis is being excited
-        INPUT_PITCH = 2,        // angle pitch axis is being excited
-        INPUT_YAW = 3,          // angle yaw axis is being excited
-        RECOVER_ROLL = 4,       // angle roll axis is being excited
-        RECOVER_PITCH = 5,      // angle pitch axis is being excited
-        RECOVER_YAW = 6,        // angle yaw axis is being excited
-        RATE_ROLL = 7,          // rate roll axis is being excited
-        RATE_PITCH = 8,         // rate pitch axis is being excited
-        RATE_YAW = 9,           // rate yaw axis is being excited
-        MIX_ROLL = 10,          // mixer roll axis is being excited
-        MIX_PITCH = 11,         // mixer pitch axis is being excited
-        MIX_YAW = 12,           // mixer pitch axis is being excited
-        MIX_THROTTLE = 13,      // mixer throttle axis is being excited
-        DISTURB_POS_LAT = 14,   // lateral body axis measured position is being excited
-        DISTURB_POS_LONG = 15,  // longitudinal body axis measured position is being excited
-        DISTURB_VEL_LAT = 16,   // lateral body axis measured velocity is being excited
-        DISTURB_VEL_LONG = 17,  // longitudinal body axis measured velocity is being excited
-        INPUT_VEL_LAT = 18,     // lateral body axis commanded velocity is being excited
-        INPUT_VEL_LONG = 19,    // longitudinal body axis commanded velocity is being excited
+        NONE = 0,           // none
+        INPUT_ROLL = 1,     // angle input roll axis is being excited
+        INPUT_PITCH = 2,    // angle pitch axis is being excited
+        INPUT_YAW = 3,      // angle yaw axis is being excited
+        RECOVER_ROLL = 4,   // angle roll axis is being excited
+        RECOVER_PITCH = 5,  // angle pitch axis is being excited
+        RECOVER_YAW = 6,    // angle yaw axis is being excited
+        RATE_ROLL = 7,      // rate roll axis is being excited
+        RATE_PITCH = 8,     // rate pitch axis is being excited
+        RATE_YAW = 9,       // rate yaw axis is being excited
+        MIX_ROLL = 10,      // mixer roll axis is being excited
+        MIX_PITCH = 11,     // mixer pitch axis is being excited
+        MIX_YAW = 12,       // mixer pitch axis is being excited
+        MIX_THROTTLE = 13,  // mixer throttle axis is being excited
     };
 
     AP_Int8 axis;               // Controls which axis are being excited. Set to non-zero to display other parameters
@@ -1751,9 +1643,7 @@ private:
     float waveform_freq_rads;   // Instantaneous waveform frequency
     float time_const_freq;      // Time at constant frequency before chirp starts
     int8_t log_subsample;       // Subsample multiple for logging.
-    Vector2f target_vel;        // target velocity for position controller modes
-    Vector2f target_pos;       // target positon
-    Vector2f input_vel_last;    // last cycle input velocity
+
     // System ID states
     enum class SystemIDModeState {
         SYSTEMID_STATE_STOPPED,
@@ -1817,7 +1707,7 @@ private:
     float free_fall_start_velz;     // vertical velocity when free fall was detected
 };
 
-#if MODE_TURTLE_ENABLED
+#if MODE_TURTLE_ENABLED == ENABLED
 class ModeTurtle : public Mode {
 
 public:
@@ -1878,7 +1768,7 @@ private:
 
 };
 
-#if MODE_FOLLOW_ENABLED
+#if AP_FOLLOW_ENABLED
 class ModeFollow : public ModeGuided {
 
 public:
@@ -2007,7 +1897,7 @@ private:
     bool is_suspended;              // true if zigzag auto is suspended
 };
 
-#if MODE_AUTOROTATE_ENABLED
+#if MODE_AUTOROTATE_ENABLED == ENABLED
 class ModeAutorotate : public Mode {
 
 public:
@@ -2040,14 +1930,18 @@ private:
     int32_t _pitch_target;          // Target pitch attitude to pass to attitude controller
     uint32_t _entry_time_start_ms;  // Time remaining until entry phase moves on to glide phase
     float _hs_decay;                // The head accerleration during the entry phase
+    float _bail_time;               // Timer for exiting the bail out phase (s)
+    uint32_t _bail_time_start_ms;   // Time at start of bail out
+    float _target_climb_rate_adjust;// Target vertical acceleration used during bail out phase
+    float _target_pitch_adjust;     // Target pitch rate used during bail out phase
 
     enum class Autorotation_Phase {
         ENTRY,
         SS_GLIDE,
         FLARE,
         TOUCH_DOWN,
-        LANDED } phase_switch;
-
+        BAIL_OUT } phase_switch;
+        
     enum class Navigation_Decision {
         USER_CONTROL_STABILISED,
         STRAIGHT_AHEAD,
@@ -2060,10 +1954,10 @@ private:
             bool ss_glide_initial          : 1;
             bool flare_initial             : 1;
             bool touch_down_initial        : 1;
-            bool landed_initial            : 1;
             bool straight_ahead_initial    : 1;
             bool level_initial             : 1;
             bool break_initial             : 1;
+            bool bail_out_initial          : 1;
             bool bad_rpm                   : 1;
     } _flags;
 

ArduCopter/mode_acro.cpp

@@ -2,7 +2,7 @@
 
 #include "mode.h"
 
-#if MODE_ACRO_ENABLED
+#if MODE_ACRO_ENABLED == ENABLED
 
 /*
  * Init and run calls for acro flight mode
@@ -162,7 +162,7 @@ void ModeAcro::get_pilot_desired_angle_rates(float roll_in, float pitch_in, floa
         }
 
         // convert earth-frame level rates to body-frame level rates
-        attitude_control->euler_rate_to_ang_vel(attitude_control->get_attitude_target_quat(), rate_ef_level_cd, rate_bf_level_cd);
+        attitude_control->euler_rate_to_ang_vel(attitude_control->get_att_target_euler_cd() * radians(0.01f), rate_ef_level_cd, rate_bf_level_cd);
 
         // combine earth frame rate corrections with rate requests
         if (g.acro_trainer == (uint8_t)Trainer::LIMITED) {

ArduCopter/mode_acro_heli.cpp

@@ -1,6 +1,6 @@
 #include "Copter.h"
 
-#if MODE_ACRO_ENABLED
+#if MODE_ACRO_ENABLED == ENABLED
 
 #if FRAME_CONFIG == HELI_FRAME
 /*
@@ -111,7 +111,7 @@ void ModeAcro_Heli::run()
             // if there is no external gyro then run the usual
             // ACRO_YAW_P gain on the input control, including
             // deadzone
-            yaw_in = get_pilot_desired_yaw_rate();
+            yaw_in = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
         }
 
         // run attitude controller
@@ -144,7 +144,7 @@ void ModeAcro_Heli::virtual_flybar( float &roll_out, float &pitch_out, float &ya
     rate_ef_level.z = 0;
 
     // convert earth-frame leak rates to body-frame leak rates
-    attitude_control->euler_rate_to_ang_vel(attitude_control->get_attitude_target_quat(), rate_ef_level, rate_bf_level);
+    attitude_control->euler_rate_to_ang_vel(attitude_control->get_att_target_euler_cd()*radians(0.01f), rate_ef_level, rate_bf_level);
 
     // combine earth frame rate corrections with rate requests
     roll_out += rate_bf_level.x;
@@ -153,4 +153,4 @@ void ModeAcro_Heli::virtual_flybar( float &roll_out, float &pitch_out, float &ya
 
 }
 #endif  //HELI_FRAME
-#endif  //MODE_ACRO_ENABLED
+#endif  //MODE_ACRO_ENABLED == ENABLED

ArduCopter/mode_althold.cpp

@@ -36,7 +36,7 @@ void ModeAltHold::run()
     get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max_cd());
 
     // get pilot's desired yaw rate
-    float target_yaw_rate = get_pilot_desired_yaw_rate();
+    float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
 
     // get pilot desired climb rate
     float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
@@ -48,22 +48,22 @@ void ModeAltHold::run()
     // Alt Hold State Machine
     switch (althold_state) {
 
-    case AltHoldModeState::MotorStopped:
+    case AltHold_MotorStopped:
         attitude_control->reset_rate_controller_I_terms();
         attitude_control->reset_yaw_target_and_rate(false);
         pos_control->relax_z_controller(0.0f);   // forces throttle output to decay to zero
         break;
 
-    case AltHoldModeState::Landed_Ground_Idle:
+    case AltHold_Landed_Ground_Idle:
         attitude_control->reset_yaw_target_and_rate();
         FALLTHROUGH;
 
-    case AltHoldModeState::Landed_Pre_Takeoff:
+    case AltHold_Landed_Pre_Takeoff:
         attitude_control->reset_rate_controller_I_terms_smoothly();
         pos_control->relax_z_controller(0.0f);   // forces throttle output to decay to zero
         break;
 
-    case AltHoldModeState::Takeoff:
+    case AltHold_Takeoff:
         // initiate take-off
         if (!takeoff.running()) {
             takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
@@ -76,10 +76,10 @@ void ModeAltHold::run()
         takeoff.do_pilot_takeoff(target_climb_rate);
         break;
 
-    case AltHoldModeState::Flying:
+    case AltHold_Flying:
         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
-#if AP_AVOIDANCE_ENABLED
+#if AC_AVOID_ENABLED == ENABLED
         // apply avoidance
         copter.avoid.adjust_roll_pitch(target_roll, target_pitch, copter.aparm.angle_max);
 #endif
@@ -87,10 +87,8 @@ void ModeAltHold::run()
         // get avoidance adjusted climb rate
         target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
 
-#if AP_RANGEFINDER_ENABLED
         // update the vertical offset based on the surface measurement
         copter.surface_tracking.update_surface_offset();
-#endif
 
         // Send the commanded climb rate to the position controller
         pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);
@@ -102,4 +100,4 @@ void ModeAltHold::run()
 
     // run the vertical position controller and set output throttle
     pos_control->update_z_controller();
-}
+}

ArduCopter/mode_auto.cpp

@@ -1,6 +1,6 @@
 #include "Copter.h"
 
-#if MODE_AUTO_ENABLED
+#if MODE_AUTO_ENABLED == ENABLED
 
 /*
  * Init and run calls for auto flight mode
@@ -139,7 +139,7 @@ void ModeAuto::run()
 
     case SubMode::NAVGUIDED:
     case SubMode::NAV_SCRIPT_TIME:
-#if AC_NAV_GUIDED || AP_SCRIPTING_ENABLED
+#if AC_NAV_GUIDED == ENABLED || AP_SCRIPTING_ENABLED
         nav_guided_run();
 #endif
         break;
@@ -164,8 +164,7 @@ void ModeAuto::run()
     }
 
     // only pretend to be in auto RTL so long as mission still thinks its in a landing sequence or the mission has completed
-    const bool auto_rtl_active = mission.get_in_landing_sequence_flag() || mission.get_in_return_path_flag() || mission.state() == AP_Mission::mission_state::MISSION_COMPLETE;
-    if (auto_RTL && !auto_rtl_active) {
+    if (auto_RTL && (!(mission.get_in_landing_sequence_flag() || mission.state() == AP_Mission::mission_state::MISSION_COMPLETE))) {
         auto_RTL = false;
         // log exit from Auto RTL
 #if HAL_LOGGING_ENABLED
@@ -202,97 +201,44 @@ void ModeAuto::set_submode(SubMode new_submode)
     }
 }
 
-bool ModeAuto::option_is_enabled(Option option) const
-{
-    return ((copter.g2.auto_options & (uint32_t)option) != 0);
-}
-
 bool ModeAuto::allows_arming(AP_Arming::Method method) const
 {
-    if (auto_RTL) {
-        return false;
-    }
-    return option_is_enabled(Option::AllowArming);
-}
+    return ((copter.g2.auto_options & (uint32_t)Options::AllowArming) != 0) && !auto_RTL;
+};
 
-#if WEATHERVANE_ENABLED
+#if WEATHERVANE_ENABLED == ENABLED
 bool ModeAuto::allows_weathervaning() const
 {
-    return option_is_enabled(Option::AllowWeatherVaning);
+    return (copter.g2.auto_options & (uint32_t)Options::AllowWeatherVaning) != 0;
 }
 #endif
 
 // Go straight to landing sequence via DO_LAND_START, if succeeds pretend to be Auto RTL mode
 bool ModeAuto::jump_to_landing_sequence_auto_RTL(ModeReason reason)
 {
-    if (!mission.jump_to_landing_sequence(get_stopping_point())) {
-        LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(Number::AUTO_RTL));
-        // make sad noise
-        if (copter.ap.initialised) {
-            AP_Notify::events.user_mode_change_failed = 1;
-        }
-        gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to AUTO RTL failed: No landing sequence found");
-        return false;
-    }
-
-    return enter_auto_rtl(reason);
-}
-
-// Join mission after DO_RETURN_PATH_START waypoint, if succeeds pretend to be Auto RTL mode
-bool ModeAuto::return_path_start_auto_RTL(ModeReason reason)
-{
-    if (!mission.jump_to_closest_mission_leg(get_stopping_point())) {
-        LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(Number::AUTO_RTL));
-        // make sad noise
-        if (copter.ap.initialised) {
-            AP_Notify::events.user_mode_change_failed = 1;
-        }
-        gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to AUTO RTL failed: No return path found");
-        return false;
-    }
-
-    return enter_auto_rtl(reason);
-}
-
-// Try join return path else do land start
-bool ModeAuto::return_path_or_jump_to_landing_sequence_auto_RTL(ModeReason reason)
-{
-    const Location stopping_point = get_stopping_point();
-    if (!mission.jump_to_closest_mission_leg(stopping_point) && !mission.jump_to_landing_sequence(stopping_point)) {
-        LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(Number::AUTO_RTL));
-        // make sad noise
-        if (copter.ap.initialised) {
-            AP_Notify::events.user_mode_change_failed = 1;
-        }
-        gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to AUTO RTL failed: No return path or landing sequence found");
-        return false;
-    }
-
-    return enter_auto_rtl(reason);
-}
-
-// Enter auto rtl pseudo mode
-bool ModeAuto::enter_auto_rtl(ModeReason reason) 
-{
-    mission.set_force_resume(true);
-
-    // if not already in auto switch to auto
-    if ((copter.flightmode == this) || set_mode(Mode::Number::AUTO, reason)) {
-        auto_RTL = true;
+    if (mission.jump_to_landing_sequence()) {
+        mission.set_force_resume(true);
+        // if not already in auto switch to auto
+        if ((copter.flightmode == &copter.mode_auto) || set_mode(Mode::Number::AUTO, reason)) {
+            auto_RTL = true;
 #if HAL_LOGGING_ENABLED
-        // log entry into AUTO RTL
-        copter.logger.Write_Mode((uint8_t)copter.flightmode->mode_number(), reason);
+            // log entry into AUTO RTL
+            copter.logger.Write_Mode((uint8_t)copter.flightmode->mode_number(), reason);
 #endif
 
-        // make happy noise
-        if (copter.ap.initialised) {
-            AP_Notify::events.user_mode_change = 1;
+            // make happy noise
+            if (copter.ap.initialised) {
+                AP_Notify::events.user_mode_change = 1;
+            }
+            return true;
         }
-        return true;
-    }
+        // mode change failed, revert force resume flag
+        mission.set_force_resume(false);
 
-    // mode change failed, revert force resume flag
-    mission.set_force_resume(false);
+        gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to AUTO RTL failed");
+    } else {
+        gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to AUTO RTL failed: No landing sequence found");
+    }
 
     LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(Number::AUTO_RTL));
     // make sad noise
@@ -427,7 +373,7 @@ bool ModeAuto::wp_start(const Location& dest_loc)
         Vector3f stopping_point;
         if (_mode == SubMode::TAKEOFF) {
             Vector3p takeoff_complete_pos;
-            if (auto_takeoff.get_completion_pos(takeoff_complete_pos)) {
+            if (auto_takeoff.get_position(takeoff_complete_pos)) {
                 stopping_point = takeoff_complete_pos.tofloat();
             }
         }
@@ -449,7 +395,7 @@ bool ModeAuto::wp_start(const Location& dest_loc)
 
     // initialise yaw
     // To-Do: reset the yaw only when the previous navigation command is not a WP.  this would allow removing the special check for ROI
-    if (auto_yaw.mode() != AutoYaw::Mode::ROI && !(auto_yaw.mode() == AutoYaw::Mode::FIXED && copter.g.wp_yaw_behavior == WP_YAW_BEHAVIOR_NONE)) {
+    if (auto_yaw.mode() != AutoYaw::Mode::ROI) {
         auto_yaw.set_mode_to_default(false);
     }
 
@@ -488,6 +434,11 @@ void ModeAuto::land_start()
     copter.landinggear.deploy_for_landing();
 #endif
 
+#if AP_FENCE_ENABLED
+    // disable the fence on landing
+    copter.fence.auto_disable_fence_for_landing();
+#endif
+
     // reset flag indicating if pilot has applied roll or pitch inputs during landing
     copter.ap.land_repo_active = false;
 
@@ -498,7 +449,7 @@ void ModeAuto::land_start()
     set_submode(SubMode::LAND);
 }
 
-// circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
+// auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
 //  we assume the caller has performed all required GPS_ok checks
 void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radius_m, bool ccw_turn)
 {
@@ -517,8 +468,8 @@ void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radi
 
     // check our distance from edge of circle
     Vector3f circle_edge_neu;
-    float dist_to_edge;
-    copter.circle_nav->get_closest_point_on_circle(circle_edge_neu, dist_to_edge);
+    copter.circle_nav->get_closest_point_on_circle(circle_edge_neu);
+    float dist_to_edge = (inertial_nav.get_position_neu_cm() - circle_edge_neu).length();
 
     // if more than 3m then fly to edge
     if (dist_to_edge > 300.0f) {
@@ -569,7 +520,7 @@ void ModeAuto::circle_start()
     set_submode(SubMode::CIRCLE);
 }
 
-#if AC_NAV_GUIDED
+#if AC_NAV_GUIDED == ENABLED
 // auto_nav_guided_start - hand over control to external navigation controller in AUTO mode
 void ModeAuto::nav_guided_start()
 {
@@ -641,7 +592,7 @@ void PayloadPlace::start_descent()
 // returns true if pilot's yaw input should be used to adjust vehicle's heading
 bool ModeAuto::use_pilot_yaw(void) const
 {
-    const bool allow_yaw_option = !option_is_enabled(Option::IgnorePilotYaw);
+    const bool allow_yaw_option = (copter.g2.auto_options.get() & uint32_t(Options::IgnorePilotYaw)) == 0;
     const bool rtl_allow_yaw = (_mode == SubMode::RTL) && copter.mode_rtl.use_pilot_yaw();
     const bool landing = _mode == SubMode::LAND;
     return allow_yaw_option || rtl_allow_yaw || landing;
@@ -671,6 +622,13 @@ bool ModeAuto::set_speed_down(float speed_down_cms)
 // start_command - this function will be called when the ap_mission lib wishes to start a new command
 bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
 {
+#if HAL_LOGGING_ENABLED
+    // To-Do: logging when new commands start/end
+    if (copter.should_log(MASK_LOG_CMD)) {
+        copter.logger.Write_Mission_Cmd(mission, cmd);
+    }
+#endif
+
     switch(cmd.id) {
 
     ///
@@ -714,7 +672,7 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
         do_spline_wp(cmd);
         break;
 
-#if AC_NAV_GUIDED
+#if AC_NAV_GUIDED == ENABLED
     case MAV_CMD_NAV_GUIDED_ENABLE:             // 92  accept navigation commands from external nav computer
         do_nav_guided_enable(cmd);
         break;
@@ -771,14 +729,24 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
         do_roi(cmd);
         break;
 
-#if HAL_MOUNT_ENABLED
     case MAV_CMD_DO_MOUNT_CONTROL:          // 205
         // point the camera to a specified angle
         do_mount_control(cmd);
         break;
-#endif
+    
+    case MAV_CMD_DO_FENCE_ENABLE:
+#if AP_FENCE_ENABLED
+        if (cmd.p1 == 0) { //disable
+            copter.fence.enable(false);
+            gcs().send_text(MAV_SEVERITY_INFO, "Fence Disabled");
+        } else { //enable fence
+            copter.fence.enable(true);
+            gcs().send_text(MAV_SEVERITY_INFO, "Fence Enabled");
+        }
+#endif //AP_FENCE_ENABLED
+        break;
 
-#if AC_NAV_GUIDED
+#if AC_NAV_GUIDED == ENABLED
     case MAV_CMD_DO_GUIDED_LIMITS:                      // 220  accept guided mode limits
         do_guided_limits(cmd);
         break;
@@ -790,7 +758,6 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
         break;
 #endif
 
-    case MAV_CMD_DO_RETURN_PATH_START:
     case MAV_CMD_DO_LAND_START:
         break;
 
@@ -824,7 +791,7 @@ void ModeAuto::exit_mission()
 bool ModeAuto::do_guided(const AP_Mission::Mission_Command& cmd)
 {
     // only process guided waypoint if we are in guided mode
-    if (!copter.flightmode->in_guided_mode()) {
+    if (copter.flightmode->mode_number() != Mode::Number::GUIDED && !(copter.flightmode->mode_number() == Mode::Number::AUTO && _mode == SubMode::NAVGUIDED)) {
         return false;
     }
 
@@ -954,7 +921,7 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
         cmd_complete = verify_spline_wp(cmd);
         break;
 
-#if AC_NAV_GUIDED
+#if AC_NAV_GUIDED == ENABLED
     case MAV_CMD_NAV_GUIDED_ENABLE:
         cmd_complete = verify_nav_guided_enable(cmd);
         break;
@@ -993,19 +960,10 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
     case MAV_CMD_DO_CHANGE_SPEED:
     case MAV_CMD_DO_SET_HOME:
     case MAV_CMD_DO_SET_ROI:
-#if HAL_MOUNT_ENABLED
     case MAV_CMD_DO_MOUNT_CONTROL:
-#endif
-#if AC_NAV_GUIDED
     case MAV_CMD_DO_GUIDED_LIMITS:
-#endif
-#if AP_FENCE_ENABLED
     case MAV_CMD_DO_FENCE_ENABLE:
-#endif
-#if AP_WINCH_ENABLED
     case MAV_CMD_DO_WINCH:
-#endif
-    case MAV_CMD_DO_RETURN_PATH_START:
     case MAV_CMD_DO_LAND_START:
         cmd_complete = true;
         break;
@@ -1033,7 +991,7 @@ void ModeAuto::takeoff_run()
 {
     // if the user doesn't want to raise the throttle we can set it automatically
     // note that this can defeat the disarm check on takeoff
-    if (option_is_enabled(Option::AllowTakeOffWithoutRaisingThrottle)) {
+    if ((copter.g2.auto_options & (int32_t)Options::AllowTakeOffWithoutRaisingThrottle) != 0) {
         copter.set_auto_armed(true);
     }
     auto_takeoff.run();
@@ -1104,7 +1062,7 @@ void ModeAuto::circle_run()
     attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
 }
 
-#if AC_NAV_GUIDED || AP_SCRIPTING_ENABLED
+#if AC_NAV_GUIDED == ENABLED || AP_SCRIPTING_ENABLED
 // auto_nav_guided_run - allows control by external navigation controller
 //      called by auto_run at 100hz or more
 void ModeAuto::nav_guided_run()
@@ -1191,10 +1149,8 @@ void ModeAuto::loiter_to_alt_run()
     // get avoidance adjusted climb rate
     target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
 
-#if AP_RANGEFINDER_ENABLED
     // update the vertical offset based on the surface measurement
     copter.surface_tracking.update_surface_offset();
-#endif
 
     // Send the commanded climb rate to the position controller
     pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);
@@ -1249,20 +1205,13 @@ void PayloadPlace::run()
     const uint32_t descent_thrust_cal_duration_ms = 2000; // milliseconds
     const uint32_t placed_check_duration_ms = 500; // how long we have to be below a throttle threshold before considering placed
 
-    auto &g2 = copter.g2;
-    const auto &g = copter.g;
-    auto *attitude_control = copter.attitude_control;
-    const auto &pos_control = copter.pos_control;
-    const auto &wp_nav = copter.wp_nav;
-
     // Vertical thrust is taken from the attitude controller before angle boost is added
+    auto *attitude_control = copter.attitude_control;
     const float thrust_level = attitude_control->get_throttle_in();
     const uint32_t now_ms = AP_HAL::millis();
 
-    // relax position target if we might be landed
     // if we discover we've landed then immediately release the load:
     if (copter.ap.land_complete || copter.ap.land_complete_maybe) {
-        pos_control->soften_for_landing_xy();
         switch (state) {
         case State::FlyToLocation:
             // this is handled in wp_run()
@@ -1284,15 +1233,13 @@ void PayloadPlace::run()
         }
     }
 
-#if AP_GRIPPER_ENABLED
+#if AP_GRIPPER_ENABLED == ENABLED
     // if pilot releases load manually:
     if (AP::gripper().valid() && AP::gripper().released()) {
         switch (state) {
         case State::FlyToLocation:
         case State::Descent_Start:
-            gcs().send_text(MAV_SEVERITY_INFO, "%s Abort: Gripper Open", prefix_str);
-            // Descent_Start has not run so we must also initalise descent_start_altitude_cm
-            descent_start_altitude_cm = pos_control->get_pos_desired_z_cm();
+            gcs().send_text(MAV_SEVERITY_INFO, "%s Manual release", prefix_str);
             state = State::Done;
             break;
         case State::Descent:
@@ -1310,6 +1257,12 @@ void PayloadPlace::run()
     }
 #endif
 
+    auto &inertial_nav = copter.inertial_nav;
+    auto &g2 = copter.g2;
+    const auto &g = copter.g;
+    const auto &wp_nav = copter.wp_nav;
+    const auto &pos_control = copter.pos_control;
+
     switch (state) {
     case State::FlyToLocation:
         if (copter.wp_nav->reached_wp_destination()) {
@@ -1319,7 +1272,7 @@ void PayloadPlace::run()
 
     case State::Descent_Start:
         descent_established_time_ms = now_ms;
-        descent_start_altitude_cm = pos_control->get_pos_desired_z_cm();
+        descent_start_altitude_cm = inertial_nav.get_position_z_up_cm();
         // limiting the decent rate to the limit set in wp_nav is not necessary but done for safety
         descent_speed_cms = MIN((is_positive(g2.pldp_descent_speed_ms)) ? g2.pldp_descent_speed_ms * 100.0 : abs(g.land_speed), wp_nav->get_default_speed_down());
         descent_thrust_level = 1.0;
@@ -1329,7 +1282,7 @@ void PayloadPlace::run()
     case State::Descent:
         // check maximum decent distance
         if (!is_zero(descent_max_cm) &&
-            descent_start_altitude_cm - pos_control->get_pos_desired_z_cm() > descent_max_cm) {
+            descent_start_altitude_cm - inertial_nav.get_position_z_up_cm() > descent_max_cm) {
             state = State::Ascent_Start;
             gcs().send_text(MAV_SEVERITY_WARNING, "%s Reached maximum descent", prefix_str);
             break;
@@ -1348,14 +1301,14 @@ void PayloadPlace::run()
             // thrust is above minimum threshold
             place_start_time_ms = now_ms;
             break;
-        } else if (is_positive(g2.pldp_range_finder_maximum_m)) {
+        } else if (is_positive(g2.pldp_range_finder_minimum_m)) {
             if (!copter.rangefinder_state.enabled) {
                 // abort payload place because rangefinder is not enabled
                 state = State::Ascent_Start;
-                gcs().send_text(MAV_SEVERITY_WARNING, "%s PLDP_RNG_MAX set and rangefinder not enabled", prefix_str);
+                gcs().send_text(MAV_SEVERITY_WARNING, "%s PLDP_RNG_MIN set and rangefinder not enabled", prefix_str);
                 break;
-            } else if (copter.rangefinder_alt_ok() && (copter.rangefinder_state.glitch_count == 0) && (copter.rangefinder_state.alt_cm > g2.pldp_range_finder_maximum_m * 100.0)) {
-                // range finder altitude is above maximum
+            } else if (copter.rangefinder_alt_ok() && (copter.rangefinder_state.glitch_count == 0) && (copter.rangefinder_state.alt_cm > g2.pldp_range_finder_minimum_m * 100.0)) {
+                // range finder altitude is above minimum
                 place_start_time_ms = now_ms;
                 break;
             }
@@ -1379,7 +1332,7 @@ void PayloadPlace::run()
     case State::Release:
         // Reinitialise vertical position controller to remove discontinuity due to touch down of payload
         pos_control->init_z_controller_no_descent();
-#if AP_GRIPPER_ENABLED
+#if AP_GRIPPER_ENABLED == ENABLED
         if (AP::gripper().valid()) {
             gcs().send_text(MAV_SEVERITY_INFO, "%s Releasing the gripper", prefix_str);
             AP::gripper().release();
@@ -1418,7 +1371,7 @@ void PayloadPlace::run()
         // vel_threshold_fraction * max velocity
         const float vel_threshold_fraction = 0.1;
         const float stop_distance = 0.5 * sq(vel_threshold_fraction * copter.pos_control->get_max_speed_up_cms()) / copter.pos_control->get_max_accel_z_cmss();
-        bool reached_altitude = pos_control->get_pos_desired_z_cm() >= descent_start_altitude_cm - stop_distance;
+        bool reached_altitude = copter.pos_control->get_pos_target_z_cm() >= descent_start_altitude_cm - stop_distance;
         if (reached_altitude) {
             state = State::Done;
         }
@@ -1441,7 +1394,8 @@ void PayloadPlace::run()
         copter.flightmode->land_run_horizontal_control();
         // update altitude target and call position controller
         pos_control->land_at_climb_rate_cm(-descent_speed_cms, true);
-        break;
+        pos_control->update_z_controller();
+        return;
     case State::Release:
     case State::Releasing:
     case State::Delay:
@@ -1449,7 +1403,8 @@ void PayloadPlace::run()
         copter.flightmode->land_run_horizontal_control();
         // update altitude target and call position controller
         pos_control->land_at_climb_rate_cm(0.0, false);
-        break;
+        pos_control->update_z_controller();
+        return;
     case State::Ascent:
     case State::Done:
         float vel = 0.0;
@@ -1457,7 +1412,6 @@ void PayloadPlace::run()
         pos_control->input_pos_vel_accel_z(descent_start_altitude_cm, vel, 0.0);
         break;
     }
-    pos_control->update_z_controller();
 }
 #endif
 
@@ -1471,8 +1425,6 @@ bool ModeAuto::shift_alt_to_current_alt(Location& target_loc) const
         (wp_nav->get_terrain_source() == AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER)) {
         int32_t curr_rngfnd_alt_cm;
         if (copter.get_rangefinder_height_interpolated_cm(curr_rngfnd_alt_cm)) {
-            // subtract position offset (if any)
-            curr_rngfnd_alt_cm -= pos_control->get_pos_offset_z_cm();
             // wp_nav is using rangefinder so use current rangefinder alt
             target_loc.set_alt_cm(MAX(curr_rngfnd_alt_cm, 200), Location::AltFrame::ABOVE_TERRAIN);
             return true;
@@ -1487,21 +1439,11 @@ bool ModeAuto::shift_alt_to_current_alt(Location& target_loc) const
         return false;
     }
 
-    // set target_loc's alt minus position offset (if any)
-    target_loc.set_alt_cm(currloc.alt - pos_control->get_pos_offset_z_cm(), currloc.get_alt_frame());
+    // set target_loc's alt
+    target_loc.set_alt_cm(currloc.alt, currloc.get_alt_frame());
     return true;
 }
 
-// subtract position controller offsets from target location
-// should be used when the location will be used as a target for the position controller
-void ModeAuto::subtract_pos_offsets(Location& target_loc) const
-{
-    // subtract position controller offsets from target location
-    const Vector3p& pos_ofs_neu_cm = pos_control->get_pos_offset_cm();
-    Vector3p pos_ofs_ned_m = Vector3p{pos_ofs_neu_cm.x * 0.01, pos_ofs_neu_cm.y * 0.01, -pos_ofs_neu_cm.z * 0.01};
-    target_loc.offset(-pos_ofs_ned_m);
-}
-
 /********************************************************************************/
 // Nav (Must) commands
 /********************************************************************************/
@@ -1513,7 +1455,6 @@ void ModeAuto::do_takeoff(const AP_Mission::Mission_Command& cmd)
     takeoff_start(cmd.content.location);
 }
 
-// return the Location portion of a command.  If the command's lat and lon and/or alt are zero the default_loc's lat,lon and/or alt are returned instead
 Location ModeAuto::loc_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc) const
 {
     Location ret(cmd.content.location);
@@ -1543,10 +1484,6 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
 {
     // calculate default location used when lat, lon or alt is zero
     Location default_loc = copter.current_loc;
-
-    // subtract position offsets
-    subtract_pos_offsets(default_loc);
-
     if (wp_nav->is_active() && wp_nav->reached_wp_destination()) {
         if (!wp_nav->get_wp_destination_loc(default_loc)) {
             // this should never happen
@@ -1667,22 +1604,32 @@ void ModeAuto::do_land(const AP_Mission::Mission_Command& cmd)
 // note: caller should set yaw_mode
 void ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
 {
-    // calculate default location used when lat, lon or alt is zero
-    Location default_loc = copter.current_loc;
+    // convert back to location
+    Location target_loc(cmd.content.location);
 
-    // subtract position offsets
-    subtract_pos_offsets(default_loc);
-
-    // use previous waypoint destination as default if available
-    if (wp_nav->is_active() && wp_nav->reached_wp_destination()) {
-        if (!wp_nav->get_wp_destination_loc(default_loc)) {
-            // this should never happen
-            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
-        }
+    // use current location if not provided
+    if (target_loc.lat == 0 && target_loc.lng == 0) {
+        // To-Do: make this simpler
+        Vector3f temp_pos;
+        copter.wp_nav->get_wp_stopping_point_xy(temp_pos.xy());
+        const Location temp_loc(temp_pos, Location::AltFrame::ABOVE_ORIGIN);
+        target_loc.lat = temp_loc.lat;
+        target_loc.lng = temp_loc.lng;
     }
 
-    // get waypoint's location from command and send to wp_nav
-    const Location target_loc = loc_from_cmd(cmd, default_loc);
+    // use current altitude if not provided
+    // To-Do: use z-axis stopping point instead of current alt
+    if (target_loc.alt == 0) {
+        // set to current altitude but in command's alt frame
+        int32_t curr_alt;
+        if (copter.current_loc.get_alt_cm(target_loc.get_alt_frame(),curr_alt)) {
+            target_loc.set_alt_cm(curr_alt, target_loc.get_alt_frame());
+        } else {
+            // default to current altitude as alt-above-home
+            target_loc.set_alt_cm(copter.current_loc.alt,
+                                  copter.current_loc.get_alt_frame());
+        }
+    }
 
     // start way point navigator and provide it the desired location
     if (!wp_start(target_loc)) {
@@ -1695,13 +1642,7 @@ void ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
 // do_circle - initiate moving in a circle
 void ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd)
 {
-    // calculate default location used when lat, lon or alt is zero
-    Location default_loc = copter.current_loc;
-
-    // subtract position offsets
-    subtract_pos_offsets(default_loc);
-
-    const Location circle_center = loc_from_cmd(cmd, default_loc);
+    const Location circle_center = loc_from_cmd(cmd, copter.current_loc);
 
     // calculate radius
     uint16_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1
@@ -1769,10 +1710,6 @@ void ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd)
 {
     // calculate default location used when lat, lon or alt is zero
     Location default_loc = copter.current_loc;
-
-    // subtract position offsets
-    subtract_pos_offsets(default_loc);
-
     if (wp_nav->is_active() && wp_nav->reached_wp_destination()) {
         if (!wp_nav->get_wp_destination_loc(default_loc)) {
             // this should never happen
@@ -1804,7 +1741,7 @@ void ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd)
 
     // initialise yaw
     // To-Do: reset the yaw only when the previous navigation command is not a WP.  this would allow removing the special check for ROI
-    if (auto_yaw.mode() != AutoYaw::Mode::ROI && !(auto_yaw.mode() == AutoYaw::Mode::FIXED && copter.g.wp_yaw_behavior == WP_YAW_BEHAVIOR_NONE)) {
+    if (auto_yaw.mode() != AutoYaw::Mode::ROI) {
         auto_yaw.set_mode_to_default(false);
     }
 
@@ -1830,7 +1767,7 @@ void ModeAuto::get_spline_from_cmd(const AP_Mission::Mission_Command& cmd, const
     }
 }
 
-#if AC_NAV_GUIDED
+#if AC_NAV_GUIDED == ENABLED
 // do_nav_guided_enable - initiate accepting commands from external nav computer
 void ModeAuto::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
 {
@@ -1936,20 +1873,15 @@ void ModeAuto::do_yaw(const AP_Mission::Mission_Command& cmd)
 void ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd)
 {
     if (cmd.content.speed.target_ms > 0) {
-        switch (cmd.content.speed.speed_type) {
-        case SPEED_TYPE_CLIMB_SPEED:
+        if (cmd.content.speed.speed_type == 2)  {
             copter.wp_nav->set_speed_up(cmd.content.speed.target_ms * 100.0f);
             desired_speed_override.up = cmd.content.speed.target_ms;
-            break;
-        case SPEED_TYPE_DESCENT_SPEED:
+        } else if (cmd.content.speed.speed_type == 3)  {
             copter.wp_nav->set_speed_down(cmd.content.speed.target_ms * 100.0f);
             desired_speed_override.down = cmd.content.speed.target_ms;
-            break;
-        case SPEED_TYPE_AIRSPEED:
-        case SPEED_TYPE_GROUNDSPEED:
+        } else {
             copter.wp_nav->set_speed_xy(cmd.content.speed.target_ms * 100.0f);
             desired_speed_override.xy = cmd.content.speed.target_ms;
-            break;
         }
     }
 }
@@ -1976,21 +1908,19 @@ void ModeAuto::do_roi(const AP_Mission::Mission_Command& cmd)
     auto_yaw.set_roi(cmd.content.location);
 }
 
-#if HAL_MOUNT_ENABLED
 // point the camera to a specified angle
 void ModeAuto::do_mount_control(const AP_Mission::Mission_Command& cmd)
 {
+#if HAL_MOUNT_ENABLED
     // if vehicle has a camera mount but it doesn't do pan control then yaw the entire vehicle instead
     if ((copter.camera_mount.get_mount_type() != AP_Mount::Type::None) &&
         !copter.camera_mount.has_pan_control()) {
-        // Per the handler in AP_Mount, DO_MOUNT_CONTROL yaw angle is in body frame, which is
-        // equivalent to an offset to the current yaw demand.
-        auto_yaw.set_yaw_angle_offset(cmd.content.mount_control.yaw);
+        auto_yaw.set_yaw_angle_rate(cmd.content.mount_control.yaw,0.0f);
     }
     // pass the target angles to the camera mount
     copter.camera_mount.set_angle_target(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw, false);
+#endif
 }
-#endif  // HAL_MOUNT_ENABLED
 
 #if AP_WINCH_ENABLED
 // control winch based on mission command
@@ -2286,7 +2216,7 @@ bool ModeAuto::verify_spline_wp(const AP_Mission::Mission_Command& cmd)
     return false;
 }
 
-#if AC_NAV_GUIDED
+#if AC_NAV_GUIDED == ENABLED
 // verify_nav_guided - check if we have breached any limits
 bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
 {
@@ -2351,7 +2281,7 @@ bool ModeAuto::resume()
 
 bool ModeAuto::paused() const
 {
-    return (wp_nav != nullptr) && wp_nav->paused();
+    return wp_nav->paused();
 }
 
 #endif

ArduCopter/mode_autorotate.cpp

@@ -11,11 +11,11 @@
 
 #include <utility>
 
-#if MODE_AUTOROTATE_ENABLED
+#if MODE_AUTOROTATE_ENABLED == ENABLED
 
 #define AUTOROTATE_ENTRY_TIME          2.0f    // (s) number of seconds that the entry phase operates for
+#define BAILOUT_MOTOR_RAMP_TIME        1.0f    // (s) time set on bailout ramp up timer for motors - See AC_MotorsHeli_Single
 #define HEAD_SPEED_TARGET_RATIO        1.0f    // Normalised target main rotor head speed (unit: -)
-#define AUTOROTATION_MIN_MOVING_SPEED  100.0    // (cm/s) minimum speed used for "is moving" check
 
 bool ModeAutorotate::init(bool ignore_checks)
 {
@@ -24,16 +24,15 @@ bool ModeAutorotate::init(bool ignore_checks)
     return false;
 #endif
 
-    // Check that mode is enabled, make sure this is the first check as this is the most
-    // important thing for users to fix if they are planning to use autorotation mode
+    // Check that mode is enabled
     if (!g2.arot.is_enable()) {
-        gcs().send_text(MAV_SEVERITY_WARNING, "Autorot Mode Not Enabled");
+        gcs().send_text(MAV_SEVERITY_INFO, "Autorot Mode Not Enabled");
         return false;
     }
 
-    // Must be armed to use mode, prevent triggering state machine on the ground
-    if (!motors->armed() || copter.ap.land_complete || copter.ap.land_complete_maybe) {
-        gcs().send_text(MAV_SEVERITY_WARNING, "Autorot: Must be Armed and Flying");
+    // Check that interlock is disengaged
+    if (motors->get_interlock()) {
+        gcs().send_text(MAV_SEVERITY_INFO, "Autorot Mode Change Fail: Interlock Engaged");
         return false;
     }
 
@@ -53,10 +52,10 @@ bool ModeAutorotate::init(bool ignore_checks)
     _flags.ss_glide_initial = true;
     _flags.flare_initial = true;
     _flags.touch_down_initial = true;
-    _flags.landed_initial = true;
     _flags.level_initial = true;
     _flags.break_initial = true;
     _flags.straight_ahead_initial = true;
+    _flags.bail_out_initial = true;
     _msg_flags.bad_rpm = true;
 
     // Setting default starting switches
@@ -75,9 +74,20 @@ bool ModeAutorotate::init(bool ignore_checks)
 
 void ModeAutorotate::run()
 {
+    // Check if interlock becomes engaged
+    if (motors->get_interlock() && !copter.ap.land_complete) {
+        phase_switch = Autorotation_Phase::BAIL_OUT;
+    } else if (motors->get_interlock() && copter.ap.land_complete) {
+        // Aircraft is landed and no need to bail out
+        set_mode(copter.prev_control_mode, ModeReason::AUTOROTATION_BAILOUT);
+    }
+
     // Current time
     uint32_t now = millis(); //milliseconds
 
+    // Initialise internal variables
+    float curr_vel_z = inertial_nav.get_velocity_z_up_cms();   // Current vertical descent
+
     //----------------------------------------------------------------
     //                  State machine logic
     //----------------------------------------------------------------
@@ -87,22 +97,12 @@ void ModeAutorotate::run()
 
     // Timer from entry phase to progress to glide phase
     if (phase_switch == Autorotation_Phase::ENTRY){
+
         if ((now - _entry_time_start_ms)/1000.0f > AUTOROTATE_ENTRY_TIME) {
             // Flight phase can be progressed to steady state glide
             phase_switch = Autorotation_Phase::SS_GLIDE;
         }
-    }
 
-    // Check if we believe we have landed. We need the landed state to zero all controls and make sure that the copter landing detector will trip
-    bool speed_check = inertial_nav.get_velocity_z_up_cms() < AUTOROTATION_MIN_MOVING_SPEED &&
-                     inertial_nav.get_speed_xy_cms() < AUTOROTATION_MIN_MOVING_SPEED;
-    if (motors->get_below_land_min_coll() && AP::ins().is_still() && speed_check) {
-        phase_switch = Autorotation_Phase::LANDED;
-    }
-
-    // Check if we are bailing out and need to re-set the spool state
-    if (motors->autorotation_bailout()) {
-        motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
     }
 
 
@@ -199,22 +199,79 @@ void ModeAutorotate::run()
         {
             break;
         }
-        case Autorotation_Phase::LANDED:
+
+        case Autorotation_Phase::BAIL_OUT:
         {
-            // Entry phase functions to be run only once
-            if (_flags.landed_initial == true) {
+        if (_flags.bail_out_initial == true) {
+                // Functions and settings to be done once are done here.
 
                 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-                    gcs().send_text(MAV_SEVERITY_INFO, "Landed");
+                    gcs().send_text(MAV_SEVERITY_INFO, "Bailing Out of Autorotation");
                 #endif
-                _flags.landed_initial = false;
+
+                // Set bail out timer remaining equal to the parameter value, bailout time 
+                // cannot be less than the motor spool-up time: BAILOUT_MOTOR_RAMP_TIME.
+                _bail_time = MAX(g2.arot.get_bail_time(),BAILOUT_MOTOR_RAMP_TIME+0.1f);
+
+                // Set bail out start time
+                _bail_time_start_ms = now;
+
+                // Set initial target vertical speed
+                _desired_v_z = curr_vel_z;
+
+                // Initialise position and desired velocity
+                if (!pos_control->is_active_z()) {
+                    pos_control->relax_z_controller(g2.arot.get_last_collective());
+                }
+
+                // Get pilot parameter limits
+                const float pilot_spd_dn = -get_pilot_speed_dn();
+                const float pilot_spd_up = g.pilot_speed_up;
+
+                float pilot_des_v_z = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
+                pilot_des_v_z = constrain_float(pilot_des_v_z, pilot_spd_dn, pilot_spd_up);
+
+                // Calculate target climb rate adjustment to transition from bail out descent speed to requested climb rate on stick.
+                _target_climb_rate_adjust = (curr_vel_z - pilot_des_v_z)/(_bail_time - BAILOUT_MOTOR_RAMP_TIME); //accounting for 0.5s motor spool time
+
+                // Calculate pitch target adjustment rate to return to level
+                _target_pitch_adjust = _pitch_target/_bail_time;
+
+                // set vertical speed and acceleration limits
+                pos_control->set_max_speed_accel_z(curr_vel_z, pilot_spd_up, fabsf(_target_climb_rate_adjust));
+                pos_control->set_correction_speed_accel_z(curr_vel_z, pilot_spd_up, fabsf(_target_climb_rate_adjust));
+
+                motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
+
+                _flags.bail_out_initial = false;
             }
-            // don't allow controller to continually ask for more pitch to build speed when we are on the ground, decay to zero smoothly
-            _pitch_target *= 0.95;
-            break;
+
+        if ((now - _bail_time_start_ms)/1000.0f >= BAILOUT_MOTOR_RAMP_TIME) {
+            // Update desired vertical speed and pitch target after the bailout motor ramp timer has completed
+            _desired_v_z -= _target_climb_rate_adjust*G_Dt;
+            _pitch_target -= _target_pitch_adjust*G_Dt;
+        }
+        // Set position controller
+        pos_control->set_pos_target_z_from_climb_rate_cm(_desired_v_z);
+
+        // Update controllers
+        pos_control->update_z_controller();
+
+        if ((now - _bail_time_start_ms)/1000.0f >= _bail_time) {
+            // Bail out timer complete.  Change flight mode. Do not revert back to auto. Prevent aircraft
+            // from continuing mission and potentially flying further away after a power failure.
+            if (copter.prev_control_mode == Mode::Number::AUTO) {
+                set_mode(Mode::Number::ALT_HOLD, ModeReason::AUTOROTATION_BAILOUT);
+            } else {
+                set_mode(copter.prev_control_mode, ModeReason::AUTOROTATION_BAILOUT);
+            }
+        }
+
+        break;
         }
     }
 
+
     switch (nav_pos_switch) {
 
         case Navigation_Decision::USER_CONTROL_STABILISED:
@@ -225,7 +282,7 @@ void ModeAutorotate::run()
             get_pilot_desired_lean_angles(pilot_roll, pilot_pitch, copter.aparm.angle_max, copter.aparm.angle_max);
 
             // Get pilot's desired yaw rate
-            float pilot_yaw_rate = get_pilot_desired_yaw_rate();
+            float pilot_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
 
             // Pitch target is calculated in autorotation phase switch above
             attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pilot_roll, _pitch_target, pilot_yaw_rate);

ArduCopter/mode_autotune.cpp

@@ -4,7 +4,7 @@
   autotune mode is a wrapper around the AC_AutoTune library
  */
 
-#if AUTOTUNE_ENABLED
+#if AUTOTUNE_ENABLED == ENABLED
 
 bool AutoTune::init()
 {
@@ -38,20 +38,30 @@ void AutoTune::run()
     // apply SIMPLE mode transform to pilot inputs
     copter.update_simple_mode();
 
-    // disarm when the landing detector says we've landed and spool state is ground idle
-    if (copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
-        copter.arming.disarm(AP_Arming::Method::LANDED);
-    }
-
-    // if not armed set throttle to zero and exit immediately
+    // reset target lean angles and heading while landed
     if (copter.ap.land_complete) {
-        copter.flightmode->make_safe_ground_handling();
-        return;
+        // we are landed, shut down
+        float target_climb_rate = get_pilot_desired_climb_rate_cms();
+
+        // set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle)
+        if (target_climb_rate < 0.0f) {
+            copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
+        } else {
+            copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
+        }
+        copter.attitude_control->reset_rate_controller_I_terms_smoothly();
+        copter.attitude_control->reset_yaw_target_and_rate();
+
+        float target_roll, target_pitch, target_yaw_rate;
+        get_pilot_desired_rp_yrate_cd(target_roll, target_pitch, target_yaw_rate);
+
+        copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
+        copter.pos_control->relax_z_controller(0.0f);
+        copter.pos_control->update_z_controller();
+    } else {
+        // run autotune mode
+        AC_AutoTune::run();
     }
-
-    // run autotune mode
-    AC_AutoTune::run();
-
 }
 
 
@@ -75,7 +85,7 @@ void AutoTune::get_pilot_desired_rp_yrate_cd(float &des_roll_cd, float &des_pitc
 {
     copter.mode_autotune.get_pilot_desired_lean_angles(des_roll_cd, des_pitch_cd, copter.aparm.angle_max,
                                                        copter.attitude_control->get_althold_lean_angle_max_cd());
-    yaw_rate_cds = copter.mode_autotune.get_pilot_desired_yaw_rate();
+    yaw_rate_cds = copter.mode_autotune.get_pilot_desired_yaw_rate(copter.channel_yaw->norm_input_dz());
 }
 
 /*
@@ -118,9 +128,19 @@ void ModeAutoTune::run()
     autotune.run();
 }
 
+void ModeAutoTune::save_tuning_gains()
+{
+    autotune.save_tuning_gains();
+}
+
 void ModeAutoTune::exit()
 {
     autotune.stop();
 }
 
-#endif  // AUTOTUNE_ENABLED
+void ModeAutoTune::reset()
+{
+    autotune.reset();
+}
+
+#endif  // AUTOTUNE_ENABLED == ENABLED

ArduCopter/mode_brake.cpp

@@ -1,6 +1,6 @@
 #include "Copter.h"
 
-#if MODE_BRAKE_ENABLED
+#if MODE_BRAKE_ENABLED == ENABLED
 
 /*
  * Init and run calls for brake flight mode

ArduCopter/mode_circle.cpp

@@ -1,7 +1,7 @@
 #include "Copter.h"
 #include <AP_Mount/AP_Mount.h>
 
-#if MODE_CIRCLE_ENABLED
+#if MODE_CIRCLE_ENABLED == ENABLED
 
 /*
  * Init and run calls for circle flight mode
@@ -22,16 +22,17 @@ bool ModeCircle::init(bool ignore_checks)
     copter.circle_nav->init();
 
 #if HAL_MOUNT_ENABLED
+    AP_Mount *s = AP_Mount::get_singleton();
+
     // Check if the CIRCLE_OPTIONS parameter have roi_at_center
     if (copter.circle_nav->roi_at_center()) {
         const Vector3p &pos { copter.circle_nav->get_center() };
         Location circle_center;
-        if (!AP::ahrs().get_location_from_origin_offset_NED(circle_center, pos * 0.01)) {
+        if (!AP::ahrs().get_location_from_origin_offset(circle_center, pos * 0.01)) {
             return false;
         }
         // point at the ground:
         circle_center.set_alt_cm(0, Location::AltFrame::ABOVE_TERRAIN);
-        AP_Mount *s = AP_Mount::get_singleton();
         s->set_roi_target(circle_center);
     }
 #endif
@@ -68,7 +69,7 @@ void ModeCircle::run()
         }
 
         // update the orbicular rate target based on pilot roll stick inputs
-        // skip if using transmitter based tuning knob for circle rate
+        // skip if using CH6 tuning knob for circle rate
         if (g.radio_tuning != TUNING_CIRCLE_RATE) {
             const float roll_stick = channel_roll->norm_input_dz();         // roll stick normalized -1 to 1
 
@@ -114,10 +115,8 @@ void ModeCircle::run()
     // set motors to full range
     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
-#if AP_RANGEFINDER_ENABLED
     // update the vertical offset based on the surface measurement
     copter.surface_tracking.update_surface_offset();
-#endif
 
     copter.failsafe_terrain_set_status(copter.circle_nav->update(target_climb_rate));
     pos_control->update_z_controller();

ArduCopter/mode_drift.cpp

@@ -1,6 +1,6 @@
 #include "Copter.h"
 
-#if MODE_DRIFT_ENABLED
+#if MODE_DRIFT_ENABLED == ENABLED
 
 /*
  * Init and run calls for drift flight mode

ArduCopter/mode_flip.cpp

@@ -1,6 +1,6 @@
 #include "Copter.h"
 
-#if MODE_FLIP_ENABLED
+#if MODE_FLIP_ENABLED == ENABLED
 
 /*
  * Init and run calls for flip flight mode

ArduCopter/mode_flowhold.cpp

@@ -1,7 +1,7 @@
 #include "Copter.h"
 #include <utility>
 
-#if MODE_FLOWHOLD_ENABLED
+#if MODE_FLOWHOLD_ENABLED == ENABLED
 
 /*
   implement FLOWHOLD mode, for position hold using optical flow
@@ -240,7 +240,7 @@ void ModeFlowHold::run()
 
     // check for filter change
     if (!is_equal(flow_filter.get_cutoff_freq(), flow_filter_hz.get())) {
-        flow_filter.set_cutoff_frequency(copter.scheduler.get_loop_rate_hz(), flow_filter_hz.get());
+        flow_filter.set_cutoff_frequency(flow_filter_hz.get());
     }
 
     // get pilot desired climb rate
@@ -248,7 +248,7 @@ void ModeFlowHold::run()
     target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), copter.g.pilot_speed_up);
 
     // get pilot's desired yaw rate
-    float target_yaw_rate = get_pilot_desired_yaw_rate();
+    float target_yaw_rate = get_pilot_desired_yaw_rate(copter.channel_yaw->norm_input_dz());
 
     // Flow Hold State Machine Determination
     AltHoldModeState flowhold_state = get_alt_hold_state(target_climb_rate);
@@ -263,7 +263,7 @@ void ModeFlowHold::run()
     // Flow Hold State Machine
     switch (flowhold_state) {
 
-    case AltHoldModeState::MotorStopped:
+    case AltHold_MotorStopped:
         copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
         copter.attitude_control->reset_rate_controller_I_terms();
         copter.attitude_control->reset_yaw_target_and_rate();
@@ -271,7 +271,7 @@ void ModeFlowHold::run()
         flow_pi_xy.reset_I();
         break;
 
-    case AltHoldModeState::Takeoff:
+    case AltHold_Takeoff:
         // set motors to full range
         copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
@@ -287,25 +287,23 @@ void ModeFlowHold::run()
         takeoff.do_pilot_takeoff(target_climb_rate);
         break;
 
-    case AltHoldModeState::Landed_Ground_Idle:
+    case AltHold_Landed_Ground_Idle:
         attitude_control->reset_yaw_target_and_rate();
         FALLTHROUGH;
 
-    case AltHoldModeState::Landed_Pre_Takeoff:
+    case AltHold_Landed_Pre_Takeoff:
         attitude_control->reset_rate_controller_I_terms_smoothly();
         pos_control->relax_z_controller(0.0f);   // forces throttle output to decay to zero
         break;
 
-    case AltHoldModeState::Flying:
+    case AltHold_Flying:
         copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
         // get avoidance adjusted climb rate
         target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
 
-#if AP_RANGEFINDER_ENABLED
         // update the vertical offset based on the surface measurement
         copter.surface_tracking.update_surface_offset();
-#endif
 
         // Send the commanded climb rate to the position controller
         pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);
@@ -334,7 +332,7 @@ void ModeFlowHold::run()
     bf_angles.x = constrain_float(bf_angles.x, -angle_max, angle_max);
     bf_angles.y = constrain_float(bf_angles.y, -angle_max, angle_max);
 
-#if AP_AVOIDANCE_ENABLED
+#if AC_AVOID_ENABLED == ENABLED
     // apply avoidance
     copter.avoid.adjust_roll_pitch(bf_angles.x, bf_angles.y, copter.aparm.angle_max);
 #endif

ArduCopter/mode_follow.cpp

@@ -1,6 +1,6 @@
 #include "Copter.h"
 
-#if MODE_FOLLOW_ENABLED
+#if MODE_FOLLOW_ENABLED == ENABLED
 
 /*
  * mode_follow.cpp - follow another mavlink-enabled vehicle by system id
@@ -10,7 +10,7 @@
  * TODO: "channel 7 option" to lock onto "pointed at" target
  * TODO: do better in terms of loitering around the moving point; may need a PID?  Maybe use loiter controller somehow?
  * TODO: extrapolate target vehicle position using its velocity and acceleration
- * TODO: ensure AP_AVOIDANCE_ENABLED is true because we rely on it velocity limiting functions
+ * TODO: ensure AC_AVOID_ENABLED is true because we rely on it velocity limiting functions
  */
 
 // initialise follow mode
@@ -172,4 +172,4 @@ bool ModeFollow::get_wp(Location &loc) const
     return true;
 }
 
-#endif // MODE_FOLLOW_ENABLED
+#endif // MODE_FOLLOW_ENABLED == ENABLED

ArduCopter/mode_guided.cpp

@@ -1,13 +1,13 @@
 #include "Copter.h"
 
-#if MODE_GUIDED_ENABLED
+#if MODE_GUIDED_ENABLED == ENABLED
 
 /*
  * Init and run calls for guided flight mode
  */
 
 static Vector3p guided_pos_target_cm;       // position target (used by posvel controller only)
-static bool guided_pos_terrain_alt;         // true if guided_pos_target_cm.z is an alt above terrain
+bool guided_pos_terrain_alt;                // true if guided_pos_target_cm.z is an alt above terrain
 static Vector3f guided_vel_target_cms;      // velocity target (used by pos_vel_accel controller and vel_accel controller)
 static Vector3f guided_accel_target_cmss;   // acceleration target (used by pos_vel_accel controller vel_accel controller and accel controller)
 static uint32_t update_time_ms;             // system time of last target update to pos_vel_accel, vel_accel or accel controller
@@ -15,7 +15,7 @@ static uint32_t update_time_ms;             // system time of last target update
 struct {
     uint32_t update_time_ms;
     Quaternion attitude_quat;
-    Vector3f ang_vel_body;
+    Vector3f ang_vel;
     float yaw_rate_cds;
     float climb_rate_cms;   // climb rate in cms.  Used if use_thrust is false
     float thrust;           // thrust from -1 to 1.  Used if use_thrust is true
@@ -30,15 +30,7 @@ struct Guided_Limit {
     float horiz_max_cm; // horizontal position limit in cm from where guided mode was initiated (0 = no limit)
     uint32_t start_time;// system time in milliseconds that control was handed to the external computer
     Vector3f start_pos; // start position as a distance from home in cm.  used for checking horiz_max limit
-} static guided_limit;
-
-// controls which controller is run (pos or vel):
-ModeGuided::SubMode ModeGuided::guided_mode = SubMode::TakeOff;
-bool ModeGuided::send_notification;     // used to send one time notification to ground station
-bool ModeGuided::takeoff_complete;      // true once takeoff has completed (used to trigger retracting of landing gear)
-
-// guided mode is paused or not
-bool ModeGuided::_paused;
+} guided_limit;
 
 // init - initialise guided controller
 bool ModeGuided::init(bool ignore_checks)
@@ -105,12 +97,6 @@ void ModeGuided::run()
     }
  }
 
-// returns true if the Guided-mode-option is set (see GUID_OPTIONS)
-bool ModeGuided::option_is_enabled(Option option) const
-{
-    return (copter.g2.guided_options.get() & (uint32_t)option) != 0;
-}
-
 bool ModeGuided::allows_arming(AP_Arming::Method method) const
 {
     // always allow arming from the ground station or scripting
@@ -119,13 +105,13 @@ bool ModeGuided::allows_arming(AP_Arming::Method method) const
     }
 
     // optionally allow arming from the transmitter
-    return option_is_enabled(Option::AllowArmingFromTX);
+    return (copter.g2.guided_options & (uint32_t)Options::AllowArmingFromTX) != 0;
 };
 
-#if WEATHERVANE_ENABLED
+#if WEATHERVANE_ENABLED == ENABLED
 bool ModeGuided::allows_weathervaning() const
 {
-    return option_is_enabled(Option::AllowWeatherVaning);
+    return (copter.g2.guided_options.get() & (uint32_t)Options::AllowWeatherVaning) != 0;
 }
 #endif
 
@@ -136,7 +122,6 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
     // calculate target altitude and frame (either alt-above-ekf-origin or alt-above-terrain)
     int32_t alt_target_cm;
     bool alt_target_terrain = false;
-#if AP_RANGEFINDER_ENABLED
     if (wp_nav->rangefinder_used_and_healthy() &&
         wp_nav->get_terrain_source() == AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER &&
         takeoff_alt_cm < copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)) {
@@ -147,9 +132,7 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
         // provide target altitude as alt-above-terrain
         alt_target_cm = takeoff_alt_cm;
         alt_target_terrain = true;
-    } else
-#endif
-    {
+    } else {
         // interpret altitude as alt-above-home
         Location target_loc = copter.current_loc;
         target_loc.set_alt_cm(takeoff_alt_cm, Location::AltFrame::ABOVE_HOME);
@@ -254,10 +237,10 @@ void ModeGuided::pos_control_start()
     pva_control_start();
 }
 
-// initialise guided mode's acceleration controller
+// initialise guided mode's velocity controller
 void ModeGuided::accel_control_start()
 {
-    // set guided_mode to acceleration controller
+    // set guided_mode to velocity controller
     guided_mode = SubMode::Accel;
 
     // initialise position controller
@@ -267,7 +250,7 @@ void ModeGuided::accel_control_start()
 // initialise guided mode's velocity and acceleration controller
 void ModeGuided::velaccel_control_start()
 {
-    // set guided_mode to velocity and acceleration controller
+    // set guided_mode to velocity controller
     guided_mode = SubMode::VelAccel;
 
     // initialise position controller
@@ -277,7 +260,7 @@ void ModeGuided::velaccel_control_start()
 // initialise guided mode's position, velocity and acceleration controller
 void ModeGuided::posvelaccel_control_start()
 {
-    // set guided_mode to position, velocity and acceleration controller
+    // set guided_mode to velocity controller
     guided_mode = SubMode::PosVelAccel;
 
     // initialise position controller
@@ -330,11 +313,14 @@ void ModeGuided::angle_control_start()
 
     // initialise targets
     guided_angle_state.update_time_ms = millis();
-    guided_angle_state.attitude_quat.from_euler(Vector3f(0.0, 0.0, attitude_control->get_att_target_euler_rad().z));
-    guided_angle_state.ang_vel_body.zero();
+    guided_angle_state.attitude_quat.initialise();
+    guided_angle_state.ang_vel.zero();
     guided_angle_state.climb_rate_cms = 0.0f;
     guided_angle_state.yaw_rate_cds = 0.0f;
     guided_angle_state.use_yaw_rate = false;
+
+    // pilot always controls yaw
+    auto_yaw.set_mode(AutoYaw::Mode::HOLD);
 }
 
 // set_destination - sets guided mode's target destination
@@ -391,10 +377,10 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa
         // convert origin to alt-above-terrain if necessary
         if (!guided_pos_terrain_alt) {
             // new destination is alt-above-terrain, previous destination was alt-above-ekf-origin
-            pos_control->init_pos_terrain_cm(origin_terr_offset);
+            pos_control->set_pos_offset_z_cm(origin_terr_offset);
         }
     } else {
-        pos_control->init_pos_terrain_cm(0.0);
+        pos_control->set_pos_offset_z_cm(0.0);
     }
 
     // set yaw state
@@ -425,14 +411,11 @@ bool ModeGuided::get_wp(Location& destination) const
     case SubMode::Pos:
         destination = Location(guided_pos_target_cm.tofloat(), guided_pos_terrain_alt ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
         return true;
-    case SubMode::Angle:
-    case SubMode::TakeOff:
-    case SubMode::Accel:
-    case SubMode::VelAccel:
-    case SubMode::PosVelAccel:
-        break;
+    default:
+        return false;
     }
 
+    // should never get here but just in case
     return false;
 }
 
@@ -476,13 +459,6 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
         return true;
     }
 
-    // set position target and zero velocity and acceleration
-    Vector3f pos_target_f;
-    bool terrain_alt;
-    if (!wp_nav->get_vector_NEU(dest_loc, pos_target_f, terrain_alt)) {
-        return false;
-    }
-
     // if configured to use position controller for position control
     // ensure we are in position control mode
     if (guided_mode != SubMode::Pos) {
@@ -492,6 +468,13 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
     // set yaw state
     set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
 
+    // set position target and zero velocity and acceleration
+    Vector3f pos_target_f;
+    bool terrain_alt;
+    if (!wp_nav->get_vector_NEU(dest_loc, pos_target_f, terrain_alt)) {
+        return false;
+    }
+
     // initialise terrain following if needed
     if (terrain_alt) {
         // get current alt above terrain
@@ -504,10 +487,10 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
         // convert origin to alt-above-terrain if necessary
         if (!guided_pos_terrain_alt) {
             // new destination is alt-above-terrain, previous destination was alt-above-ekf-origin
-            pos_control->init_pos_terrain_cm(origin_terr_offset);
+            pos_control->set_pos_offset_z_cm(origin_terr_offset);
         }
     } else {
-        pos_control->init_pos_terrain_cm(0.0);
+        pos_control->set_pos_offset_z_cm(0.0);
     }
 
     guided_pos_target_cm = pos_target_f.topostype();
@@ -529,7 +512,7 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
 // set_velaccel - sets guided mode's target velocity and acceleration
 void ModeGuided::set_accel(const Vector3f& acceleration, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request)
 {
-    // check we are in acceleration control mode
+    // check we are in velocity control mode
     if (guided_mode != SubMode::Accel) {
         accel_control_start();
     }
@@ -561,7 +544,7 @@ void ModeGuided::set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_
 // set_velaccel - sets guided mode's target velocity and acceleration
 void ModeGuided::set_velaccel(const Vector3f& velocity, const Vector3f& acceleration, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request)
 {
-    // check we are in velocity and acceleration control mode
+    // check we are in velocity control mode
     if (guided_mode != SubMode::VelAccel) {
         velaccel_control_start();
     }
@@ -603,7 +586,7 @@ bool ModeGuided::set_destination_posvelaccel(const Vector3f& destination, const
     }
 #endif
 
-    // check we are in position, velocity and acceleration control mode
+    // check we are in velocity control mode
     if (guided_mode != SubMode::PosVelAccel) {
         posvelaccel_control_start();
     }
@@ -627,46 +610,43 @@ bool ModeGuided::set_destination_posvelaccel(const Vector3f& destination, const
 // returns true if GUIDED_OPTIONS param suggests SET_ATTITUDE_TARGET's "thrust" field should be interpreted as thrust instead of climb rate
 bool ModeGuided::set_attitude_target_provides_thrust() const
 {
-    return option_is_enabled(Option::SetAttitudeTarget_ThrustAsThrust);
+    return ((copter.g2.guided_options.get() & uint32_t(Options::SetAttitudeTarget_ThrustAsThrust)) != 0);
 }
 
 // returns true if GUIDED_OPTIONS param specifies position should be controlled (when velocity and/or acceleration control is active)
 bool ModeGuided::stabilizing_pos_xy() const
 {
-    return !option_is_enabled(Option::DoNotStabilizePositionXY);
+    return !((copter.g2.guided_options.get() & uint32_t(Options::DoNotStabilizePositionXY)) != 0);
 }
 
 // returns true if GUIDED_OPTIONS param specifies velocity should  be controlled (when acceleration control is active)
 bool ModeGuided::stabilizing_vel_xy() const
 {
-    return !option_is_enabled(Option::DoNotStabilizeVelocityXY);
+    return !((copter.g2.guided_options.get() & uint32_t(Options::DoNotStabilizeVelocityXY)) != 0);
 }
 
 // returns true if GUIDED_OPTIONS param specifies waypoint navigation should be used for position control (allow path planning to be used but updates must be slower)
 bool ModeGuided::use_wpnav_for_position_control() const
 {
-    return option_is_enabled(Option::WPNavUsedForPosControl);
+    return ((copter.g2.guided_options.get() & uint32_t(Options::WPNavUsedForPosControl)) != 0);
 }
 
 // Sets guided's angular target submode: Using a rotation quaternion, angular velocity, and climbrate or thrust (depends on user option)
-// attitude_quat: IF zero: ang_vel_body (body frame angular velocity) must be provided even if all zeroes
-//                IF non-zero: attitude_control is performed using both the attitude quaternion and body frame angular velocity
-// ang_vel_body: body frame angular velocity (rad/s)
+// attitude_quat: IF zero: ang_vel (angular velocity) must be provided even if all zeroes
+//                IF non-zero: attitude_control is performed using both the attitude quaternion and angular velocity
+// ang_vel: angular velocity (rad/s)
 // climb_rate_cms_or_thrust: represents either the climb_rate (cm/s) or thrust scaled from [0, 1], unitless
 // use_thrust: IF true: climb_rate_cms_or_thrust represents thrust
 //             IF false: climb_rate_cms_or_thrust represents climb_rate (cm/s)
-void ModeGuided::set_angle(const Quaternion &attitude_quat, const Vector3f &ang_vel_body, float climb_rate_cms_or_thrust, bool use_thrust)
+void ModeGuided::set_angle(const Quaternion &attitude_quat, const Vector3f &ang_vel, float climb_rate_cms_or_thrust, bool use_thrust)
 {
     // check we are in velocity control mode
     if (guided_mode != SubMode::Angle) {
         angle_control_start();
-    } else if (!use_thrust && guided_angle_state.use_thrust) {
-        // Already angle control but changing from thrust to climb rate
-        pos_control->init_z_controller();
     }
 
     guided_angle_state.attitude_quat = attitude_quat;
-    guided_angle_state.ang_vel_body = ang_vel_body;
+    guided_angle_state.ang_vel = ang_vel;
 
     guided_angle_state.use_thrust = use_thrust;
     if (use_thrust) {
@@ -685,7 +665,7 @@ void ModeGuided::set_angle(const Quaternion &attitude_quat, const Vector3f &ang_
 
 #if HAL_LOGGING_ENABLED
     // log target
-    copter.Log_Write_Guided_Attitude_Target(guided_mode, roll_rad, pitch_rad, yaw_rad, ang_vel_body, guided_angle_state.thrust, guided_angle_state.climb_rate_cms * 0.01);
+    copter.Log_Write_Guided_Attitude_Target(guided_mode, roll_rad, pitch_rad, yaw_rad, ang_vel, guided_angle_state.thrust, guided_angle_state.climb_rate_cms * 0.01);
 #endif
 }
 
@@ -696,9 +676,6 @@ void ModeGuided::takeoff_run()
     auto_takeoff.run();
     if (auto_takeoff.complete && !takeoff_complete) {
         takeoff_complete = true;
-#if AP_FENCE_ENABLED
-        copter.fence.auto_enable_fence_after_takeoff();
-#endif
 #if AP_LANDINGGEAR_ENABLED
         // optionally retract landing gear
         copter.landinggear.retract_after_takeoff();
@@ -823,7 +800,7 @@ void ModeGuided::velaccel_control_run()
     }
 
     bool do_avoid = false;
-#if AP_AVOIDANCE_ENABLED
+#if AC_AVOID_ENABLED
     // limit the velocity for obstacle/fence avoidance
     copter.avoid.adjust_velocity(guided_vel_target_cms, pos_control->get_pos_xy_p().kP(), pos_control->get_max_accel_xy_cmss(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z_cmss(), G_Dt);
     do_avoid = copter.avoid.limits_active();
@@ -833,7 +810,8 @@ void ModeGuided::velaccel_control_run()
 
     if (!stabilizing_vel_xy() && !do_avoid) {
         // set the current commanded xy vel to the desired vel
-        guided_vel_target_cms.xy() = pos_control->get_vel_desired_cms().xy();
+        guided_vel_target_cms.x = pos_control->get_vel_desired_cms().x;
+        guided_vel_target_cms.y = pos_control->get_vel_desired_cms().y;
     }
     pos_control->input_vel_accel_xy(guided_vel_target_cms.xy(), guided_accel_target_cmss.xy(), false);
     if (!stabilizing_vel_xy() && !do_avoid) {
@@ -910,11 +888,14 @@ void ModeGuided::posvelaccel_control_run()
     // send position and velocity targets to position controller
     if (!stabilizing_vel_xy()) {
         // set the current commanded xy pos to the target pos and xy vel to the desired vel
-        guided_pos_target_cm.xy() = pos_control->get_pos_desired_cm().xy();
-        guided_vel_target_cms.xy() = pos_control->get_vel_desired_cms().xy();
+        guided_pos_target_cm.x = pos_control->get_pos_target_cm().x;
+        guided_pos_target_cm.y = pos_control->get_pos_target_cm().y;
+        guided_vel_target_cms.x = pos_control->get_vel_desired_cms().x;
+        guided_vel_target_cms.y = pos_control->get_vel_desired_cms().y;
     } else if (!stabilizing_pos_xy()) {
         // set the current commanded xy pos to the target pos
-        guided_pos_target_cm.xy() = pos_control->get_pos_desired_cm().xy();
+        guided_pos_target_cm.x = pos_control->get_pos_target_cm().x;
+        guided_pos_target_cm.y = pos_control->get_pos_target_cm().y;
     }
     pos_control->input_pos_vel_accel_xy(guided_pos_target_cm.xy(), guided_vel_target_cms.xy(), guided_accel_target_cmss.xy(), false);
     if (!stabilizing_vel_xy()) {
@@ -958,8 +939,8 @@ void ModeGuided::angle_control_run()
     // check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds
     uint32_t tnow = millis();
     if (tnow - guided_angle_state.update_time_ms > get_timeout_ms()) {
-        guided_angle_state.attitude_quat.from_euler(Vector3f(0.0, 0.0, attitude_control->get_att_target_euler_rad().z));
-        guided_angle_state.ang_vel_body.zero();
+        guided_angle_state.attitude_quat.initialise();
+        guided_angle_state.ang_vel.zero();
         climb_rate_cms = 0.0f;
         if (guided_angle_state.use_thrust) {
             // initialise vertical velocity controller
@@ -982,8 +963,8 @@ void ModeGuided::angle_control_run()
     }
 
     // TODO: use get_alt_hold_state
-    // landed with positive desired climb rate or thrust, takeoff
-    if (copter.ap.land_complete && positive_thrust_or_climbrate) {
+    // landed with positive desired climb rate, takeoff
+    if (copter.ap.land_complete && (guided_angle_state.climb_rate_cms > 0.0f)) {
         zero_throttle_and_relax_ac();
         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
         if (motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
@@ -998,9 +979,9 @@ void ModeGuided::angle_control_run()
 
     // call attitude controller
     if (guided_angle_state.attitude_quat.is_zero()) {
-        attitude_control->input_rate_bf_roll_pitch_yaw(ToDeg(guided_angle_state.ang_vel_body.x) * 100.0f, ToDeg(guided_angle_state.ang_vel_body.y) * 100.0f, ToDeg(guided_angle_state.ang_vel_body.z) * 100.0f);
+        attitude_control->input_rate_bf_roll_pitch_yaw(ToDeg(guided_angle_state.ang_vel.x) * 100.0f, ToDeg(guided_angle_state.ang_vel.y) * 100.0f, ToDeg(guided_angle_state.ang_vel.z) * 100.0f);
     } else {
-        attitude_control->input_quaternion(guided_angle_state.attitude_quat, guided_angle_state.ang_vel_body);
+        attitude_control->input_quaternion(guided_angle_state.attitude_quat, guided_angle_state.ang_vel);
     }
 
     // call position controller
@@ -1031,7 +1012,7 @@ void ModeGuided::set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, fl
 // returns true if pilot's yaw input should be used to adjust vehicle's heading
 bool ModeGuided::use_pilot_yaw(void) const
 {
-    return !option_is_enabled(Option::IgnorePilotYaw);
+    return (copter.g2.guided_options.get() & uint32_t(Options::IgnorePilotYaw)) == 0;
 }
 
 // Guided Limit code

ArduCopter/mode_guided_custom.cpp

@@ -1,23 +0,0 @@
-#include "Copter.h"
-
-#if MODE_GUIDED_ENABLED && AP_SCRIPTING_ENABLED
-// constructor registers custom number and names
-ModeGuidedCustom::ModeGuidedCustom(const Number _number, const char* _full_name, const char* _short_name):
-    number(_number),
-    full_name(_full_name),
-    short_name(_short_name)
-{
-}
-
-bool ModeGuidedCustom::init(bool ignore_checks)
-{
-    // Stript can block entry
-    if (!state.allow_entry) {
-        return false;
-    }
-
-    // Guided entry checks must also pass
-    return ModeGuided::init(ignore_checks);
-}
-
-#endif

ArduCopter/mode_guided_nogps.cpp

@@ -1,6 +1,6 @@
 #include "Copter.h"
 
-#if MODE_GUIDED_NOGPS_ENABLED
+#if MODE_GUIDED_NOGPS_ENABLED == ENABLED
 
 /*
  * Init and run calls for guided_nogps flight mode